root/drivers/net/ethernet/ti/cpsw.c

/* [<][>][^][v][top][bottom][index][help] */

DEFINITIONS

This source file includes following definitions.
  1. cpsw_set_promiscious
  2. cpsw_set_mc
  3. cpsw_update_vlan_mc
  4. cpsw_add_mc_addr
  5. cpsw_del_mc_addr
  6. cpsw_purge_vlan_mc
  7. cpsw_purge_all_mc
  8. cpsw_ndo_set_rx_mode
  9. cpsw_intr_enable
  10. cpsw_intr_disable
  11. cpsw_is_xdpf_handle
  12. cpsw_xdpf_to_handle
  13. cpsw_handle_to_xdpf
  14. cpsw_tx_handler
  15. cpsw_rx_vlan_encap
  16. cpsw_xdp_tx_frame
  17. cpsw_run_xdp
  18. cpsw_rxbuf_total_len
  19. cpsw_create_page_pool
  20. cpsw_ndev_create_xdp_rxq
  21. cpsw_ndev_destroy_xdp_rxq
  22. cpsw_create_rx_pool
  23. cpsw_destroy_xdp_rxqs
  24. cpsw_create_xdp_rxqs
  25. cpsw_rx_handler
  26. cpsw_split_res
  27. cpsw_tx_interrupt
  28. cpsw_rx_interrupt
  29. cpsw_tx_mq_poll
  30. cpsw_tx_poll
  31. cpsw_rx_mq_poll
  32. cpsw_rx_poll
  33. soft_reset
  34. cpsw_set_slave_mac
  35. cpsw_shp_is_off
  36. cpsw_fifo_shp_on
  37. _cpsw_adjust_link
  38. cpsw_get_common_speed
  39. cpsw_need_resplit
  40. cpsw_adjust_link
  41. cpsw_add_dual_emac_def_ale_entries
  42. cpsw_slave_open
  43. cpsw_add_default_vlan
  44. cpsw_init_host_port
  45. cpsw_fill_rx_channels
  46. cpsw_slave_stop
  47. cpsw_tc_to_fifo
  48. cpsw_set_fifo_bw
  49. cpsw_set_fifo_rlimit
  50. cpsw_set_cbs
  51. cpsw_cbs_resume
  52. cpsw_mqprio_resume
  53. cpsw_restore_vlans
  54. cpsw_restore
  55. cpsw_ndo_open
  56. cpsw_ndo_stop
  57. cpsw_ndo_start_xmit
  58. cpsw_hwtstamp_v1
  59. cpsw_hwtstamp_v2
  60. cpsw_hwtstamp_set
  61. cpsw_hwtstamp_get
  62. cpsw_hwtstamp_get
  63. cpsw_hwtstamp_set
  64. cpsw_ndo_ioctl
  65. cpsw_ndo_tx_timeout
  66. cpsw_ndo_set_mac_address
  67. cpsw_add_vlan_ale_entry
  68. cpsw_ndo_vlan_rx_add_vid
  69. cpsw_ndo_vlan_rx_kill_vid
  70. cpsw_ndo_set_tx_maxrate
  71. cpsw_set_mqprio
  72. cpsw_ndo_setup_tc
  73. cpsw_xdp_prog_setup
  74. cpsw_ndo_bpf
  75. cpsw_ndo_xdp_xmit
  76. cpsw_ndo_poll_controller
  77. cpsw_get_drvinfo
  78. cpsw_set_pauseparam
  79. cpsw_set_channels
  80. cpsw_probe_dt
  81. cpsw_remove_dt
  82. cpsw_probe_dual_emac
  83. cpsw_probe
  84. cpsw_remove
  85. cpsw_suspend
  86. cpsw_resume

   1 // SPDX-License-Identifier: GPL-2.0
   2 /*
   3  * Texas Instruments Ethernet Switch Driver
   4  *
   5  * Copyright (C) 2012 Texas Instruments
   6  *
   7  */
   8 
   9 #include <linux/kernel.h>
  10 #include <linux/io.h>
  11 #include <linux/clk.h>
  12 #include <linux/timer.h>
  13 #include <linux/module.h>
  14 #include <linux/platform_device.h>
  15 #include <linux/irqreturn.h>
  16 #include <linux/interrupt.h>
  17 #include <linux/if_ether.h>
  18 #include <linux/etherdevice.h>
  19 #include <linux/netdevice.h>
  20 #include <linux/net_tstamp.h>
  21 #include <linux/phy.h>
  22 #include <linux/phy/phy.h>
  23 #include <linux/workqueue.h>
  24 #include <linux/delay.h>
  25 #include <linux/pm_runtime.h>
  26 #include <linux/gpio/consumer.h>
  27 #include <linux/of.h>
  28 #include <linux/of_mdio.h>
  29 #include <linux/of_net.h>
  30 #include <linux/of_device.h>
  31 #include <linux/if_vlan.h>
  32 #include <linux/kmemleak.h>
  33 #include <linux/sys_soc.h>
  34 #include <net/page_pool.h>
  35 #include <linux/bpf.h>
  36 #include <linux/bpf_trace.h>
  37 #include <linux/filter.h>
  38 
  39 #include <linux/pinctrl/consumer.h>
  40 #include <net/pkt_cls.h>
  41 
  42 #include "cpsw.h"
  43 #include "cpsw_ale.h"
  44 #include "cpsw_priv.h"
  45 #include "cpsw_sl.h"
  46 #include "cpts.h"
  47 #include "davinci_cpdma.h"
  48 
  49 #include <net/pkt_sched.h>
  50 
  51 static int debug_level;
  52 module_param(debug_level, int, 0);
  53 MODULE_PARM_DESC(debug_level, "cpsw debug level (NETIF_MSG bits)");
  54 
  55 static int ale_ageout = 10;
  56 module_param(ale_ageout, int, 0);
  57 MODULE_PARM_DESC(ale_ageout, "cpsw ale ageout interval (seconds)");
  58 
  59 static int rx_packet_max = CPSW_MAX_PACKET_SIZE;
  60 module_param(rx_packet_max, int, 0);
  61 MODULE_PARM_DESC(rx_packet_max, "maximum receive packet size (bytes)");
  62 
  63 static int descs_pool_size = CPSW_CPDMA_DESCS_POOL_SIZE_DEFAULT;
  64 module_param(descs_pool_size, int, 0444);
  65 MODULE_PARM_DESC(descs_pool_size, "Number of CPDMA CPPI descriptors in pool");
  66 
  67 /* The buf includes headroom compatible with both skb and xdpf */
  68 #define CPSW_HEADROOM_NA (max(XDP_PACKET_HEADROOM, NET_SKB_PAD) + NET_IP_ALIGN)
  69 #define CPSW_HEADROOM  ALIGN(CPSW_HEADROOM_NA, sizeof(long))
  70 
  71 #define for_each_slave(priv, func, arg...)                              \
  72         do {                                                            \
  73                 struct cpsw_slave *slave;                               \
  74                 struct cpsw_common *cpsw = (priv)->cpsw;                \
  75                 int n;                                                  \
  76                 if (cpsw->data.dual_emac)                               \
  77                         (func)((cpsw)->slaves + priv->emac_port, ##arg);\
  78                 else                                                    \
  79                         for (n = cpsw->data.slaves,                     \
  80                                         slave = cpsw->slaves;           \
  81                                         n; n--)                         \
  82                                 (func)(slave++, ##arg);                 \
  83         } while (0)
  84 
  85 #define CPSW_XMETA_OFFSET       ALIGN(sizeof(struct xdp_frame), sizeof(long))
  86 
  87 #define CPSW_XDP_CONSUMED               1
  88 #define CPSW_XDP_PASS                   0
  89 
  90 static int cpsw_ndo_vlan_rx_add_vid(struct net_device *ndev,
  91                                     __be16 proto, u16 vid);
  92 
  93 static void cpsw_set_promiscious(struct net_device *ndev, bool enable)
  94 {
  95         struct cpsw_common *cpsw = ndev_to_cpsw(ndev);
  96         struct cpsw_ale *ale = cpsw->ale;
  97         int i;
  98 
  99         if (cpsw->data.dual_emac) {
 100                 bool flag = false;
 101 
 102                 /* Enabling promiscuous mode for one interface will be
 103                  * common for both the interface as the interface shares
 104                  * the same hardware resource.
 105                  */
 106                 for (i = 0; i < cpsw->data.slaves; i++)
 107                         if (cpsw->slaves[i].ndev->flags & IFF_PROMISC)
 108                                 flag = true;
 109 
 110                 if (!enable && flag) {
 111                         enable = true;
 112                         dev_err(&ndev->dev, "promiscuity not disabled as the other interface is still in promiscuity mode\n");
 113                 }
 114 
 115                 if (enable) {
 116                         /* Enable Bypass */
 117                         cpsw_ale_control_set(ale, 0, ALE_BYPASS, 1);
 118 
 119                         dev_dbg(&ndev->dev, "promiscuity enabled\n");
 120                 } else {
 121                         /* Disable Bypass */
 122                         cpsw_ale_control_set(ale, 0, ALE_BYPASS, 0);
 123                         dev_dbg(&ndev->dev, "promiscuity disabled\n");
 124                 }
 125         } else {
 126                 if (enable) {
 127                         unsigned long timeout = jiffies + HZ;
 128 
 129                         /* Disable Learn for all ports (host is port 0 and slaves are port 1 and up */
 130                         for (i = 0; i <= cpsw->data.slaves; i++) {
 131                                 cpsw_ale_control_set(ale, i,
 132                                                      ALE_PORT_NOLEARN, 1);
 133                                 cpsw_ale_control_set(ale, i,
 134                                                      ALE_PORT_NO_SA_UPDATE, 1);
 135                         }
 136 
 137                         /* Clear All Untouched entries */
 138                         cpsw_ale_control_set(ale, 0, ALE_AGEOUT, 1);
 139                         do {
 140                                 cpu_relax();
 141                                 if (cpsw_ale_control_get(ale, 0, ALE_AGEOUT))
 142                                         break;
 143                         } while (time_after(timeout, jiffies));
 144                         cpsw_ale_control_set(ale, 0, ALE_AGEOUT, 1);
 145 
 146                         /* Clear all mcast from ALE */
 147                         cpsw_ale_flush_multicast(ale, ALE_ALL_PORTS, -1);
 148                         __hw_addr_ref_unsync_dev(&ndev->mc, ndev, NULL);
 149 
 150                         /* Flood All Unicast Packets to Host port */
 151                         cpsw_ale_control_set(ale, 0, ALE_P0_UNI_FLOOD, 1);
 152                         dev_dbg(&ndev->dev, "promiscuity enabled\n");
 153                 } else {
 154                         /* Don't Flood All Unicast Packets to Host port */
 155                         cpsw_ale_control_set(ale, 0, ALE_P0_UNI_FLOOD, 0);
 156 
 157                         /* Enable Learn for all ports (host is port 0 and slaves are port 1 and up */
 158                         for (i = 0; i <= cpsw->data.slaves; i++) {
 159                                 cpsw_ale_control_set(ale, i,
 160                                                      ALE_PORT_NOLEARN, 0);
 161                                 cpsw_ale_control_set(ale, i,
 162                                                      ALE_PORT_NO_SA_UPDATE, 0);
 163                         }
 164                         dev_dbg(&ndev->dev, "promiscuity disabled\n");
 165                 }
 166         }
 167 }
 168 
 169 /**
 170  * cpsw_set_mc - adds multicast entry to the table if it's not added or deletes
 171  * if it's not deleted
 172  * @ndev: device to sync
 173  * @addr: address to be added or deleted
 174  * @vid: vlan id, if vid < 0 set/unset address for real device
 175  * @add: add address if the flag is set or remove otherwise
 176  */
 177 static int cpsw_set_mc(struct net_device *ndev, const u8 *addr,
 178                        int vid, int add)
 179 {
 180         struct cpsw_priv *priv = netdev_priv(ndev);
 181         struct cpsw_common *cpsw = priv->cpsw;
 182         int mask, flags, ret;
 183 
 184         if (vid < 0) {
 185                 if (cpsw->data.dual_emac)
 186                         vid = cpsw->slaves[priv->emac_port].port_vlan;
 187                 else
 188                         vid = 0;
 189         }
 190 
 191         mask = cpsw->data.dual_emac ? ALE_PORT_HOST : ALE_ALL_PORTS;
 192         flags = vid ? ALE_VLAN : 0;
 193 
 194         if (add)
 195                 ret = cpsw_ale_add_mcast(cpsw->ale, addr, mask, flags, vid, 0);
 196         else
 197                 ret = cpsw_ale_del_mcast(cpsw->ale, addr, 0, flags, vid);
 198 
 199         return ret;
 200 }
 201 
 202 static int cpsw_update_vlan_mc(struct net_device *vdev, int vid, void *ctx)
 203 {
 204         struct addr_sync_ctx *sync_ctx = ctx;
 205         struct netdev_hw_addr *ha;
 206         int found = 0, ret = 0;
 207 
 208         if (!vdev || !(vdev->flags & IFF_UP))
 209                 return 0;
 210 
 211         /* vlan address is relevant if its sync_cnt != 0 */
 212         netdev_for_each_mc_addr(ha, vdev) {
 213                 if (ether_addr_equal(ha->addr, sync_ctx->addr)) {
 214                         found = ha->sync_cnt;
 215                         break;
 216                 }
 217         }
 218 
 219         if (found)
 220                 sync_ctx->consumed++;
 221 
 222         if (sync_ctx->flush) {
 223                 if (!found)
 224                         cpsw_set_mc(sync_ctx->ndev, sync_ctx->addr, vid, 0);
 225                 return 0;
 226         }
 227 
 228         if (found)
 229                 ret = cpsw_set_mc(sync_ctx->ndev, sync_ctx->addr, vid, 1);
 230 
 231         return ret;
 232 }
 233 
 234 static int cpsw_add_mc_addr(struct net_device *ndev, const u8 *addr, int num)
 235 {
 236         struct addr_sync_ctx sync_ctx;
 237         int ret;
 238 
 239         sync_ctx.consumed = 0;
 240         sync_ctx.addr = addr;
 241         sync_ctx.ndev = ndev;
 242         sync_ctx.flush = 0;
 243 
 244         ret = vlan_for_each(ndev, cpsw_update_vlan_mc, &sync_ctx);
 245         if (sync_ctx.consumed < num && !ret)
 246                 ret = cpsw_set_mc(ndev, addr, -1, 1);
 247 
 248         return ret;
 249 }
 250 
 251 static int cpsw_del_mc_addr(struct net_device *ndev, const u8 *addr, int num)
 252 {
 253         struct addr_sync_ctx sync_ctx;
 254 
 255         sync_ctx.consumed = 0;
 256         sync_ctx.addr = addr;
 257         sync_ctx.ndev = ndev;
 258         sync_ctx.flush = 1;
 259 
 260         vlan_for_each(ndev, cpsw_update_vlan_mc, &sync_ctx);
 261         if (sync_ctx.consumed == num)
 262                 cpsw_set_mc(ndev, addr, -1, 0);
 263 
 264         return 0;
 265 }
 266 
 267 static int cpsw_purge_vlan_mc(struct net_device *vdev, int vid, void *ctx)
 268 {
 269         struct addr_sync_ctx *sync_ctx = ctx;
 270         struct netdev_hw_addr *ha;
 271         int found = 0;
 272 
 273         if (!vdev || !(vdev->flags & IFF_UP))
 274                 return 0;
 275 
 276         /* vlan address is relevant if its sync_cnt != 0 */
 277         netdev_for_each_mc_addr(ha, vdev) {
 278                 if (ether_addr_equal(ha->addr, sync_ctx->addr)) {
 279                         found = ha->sync_cnt;
 280                         break;
 281                 }
 282         }
 283 
 284         if (!found)
 285                 return 0;
 286 
 287         sync_ctx->consumed++;
 288         cpsw_set_mc(sync_ctx->ndev, sync_ctx->addr, vid, 0);
 289         return 0;
 290 }
 291 
 292 static int cpsw_purge_all_mc(struct net_device *ndev, const u8 *addr, int num)
 293 {
 294         struct addr_sync_ctx sync_ctx;
 295 
 296         sync_ctx.addr = addr;
 297         sync_ctx.ndev = ndev;
 298         sync_ctx.consumed = 0;
 299 
 300         vlan_for_each(ndev, cpsw_purge_vlan_mc, &sync_ctx);
 301         if (sync_ctx.consumed < num)
 302                 cpsw_set_mc(ndev, addr, -1, 0);
 303 
 304         return 0;
 305 }
 306 
 307 static void cpsw_ndo_set_rx_mode(struct net_device *ndev)
 308 {
 309         struct cpsw_priv *priv = netdev_priv(ndev);
 310         struct cpsw_common *cpsw = priv->cpsw;
 311         int slave_port = -1;
 312 
 313         if (cpsw->data.dual_emac)
 314                 slave_port = priv->emac_port + 1;
 315 
 316         if (ndev->flags & IFF_PROMISC) {
 317                 /* Enable promiscuous mode */
 318                 cpsw_set_promiscious(ndev, true);
 319                 cpsw_ale_set_allmulti(cpsw->ale, IFF_ALLMULTI, slave_port);
 320                 return;
 321         } else {
 322                 /* Disable promiscuous mode */
 323                 cpsw_set_promiscious(ndev, false);
 324         }
 325 
 326         /* Restore allmulti on vlans if necessary */
 327         cpsw_ale_set_allmulti(cpsw->ale,
 328                               ndev->flags & IFF_ALLMULTI, slave_port);
 329 
 330         /* add/remove mcast address either for real netdev or for vlan */
 331         __hw_addr_ref_sync_dev(&ndev->mc, ndev, cpsw_add_mc_addr,
 332                                cpsw_del_mc_addr);
 333 }
 334 
 335 void cpsw_intr_enable(struct cpsw_common *cpsw)
 336 {
 337         writel_relaxed(0xFF, &cpsw->wr_regs->tx_en);
 338         writel_relaxed(0xFF, &cpsw->wr_regs->rx_en);
 339 
 340         cpdma_ctlr_int_ctrl(cpsw->dma, true);
 341         return;
 342 }
 343 
 344 void cpsw_intr_disable(struct cpsw_common *cpsw)
 345 {
 346         writel_relaxed(0, &cpsw->wr_regs->tx_en);
 347         writel_relaxed(0, &cpsw->wr_regs->rx_en);
 348 
 349         cpdma_ctlr_int_ctrl(cpsw->dma, false);
 350         return;
 351 }
 352 
 353 static int cpsw_is_xdpf_handle(void *handle)
 354 {
 355         return (unsigned long)handle & BIT(0);
 356 }
 357 
 358 static void *cpsw_xdpf_to_handle(struct xdp_frame *xdpf)
 359 {
 360         return (void *)((unsigned long)xdpf | BIT(0));
 361 }
 362 
 363 static struct xdp_frame *cpsw_handle_to_xdpf(void *handle)
 364 {
 365         return (struct xdp_frame *)((unsigned long)handle & ~BIT(0));
 366 }
 367 
 368 struct __aligned(sizeof(long)) cpsw_meta_xdp {
 369         struct net_device *ndev;
 370         int ch;
 371 };
 372 
 373 void cpsw_tx_handler(void *token, int len, int status)
 374 {
 375         struct cpsw_meta_xdp    *xmeta;
 376         struct xdp_frame        *xdpf;
 377         struct net_device       *ndev;
 378         struct netdev_queue     *txq;
 379         struct sk_buff          *skb;
 380         int                     ch;
 381 
 382         if (cpsw_is_xdpf_handle(token)) {
 383                 xdpf = cpsw_handle_to_xdpf(token);
 384                 xmeta = (void *)xdpf + CPSW_XMETA_OFFSET;
 385                 ndev = xmeta->ndev;
 386                 ch = xmeta->ch;
 387                 xdp_return_frame(xdpf);
 388         } else {
 389                 skb = token;
 390                 ndev = skb->dev;
 391                 ch = skb_get_queue_mapping(skb);
 392                 cpts_tx_timestamp(ndev_to_cpsw(ndev)->cpts, skb);
 393                 dev_kfree_skb_any(skb);
 394         }
 395 
 396         /* Check whether the queue is stopped due to stalled tx dma, if the
 397          * queue is stopped then start the queue as we have free desc for tx
 398          */
 399         txq = netdev_get_tx_queue(ndev, ch);
 400         if (unlikely(netif_tx_queue_stopped(txq)))
 401                 netif_tx_wake_queue(txq);
 402 
 403         ndev->stats.tx_packets++;
 404         ndev->stats.tx_bytes += len;
 405 }
 406 
 407 static void cpsw_rx_vlan_encap(struct sk_buff *skb)
 408 {
 409         struct cpsw_priv *priv = netdev_priv(skb->dev);
 410         struct cpsw_common *cpsw = priv->cpsw;
 411         u32 rx_vlan_encap_hdr = *((u32 *)skb->data);
 412         u16 vtag, vid, prio, pkt_type;
 413 
 414         /* Remove VLAN header encapsulation word */
 415         skb_pull(skb, CPSW_RX_VLAN_ENCAP_HDR_SIZE);
 416 
 417         pkt_type = (rx_vlan_encap_hdr >>
 418                     CPSW_RX_VLAN_ENCAP_HDR_PKT_TYPE_SHIFT) &
 419                     CPSW_RX_VLAN_ENCAP_HDR_PKT_TYPE_MSK;
 420         /* Ignore unknown & Priority-tagged packets*/
 421         if (pkt_type == CPSW_RX_VLAN_ENCAP_HDR_PKT_RESERV ||
 422             pkt_type == CPSW_RX_VLAN_ENCAP_HDR_PKT_PRIO_TAG)
 423                 return;
 424 
 425         vid = (rx_vlan_encap_hdr >>
 426                CPSW_RX_VLAN_ENCAP_HDR_VID_SHIFT) &
 427                VLAN_VID_MASK;
 428         /* Ignore vid 0 and pass packet as is */
 429         if (!vid)
 430                 return;
 431         /* Ignore default vlans in dual mac mode */
 432         if (cpsw->data.dual_emac &&
 433             vid == cpsw->slaves[priv->emac_port].port_vlan)
 434                 return;
 435 
 436         prio = (rx_vlan_encap_hdr >>
 437                 CPSW_RX_VLAN_ENCAP_HDR_PRIO_SHIFT) &
 438                 CPSW_RX_VLAN_ENCAP_HDR_PRIO_MSK;
 439 
 440         vtag = (prio << VLAN_PRIO_SHIFT) | vid;
 441         __vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), vtag);
 442 
 443         /* strip vlan tag for VLAN-tagged packet */
 444         if (pkt_type == CPSW_RX_VLAN_ENCAP_HDR_PKT_VLAN_TAG) {
 445                 memmove(skb->data + VLAN_HLEN, skb->data, 2 * ETH_ALEN);
 446                 skb_pull(skb, VLAN_HLEN);
 447         }
 448 }
 449 
 450 static int cpsw_xdp_tx_frame(struct cpsw_priv *priv, struct xdp_frame *xdpf,
 451                              struct page *page)
 452 {
 453         struct cpsw_common *cpsw = priv->cpsw;
 454         struct cpsw_meta_xdp *xmeta;
 455         struct cpdma_chan *txch;
 456         dma_addr_t dma;
 457         int ret, port;
 458 
 459         xmeta = (void *)xdpf + CPSW_XMETA_OFFSET;
 460         xmeta->ndev = priv->ndev;
 461         xmeta->ch = 0;
 462         txch = cpsw->txv[0].ch;
 463 
 464         port = priv->emac_port + cpsw->data.dual_emac;
 465         if (page) {
 466                 dma = page_pool_get_dma_addr(page);
 467                 dma += xdpf->headroom + sizeof(struct xdp_frame);
 468                 ret = cpdma_chan_submit_mapped(txch, cpsw_xdpf_to_handle(xdpf),
 469                                                dma, xdpf->len, port);
 470         } else {
 471                 if (sizeof(*xmeta) > xdpf->headroom) {
 472                         xdp_return_frame_rx_napi(xdpf);
 473                         return -EINVAL;
 474                 }
 475 
 476                 ret = cpdma_chan_submit(txch, cpsw_xdpf_to_handle(xdpf),
 477                                         xdpf->data, xdpf->len, port);
 478         }
 479 
 480         if (ret) {
 481                 priv->ndev->stats.tx_dropped++;
 482                 xdp_return_frame_rx_napi(xdpf);
 483         }
 484 
 485         return ret;
 486 }
 487 
 488 static int cpsw_run_xdp(struct cpsw_priv *priv, int ch, struct xdp_buff *xdp,
 489                         struct page *page)
 490 {
 491         struct cpsw_common *cpsw = priv->cpsw;
 492         struct net_device *ndev = priv->ndev;
 493         int ret = CPSW_XDP_CONSUMED;
 494         struct xdp_frame *xdpf;
 495         struct bpf_prog *prog;
 496         u32 act;
 497 
 498         rcu_read_lock();
 499 
 500         prog = READ_ONCE(priv->xdp_prog);
 501         if (!prog) {
 502                 ret = CPSW_XDP_PASS;
 503                 goto out;
 504         }
 505 
 506         act = bpf_prog_run_xdp(prog, xdp);
 507         switch (act) {
 508         case XDP_PASS:
 509                 ret = CPSW_XDP_PASS;
 510                 break;
 511         case XDP_TX:
 512                 xdpf = convert_to_xdp_frame(xdp);
 513                 if (unlikely(!xdpf))
 514                         goto drop;
 515 
 516                 cpsw_xdp_tx_frame(priv, xdpf, page);
 517                 break;
 518         case XDP_REDIRECT:
 519                 if (xdp_do_redirect(ndev, xdp, prog))
 520                         goto drop;
 521 
 522                 /*  Have to flush here, per packet, instead of doing it in bulk
 523                  *  at the end of the napi handler. The RX devices on this
 524                  *  particular hardware is sharing a common queue, so the
 525                  *  incoming device might change per packet.
 526                  */
 527                 xdp_do_flush_map();
 528                 break;
 529         default:
 530                 bpf_warn_invalid_xdp_action(act);
 531                 /* fall through */
 532         case XDP_ABORTED:
 533                 trace_xdp_exception(ndev, prog, act);
 534                 /* fall through -- handle aborts by dropping packet */
 535         case XDP_DROP:
 536                 goto drop;
 537         }
 538 out:
 539         rcu_read_unlock();
 540         return ret;
 541 drop:
 542         rcu_read_unlock();
 543         page_pool_recycle_direct(cpsw->page_pool[ch], page);
 544         return ret;
 545 }
 546 
 547 static unsigned int cpsw_rxbuf_total_len(unsigned int len)
 548 {
 549         len += CPSW_HEADROOM;
 550         len += SKB_DATA_ALIGN(sizeof(struct skb_shared_info));
 551 
 552         return SKB_DATA_ALIGN(len);
 553 }
 554 
 555 static struct page_pool *cpsw_create_page_pool(struct cpsw_common *cpsw,
 556                                                int size)
 557 {
 558         struct page_pool_params pp_params;
 559         struct page_pool *pool;
 560 
 561         pp_params.order = 0;
 562         pp_params.flags = PP_FLAG_DMA_MAP;
 563         pp_params.pool_size = size;
 564         pp_params.nid = NUMA_NO_NODE;
 565         pp_params.dma_dir = DMA_BIDIRECTIONAL;
 566         pp_params.dev = cpsw->dev;
 567 
 568         pool = page_pool_create(&pp_params);
 569         if (IS_ERR(pool))
 570                 dev_err(cpsw->dev, "cannot create rx page pool\n");
 571 
 572         return pool;
 573 }
 574 
 575 static int cpsw_ndev_create_xdp_rxq(struct cpsw_priv *priv, int ch)
 576 {
 577         struct cpsw_common *cpsw = priv->cpsw;
 578         struct xdp_rxq_info *rxq;
 579         struct page_pool *pool;
 580         int ret;
 581 
 582         pool = cpsw->page_pool[ch];
 583         rxq = &priv->xdp_rxq[ch];
 584 
 585         ret = xdp_rxq_info_reg(rxq, priv->ndev, ch);
 586         if (ret)
 587                 return ret;
 588 
 589         ret = xdp_rxq_info_reg_mem_model(rxq, MEM_TYPE_PAGE_POOL, pool);
 590         if (ret)
 591                 xdp_rxq_info_unreg(rxq);
 592 
 593         return ret;
 594 }
 595 
 596 static void cpsw_ndev_destroy_xdp_rxq(struct cpsw_priv *priv, int ch)
 597 {
 598         struct xdp_rxq_info *rxq = &priv->xdp_rxq[ch];
 599 
 600         if (!xdp_rxq_info_is_reg(rxq))
 601                 return;
 602 
 603         xdp_rxq_info_unreg(rxq);
 604 }
 605 
 606 static int cpsw_create_rx_pool(struct cpsw_common *cpsw, int ch)
 607 {
 608         struct page_pool *pool;
 609         int ret = 0, pool_size;
 610 
 611         pool_size = cpdma_chan_get_rx_buf_num(cpsw->rxv[ch].ch);
 612         pool = cpsw_create_page_pool(cpsw, pool_size);
 613         if (IS_ERR(pool))
 614                 ret = PTR_ERR(pool);
 615         else
 616                 cpsw->page_pool[ch] = pool;
 617 
 618         return ret;
 619 }
 620 
 621 void cpsw_destroy_xdp_rxqs(struct cpsw_common *cpsw)
 622 {
 623         struct net_device *ndev;
 624         int i, ch;
 625 
 626         for (ch = 0; ch < cpsw->rx_ch_num; ch++) {
 627                 for (i = 0; i < cpsw->data.slaves; i++) {
 628                         ndev = cpsw->slaves[i].ndev;
 629                         if (!ndev)
 630                                 continue;
 631 
 632                         cpsw_ndev_destroy_xdp_rxq(netdev_priv(ndev), ch);
 633                 }
 634 
 635                 page_pool_destroy(cpsw->page_pool[ch]);
 636                 cpsw->page_pool[ch] = NULL;
 637         }
 638 }
 639 
 640 int cpsw_create_xdp_rxqs(struct cpsw_common *cpsw)
 641 {
 642         struct net_device *ndev;
 643         int i, ch, ret;
 644 
 645         for (ch = 0; ch < cpsw->rx_ch_num; ch++) {
 646                 ret = cpsw_create_rx_pool(cpsw, ch);
 647                 if (ret)
 648                         goto err_cleanup;
 649 
 650                 /* using same page pool is allowed as no running rx handlers
 651                  * simultaneously for both ndevs
 652                  */
 653                 for (i = 0; i < cpsw->data.slaves; i++) {
 654                         ndev = cpsw->slaves[i].ndev;
 655                         if (!ndev)
 656                                 continue;
 657 
 658                         ret = cpsw_ndev_create_xdp_rxq(netdev_priv(ndev), ch);
 659                         if (ret)
 660                                 goto err_cleanup;
 661                 }
 662         }
 663 
 664         return 0;
 665 
 666 err_cleanup:
 667         cpsw_destroy_xdp_rxqs(cpsw);
 668 
 669         return ret;
 670 }
 671 
 672 static void cpsw_rx_handler(void *token, int len, int status)
 673 {
 674         struct page             *new_page, *page = token;
 675         void                    *pa = page_address(page);
 676         struct cpsw_meta_xdp    *xmeta = pa + CPSW_XMETA_OFFSET;
 677         struct cpsw_common      *cpsw = ndev_to_cpsw(xmeta->ndev);
 678         int                     pkt_size = cpsw->rx_packet_max;
 679         int                     ret = 0, port, ch = xmeta->ch;
 680         int                     headroom = CPSW_HEADROOM;
 681         struct net_device       *ndev = xmeta->ndev;
 682         struct cpsw_priv        *priv;
 683         struct page_pool        *pool;
 684         struct sk_buff          *skb;
 685         struct xdp_buff         xdp;
 686         dma_addr_t              dma;
 687 
 688         if (cpsw->data.dual_emac && status >= 0) {
 689                 port = CPDMA_RX_SOURCE_PORT(status);
 690                 if (port)
 691                         ndev = cpsw->slaves[--port].ndev;
 692         }
 693 
 694         priv = netdev_priv(ndev);
 695         pool = cpsw->page_pool[ch];
 696         if (unlikely(status < 0) || unlikely(!netif_running(ndev))) {
 697                 /* In dual emac mode check for all interfaces */
 698                 if (cpsw->data.dual_emac && cpsw->usage_count &&
 699                     (status >= 0)) {
 700                         /* The packet received is for the interface which
 701                          * is already down and the other interface is up
 702                          * and running, instead of freeing which results
 703                          * in reducing of the number of rx descriptor in
 704                          * DMA engine, requeue page back to cpdma.
 705                          */
 706                         new_page = page;
 707                         goto requeue;
 708                 }
 709 
 710                 /* the interface is going down, pages are purged */
 711                 page_pool_recycle_direct(pool, page);
 712                 return;
 713         }
 714 
 715         new_page = page_pool_dev_alloc_pages(pool);
 716         if (unlikely(!new_page)) {
 717                 new_page = page;
 718                 ndev->stats.rx_dropped++;
 719                 goto requeue;
 720         }
 721 
 722         if (priv->xdp_prog) {
 723                 if (status & CPDMA_RX_VLAN_ENCAP) {
 724                         xdp.data = pa + CPSW_HEADROOM +
 725                                    CPSW_RX_VLAN_ENCAP_HDR_SIZE;
 726                         xdp.data_end = xdp.data + len -
 727                                        CPSW_RX_VLAN_ENCAP_HDR_SIZE;
 728                 } else {
 729                         xdp.data = pa + CPSW_HEADROOM;
 730                         xdp.data_end = xdp.data + len;
 731                 }
 732 
 733                 xdp_set_data_meta_invalid(&xdp);
 734 
 735                 xdp.data_hard_start = pa;
 736                 xdp.rxq = &priv->xdp_rxq[ch];
 737 
 738                 ret = cpsw_run_xdp(priv, ch, &xdp, page);
 739                 if (ret != CPSW_XDP_PASS)
 740                         goto requeue;
 741 
 742                 /* XDP prog might have changed packet data and boundaries */
 743                 len = xdp.data_end - xdp.data;
 744                 headroom = xdp.data - xdp.data_hard_start;
 745 
 746                 /* XDP prog can modify vlan tag, so can't use encap header */
 747                 status &= ~CPDMA_RX_VLAN_ENCAP;
 748         }
 749 
 750         /* pass skb to netstack if no XDP prog or returned XDP_PASS */
 751         skb = build_skb(pa, cpsw_rxbuf_total_len(pkt_size));
 752         if (!skb) {
 753                 ndev->stats.rx_dropped++;
 754                 page_pool_recycle_direct(pool, page);
 755                 goto requeue;
 756         }
 757 
 758         skb_reserve(skb, headroom);
 759         skb_put(skb, len);
 760         skb->dev = ndev;
 761         if (status & CPDMA_RX_VLAN_ENCAP)
 762                 cpsw_rx_vlan_encap(skb);
 763         if (priv->rx_ts_enabled)
 764                 cpts_rx_timestamp(cpsw->cpts, skb);
 765         skb->protocol = eth_type_trans(skb, ndev);
 766 
 767         /* unmap page as no netstack skb page recycling */
 768         page_pool_release_page(pool, page);
 769         netif_receive_skb(skb);
 770 
 771         ndev->stats.rx_bytes += len;
 772         ndev->stats.rx_packets++;
 773 
 774 requeue:
 775         xmeta = page_address(new_page) + CPSW_XMETA_OFFSET;
 776         xmeta->ndev = ndev;
 777         xmeta->ch = ch;
 778 
 779         dma = page_pool_get_dma_addr(new_page) + CPSW_HEADROOM;
 780         ret = cpdma_chan_submit_mapped(cpsw->rxv[ch].ch, new_page, dma,
 781                                        pkt_size, 0);
 782         if (ret < 0) {
 783                 WARN_ON(ret == -ENOMEM);
 784                 page_pool_recycle_direct(pool, new_page);
 785         }
 786 }
 787 
 788 void cpsw_split_res(struct cpsw_common *cpsw)
 789 {
 790         u32 consumed_rate = 0, bigest_rate = 0;
 791         struct cpsw_vector *txv = cpsw->txv;
 792         int i, ch_weight, rlim_ch_num = 0;
 793         int budget, bigest_rate_ch = 0;
 794         u32 ch_rate, max_rate;
 795         int ch_budget = 0;
 796 
 797         for (i = 0; i < cpsw->tx_ch_num; i++) {
 798                 ch_rate = cpdma_chan_get_rate(txv[i].ch);
 799                 if (!ch_rate)
 800                         continue;
 801 
 802                 rlim_ch_num++;
 803                 consumed_rate += ch_rate;
 804         }
 805 
 806         if (cpsw->tx_ch_num == rlim_ch_num) {
 807                 max_rate = consumed_rate;
 808         } else if (!rlim_ch_num) {
 809                 ch_budget = CPSW_POLL_WEIGHT / cpsw->tx_ch_num;
 810                 bigest_rate = 0;
 811                 max_rate = consumed_rate;
 812         } else {
 813                 max_rate = cpsw->speed * 1000;
 814 
 815                 /* if max_rate is less then expected due to reduced link speed,
 816                  * split proportionally according next potential max speed
 817                  */
 818                 if (max_rate < consumed_rate)
 819                         max_rate *= 10;
 820 
 821                 if (max_rate < consumed_rate)
 822                         max_rate *= 10;
 823 
 824                 ch_budget = (consumed_rate * CPSW_POLL_WEIGHT) / max_rate;
 825                 ch_budget = (CPSW_POLL_WEIGHT - ch_budget) /
 826                             (cpsw->tx_ch_num - rlim_ch_num);
 827                 bigest_rate = (max_rate - consumed_rate) /
 828                               (cpsw->tx_ch_num - rlim_ch_num);
 829         }
 830 
 831         /* split tx weight/budget */
 832         budget = CPSW_POLL_WEIGHT;
 833         for (i = 0; i < cpsw->tx_ch_num; i++) {
 834                 ch_rate = cpdma_chan_get_rate(txv[i].ch);
 835                 if (ch_rate) {
 836                         txv[i].budget = (ch_rate * CPSW_POLL_WEIGHT) / max_rate;
 837                         if (!txv[i].budget)
 838                                 txv[i].budget++;
 839                         if (ch_rate > bigest_rate) {
 840                                 bigest_rate_ch = i;
 841                                 bigest_rate = ch_rate;
 842                         }
 843 
 844                         ch_weight = (ch_rate * 100) / max_rate;
 845                         if (!ch_weight)
 846                                 ch_weight++;
 847                         cpdma_chan_set_weight(cpsw->txv[i].ch, ch_weight);
 848                 } else {
 849                         txv[i].budget = ch_budget;
 850                         if (!bigest_rate_ch)
 851                                 bigest_rate_ch = i;
 852                         cpdma_chan_set_weight(cpsw->txv[i].ch, 0);
 853                 }
 854 
 855                 budget -= txv[i].budget;
 856         }
 857 
 858         if (budget)
 859                 txv[bigest_rate_ch].budget += budget;
 860 
 861         /* split rx budget */
 862         budget = CPSW_POLL_WEIGHT;
 863         ch_budget = budget / cpsw->rx_ch_num;
 864         for (i = 0; i < cpsw->rx_ch_num; i++) {
 865                 cpsw->rxv[i].budget = ch_budget;
 866                 budget -= ch_budget;
 867         }
 868 
 869         if (budget)
 870                 cpsw->rxv[0].budget += budget;
 871 }
 872 
 873 static irqreturn_t cpsw_tx_interrupt(int irq, void *dev_id)
 874 {
 875         struct cpsw_common *cpsw = dev_id;
 876 
 877         writel(0, &cpsw->wr_regs->tx_en);
 878         cpdma_ctlr_eoi(cpsw->dma, CPDMA_EOI_TX);
 879 
 880         if (cpsw->quirk_irq) {
 881                 disable_irq_nosync(cpsw->irqs_table[1]);
 882                 cpsw->tx_irq_disabled = true;
 883         }
 884 
 885         napi_schedule(&cpsw->napi_tx);
 886         return IRQ_HANDLED;
 887 }
 888 
 889 static irqreturn_t cpsw_rx_interrupt(int irq, void *dev_id)
 890 {
 891         struct cpsw_common *cpsw = dev_id;
 892 
 893         writel(0, &cpsw->wr_regs->rx_en);
 894         cpdma_ctlr_eoi(cpsw->dma, CPDMA_EOI_RX);
 895 
 896         if (cpsw->quirk_irq) {
 897                 disable_irq_nosync(cpsw->irqs_table[0]);
 898                 cpsw->rx_irq_disabled = true;
 899         }
 900 
 901         napi_schedule(&cpsw->napi_rx);
 902         return IRQ_HANDLED;
 903 }
 904 
 905 static int cpsw_tx_mq_poll(struct napi_struct *napi_tx, int budget)
 906 {
 907         u32                     ch_map;
 908         int                     num_tx, cur_budget, ch;
 909         struct cpsw_common      *cpsw = napi_to_cpsw(napi_tx);
 910         struct cpsw_vector      *txv;
 911 
 912         /* process every unprocessed channel */
 913         ch_map = cpdma_ctrl_txchs_state(cpsw->dma);
 914         for (ch = 0, num_tx = 0; ch_map & 0xff; ch_map <<= 1, ch++) {
 915                 if (!(ch_map & 0x80))
 916                         continue;
 917 
 918                 txv = &cpsw->txv[ch];
 919                 if (unlikely(txv->budget > budget - num_tx))
 920                         cur_budget = budget - num_tx;
 921                 else
 922                         cur_budget = txv->budget;
 923 
 924                 num_tx += cpdma_chan_process(txv->ch, cur_budget);
 925                 if (num_tx >= budget)
 926                         break;
 927         }
 928 
 929         if (num_tx < budget) {
 930                 napi_complete(napi_tx);
 931                 writel(0xff, &cpsw->wr_regs->tx_en);
 932         }
 933 
 934         return num_tx;
 935 }
 936 
 937 static int cpsw_tx_poll(struct napi_struct *napi_tx, int budget)
 938 {
 939         struct cpsw_common *cpsw = napi_to_cpsw(napi_tx);
 940         int num_tx;
 941 
 942         num_tx = cpdma_chan_process(cpsw->txv[0].ch, budget);
 943         if (num_tx < budget) {
 944                 napi_complete(napi_tx);
 945                 writel(0xff, &cpsw->wr_regs->tx_en);
 946                 if (cpsw->tx_irq_disabled) {
 947                         cpsw->tx_irq_disabled = false;
 948                         enable_irq(cpsw->irqs_table[1]);
 949                 }
 950         }
 951 
 952         return num_tx;
 953 }
 954 
 955 static int cpsw_rx_mq_poll(struct napi_struct *napi_rx, int budget)
 956 {
 957         u32                     ch_map;
 958         int                     num_rx, cur_budget, ch;
 959         struct cpsw_common      *cpsw = napi_to_cpsw(napi_rx);
 960         struct cpsw_vector      *rxv;
 961 
 962         /* process every unprocessed channel */
 963         ch_map = cpdma_ctrl_rxchs_state(cpsw->dma);
 964         for (ch = 0, num_rx = 0; ch_map; ch_map >>= 1, ch++) {
 965                 if (!(ch_map & 0x01))
 966                         continue;
 967 
 968                 rxv = &cpsw->rxv[ch];
 969                 if (unlikely(rxv->budget > budget - num_rx))
 970                         cur_budget = budget - num_rx;
 971                 else
 972                         cur_budget = rxv->budget;
 973 
 974                 num_rx += cpdma_chan_process(rxv->ch, cur_budget);
 975                 if (num_rx >= budget)
 976                         break;
 977         }
 978 
 979         if (num_rx < budget) {
 980                 napi_complete_done(napi_rx, num_rx);
 981                 writel(0xff, &cpsw->wr_regs->rx_en);
 982         }
 983 
 984         return num_rx;
 985 }
 986 
 987 static int cpsw_rx_poll(struct napi_struct *napi_rx, int budget)
 988 {
 989         struct cpsw_common *cpsw = napi_to_cpsw(napi_rx);
 990         int num_rx;
 991 
 992         num_rx = cpdma_chan_process(cpsw->rxv[0].ch, budget);
 993         if (num_rx < budget) {
 994                 napi_complete_done(napi_rx, num_rx);
 995                 writel(0xff, &cpsw->wr_regs->rx_en);
 996                 if (cpsw->rx_irq_disabled) {
 997                         cpsw->rx_irq_disabled = false;
 998                         enable_irq(cpsw->irqs_table[0]);
 999                 }
1000         }
1001 
1002         return num_rx;
1003 }
1004 
1005 static inline void soft_reset(const char *module, void __iomem *reg)
1006 {
1007         unsigned long timeout = jiffies + HZ;
1008 
1009         writel_relaxed(1, reg);
1010         do {
1011                 cpu_relax();
1012         } while ((readl_relaxed(reg) & 1) && time_after(timeout, jiffies));
1013 
1014         WARN(readl_relaxed(reg) & 1, "failed to soft-reset %s\n", module);
1015 }
1016 
1017 static void cpsw_set_slave_mac(struct cpsw_slave *slave,
1018                                struct cpsw_priv *priv)
1019 {
1020         slave_write(slave, mac_hi(priv->mac_addr), SA_HI);
1021         slave_write(slave, mac_lo(priv->mac_addr), SA_LO);
1022 }
1023 
1024 static bool cpsw_shp_is_off(struct cpsw_priv *priv)
1025 {
1026         struct cpsw_common *cpsw = priv->cpsw;
1027         struct cpsw_slave *slave;
1028         u32 shift, mask, val;
1029 
1030         val = readl_relaxed(&cpsw->regs->ptype);
1031 
1032         slave = &cpsw->slaves[cpsw_slave_index(cpsw, priv)];
1033         shift = CPSW_FIFO_SHAPE_EN_SHIFT + 3 * slave->slave_num;
1034         mask = 7 << shift;
1035         val = val & mask;
1036 
1037         return !val;
1038 }
1039 
1040 static void cpsw_fifo_shp_on(struct cpsw_priv *priv, int fifo, int on)
1041 {
1042         struct cpsw_common *cpsw = priv->cpsw;
1043         struct cpsw_slave *slave;
1044         u32 shift, mask, val;
1045 
1046         val = readl_relaxed(&cpsw->regs->ptype);
1047 
1048         slave = &cpsw->slaves[cpsw_slave_index(cpsw, priv)];
1049         shift = CPSW_FIFO_SHAPE_EN_SHIFT + 3 * slave->slave_num;
1050         mask = (1 << --fifo) << shift;
1051         val = on ? val | mask : val & ~mask;
1052 
1053         writel_relaxed(val, &cpsw->regs->ptype);
1054 }
1055 
1056 static void _cpsw_adjust_link(struct cpsw_slave *slave,
1057                               struct cpsw_priv *priv, bool *link)
1058 {
1059         struct phy_device       *phy = slave->phy;
1060         u32                     mac_control = 0;
1061         u32                     slave_port;
1062         struct cpsw_common *cpsw = priv->cpsw;
1063 
1064         if (!phy)
1065                 return;
1066 
1067         slave_port = cpsw_get_slave_port(slave->slave_num);
1068 
1069         if (phy->link) {
1070                 mac_control = CPSW_SL_CTL_GMII_EN;
1071 
1072                 if (phy->speed == 1000)
1073                         mac_control |= CPSW_SL_CTL_GIG;
1074                 if (phy->duplex)
1075                         mac_control |= CPSW_SL_CTL_FULLDUPLEX;
1076 
1077                 /* set speed_in input in case RMII mode is used in 100Mbps */
1078                 if (phy->speed == 100)
1079                         mac_control |= CPSW_SL_CTL_IFCTL_A;
1080                 /* in band mode only works in 10Mbps RGMII mode */
1081                 else if ((phy->speed == 10) && phy_interface_is_rgmii(phy))
1082                         mac_control |= CPSW_SL_CTL_EXT_EN; /* In Band mode */
1083 
1084                 if (priv->rx_pause)
1085                         mac_control |= CPSW_SL_CTL_RX_FLOW_EN;
1086 
1087                 if (priv->tx_pause)
1088                         mac_control |= CPSW_SL_CTL_TX_FLOW_EN;
1089 
1090                 if (mac_control != slave->mac_control)
1091                         cpsw_sl_ctl_set(slave->mac_sl, mac_control);
1092 
1093                 /* enable forwarding */
1094                 cpsw_ale_control_set(cpsw->ale, slave_port,
1095                                      ALE_PORT_STATE, ALE_PORT_STATE_FORWARD);
1096 
1097                 *link = true;
1098 
1099                 if (priv->shp_cfg_speed &&
1100                     priv->shp_cfg_speed != slave->phy->speed &&
1101                     !cpsw_shp_is_off(priv))
1102                         dev_warn(priv->dev,
1103                                  "Speed was changed, CBS shaper speeds are changed!");
1104         } else {
1105                 mac_control = 0;
1106                 /* disable forwarding */
1107                 cpsw_ale_control_set(cpsw->ale, slave_port,
1108                                      ALE_PORT_STATE, ALE_PORT_STATE_DISABLE);
1109 
1110                 cpsw_sl_wait_for_idle(slave->mac_sl, 100);
1111 
1112                 cpsw_sl_ctl_reset(slave->mac_sl);
1113         }
1114 
1115         if (mac_control != slave->mac_control)
1116                 phy_print_status(phy);
1117 
1118         slave->mac_control = mac_control;
1119 }
1120 
1121 static int cpsw_get_common_speed(struct cpsw_common *cpsw)
1122 {
1123         int i, speed;
1124 
1125         for (i = 0, speed = 0; i < cpsw->data.slaves; i++)
1126                 if (cpsw->slaves[i].phy && cpsw->slaves[i].phy->link)
1127                         speed += cpsw->slaves[i].phy->speed;
1128 
1129         return speed;
1130 }
1131 
1132 static int cpsw_need_resplit(struct cpsw_common *cpsw)
1133 {
1134         int i, rlim_ch_num;
1135         int speed, ch_rate;
1136 
1137         /* re-split resources only in case speed was changed */
1138         speed = cpsw_get_common_speed(cpsw);
1139         if (speed == cpsw->speed || !speed)
1140                 return 0;
1141 
1142         cpsw->speed = speed;
1143 
1144         for (i = 0, rlim_ch_num = 0; i < cpsw->tx_ch_num; i++) {
1145                 ch_rate = cpdma_chan_get_rate(cpsw->txv[i].ch);
1146                 if (!ch_rate)
1147                         break;
1148 
1149                 rlim_ch_num++;
1150         }
1151 
1152         /* cases not dependent on speed */
1153         if (!rlim_ch_num || rlim_ch_num == cpsw->tx_ch_num)
1154                 return 0;
1155 
1156         return 1;
1157 }
1158 
1159 static void cpsw_adjust_link(struct net_device *ndev)
1160 {
1161         struct cpsw_priv        *priv = netdev_priv(ndev);
1162         struct cpsw_common      *cpsw = priv->cpsw;
1163         bool                    link = false;
1164 
1165         for_each_slave(priv, _cpsw_adjust_link, priv, &link);
1166 
1167         if (link) {
1168                 if (cpsw_need_resplit(cpsw))
1169                         cpsw_split_res(cpsw);
1170 
1171                 netif_carrier_on(ndev);
1172                 if (netif_running(ndev))
1173                         netif_tx_wake_all_queues(ndev);
1174         } else {
1175                 netif_carrier_off(ndev);
1176                 netif_tx_stop_all_queues(ndev);
1177         }
1178 }
1179 
1180 static inline void cpsw_add_dual_emac_def_ale_entries(
1181                 struct cpsw_priv *priv, struct cpsw_slave *slave,
1182                 u32 slave_port)
1183 {
1184         struct cpsw_common *cpsw = priv->cpsw;
1185         u32 port_mask = 1 << slave_port | ALE_PORT_HOST;
1186 
1187         if (cpsw->version == CPSW_VERSION_1)
1188                 slave_write(slave, slave->port_vlan, CPSW1_PORT_VLAN);
1189         else
1190                 slave_write(slave, slave->port_vlan, CPSW2_PORT_VLAN);
1191         cpsw_ale_add_vlan(cpsw->ale, slave->port_vlan, port_mask,
1192                           port_mask, port_mask, 0);
1193         cpsw_ale_add_mcast(cpsw->ale, priv->ndev->broadcast,
1194                            ALE_PORT_HOST, ALE_VLAN, slave->port_vlan, 0);
1195         cpsw_ale_add_ucast(cpsw->ale, priv->mac_addr,
1196                            HOST_PORT_NUM, ALE_VLAN |
1197                            ALE_SECURE, slave->port_vlan);
1198         cpsw_ale_control_set(cpsw->ale, slave_port,
1199                              ALE_PORT_DROP_UNKNOWN_VLAN, 1);
1200 }
1201 
1202 static void cpsw_slave_open(struct cpsw_slave *slave, struct cpsw_priv *priv)
1203 {
1204         u32 slave_port;
1205         struct phy_device *phy;
1206         struct cpsw_common *cpsw = priv->cpsw;
1207 
1208         cpsw_sl_reset(slave->mac_sl, 100);
1209         cpsw_sl_ctl_reset(slave->mac_sl);
1210 
1211         /* setup priority mapping */
1212         cpsw_sl_reg_write(slave->mac_sl, CPSW_SL_RX_PRI_MAP,
1213                           RX_PRIORITY_MAPPING);
1214 
1215         switch (cpsw->version) {
1216         case CPSW_VERSION_1:
1217                 slave_write(slave, TX_PRIORITY_MAPPING, CPSW1_TX_PRI_MAP);
1218                 /* Increase RX FIFO size to 5 for supporting fullduplex
1219                  * flow control mode
1220                  */
1221                 slave_write(slave,
1222                             (CPSW_MAX_BLKS_TX << CPSW_MAX_BLKS_TX_SHIFT) |
1223                             CPSW_MAX_BLKS_RX, CPSW1_MAX_BLKS);
1224                 break;
1225         case CPSW_VERSION_2:
1226         case CPSW_VERSION_3:
1227         case CPSW_VERSION_4:
1228                 slave_write(slave, TX_PRIORITY_MAPPING, CPSW2_TX_PRI_MAP);
1229                 /* Increase RX FIFO size to 5 for supporting fullduplex
1230                  * flow control mode
1231                  */
1232                 slave_write(slave,
1233                             (CPSW_MAX_BLKS_TX << CPSW_MAX_BLKS_TX_SHIFT) |
1234                             CPSW_MAX_BLKS_RX, CPSW2_MAX_BLKS);
1235                 break;
1236         }
1237 
1238         /* setup max packet size, and mac address */
1239         cpsw_sl_reg_write(slave->mac_sl, CPSW_SL_RX_MAXLEN,
1240                           cpsw->rx_packet_max);
1241         cpsw_set_slave_mac(slave, priv);
1242 
1243         slave->mac_control = 0; /* no link yet */
1244 
1245         slave_port = cpsw_get_slave_port(slave->slave_num);
1246 
1247         if (cpsw->data.dual_emac)
1248                 cpsw_add_dual_emac_def_ale_entries(priv, slave, slave_port);
1249         else
1250                 cpsw_ale_add_mcast(cpsw->ale, priv->ndev->broadcast,
1251                                    1 << slave_port, 0, 0, ALE_MCAST_FWD_2);
1252 
1253         if (slave->data->phy_node) {
1254                 phy = of_phy_connect(priv->ndev, slave->data->phy_node,
1255                                  &cpsw_adjust_link, 0, slave->data->phy_if);
1256                 if (!phy) {
1257                         dev_err(priv->dev, "phy \"%pOF\" not found on slave %d\n",
1258                                 slave->data->phy_node,
1259                                 slave->slave_num);
1260                         return;
1261                 }
1262         } else {
1263                 phy = phy_connect(priv->ndev, slave->data->phy_id,
1264                                  &cpsw_adjust_link, slave->data->phy_if);
1265                 if (IS_ERR(phy)) {
1266                         dev_err(priv->dev,
1267                                 "phy \"%s\" not found on slave %d, err %ld\n",
1268                                 slave->data->phy_id, slave->slave_num,
1269                                 PTR_ERR(phy));
1270                         return;
1271                 }
1272         }
1273 
1274         slave->phy = phy;
1275 
1276         phy_attached_info(slave->phy);
1277 
1278         phy_start(slave->phy);
1279 
1280         /* Configure GMII_SEL register */
1281         if (!IS_ERR(slave->data->ifphy))
1282                 phy_set_mode_ext(slave->data->ifphy, PHY_MODE_ETHERNET,
1283                                  slave->data->phy_if);
1284         else
1285                 cpsw_phy_sel(cpsw->dev, slave->phy->interface,
1286                              slave->slave_num);
1287 }
1288 
1289 static inline void cpsw_add_default_vlan(struct cpsw_priv *priv)
1290 {
1291         struct cpsw_common *cpsw = priv->cpsw;
1292         const int vlan = cpsw->data.default_vlan;
1293         u32 reg;
1294         int i;
1295         int unreg_mcast_mask;
1296 
1297         reg = (cpsw->version == CPSW_VERSION_1) ? CPSW1_PORT_VLAN :
1298                CPSW2_PORT_VLAN;
1299 
1300         writel(vlan, &cpsw->host_port_regs->port_vlan);
1301 
1302         for (i = 0; i < cpsw->data.slaves; i++)
1303                 slave_write(cpsw->slaves + i, vlan, reg);
1304 
1305         if (priv->ndev->flags & IFF_ALLMULTI)
1306                 unreg_mcast_mask = ALE_ALL_PORTS;
1307         else
1308                 unreg_mcast_mask = ALE_PORT_1 | ALE_PORT_2;
1309 
1310         cpsw_ale_add_vlan(cpsw->ale, vlan, ALE_ALL_PORTS,
1311                           ALE_ALL_PORTS, ALE_ALL_PORTS,
1312                           unreg_mcast_mask);
1313 }
1314 
1315 static void cpsw_init_host_port(struct cpsw_priv *priv)
1316 {
1317         u32 fifo_mode;
1318         u32 control_reg;
1319         struct cpsw_common *cpsw = priv->cpsw;
1320 
1321         /* soft reset the controller and initialize ale */
1322         soft_reset("cpsw", &cpsw->regs->soft_reset);
1323         cpsw_ale_start(cpsw->ale);
1324 
1325         /* switch to vlan unaware mode */
1326         cpsw_ale_control_set(cpsw->ale, HOST_PORT_NUM, ALE_VLAN_AWARE,
1327                              CPSW_ALE_VLAN_AWARE);
1328         control_reg = readl(&cpsw->regs->control);
1329         control_reg |= CPSW_VLAN_AWARE | CPSW_RX_VLAN_ENCAP;
1330         writel(control_reg, &cpsw->regs->control);
1331         fifo_mode = (cpsw->data.dual_emac) ? CPSW_FIFO_DUAL_MAC_MODE :
1332                      CPSW_FIFO_NORMAL_MODE;
1333         writel(fifo_mode, &cpsw->host_port_regs->tx_in_ctl);
1334 
1335         /* setup host port priority mapping */
1336         writel_relaxed(CPDMA_TX_PRIORITY_MAP,
1337                        &cpsw->host_port_regs->cpdma_tx_pri_map);
1338         writel_relaxed(0, &cpsw->host_port_regs->cpdma_rx_chan_map);
1339 
1340         cpsw_ale_control_set(cpsw->ale, HOST_PORT_NUM,
1341                              ALE_PORT_STATE, ALE_PORT_STATE_FORWARD);
1342 
1343         if (!cpsw->data.dual_emac) {
1344                 cpsw_ale_add_ucast(cpsw->ale, priv->mac_addr, HOST_PORT_NUM,
1345                                    0, 0);
1346                 cpsw_ale_add_mcast(cpsw->ale, priv->ndev->broadcast,
1347                                    ALE_PORT_HOST, 0, 0, ALE_MCAST_FWD_2);
1348         }
1349 }
1350 
1351 int cpsw_fill_rx_channels(struct cpsw_priv *priv)
1352 {
1353         struct cpsw_common *cpsw = priv->cpsw;
1354         struct cpsw_meta_xdp *xmeta;
1355         struct page_pool *pool;
1356         struct page *page;
1357         int ch_buf_num;
1358         int ch, i, ret;
1359         dma_addr_t dma;
1360 
1361         for (ch = 0; ch < cpsw->rx_ch_num; ch++) {
1362                 pool = cpsw->page_pool[ch];
1363                 ch_buf_num = cpdma_chan_get_rx_buf_num(cpsw->rxv[ch].ch);
1364                 for (i = 0; i < ch_buf_num; i++) {
1365                         page = page_pool_dev_alloc_pages(pool);
1366                         if (!page) {
1367                                 cpsw_err(priv, ifup, "allocate rx page err\n");
1368                                 return -ENOMEM;
1369                         }
1370 
1371                         xmeta = page_address(page) + CPSW_XMETA_OFFSET;
1372                         xmeta->ndev = priv->ndev;
1373                         xmeta->ch = ch;
1374 
1375                         dma = page_pool_get_dma_addr(page) + CPSW_HEADROOM;
1376                         ret = cpdma_chan_idle_submit_mapped(cpsw->rxv[ch].ch,
1377                                                             page, dma,
1378                                                             cpsw->rx_packet_max,
1379                                                             0);
1380                         if (ret < 0) {
1381                                 cpsw_err(priv, ifup,
1382                                          "cannot submit page to channel %d rx, error %d\n",
1383                                          ch, ret);
1384                                 page_pool_recycle_direct(pool, page);
1385                                 return ret;
1386                         }
1387                 }
1388 
1389                 cpsw_info(priv, ifup, "ch %d rx, submitted %d descriptors\n",
1390                           ch, ch_buf_num);
1391         }
1392 
1393         return 0;
1394 }
1395 
1396 static void cpsw_slave_stop(struct cpsw_slave *slave, struct cpsw_common *cpsw)
1397 {
1398         u32 slave_port;
1399 
1400         slave_port = cpsw_get_slave_port(slave->slave_num);
1401 
1402         if (!slave->phy)
1403                 return;
1404         phy_stop(slave->phy);
1405         phy_disconnect(slave->phy);
1406         slave->phy = NULL;
1407         cpsw_ale_control_set(cpsw->ale, slave_port,
1408                              ALE_PORT_STATE, ALE_PORT_STATE_DISABLE);
1409         cpsw_sl_reset(slave->mac_sl, 100);
1410         cpsw_sl_ctl_reset(slave->mac_sl);
1411 }
1412 
1413 static int cpsw_tc_to_fifo(int tc, int num_tc)
1414 {
1415         if (tc == num_tc - 1)
1416                 return 0;
1417 
1418         return CPSW_FIFO_SHAPERS_NUM - tc;
1419 }
1420 
1421 static int cpsw_set_fifo_bw(struct cpsw_priv *priv, int fifo, int bw)
1422 {
1423         struct cpsw_common *cpsw = priv->cpsw;
1424         u32 val = 0, send_pct, shift;
1425         struct cpsw_slave *slave;
1426         int pct = 0, i;
1427 
1428         if (bw > priv->shp_cfg_speed * 1000)
1429                 goto err;
1430 
1431         /* shaping has to stay enabled for highest fifos linearly
1432          * and fifo bw no more then interface can allow
1433          */
1434         slave = &cpsw->slaves[cpsw_slave_index(cpsw, priv)];
1435         send_pct = slave_read(slave, SEND_PERCENT);
1436         for (i = CPSW_FIFO_SHAPERS_NUM; i > 0; i--) {
1437                 if (!bw) {
1438                         if (i >= fifo || !priv->fifo_bw[i])
1439                                 continue;
1440 
1441                         dev_warn(priv->dev, "Prev FIFO%d is shaped", i);
1442                         continue;
1443                 }
1444 
1445                 if (!priv->fifo_bw[i] && i > fifo) {
1446                         dev_err(priv->dev, "Upper FIFO%d is not shaped", i);
1447                         return -EINVAL;
1448                 }
1449 
1450                 shift = (i - 1) * 8;
1451                 if (i == fifo) {
1452                         send_pct &= ~(CPSW_PCT_MASK << shift);
1453                         val = DIV_ROUND_UP(bw, priv->shp_cfg_speed * 10);
1454                         if (!val)
1455                                 val = 1;
1456 
1457                         send_pct |= val << shift;
1458                         pct += val;
1459                         continue;
1460                 }
1461 
1462                 if (priv->fifo_bw[i])
1463                         pct += (send_pct >> shift) & CPSW_PCT_MASK;
1464         }
1465 
1466         if (pct >= 100)
1467                 goto err;
1468 
1469         slave_write(slave, send_pct, SEND_PERCENT);
1470         priv->fifo_bw[fifo] = bw;
1471 
1472         dev_warn(priv->dev, "set FIFO%d bw = %d\n", fifo,
1473                  DIV_ROUND_CLOSEST(val * priv->shp_cfg_speed, 100));
1474 
1475         return 0;
1476 err:
1477         dev_err(priv->dev, "Bandwidth doesn't fit in tc configuration");
1478         return -EINVAL;
1479 }
1480 
1481 static int cpsw_set_fifo_rlimit(struct cpsw_priv *priv, int fifo, int bw)
1482 {
1483         struct cpsw_common *cpsw = priv->cpsw;
1484         struct cpsw_slave *slave;
1485         u32 tx_in_ctl_rg, val;
1486         int ret;
1487 
1488         ret = cpsw_set_fifo_bw(priv, fifo, bw);
1489         if (ret)
1490                 return ret;
1491 
1492         slave = &cpsw->slaves[cpsw_slave_index(cpsw, priv)];
1493         tx_in_ctl_rg = cpsw->version == CPSW_VERSION_1 ?
1494                        CPSW1_TX_IN_CTL : CPSW2_TX_IN_CTL;
1495 
1496         if (!bw)
1497                 cpsw_fifo_shp_on(priv, fifo, bw);
1498 
1499         val = slave_read(slave, tx_in_ctl_rg);
1500         if (cpsw_shp_is_off(priv)) {
1501                 /* disable FIFOs rate limited queues */
1502                 val &= ~(0xf << CPSW_FIFO_RATE_EN_SHIFT);
1503 
1504                 /* set type of FIFO queues to normal priority mode */
1505                 val &= ~(3 << CPSW_FIFO_QUEUE_TYPE_SHIFT);
1506 
1507                 /* set type of FIFO queues to be rate limited */
1508                 if (bw)
1509                         val |= 2 << CPSW_FIFO_QUEUE_TYPE_SHIFT;
1510                 else
1511                         priv->shp_cfg_speed = 0;
1512         }
1513 
1514         /* toggle a FIFO rate limited queue */
1515         if (bw)
1516                 val |= BIT(fifo + CPSW_FIFO_RATE_EN_SHIFT);
1517         else
1518                 val &= ~BIT(fifo + CPSW_FIFO_RATE_EN_SHIFT);
1519         slave_write(slave, val, tx_in_ctl_rg);
1520 
1521         /* FIFO transmit shape enable */
1522         cpsw_fifo_shp_on(priv, fifo, bw);
1523         return 0;
1524 }
1525 
1526 /* Defaults:
1527  * class A - prio 3
1528  * class B - prio 2
1529  * shaping for class A should be set first
1530  */
1531 static int cpsw_set_cbs(struct net_device *ndev,
1532                         struct tc_cbs_qopt_offload *qopt)
1533 {
1534         struct cpsw_priv *priv = netdev_priv(ndev);
1535         struct cpsw_common *cpsw = priv->cpsw;
1536         struct cpsw_slave *slave;
1537         int prev_speed = 0;
1538         int tc, ret, fifo;
1539         u32 bw = 0;
1540 
1541         tc = netdev_txq_to_tc(priv->ndev, qopt->queue);
1542 
1543         /* enable channels in backward order, as highest FIFOs must be rate
1544          * limited first and for compliance with CPDMA rate limited channels
1545          * that also used in bacward order. FIFO0 cannot be rate limited.
1546          */
1547         fifo = cpsw_tc_to_fifo(tc, ndev->num_tc);
1548         if (!fifo) {
1549                 dev_err(priv->dev, "Last tc%d can't be rate limited", tc);
1550                 return -EINVAL;
1551         }
1552 
1553         /* do nothing, it's disabled anyway */
1554         if (!qopt->enable && !priv->fifo_bw[fifo])
1555                 return 0;
1556 
1557         /* shapers can be set if link speed is known */
1558         slave = &cpsw->slaves[cpsw_slave_index(cpsw, priv)];
1559         if (slave->phy && slave->phy->link) {
1560                 if (priv->shp_cfg_speed &&
1561                     priv->shp_cfg_speed != slave->phy->speed)
1562                         prev_speed = priv->shp_cfg_speed;
1563 
1564                 priv->shp_cfg_speed = slave->phy->speed;
1565         }
1566 
1567         if (!priv->shp_cfg_speed) {
1568                 dev_err(priv->dev, "Link speed is not known");
1569                 return -1;
1570         }
1571 
1572         ret = pm_runtime_get_sync(cpsw->dev);
1573         if (ret < 0) {
1574                 pm_runtime_put_noidle(cpsw->dev);
1575                 return ret;
1576         }
1577 
1578         bw = qopt->enable ? qopt->idleslope : 0;
1579         ret = cpsw_set_fifo_rlimit(priv, fifo, bw);
1580         if (ret) {
1581                 priv->shp_cfg_speed = prev_speed;
1582                 prev_speed = 0;
1583         }
1584 
1585         if (bw && prev_speed)
1586                 dev_warn(priv->dev,
1587                          "Speed was changed, CBS shaper speeds are changed!");
1588 
1589         pm_runtime_put_sync(cpsw->dev);
1590         return ret;
1591 }
1592 
1593 static void cpsw_cbs_resume(struct cpsw_slave *slave, struct cpsw_priv *priv)
1594 {
1595         int fifo, bw;
1596 
1597         for (fifo = CPSW_FIFO_SHAPERS_NUM; fifo > 0; fifo--) {
1598                 bw = priv->fifo_bw[fifo];
1599                 if (!bw)
1600                         continue;
1601 
1602                 cpsw_set_fifo_rlimit(priv, fifo, bw);
1603         }
1604 }
1605 
1606 static void cpsw_mqprio_resume(struct cpsw_slave *slave, struct cpsw_priv *priv)
1607 {
1608         struct cpsw_common *cpsw = priv->cpsw;
1609         u32 tx_prio_map = 0;
1610         int i, tc, fifo;
1611         u32 tx_prio_rg;
1612 
1613         if (!priv->mqprio_hw)
1614                 return;
1615 
1616         for (i = 0; i < 8; i++) {
1617                 tc = netdev_get_prio_tc_map(priv->ndev, i);
1618                 fifo = CPSW_FIFO_SHAPERS_NUM - tc;
1619                 tx_prio_map |= fifo << (4 * i);
1620         }
1621 
1622         tx_prio_rg = cpsw->version == CPSW_VERSION_1 ?
1623                      CPSW1_TX_PRI_MAP : CPSW2_TX_PRI_MAP;
1624 
1625         slave_write(slave, tx_prio_map, tx_prio_rg);
1626 }
1627 
1628 static int cpsw_restore_vlans(struct net_device *vdev, int vid, void *arg)
1629 {
1630         struct cpsw_priv *priv = arg;
1631 
1632         if (!vdev)
1633                 return 0;
1634 
1635         cpsw_ndo_vlan_rx_add_vid(priv->ndev, 0, vid);
1636         return 0;
1637 }
1638 
1639 /* restore resources after port reset */
1640 static void cpsw_restore(struct cpsw_priv *priv)
1641 {
1642         /* restore vlan configurations */
1643         vlan_for_each(priv->ndev, cpsw_restore_vlans, priv);
1644 
1645         /* restore MQPRIO offload */
1646         for_each_slave(priv, cpsw_mqprio_resume, priv);
1647 
1648         /* restore CBS offload */
1649         for_each_slave(priv, cpsw_cbs_resume, priv);
1650 }
1651 
1652 static int cpsw_ndo_open(struct net_device *ndev)
1653 {
1654         struct cpsw_priv *priv = netdev_priv(ndev);
1655         struct cpsw_common *cpsw = priv->cpsw;
1656         int ret;
1657         u32 reg;
1658 
1659         ret = pm_runtime_get_sync(cpsw->dev);
1660         if (ret < 0) {
1661                 pm_runtime_put_noidle(cpsw->dev);
1662                 return ret;
1663         }
1664 
1665         netif_carrier_off(ndev);
1666 
1667         /* Notify the stack of the actual queue counts. */
1668         ret = netif_set_real_num_tx_queues(ndev, cpsw->tx_ch_num);
1669         if (ret) {
1670                 dev_err(priv->dev, "cannot set real number of tx queues\n");
1671                 goto err_cleanup;
1672         }
1673 
1674         ret = netif_set_real_num_rx_queues(ndev, cpsw->rx_ch_num);
1675         if (ret) {
1676                 dev_err(priv->dev, "cannot set real number of rx queues\n");
1677                 goto err_cleanup;
1678         }
1679 
1680         reg = cpsw->version;
1681 
1682         dev_info(priv->dev, "initializing cpsw version %d.%d (%d)\n",
1683                  CPSW_MAJOR_VERSION(reg), CPSW_MINOR_VERSION(reg),
1684                  CPSW_RTL_VERSION(reg));
1685 
1686         /* Initialize host and slave ports */
1687         if (!cpsw->usage_count)
1688                 cpsw_init_host_port(priv);
1689         for_each_slave(priv, cpsw_slave_open, priv);
1690 
1691         /* Add default VLAN */
1692         if (!cpsw->data.dual_emac)
1693                 cpsw_add_default_vlan(priv);
1694         else
1695                 cpsw_ale_add_vlan(cpsw->ale, cpsw->data.default_vlan,
1696                                   ALE_ALL_PORTS, ALE_ALL_PORTS, 0, 0);
1697 
1698         /* initialize shared resources for every ndev */
1699         if (!cpsw->usage_count) {
1700                 /* disable priority elevation */
1701                 writel_relaxed(0, &cpsw->regs->ptype);
1702 
1703                 /* enable statistics collection only on all ports */
1704                 writel_relaxed(0x7, &cpsw->regs->stat_port_en);
1705 
1706                 /* Enable internal fifo flow control */
1707                 writel(0x7, &cpsw->regs->flow_control);
1708 
1709                 napi_enable(&cpsw->napi_rx);
1710                 napi_enable(&cpsw->napi_tx);
1711 
1712                 if (cpsw->tx_irq_disabled) {
1713                         cpsw->tx_irq_disabled = false;
1714                         enable_irq(cpsw->irqs_table[1]);
1715                 }
1716 
1717                 if (cpsw->rx_irq_disabled) {
1718                         cpsw->rx_irq_disabled = false;
1719                         enable_irq(cpsw->irqs_table[0]);
1720                 }
1721 
1722                 /* create rxqs for both infs in dual mac as they use same pool
1723                  * and must be destroyed together when no users.
1724                  */
1725                 ret = cpsw_create_xdp_rxqs(cpsw);
1726                 if (ret < 0)
1727                         goto err_cleanup;
1728 
1729                 ret = cpsw_fill_rx_channels(priv);
1730                 if (ret < 0)
1731                         goto err_cleanup;
1732 
1733                 if (cpts_register(cpsw->cpts))
1734                         dev_err(priv->dev, "error registering cpts device\n");
1735 
1736         }
1737 
1738         cpsw_restore(priv);
1739 
1740         /* Enable Interrupt pacing if configured */
1741         if (cpsw->coal_intvl != 0) {
1742                 struct ethtool_coalesce coal;
1743 
1744                 coal.rx_coalesce_usecs = cpsw->coal_intvl;
1745                 cpsw_set_coalesce(ndev, &coal);
1746         }
1747 
1748         cpdma_ctlr_start(cpsw->dma);
1749         cpsw_intr_enable(cpsw);
1750         cpsw->usage_count++;
1751 
1752         return 0;
1753 
1754 err_cleanup:
1755         if (!cpsw->usage_count) {
1756                 cpdma_ctlr_stop(cpsw->dma);
1757                 cpsw_destroy_xdp_rxqs(cpsw);
1758         }
1759 
1760         for_each_slave(priv, cpsw_slave_stop, cpsw);
1761         pm_runtime_put_sync(cpsw->dev);
1762         netif_carrier_off(priv->ndev);
1763         return ret;
1764 }
1765 
1766 static int cpsw_ndo_stop(struct net_device *ndev)
1767 {
1768         struct cpsw_priv *priv = netdev_priv(ndev);
1769         struct cpsw_common *cpsw = priv->cpsw;
1770 
1771         cpsw_info(priv, ifdown, "shutting down cpsw device\n");
1772         __hw_addr_ref_unsync_dev(&ndev->mc, ndev, cpsw_purge_all_mc);
1773         netif_tx_stop_all_queues(priv->ndev);
1774         netif_carrier_off(priv->ndev);
1775 
1776         if (cpsw->usage_count <= 1) {
1777                 napi_disable(&cpsw->napi_rx);
1778                 napi_disable(&cpsw->napi_tx);
1779                 cpts_unregister(cpsw->cpts);
1780                 cpsw_intr_disable(cpsw);
1781                 cpdma_ctlr_stop(cpsw->dma);
1782                 cpsw_ale_stop(cpsw->ale);
1783                 cpsw_destroy_xdp_rxqs(cpsw);
1784         }
1785         for_each_slave(priv, cpsw_slave_stop, cpsw);
1786 
1787         if (cpsw_need_resplit(cpsw))
1788                 cpsw_split_res(cpsw);
1789 
1790         cpsw->usage_count--;
1791         pm_runtime_put_sync(cpsw->dev);
1792         return 0;
1793 }
1794 
1795 static netdev_tx_t cpsw_ndo_start_xmit(struct sk_buff *skb,
1796                                        struct net_device *ndev)
1797 {
1798         struct cpsw_priv *priv = netdev_priv(ndev);
1799         struct cpsw_common *cpsw = priv->cpsw;
1800         struct cpts *cpts = cpsw->cpts;
1801         struct netdev_queue *txq;
1802         struct cpdma_chan *txch;
1803         int ret, q_idx;
1804 
1805         if (skb_padto(skb, CPSW_MIN_PACKET_SIZE)) {
1806                 cpsw_err(priv, tx_err, "packet pad failed\n");
1807                 ndev->stats.tx_dropped++;
1808                 return NET_XMIT_DROP;
1809         }
1810 
1811         if (skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP &&
1812             priv->tx_ts_enabled && cpts_can_timestamp(cpts, skb))
1813                 skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
1814 
1815         q_idx = skb_get_queue_mapping(skb);
1816         if (q_idx >= cpsw->tx_ch_num)
1817                 q_idx = q_idx % cpsw->tx_ch_num;
1818 
1819         txch = cpsw->txv[q_idx].ch;
1820         txq = netdev_get_tx_queue(ndev, q_idx);
1821         skb_tx_timestamp(skb);
1822         ret = cpdma_chan_submit(txch, skb, skb->data, skb->len,
1823                                 priv->emac_port + cpsw->data.dual_emac);
1824         if (unlikely(ret != 0)) {
1825                 cpsw_err(priv, tx_err, "desc submit failed\n");
1826                 goto fail;
1827         }
1828 
1829         /* If there is no more tx desc left free then we need to
1830          * tell the kernel to stop sending us tx frames.
1831          */
1832         if (unlikely(!cpdma_check_free_tx_desc(txch))) {
1833                 netif_tx_stop_queue(txq);
1834 
1835                 /* Barrier, so that stop_queue visible to other cpus */
1836                 smp_mb__after_atomic();
1837 
1838                 if (cpdma_check_free_tx_desc(txch))
1839                         netif_tx_wake_queue(txq);
1840         }
1841 
1842         return NETDEV_TX_OK;
1843 fail:
1844         ndev->stats.tx_dropped++;
1845         netif_tx_stop_queue(txq);
1846 
1847         /* Barrier, so that stop_queue visible to other cpus */
1848         smp_mb__after_atomic();
1849 
1850         if (cpdma_check_free_tx_desc(txch))
1851                 netif_tx_wake_queue(txq);
1852 
1853         return NETDEV_TX_BUSY;
1854 }
1855 
1856 #if IS_ENABLED(CONFIG_TI_CPTS)
1857 
1858 static void cpsw_hwtstamp_v1(struct cpsw_priv *priv)
1859 {
1860         struct cpsw_common *cpsw = priv->cpsw;
1861         struct cpsw_slave *slave = &cpsw->slaves[cpsw->data.active_slave];
1862         u32 ts_en, seq_id;
1863 
1864         if (!priv->tx_ts_enabled && !priv->rx_ts_enabled) {
1865                 slave_write(slave, 0, CPSW1_TS_CTL);
1866                 return;
1867         }
1868 
1869         seq_id = (30 << CPSW_V1_SEQ_ID_OFS_SHIFT) | ETH_P_1588;
1870         ts_en = EVENT_MSG_BITS << CPSW_V1_MSG_TYPE_OFS;
1871 
1872         if (priv->tx_ts_enabled)
1873                 ts_en |= CPSW_V1_TS_TX_EN;
1874 
1875         if (priv->rx_ts_enabled)
1876                 ts_en |= CPSW_V1_TS_RX_EN;
1877 
1878         slave_write(slave, ts_en, CPSW1_TS_CTL);
1879         slave_write(slave, seq_id, CPSW1_TS_SEQ_LTYPE);
1880 }
1881 
1882 static void cpsw_hwtstamp_v2(struct cpsw_priv *priv)
1883 {
1884         struct cpsw_slave *slave;
1885         struct cpsw_common *cpsw = priv->cpsw;
1886         u32 ctrl, mtype;
1887 
1888         slave = &cpsw->slaves[cpsw_slave_index(cpsw, priv)];
1889 
1890         ctrl = slave_read(slave, CPSW2_CONTROL);
1891         switch (cpsw->version) {
1892         case CPSW_VERSION_2:
1893                 ctrl &= ~CTRL_V2_ALL_TS_MASK;
1894 
1895                 if (priv->tx_ts_enabled)
1896                         ctrl |= CTRL_V2_TX_TS_BITS;
1897 
1898                 if (priv->rx_ts_enabled)
1899                         ctrl |= CTRL_V2_RX_TS_BITS;
1900                 break;
1901         case CPSW_VERSION_3:
1902         default:
1903                 ctrl &= ~CTRL_V3_ALL_TS_MASK;
1904 
1905                 if (priv->tx_ts_enabled)
1906                         ctrl |= CTRL_V3_TX_TS_BITS;
1907 
1908                 if (priv->rx_ts_enabled)
1909                         ctrl |= CTRL_V3_RX_TS_BITS;
1910                 break;
1911         }
1912 
1913         mtype = (30 << TS_SEQ_ID_OFFSET_SHIFT) | EVENT_MSG_BITS;
1914 
1915         slave_write(slave, mtype, CPSW2_TS_SEQ_MTYPE);
1916         slave_write(slave, ctrl, CPSW2_CONTROL);
1917         writel_relaxed(ETH_P_1588, &cpsw->regs->ts_ltype);
1918         writel_relaxed(ETH_P_8021Q, &cpsw->regs->vlan_ltype);
1919 }
1920 
1921 static int cpsw_hwtstamp_set(struct net_device *dev, struct ifreq *ifr)
1922 {
1923         struct cpsw_priv *priv = netdev_priv(dev);
1924         struct hwtstamp_config cfg;
1925         struct cpsw_common *cpsw = priv->cpsw;
1926 
1927         if (cpsw->version != CPSW_VERSION_1 &&
1928             cpsw->version != CPSW_VERSION_2 &&
1929             cpsw->version != CPSW_VERSION_3)
1930                 return -EOPNOTSUPP;
1931 
1932         if (copy_from_user(&cfg, ifr->ifr_data, sizeof(cfg)))
1933                 return -EFAULT;
1934 
1935         /* reserved for future extensions */
1936         if (cfg.flags)
1937                 return -EINVAL;
1938 
1939         if (cfg.tx_type != HWTSTAMP_TX_OFF && cfg.tx_type != HWTSTAMP_TX_ON)
1940                 return -ERANGE;
1941 
1942         switch (cfg.rx_filter) {
1943         case HWTSTAMP_FILTER_NONE:
1944                 priv->rx_ts_enabled = 0;
1945                 break;
1946         case HWTSTAMP_FILTER_ALL:
1947         case HWTSTAMP_FILTER_NTP_ALL:
1948                 return -ERANGE;
1949         case HWTSTAMP_FILTER_PTP_V1_L4_EVENT:
1950         case HWTSTAMP_FILTER_PTP_V1_L4_SYNC:
1951         case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ:
1952                 priv->rx_ts_enabled = HWTSTAMP_FILTER_PTP_V1_L4_EVENT;
1953                 cfg.rx_filter = HWTSTAMP_FILTER_PTP_V1_L4_EVENT;
1954                 break;
1955         case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
1956         case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
1957         case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
1958         case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
1959         case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
1960         case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
1961         case HWTSTAMP_FILTER_PTP_V2_EVENT:
1962         case HWTSTAMP_FILTER_PTP_V2_SYNC:
1963         case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
1964                 priv->rx_ts_enabled = HWTSTAMP_FILTER_PTP_V2_EVENT;
1965                 cfg.rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
1966                 break;
1967         default:
1968                 return -ERANGE;
1969         }
1970 
1971         priv->tx_ts_enabled = cfg.tx_type == HWTSTAMP_TX_ON;
1972 
1973         switch (cpsw->version) {
1974         case CPSW_VERSION_1:
1975                 cpsw_hwtstamp_v1(priv);
1976                 break;
1977         case CPSW_VERSION_2:
1978         case CPSW_VERSION_3:
1979                 cpsw_hwtstamp_v2(priv);
1980                 break;
1981         default:
1982                 WARN_ON(1);
1983         }
1984 
1985         return copy_to_user(ifr->ifr_data, &cfg, sizeof(cfg)) ? -EFAULT : 0;
1986 }
1987 
1988 static int cpsw_hwtstamp_get(struct net_device *dev, struct ifreq *ifr)
1989 {
1990         struct cpsw_common *cpsw = ndev_to_cpsw(dev);
1991         struct cpsw_priv *priv = netdev_priv(dev);
1992         struct hwtstamp_config cfg;
1993 
1994         if (cpsw->version != CPSW_VERSION_1 &&
1995             cpsw->version != CPSW_VERSION_2 &&
1996             cpsw->version != CPSW_VERSION_3)
1997                 return -EOPNOTSUPP;
1998 
1999         cfg.flags = 0;
2000         cfg.tx_type = priv->tx_ts_enabled ? HWTSTAMP_TX_ON : HWTSTAMP_TX_OFF;
2001         cfg.rx_filter = priv->rx_ts_enabled;
2002 
2003         return copy_to_user(ifr->ifr_data, &cfg, sizeof(cfg)) ? -EFAULT : 0;
2004 }
2005 #else
2006 static int cpsw_hwtstamp_get(struct net_device *dev, struct ifreq *ifr)
2007 {
2008         return -EOPNOTSUPP;
2009 }
2010 
2011 static int cpsw_hwtstamp_set(struct net_device *dev, struct ifreq *ifr)
2012 {
2013         return -EOPNOTSUPP;
2014 }
2015 #endif /*CONFIG_TI_CPTS*/
2016 
2017 static int cpsw_ndo_ioctl(struct net_device *dev, struct ifreq *req, int cmd)
2018 {
2019         struct cpsw_priv *priv = netdev_priv(dev);
2020         struct cpsw_common *cpsw = priv->cpsw;
2021         int slave_no = cpsw_slave_index(cpsw, priv);
2022 
2023         if (!netif_running(dev))
2024                 return -EINVAL;
2025 
2026         switch (cmd) {
2027         case SIOCSHWTSTAMP:
2028                 return cpsw_hwtstamp_set(dev, req);
2029         case SIOCGHWTSTAMP:
2030                 return cpsw_hwtstamp_get(dev, req);
2031         }
2032 
2033         if (!cpsw->slaves[slave_no].phy)
2034                 return -EOPNOTSUPP;
2035         return phy_mii_ioctl(cpsw->slaves[slave_no].phy, req, cmd);
2036 }
2037 
2038 static void cpsw_ndo_tx_timeout(struct net_device *ndev)
2039 {
2040         struct cpsw_priv *priv = netdev_priv(ndev);
2041         struct cpsw_common *cpsw = priv->cpsw;
2042         int ch;
2043 
2044         cpsw_err(priv, tx_err, "transmit timeout, restarting dma\n");
2045         ndev->stats.tx_errors++;
2046         cpsw_intr_disable(cpsw);
2047         for (ch = 0; ch < cpsw->tx_ch_num; ch++) {
2048                 cpdma_chan_stop(cpsw->txv[ch].ch);
2049                 cpdma_chan_start(cpsw->txv[ch].ch);
2050         }
2051 
2052         cpsw_intr_enable(cpsw);
2053         netif_trans_update(ndev);
2054         netif_tx_wake_all_queues(ndev);
2055 }
2056 
2057 static int cpsw_ndo_set_mac_address(struct net_device *ndev, void *p)
2058 {
2059         struct cpsw_priv *priv = netdev_priv(ndev);
2060         struct sockaddr *addr = (struct sockaddr *)p;
2061         struct cpsw_common *cpsw = priv->cpsw;
2062         int flags = 0;
2063         u16 vid = 0;
2064         int ret;
2065 
2066         if (!is_valid_ether_addr(addr->sa_data))
2067                 return -EADDRNOTAVAIL;
2068 
2069         ret = pm_runtime_get_sync(cpsw->dev);
2070         if (ret < 0) {
2071                 pm_runtime_put_noidle(cpsw->dev);
2072                 return ret;
2073         }
2074 
2075         if (cpsw->data.dual_emac) {
2076                 vid = cpsw->slaves[priv->emac_port].port_vlan;
2077                 flags = ALE_VLAN;
2078         }
2079 
2080         cpsw_ale_del_ucast(cpsw->ale, priv->mac_addr, HOST_PORT_NUM,
2081                            flags, vid);
2082         cpsw_ale_add_ucast(cpsw->ale, addr->sa_data, HOST_PORT_NUM,
2083                            flags, vid);
2084 
2085         memcpy(priv->mac_addr, addr->sa_data, ETH_ALEN);
2086         memcpy(ndev->dev_addr, priv->mac_addr, ETH_ALEN);
2087         for_each_slave(priv, cpsw_set_slave_mac, priv);
2088 
2089         pm_runtime_put(cpsw->dev);
2090 
2091         return 0;
2092 }
2093 
2094 static inline int cpsw_add_vlan_ale_entry(struct cpsw_priv *priv,
2095                                 unsigned short vid)
2096 {
2097         int ret;
2098         int unreg_mcast_mask = 0;
2099         int mcast_mask;
2100         u32 port_mask;
2101         struct cpsw_common *cpsw = priv->cpsw;
2102 
2103         if (cpsw->data.dual_emac) {
2104                 port_mask = (1 << (priv->emac_port + 1)) | ALE_PORT_HOST;
2105 
2106                 mcast_mask = ALE_PORT_HOST;
2107                 if (priv->ndev->flags & IFF_ALLMULTI)
2108                         unreg_mcast_mask = mcast_mask;
2109         } else {
2110                 port_mask = ALE_ALL_PORTS;
2111                 mcast_mask = port_mask;
2112 
2113                 if (priv->ndev->flags & IFF_ALLMULTI)
2114                         unreg_mcast_mask = ALE_ALL_PORTS;
2115                 else
2116                         unreg_mcast_mask = ALE_PORT_1 | ALE_PORT_2;
2117         }
2118 
2119         ret = cpsw_ale_add_vlan(cpsw->ale, vid, port_mask, 0, port_mask,
2120                                 unreg_mcast_mask);
2121         if (ret != 0)
2122                 return ret;
2123 
2124         ret = cpsw_ale_add_ucast(cpsw->ale, priv->mac_addr,
2125                                  HOST_PORT_NUM, ALE_VLAN, vid);
2126         if (ret != 0)
2127                 goto clean_vid;
2128 
2129         ret = cpsw_ale_add_mcast(cpsw->ale, priv->ndev->broadcast,
2130                                  mcast_mask, ALE_VLAN, vid, 0);
2131         if (ret != 0)
2132                 goto clean_vlan_ucast;
2133         return 0;
2134 
2135 clean_vlan_ucast:
2136         cpsw_ale_del_ucast(cpsw->ale, priv->mac_addr,
2137                            HOST_PORT_NUM, ALE_VLAN, vid);
2138 clean_vid:
2139         cpsw_ale_del_vlan(cpsw->ale, vid, 0);
2140         return ret;
2141 }
2142 
2143 static int cpsw_ndo_vlan_rx_add_vid(struct net_device *ndev,
2144                                     __be16 proto, u16 vid)
2145 {
2146         struct cpsw_priv *priv = netdev_priv(ndev);
2147         struct cpsw_common *cpsw = priv->cpsw;
2148         int ret;
2149 
2150         if (vid == cpsw->data.default_vlan)
2151                 return 0;
2152 
2153         ret = pm_runtime_get_sync(cpsw->dev);
2154         if (ret < 0) {
2155                 pm_runtime_put_noidle(cpsw->dev);
2156                 return ret;
2157         }
2158 
2159         if (cpsw->data.dual_emac) {
2160                 /* In dual EMAC, reserved VLAN id should not be used for
2161                  * creating VLAN interfaces as this can break the dual
2162                  * EMAC port separation
2163                  */
2164                 int i;
2165 
2166                 for (i = 0; i < cpsw->data.slaves; i++) {
2167                         if (vid == cpsw->slaves[i].port_vlan) {
2168                                 ret = -EINVAL;
2169                                 goto err;
2170                         }
2171                 }
2172         }
2173 
2174         dev_info(priv->dev, "Adding vlanid %d to vlan filter\n", vid);
2175         ret = cpsw_add_vlan_ale_entry(priv, vid);
2176 err:
2177         pm_runtime_put(cpsw->dev);
2178         return ret;
2179 }
2180 
2181 static int cpsw_ndo_vlan_rx_kill_vid(struct net_device *ndev,
2182                                      __be16 proto, u16 vid)
2183 {
2184         struct cpsw_priv *priv = netdev_priv(ndev);
2185         struct cpsw_common *cpsw = priv->cpsw;
2186         int ret;
2187 
2188         if (vid == cpsw->data.default_vlan)
2189                 return 0;
2190 
2191         ret = pm_runtime_get_sync(cpsw->dev);
2192         if (ret < 0) {
2193                 pm_runtime_put_noidle(cpsw->dev);
2194                 return ret;
2195         }
2196 
2197         if (cpsw->data.dual_emac) {
2198                 int i;
2199 
2200                 for (i = 0; i < cpsw->data.slaves; i++) {
2201                         if (vid == cpsw->slaves[i].port_vlan)
2202                                 goto err;
2203                 }
2204         }
2205 
2206         dev_info(priv->dev, "removing vlanid %d from vlan filter\n", vid);
2207         ret = cpsw_ale_del_vlan(cpsw->ale, vid, 0);
2208         ret |= cpsw_ale_del_ucast(cpsw->ale, priv->mac_addr,
2209                                   HOST_PORT_NUM, ALE_VLAN, vid);
2210         ret |= cpsw_ale_del_mcast(cpsw->ale, priv->ndev->broadcast,
2211                                   0, ALE_VLAN, vid);
2212         ret |= cpsw_ale_flush_multicast(cpsw->ale, 0, vid);
2213 err:
2214         pm_runtime_put(cpsw->dev);
2215         return ret;
2216 }
2217 
2218 static int cpsw_ndo_set_tx_maxrate(struct net_device *ndev, int queue, u32 rate)
2219 {
2220         struct cpsw_priv *priv = netdev_priv(ndev);
2221         struct cpsw_common *cpsw = priv->cpsw;
2222         struct cpsw_slave *slave;
2223         u32 min_rate;
2224         u32 ch_rate;
2225         int i, ret;
2226 
2227         ch_rate = netdev_get_tx_queue(ndev, queue)->tx_maxrate;
2228         if (ch_rate == rate)
2229                 return 0;
2230 
2231         ch_rate = rate * 1000;
2232         min_rate = cpdma_chan_get_min_rate(cpsw->dma);
2233         if ((ch_rate < min_rate && ch_rate)) {
2234                 dev_err(priv->dev, "The channel rate cannot be less than %dMbps",
2235                         min_rate);
2236                 return -EINVAL;
2237         }
2238 
2239         if (rate > cpsw->speed) {
2240                 dev_err(priv->dev, "The channel rate cannot be more than 2Gbps");
2241                 return -EINVAL;
2242         }
2243 
2244         ret = pm_runtime_get_sync(cpsw->dev);
2245         if (ret < 0) {
2246                 pm_runtime_put_noidle(cpsw->dev);
2247                 return ret;
2248         }
2249 
2250         ret = cpdma_chan_set_rate(cpsw->txv[queue].ch, ch_rate);
2251         pm_runtime_put(cpsw->dev);
2252 
2253         if (ret)
2254                 return ret;
2255 
2256         /* update rates for slaves tx queues */
2257         for (i = 0; i < cpsw->data.slaves; i++) {
2258                 slave = &cpsw->slaves[i];
2259                 if (!slave->ndev)
2260                         continue;
2261 
2262                 netdev_get_tx_queue(slave->ndev, queue)->tx_maxrate = rate;
2263         }
2264 
2265         cpsw_split_res(cpsw);
2266         return ret;
2267 }
2268 
2269 static int cpsw_set_mqprio(struct net_device *ndev, void *type_data)
2270 {
2271         struct tc_mqprio_qopt_offload *mqprio = type_data;
2272         struct cpsw_priv *priv = netdev_priv(ndev);
2273         struct cpsw_common *cpsw = priv->cpsw;
2274         int fifo, num_tc, count, offset;
2275         struct cpsw_slave *slave;
2276         u32 tx_prio_map = 0;
2277         int i, tc, ret;
2278 
2279         num_tc = mqprio->qopt.num_tc;
2280         if (num_tc > CPSW_TC_NUM)
2281                 return -EINVAL;
2282 
2283         if (mqprio->mode != TC_MQPRIO_MODE_DCB)
2284                 return -EINVAL;
2285 
2286         ret = pm_runtime_get_sync(cpsw->dev);
2287         if (ret < 0) {
2288                 pm_runtime_put_noidle(cpsw->dev);
2289                 return ret;
2290         }
2291 
2292         if (num_tc) {
2293                 for (i = 0; i < 8; i++) {
2294                         tc = mqprio->qopt.prio_tc_map[i];
2295                         fifo = cpsw_tc_to_fifo(tc, num_tc);
2296                         tx_prio_map |= fifo << (4 * i);
2297                 }
2298 
2299                 netdev_set_num_tc(ndev, num_tc);
2300                 for (i = 0; i < num_tc; i++) {
2301                         count = mqprio->qopt.count[i];
2302                         offset = mqprio->qopt.offset[i];
2303                         netdev_set_tc_queue(ndev, i, count, offset);
2304                 }
2305         }
2306 
2307         if (!mqprio->qopt.hw) {
2308                 /* restore default configuration */
2309                 netdev_reset_tc(ndev);
2310                 tx_prio_map = TX_PRIORITY_MAPPING;
2311         }
2312 
2313         priv->mqprio_hw = mqprio->qopt.hw;
2314 
2315         offset = cpsw->version == CPSW_VERSION_1 ?
2316                  CPSW1_TX_PRI_MAP : CPSW2_TX_PRI_MAP;
2317 
2318         slave = &cpsw->slaves[cpsw_slave_index(cpsw, priv)];
2319         slave_write(slave, tx_prio_map, offset);
2320 
2321         pm_runtime_put_sync(cpsw->dev);
2322 
2323         return 0;
2324 }
2325 
2326 static int cpsw_ndo_setup_tc(struct net_device *ndev, enum tc_setup_type type,
2327                              void *type_data)
2328 {
2329         switch (type) {
2330         case TC_SETUP_QDISC_CBS:
2331                 return cpsw_set_cbs(ndev, type_data);
2332 
2333         case TC_SETUP_QDISC_MQPRIO:
2334                 return cpsw_set_mqprio(ndev, type_data);
2335 
2336         default:
2337                 return -EOPNOTSUPP;
2338         }
2339 }
2340 
2341 static int cpsw_xdp_prog_setup(struct cpsw_priv *priv, struct netdev_bpf *bpf)
2342 {
2343         struct bpf_prog *prog = bpf->prog;
2344 
2345         if (!priv->xdpi.prog && !prog)
2346                 return 0;
2347 
2348         if (!xdp_attachment_flags_ok(&priv->xdpi, bpf))
2349                 return -EBUSY;
2350 
2351         WRITE_ONCE(priv->xdp_prog, prog);
2352 
2353         xdp_attachment_setup(&priv->xdpi, bpf);
2354 
2355         return 0;
2356 }
2357 
2358 static int cpsw_ndo_bpf(struct net_device *ndev, struct netdev_bpf *bpf)
2359 {
2360         struct cpsw_priv *priv = netdev_priv(ndev);
2361 
2362         switch (bpf->command) {
2363         case XDP_SETUP_PROG:
2364                 return cpsw_xdp_prog_setup(priv, bpf);
2365 
2366         case XDP_QUERY_PROG:
2367                 return xdp_attachment_query(&priv->xdpi, bpf);
2368 
2369         default:
2370                 return -EINVAL;
2371         }
2372 }
2373 
2374 static int cpsw_ndo_xdp_xmit(struct net_device *ndev, int n,
2375                              struct xdp_frame **frames, u32 flags)
2376 {
2377         struct cpsw_priv *priv = netdev_priv(ndev);
2378         struct xdp_frame *xdpf;
2379         int i, drops = 0;
2380 
2381         if (unlikely(flags & ~XDP_XMIT_FLAGS_MASK))
2382                 return -EINVAL;
2383 
2384         for (i = 0; i < n; i++) {
2385                 xdpf = frames[i];
2386                 if (xdpf->len < CPSW_MIN_PACKET_SIZE) {
2387                         xdp_return_frame_rx_napi(xdpf);
2388                         drops++;
2389                         continue;
2390                 }
2391 
2392                 if (cpsw_xdp_tx_frame(priv, xdpf, NULL))
2393                         drops++;
2394         }
2395 
2396         return n - drops;
2397 }
2398 
2399 #ifdef CONFIG_NET_POLL_CONTROLLER
2400 static void cpsw_ndo_poll_controller(struct net_device *ndev)
2401 {
2402         struct cpsw_common *cpsw = ndev_to_cpsw(ndev);
2403 
2404         cpsw_intr_disable(cpsw);
2405         cpsw_rx_interrupt(cpsw->irqs_table[0], cpsw);
2406         cpsw_tx_interrupt(cpsw->irqs_table[1], cpsw);
2407         cpsw_intr_enable(cpsw);
2408 }
2409 #endif
2410 
2411 static const struct net_device_ops cpsw_netdev_ops = {
2412         .ndo_open               = cpsw_ndo_open,
2413         .ndo_stop               = cpsw_ndo_stop,
2414         .ndo_start_xmit         = cpsw_ndo_start_xmit,
2415         .ndo_set_mac_address    = cpsw_ndo_set_mac_address,
2416         .ndo_do_ioctl           = cpsw_ndo_ioctl,
2417         .ndo_validate_addr      = eth_validate_addr,
2418         .ndo_tx_timeout         = cpsw_ndo_tx_timeout,
2419         .ndo_set_rx_mode        = cpsw_ndo_set_rx_mode,
2420         .ndo_set_tx_maxrate     = cpsw_ndo_set_tx_maxrate,
2421 #ifdef CONFIG_NET_POLL_CONTROLLER
2422         .ndo_poll_controller    = cpsw_ndo_poll_controller,
2423 #endif
2424         .ndo_vlan_rx_add_vid    = cpsw_ndo_vlan_rx_add_vid,
2425         .ndo_vlan_rx_kill_vid   = cpsw_ndo_vlan_rx_kill_vid,
2426         .ndo_setup_tc           = cpsw_ndo_setup_tc,
2427         .ndo_bpf                = cpsw_ndo_bpf,
2428         .ndo_xdp_xmit           = cpsw_ndo_xdp_xmit,
2429 };
2430 
2431 static void cpsw_get_drvinfo(struct net_device *ndev,
2432                              struct ethtool_drvinfo *info)
2433 {
2434         struct cpsw_common *cpsw = ndev_to_cpsw(ndev);
2435         struct platform_device  *pdev = to_platform_device(cpsw->dev);
2436 
2437         strlcpy(info->driver, "cpsw", sizeof(info->driver));
2438         strlcpy(info->version, "1.0", sizeof(info->version));
2439         strlcpy(info->bus_info, pdev->name, sizeof(info->bus_info));
2440 }
2441 
2442 static int cpsw_set_pauseparam(struct net_device *ndev,
2443                                struct ethtool_pauseparam *pause)
2444 {
2445         struct cpsw_priv *priv = netdev_priv(ndev);
2446         bool link;
2447 
2448         priv->rx_pause = pause->rx_pause ? true : false;
2449         priv->tx_pause = pause->tx_pause ? true : false;
2450 
2451         for_each_slave(priv, _cpsw_adjust_link, priv, &link);
2452         return 0;
2453 }
2454 
2455 static int cpsw_set_channels(struct net_device *ndev,
2456                              struct ethtool_channels *chs)
2457 {
2458         return cpsw_set_channels_common(ndev, chs, cpsw_rx_handler);
2459 }
2460 
2461 static const struct ethtool_ops cpsw_ethtool_ops = {
2462         .get_drvinfo    = cpsw_get_drvinfo,
2463         .get_msglevel   = cpsw_get_msglevel,
2464         .set_msglevel   = cpsw_set_msglevel,
2465         .get_link       = ethtool_op_get_link,
2466         .get_ts_info    = cpsw_get_ts_info,
2467         .get_coalesce   = cpsw_get_coalesce,
2468         .set_coalesce   = cpsw_set_coalesce,
2469         .get_sset_count         = cpsw_get_sset_count,
2470         .get_strings            = cpsw_get_strings,
2471         .get_ethtool_stats      = cpsw_get_ethtool_stats,
2472         .get_pauseparam         = cpsw_get_pauseparam,
2473         .set_pauseparam         = cpsw_set_pauseparam,
2474         .get_wol        = cpsw_get_wol,
2475         .set_wol        = cpsw_set_wol,
2476         .get_regs_len   = cpsw_get_regs_len,
2477         .get_regs       = cpsw_get_regs,
2478         .begin          = cpsw_ethtool_op_begin,
2479         .complete       = cpsw_ethtool_op_complete,
2480         .get_channels   = cpsw_get_channels,
2481         .set_channels   = cpsw_set_channels,
2482         .get_link_ksettings     = cpsw_get_link_ksettings,
2483         .set_link_ksettings     = cpsw_set_link_ksettings,
2484         .get_eee        = cpsw_get_eee,
2485         .set_eee        = cpsw_set_eee,
2486         .nway_reset     = cpsw_nway_reset,
2487         .get_ringparam = cpsw_get_ringparam,
2488         .set_ringparam = cpsw_set_ringparam,
2489 };
2490 
2491 static int cpsw_probe_dt(struct cpsw_platform_data *data,
2492                          struct platform_device *pdev)
2493 {
2494         struct device_node *node = pdev->dev.of_node;
2495         struct device_node *slave_node;
2496         int i = 0, ret;
2497         u32 prop;
2498 
2499         if (!node)
2500                 return -EINVAL;
2501 
2502         if (of_property_read_u32(node, "slaves", &prop)) {
2503                 dev_err(&pdev->dev, "Missing slaves property in the DT.\n");
2504                 return -EINVAL;
2505         }
2506         data->slaves = prop;
2507 
2508         if (of_property_read_u32(node, "active_slave", &prop)) {
2509                 dev_err(&pdev->dev, "Missing active_slave property in the DT.\n");
2510                 return -EINVAL;
2511         }
2512         data->active_slave = prop;
2513 
2514         data->slave_data = devm_kcalloc(&pdev->dev,
2515                                         data->slaves,
2516                                         sizeof(struct cpsw_slave_data),
2517                                         GFP_KERNEL);
2518         if (!data->slave_data)
2519                 return -ENOMEM;
2520 
2521         if (of_property_read_u32(node, "cpdma_channels", &prop)) {
2522                 dev_err(&pdev->dev, "Missing cpdma_channels property in the DT.\n");
2523                 return -EINVAL;
2524         }
2525         data->channels = prop;
2526 
2527         if (of_property_read_u32(node, "ale_entries", &prop)) {
2528                 dev_err(&pdev->dev, "Missing ale_entries property in the DT.\n");
2529                 return -EINVAL;
2530         }
2531         data->ale_entries = prop;
2532 
2533         if (of_property_read_u32(node, "bd_ram_size", &prop)) {
2534                 dev_err(&pdev->dev, "Missing bd_ram_size property in the DT.\n");
2535                 return -EINVAL;
2536         }
2537         data->bd_ram_size = prop;
2538 
2539         if (of_property_read_u32(node, "mac_control", &prop)) {
2540                 dev_err(&pdev->dev, "Missing mac_control property in the DT.\n");
2541                 return -EINVAL;
2542         }
2543         data->mac_control = prop;
2544 
2545         if (of_property_read_bool(node, "dual_emac"))
2546                 data->dual_emac = 1;
2547 
2548         /*
2549          * Populate all the child nodes here...
2550          */
2551         ret = of_platform_populate(node, NULL, NULL, &pdev->dev);
2552         /* We do not want to force this, as in some cases may not have child */
2553         if (ret)
2554                 dev_warn(&pdev->dev, "Doesn't have any child node\n");
2555 
2556         for_each_available_child_of_node(node, slave_node) {
2557                 struct cpsw_slave_data *slave_data = data->slave_data + i;
2558                 const void *mac_addr = NULL;
2559                 int lenp;
2560                 const __be32 *parp;
2561 
2562                 /* This is no slave child node, continue */
2563                 if (!of_node_name_eq(slave_node, "slave"))
2564                         continue;
2565 
2566                 slave_data->ifphy = devm_of_phy_get(&pdev->dev, slave_node,
2567                                                     NULL);
2568                 if (!IS_ENABLED(CONFIG_TI_CPSW_PHY_SEL) &&
2569                     IS_ERR(slave_data->ifphy)) {
2570                         ret = PTR_ERR(slave_data->ifphy);
2571                         dev_err(&pdev->dev,
2572                                 "%d: Error retrieving port phy: %d\n", i, ret);
2573                         goto err_node_put;
2574                 }
2575 
2576                 slave_data->slave_node = slave_node;
2577                 slave_data->phy_node = of_parse_phandle(slave_node,
2578                                                         "phy-handle", 0);
2579                 parp = of_get_property(slave_node, "phy_id", &lenp);
2580                 if (slave_data->phy_node) {
2581                         dev_dbg(&pdev->dev,
2582                                 "slave[%d] using phy-handle=\"%pOF\"\n",
2583                                 i, slave_data->phy_node);
2584                 } else if (of_phy_is_fixed_link(slave_node)) {
2585                         /* In the case of a fixed PHY, the DT node associated
2586                          * to the PHY is the Ethernet MAC DT node.
2587                          */
2588                         ret = of_phy_register_fixed_link(slave_node);
2589                         if (ret) {
2590                                 if (ret != -EPROBE_DEFER)
2591                                         dev_err(&pdev->dev, "failed to register fixed-link phy: %d\n", ret);
2592                                 goto err_node_put;
2593                         }
2594                         slave_data->phy_node = of_node_get(slave_node);
2595                 } else if (parp) {
2596                         u32 phyid;
2597                         struct device_node *mdio_node;
2598                         struct platform_device *mdio;
2599 
2600                         if (lenp != (sizeof(__be32) * 2)) {
2601                                 dev_err(&pdev->dev, "Invalid slave[%d] phy_id property\n", i);
2602                                 goto no_phy_slave;
2603                         }
2604                         mdio_node = of_find_node_by_phandle(be32_to_cpup(parp));
2605                         phyid = be32_to_cpup(parp+1);
2606                         mdio = of_find_device_by_node(mdio_node);
2607                         of_node_put(mdio_node);
2608                         if (!mdio) {
2609                                 dev_err(&pdev->dev, "Missing mdio platform device\n");
2610                                 ret = -EINVAL;
2611                                 goto err_node_put;
2612                         }
2613                         snprintf(slave_data->phy_id, sizeof(slave_data->phy_id),
2614                                  PHY_ID_FMT, mdio->name, phyid);
2615                         put_device(&mdio->dev);
2616                 } else {
2617                         dev_err(&pdev->dev,
2618                                 "No slave[%d] phy_id, phy-handle, or fixed-link property\n",
2619                                 i);
2620                         goto no_phy_slave;
2621                 }
2622                 slave_data->phy_if = of_get_phy_mode(slave_node);
2623                 if (slave_data->phy_if < 0) {
2624                         dev_err(&pdev->dev, "Missing or malformed slave[%d] phy-mode property\n",
2625                                 i);
2626                         ret = slave_data->phy_if;
2627                         goto err_node_put;
2628                 }
2629 
2630 no_phy_slave:
2631                 mac_addr = of_get_mac_address(slave_node);
2632                 if (!IS_ERR(mac_addr)) {
2633                         ether_addr_copy(slave_data->mac_addr, mac_addr);
2634                 } else {
2635                         ret = ti_cm_get_macid(&pdev->dev, i,
2636                                               slave_data->mac_addr);
2637                         if (ret)
2638                                 goto err_node_put;
2639                 }
2640                 if (data->dual_emac) {
2641                         if (of_property_read_u32(slave_node, "dual_emac_res_vlan",
2642                                                  &prop)) {
2643                                 dev_err(&pdev->dev, "Missing dual_emac_res_vlan in DT.\n");
2644                                 slave_data->dual_emac_res_vlan = i+1;
2645                                 dev_err(&pdev->dev, "Using %d as Reserved VLAN for %d slave\n",
2646                                         slave_data->dual_emac_res_vlan, i);
2647                         } else {
2648                                 slave_data->dual_emac_res_vlan = prop;
2649                         }
2650                 }
2651 
2652                 i++;
2653                 if (i == data->slaves) {
2654                         ret = 0;
2655                         goto err_node_put;
2656                 }
2657         }
2658 
2659         return 0;
2660 
2661 err_node_put:
2662         of_node_put(slave_node);
2663         return ret;
2664 }
2665 
2666 static void cpsw_remove_dt(struct platform_device *pdev)
2667 {
2668         struct cpsw_common *cpsw = platform_get_drvdata(pdev);
2669         struct cpsw_platform_data *data = &cpsw->data;
2670         struct device_node *node = pdev->dev.of_node;
2671         struct device_node *slave_node;
2672         int i = 0;
2673 
2674         for_each_available_child_of_node(node, slave_node) {
2675                 struct cpsw_slave_data *slave_data = &data->slave_data[i];
2676 
2677                 if (!of_node_name_eq(slave_node, "slave"))
2678                         continue;
2679 
2680                 if (of_phy_is_fixed_link(slave_node))
2681                         of_phy_deregister_fixed_link(slave_node);
2682 
2683                 of_node_put(slave_data->phy_node);
2684 
2685                 i++;
2686                 if (i == data->slaves) {
2687                         of_node_put(slave_node);
2688                         break;
2689                 }
2690         }
2691 
2692         of_platform_depopulate(&pdev->dev);
2693 }
2694 
2695 static int cpsw_probe_dual_emac(struct cpsw_priv *priv)
2696 {
2697         struct cpsw_common              *cpsw = priv->cpsw;
2698         struct cpsw_platform_data       *data = &cpsw->data;
2699         struct net_device               *ndev;
2700         struct cpsw_priv                *priv_sl2;
2701         int ret = 0;
2702 
2703         ndev = devm_alloc_etherdev_mqs(cpsw->dev, sizeof(struct cpsw_priv),
2704                                        CPSW_MAX_QUEUES, CPSW_MAX_QUEUES);
2705         if (!ndev) {
2706                 dev_err(cpsw->dev, "cpsw: error allocating net_device\n");
2707                 return -ENOMEM;
2708         }
2709 
2710         priv_sl2 = netdev_priv(ndev);
2711         priv_sl2->cpsw = cpsw;
2712         priv_sl2->ndev = ndev;
2713         priv_sl2->dev  = &ndev->dev;
2714         priv_sl2->msg_enable = netif_msg_init(debug_level, CPSW_DEBUG);
2715 
2716         if (is_valid_ether_addr(data->slave_data[1].mac_addr)) {
2717                 memcpy(priv_sl2->mac_addr, data->slave_data[1].mac_addr,
2718                         ETH_ALEN);
2719                 dev_info(cpsw->dev, "cpsw: Detected MACID = %pM\n",
2720                          priv_sl2->mac_addr);
2721         } else {
2722                 eth_random_addr(priv_sl2->mac_addr);
2723                 dev_info(cpsw->dev, "cpsw: Random MACID = %pM\n",
2724                          priv_sl2->mac_addr);
2725         }
2726         memcpy(ndev->dev_addr, priv_sl2->mac_addr, ETH_ALEN);
2727 
2728         priv_sl2->emac_port = 1;
2729         cpsw->slaves[1].ndev = ndev;
2730         ndev->features |= NETIF_F_HW_VLAN_CTAG_FILTER | NETIF_F_HW_VLAN_CTAG_RX;
2731 
2732         ndev->netdev_ops = &cpsw_netdev_ops;
2733         ndev->ethtool_ops = &cpsw_ethtool_ops;
2734 
2735         /* register the network device */
2736         SET_NETDEV_DEV(ndev, cpsw->dev);
2737         ndev->dev.of_node = cpsw->slaves[1].data->slave_node;
2738         ret = register_netdev(ndev);
2739         if (ret)
2740                 dev_err(cpsw->dev, "cpsw: error registering net device\n");
2741 
2742         return ret;
2743 }
2744 
2745 static const struct of_device_id cpsw_of_mtable[] = {
2746         { .compatible = "ti,cpsw"},
2747         { .compatible = "ti,am335x-cpsw"},
2748         { .compatible = "ti,am4372-cpsw"},
2749         { .compatible = "ti,dra7-cpsw"},
2750         { /* sentinel */ },
2751 };
2752 MODULE_DEVICE_TABLE(of, cpsw_of_mtable);
2753 
2754 static const struct soc_device_attribute cpsw_soc_devices[] = {
2755         { .family = "AM33xx", .revision = "ES1.0"},
2756         { /* sentinel */ }
2757 };
2758 
2759 static int cpsw_probe(struct platform_device *pdev)
2760 {
2761         struct device                   *dev = &pdev->dev;
2762         struct clk                      *clk;
2763         struct cpsw_platform_data       *data;
2764         struct net_device               *ndev;
2765         struct cpsw_priv                *priv;
2766         void __iomem                    *ss_regs;
2767         struct resource                 *ss_res;
2768         struct gpio_descs               *mode;
2769         const struct soc_device_attribute *soc;
2770         struct cpsw_common              *cpsw;
2771         int ret = 0, ch;
2772         int irq;
2773 
2774         cpsw = devm_kzalloc(dev, sizeof(struct cpsw_common), GFP_KERNEL);
2775         if (!cpsw)
2776                 return -ENOMEM;
2777 
2778         platform_set_drvdata(pdev, cpsw);
2779         cpsw->dev = dev;
2780 
2781         mode = devm_gpiod_get_array_optional(dev, "mode", GPIOD_OUT_LOW);
2782         if (IS_ERR(mode)) {
2783                 ret = PTR_ERR(mode);
2784                 dev_err(dev, "gpio request failed, ret %d\n", ret);
2785                 return ret;
2786         }
2787 
2788         clk = devm_clk_get(dev, "fck");
2789         if (IS_ERR(clk)) {
2790                 ret = PTR_ERR(clk);
2791                 dev_err(dev, "fck is not found %d\n", ret);
2792                 return ret;
2793         }
2794         cpsw->bus_freq_mhz = clk_get_rate(clk) / 1000000;
2795 
2796         ss_res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
2797         ss_regs = devm_ioremap_resource(dev, ss_res);
2798         if (IS_ERR(ss_regs))
2799                 return PTR_ERR(ss_regs);
2800         cpsw->regs = ss_regs;
2801 
2802         cpsw->wr_regs = devm_platform_ioremap_resource(pdev, 1);
2803         if (IS_ERR(cpsw->wr_regs))
2804                 return PTR_ERR(cpsw->wr_regs);
2805 
2806         /* RX IRQ */
2807         irq = platform_get_irq(pdev, 1);
2808         if (irq < 0)
2809                 return irq;
2810         cpsw->irqs_table[0] = irq;
2811 
2812         /* TX IRQ */
2813         irq = platform_get_irq(pdev, 2);
2814         if (irq < 0)
2815                 return irq;
2816         cpsw->irqs_table[1] = irq;
2817 
2818         /*
2819          * This may be required here for child devices.
2820          */
2821         pm_runtime_enable(dev);
2822 
2823         /* Need to enable clocks with runtime PM api to access module
2824          * registers
2825          */
2826         ret = pm_runtime_get_sync(dev);
2827         if (ret < 0) {
2828                 pm_runtime_put_noidle(dev);
2829                 goto clean_runtime_disable_ret;
2830         }
2831 
2832         ret = cpsw_probe_dt(&cpsw->data, pdev);
2833         if (ret)
2834                 goto clean_dt_ret;
2835 
2836         soc = soc_device_match(cpsw_soc_devices);
2837         if (soc)
2838                 cpsw->quirk_irq = 1;
2839 
2840         data = &cpsw->data;
2841         cpsw->slaves = devm_kcalloc(dev,
2842                                     data->slaves, sizeof(struct cpsw_slave),
2843                                     GFP_KERNEL);
2844         if (!cpsw->slaves) {
2845                 ret = -ENOMEM;
2846                 goto clean_dt_ret;
2847         }
2848 
2849         cpsw->rx_packet_max = max(rx_packet_max, CPSW_MAX_PACKET_SIZE);
2850         cpsw->descs_pool_size = descs_pool_size;
2851 
2852         ret = cpsw_init_common(cpsw, ss_regs, ale_ageout,
2853                                ss_res->start + CPSW2_BD_OFFSET,
2854                                descs_pool_size);
2855         if (ret)
2856                 goto clean_dt_ret;
2857 
2858         ch = cpsw->quirk_irq ? 0 : 7;
2859         cpsw->txv[0].ch = cpdma_chan_create(cpsw->dma, ch, cpsw_tx_handler, 0);
2860         if (IS_ERR(cpsw->txv[0].ch)) {
2861                 dev_err(dev, "error initializing tx dma channel\n");
2862                 ret = PTR_ERR(cpsw->txv[0].ch);
2863                 goto clean_cpts;
2864         }
2865 
2866         cpsw->rxv[0].ch = cpdma_chan_create(cpsw->dma, 0, cpsw_rx_handler, 1);
2867         if (IS_ERR(cpsw->rxv[0].ch)) {
2868                 dev_err(dev, "error initializing rx dma channel\n");
2869                 ret = PTR_ERR(cpsw->rxv[0].ch);
2870                 goto clean_cpts;
2871         }
2872         cpsw_split_res(cpsw);
2873 
2874         /* setup netdev */
2875         ndev = devm_alloc_etherdev_mqs(dev, sizeof(struct cpsw_priv),
2876                                        CPSW_MAX_QUEUES, CPSW_MAX_QUEUES);
2877         if (!ndev) {
2878                 dev_err(dev, "error allocating net_device\n");
2879                 goto clean_cpts;
2880         }
2881 
2882         priv = netdev_priv(ndev);
2883         priv->cpsw = cpsw;
2884         priv->ndev = ndev;
2885         priv->dev  = dev;
2886         priv->msg_enable = netif_msg_init(debug_level, CPSW_DEBUG);
2887         priv->emac_port = 0;
2888 
2889         if (is_valid_ether_addr(data->slave_data[0].mac_addr)) {
2890                 memcpy(priv->mac_addr, data->slave_data[0].mac_addr, ETH_ALEN);
2891                 dev_info(dev, "Detected MACID = %pM\n", priv->mac_addr);
2892         } else {
2893                 eth_random_addr(priv->mac_addr);
2894                 dev_info(dev, "Random MACID = %pM\n", priv->mac_addr);
2895         }
2896 
2897         memcpy(ndev->dev_addr, priv->mac_addr, ETH_ALEN);
2898 
2899         cpsw->slaves[0].ndev = ndev;
2900 
2901         ndev->features |= NETIF_F_HW_VLAN_CTAG_FILTER | NETIF_F_HW_VLAN_CTAG_RX;
2902 
2903         ndev->netdev_ops = &cpsw_netdev_ops;
2904         ndev->ethtool_ops = &cpsw_ethtool_ops;
2905         netif_napi_add(ndev, &cpsw->napi_rx,
2906                        cpsw->quirk_irq ? cpsw_rx_poll : cpsw_rx_mq_poll,
2907                        CPSW_POLL_WEIGHT);
2908         netif_tx_napi_add(ndev, &cpsw->napi_tx,
2909                           cpsw->quirk_irq ? cpsw_tx_poll : cpsw_tx_mq_poll,
2910                           CPSW_POLL_WEIGHT);
2911 
2912         /* register the network device */
2913         SET_NETDEV_DEV(ndev, dev);
2914         ndev->dev.of_node = cpsw->slaves[0].data->slave_node;
2915         ret = register_netdev(ndev);
2916         if (ret) {
2917                 dev_err(dev, "error registering net device\n");
2918                 ret = -ENODEV;
2919                 goto clean_cpts;
2920         }
2921 
2922         if (cpsw->data.dual_emac) {
2923                 ret = cpsw_probe_dual_emac(priv);
2924                 if (ret) {
2925                         cpsw_err(priv, probe, "error probe slave 2 emac interface\n");
2926                         goto clean_unregister_netdev_ret;
2927                 }
2928         }
2929 
2930         /* Grab RX and TX IRQs. Note that we also have RX_THRESHOLD and
2931          * MISC IRQs which are always kept disabled with this driver so
2932          * we will not request them.
2933          *
2934          * If anyone wants to implement support for those, make sure to
2935          * first request and append them to irqs_table array.
2936          */
2937         ret = devm_request_irq(dev, cpsw->irqs_table[0], cpsw_rx_interrupt,
2938                                0, dev_name(dev), cpsw);
2939         if (ret < 0) {
2940                 dev_err(dev, "error attaching irq (%d)\n", ret);
2941                 goto clean_unregister_netdev_ret;
2942         }
2943 
2944 
2945         ret = devm_request_irq(dev, cpsw->irqs_table[1], cpsw_tx_interrupt,
2946                                0, dev_name(&pdev->dev), cpsw);
2947         if (ret < 0) {
2948                 dev_err(dev, "error attaching irq (%d)\n", ret);
2949                 goto clean_unregister_netdev_ret;
2950         }
2951 
2952         cpsw_notice(priv, probe,
2953                     "initialized device (regs %pa, irq %d, pool size %d)\n",
2954                     &ss_res->start, cpsw->irqs_table[0], descs_pool_size);
2955 
2956         pm_runtime_put(&pdev->dev);
2957 
2958         return 0;
2959 
2960 clean_unregister_netdev_ret:
2961         unregister_netdev(ndev);
2962 clean_cpts:
2963         cpts_release(cpsw->cpts);
2964         cpdma_ctlr_destroy(cpsw->dma);
2965 clean_dt_ret:
2966         cpsw_remove_dt(pdev);
2967         pm_runtime_put_sync(&pdev->dev);
2968 clean_runtime_disable_ret:
2969         pm_runtime_disable(&pdev->dev);
2970         return ret;
2971 }
2972 
2973 static int cpsw_remove(struct platform_device *pdev)
2974 {
2975         struct cpsw_common *cpsw = platform_get_drvdata(pdev);
2976         int i, ret;
2977 
2978         ret = pm_runtime_get_sync(&pdev->dev);
2979         if (ret < 0) {
2980                 pm_runtime_put_noidle(&pdev->dev);
2981                 return ret;
2982         }
2983 
2984         for (i = 0; i < cpsw->data.slaves; i++)
2985                 if (cpsw->slaves[i].ndev)
2986                         unregister_netdev(cpsw->slaves[i].ndev);
2987 
2988         cpts_release(cpsw->cpts);
2989         cpdma_ctlr_destroy(cpsw->dma);
2990         cpsw_remove_dt(pdev);
2991         pm_runtime_put_sync(&pdev->dev);
2992         pm_runtime_disable(&pdev->dev);
2993         return 0;
2994 }
2995 
2996 #ifdef CONFIG_PM_SLEEP
2997 static int cpsw_suspend(struct device *dev)
2998 {
2999         struct cpsw_common *cpsw = dev_get_drvdata(dev);
3000         int i;
3001 
3002         rtnl_lock();
3003 
3004         for (i = 0; i < cpsw->data.slaves; i++)
3005                 if (cpsw->slaves[i].ndev)
3006                         if (netif_running(cpsw->slaves[i].ndev))
3007                                 cpsw_ndo_stop(cpsw->slaves[i].ndev);
3008 
3009         rtnl_unlock();
3010 
3011         /* Select sleep pin state */
3012         pinctrl_pm_select_sleep_state(dev);
3013 
3014         return 0;
3015 }
3016 
3017 static int cpsw_resume(struct device *dev)
3018 {
3019         struct cpsw_common *cpsw = dev_get_drvdata(dev);
3020         int i;
3021 
3022         /* Select default pin state */
3023         pinctrl_pm_select_default_state(dev);
3024 
3025         /* shut up ASSERT_RTNL() warning in netif_set_real_num_tx/rx_queues */
3026         rtnl_lock();
3027 
3028         for (i = 0; i < cpsw->data.slaves; i++)
3029                 if (cpsw->slaves[i].ndev)
3030                         if (netif_running(cpsw->slaves[i].ndev))
3031                                 cpsw_ndo_open(cpsw->slaves[i].ndev);
3032 
3033         rtnl_unlock();
3034 
3035         return 0;
3036 }
3037 #endif
3038 
3039 static SIMPLE_DEV_PM_OPS(cpsw_pm_ops, cpsw_suspend, cpsw_resume);
3040 
3041 static struct platform_driver cpsw_driver = {
3042         .driver = {
3043                 .name    = "cpsw",
3044                 .pm      = &cpsw_pm_ops,
3045                 .of_match_table = cpsw_of_mtable,
3046         },
3047         .probe = cpsw_probe,
3048         .remove = cpsw_remove,
3049 };
3050 
3051 module_platform_driver(cpsw_driver);
3052 
3053 MODULE_LICENSE("GPL");
3054 MODULE_AUTHOR("Cyril Chemparathy <cyril@ti.com>");
3055 MODULE_AUTHOR("Mugunthan V N <mugunthanvnm@ti.com>");
3056 MODULE_DESCRIPTION("TI CPSW Ethernet driver");

/* [<][>][^][v][top][bottom][index][help] */