drivers/net/ethernet/sun/sunvnet.c

   1 /* sunvnet.c: Sun LDOM Virtual Network Driver.
   2  *
   3  * Copyright (C) 2007, 2008 David S. Miller <davem@davemloft.net>
   4  */
   5
   6 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
   7
   8 #include <linux/module.h>
   9 #include <linux/kernel.h>
  10 #include <linux/types.h>
  11 #include <linux/slab.h>
  12 #include <linux/delay.h>
  13 #include <linux/init.h>
  14 #include <linux/netdevice.h>
  15 #include <linux/ethtool.h>
  16 #include <linux/etherdevice.h>
  17 #include <linux/mutex.h>
  18 #include <linux/if_vlan.h>
  19
  20 #if IS_ENABLED(CONFIG_IPV6)
  21 #include <linux/icmpv6.h>
  22 #endif
  23
  24 #include <net/icmp.h>
  25 #include <net/route.h>
  26
  27 #include <asm/vio.h>
  28 #include <asm/ldc.h>
  29
  30 #include "sunvnet.h"
  31
  32 #define DRV_MODULE_NAME         "sunvnet"
  33 #define DRV_MODULE_VERSION      "1.0"
  34 #define DRV_MODULE_RELDATE      "June 25, 2007"
  35
  36 static char version[] =
  37         DRV_MODULE_NAME ".c:v" DRV_MODULE_VERSION " (" DRV_MODULE_RELDATE ")\n";
  38 MODULE_AUTHOR("David S. Miller (davem@davemloft.net)");
  39 MODULE_DESCRIPTION("Sun LDOM virtual network driver");
  40 MODULE_LICENSE("GPL");
  41 MODULE_VERSION(DRV_MODULE_VERSION);
  42
  43 #define VNET_MAX_TXQS           16
  44
  45 /* Heuristic for the number of times to exponentially backoff and
  46  * retry sending an LDC trigger when EAGAIN is encountered
  47  */
  48 #define VNET_MAX_RETRIES        10
  49
  50 static int __vnet_tx_trigger(struct vnet_port *port, u32 start);
  51
  52 /* Ordered from largest major to lowest */
  53 static struct vio_version vnet_versions[] = {
  54         { .major = 1, .minor = 6 },
  55         { .major = 1, .minor = 0 },
  56 };
  57
  58 static inline u32 vnet_tx_dring_avail(struct vio_dring_state *dr)
  59 {
  60         return vio_dring_avail(dr, VNET_TX_RING_SIZE);
  61 }
  62
  63 static int vnet_handle_unknown(struct vnet_port *port, void *arg)
  64 {
  65         struct vio_msg_tag *pkt = arg;
  66
  67         pr_err("Received unknown msg [%02x:%02x:%04x:%08x]\n",
  68                pkt->type, pkt->stype, pkt->stype_env, pkt->sid);
  69         pr_err("Resetting connection\n");
  70
  71         ldc_disconnect(port->vio.lp);
  72
  73         return -ECONNRESET;
  74 }
  75
  76 static int vnet_send_attr(struct vio_driver_state *vio)
  77 {
  78         struct vnet_port *port = to_vnet_port(vio);
  79         struct net_device *dev = port->vp->dev;
  80         struct vio_net_attr_info pkt;
  81         int framelen = ETH_FRAME_LEN;
  82         int i;
  83
  84         memset(&pkt, 0, sizeof(pkt));
  85         pkt.tag.type = VIO_TYPE_CTRL;
  86         pkt.tag.stype = VIO_SUBTYPE_INFO;
  87         pkt.tag.stype_env = VIO_ATTR_INFO;
  88         pkt.tag.sid = vio_send_sid(vio);
  89         if (vio_version_before(vio, 1, 2))
  90                 pkt.xfer_mode = VIO_DRING_MODE;
  91         else
  92                 pkt.xfer_mode = VIO_NEW_DRING_MODE;
  93         pkt.addr_type = VNET_ADDR_ETHERMAC;
  94         pkt.ack_freq = 0;
  95         for (i = 0; i < 6; i++)
  96                 pkt.addr |= (u64)dev->dev_addr[i] << ((5 - i) * 8);
  97         if (vio_version_after(vio, 1, 3)) {
  98                 if (port->rmtu) {
  99                         port->rmtu = min(VNET_MAXPACKET, port->rmtu);
 100                         pkt.mtu = port->rmtu;
 101                 } else {
 102                         port->rmtu = VNET_MAXPACKET;
 103                         pkt.mtu = port->rmtu;
 104                 }
 105                 if (vio_version_after_eq(vio, 1, 6))
 106                         pkt.options = VIO_TX_DRING;
 107         } else if (vio_version_before(vio, 1, 3)) {
 108                 pkt.mtu = framelen;
 109         } else { /* v1.3 */
 110                 pkt.mtu = framelen + VLAN_HLEN;
 111         }
 112
 113         pkt.plnk_updt = PHYSLINK_UPDATE_NONE;
 114         pkt.cflags = 0;
 115
 116         viodbg(HS, "SEND NET ATTR xmode[0x%x] atype[0x%x] addr[%llx] "
 117                "ackfreq[%u] plnk_updt[0x%02x] opts[0x%02x] mtu[%llu] "
 118                "cflags[0x%04x] lso_max[%u]\n",
 119                pkt.xfer_mode, pkt.addr_type,
 120                (unsigned long long)pkt.addr,
 121                pkt.ack_freq, pkt.plnk_updt, pkt.options,
 122                (unsigned long long)pkt.mtu, pkt.cflags, pkt.ipv4_lso_maxlen);
 123
 124
 125         return vio_ldc_send(vio, &pkt, sizeof(pkt));
 126 }
 127
 128 static int handle_attr_info(struct vio_driver_state *vio,
 129                             struct vio_net_attr_info *pkt)
 130 {
 131         struct vnet_port *port = to_vnet_port(vio);
 132         u64     localmtu;
 133         u8      xfer_mode;
 134
 135         viodbg(HS, "GOT NET ATTR xmode[0x%x] atype[0x%x] addr[%llx] "
 136                "ackfreq[%u] plnk_updt[0x%02x] opts[0x%02x] mtu[%llu] "
 137                " (rmtu[%llu]) cflags[0x%04x] lso_max[%u]\n",
 138                pkt->xfer_mode, pkt->addr_type,
 139                (unsigned long long)pkt->addr,
 140                pkt->ack_freq, pkt->plnk_updt, pkt->options,
 141                (unsigned long long)pkt->mtu, port->rmtu, pkt->cflags,
 142                pkt->ipv4_lso_maxlen);
 143
 144         pkt->tag.sid = vio_send_sid(vio);
 145
 146         xfer_mode = pkt->xfer_mode;
 147         /* for version < 1.2, VIO_DRING_MODE = 0x3 and no bitmask */
 148         if (vio_version_before(vio, 1, 2) && xfer_mode == VIO_DRING_MODE)
 149                 xfer_mode = VIO_NEW_DRING_MODE;
 150
 151         /* MTU negotiation:
 152          *      < v1.3 - ETH_FRAME_LEN exactly
 153          *      > v1.3 - MIN(pkt.mtu, VNET_MAXPACKET, port->rmtu) and change
 154          *                      pkt->mtu for ACK
 155          *      = v1.3 - ETH_FRAME_LEN + VLAN_HLEN exactly
 156          */
 157         if (vio_version_before(vio, 1, 3)) {
 158                 localmtu = ETH_FRAME_LEN;
 159         } else if (vio_version_after(vio, 1, 3)) {
 160                 localmtu = port->rmtu ? port->rmtu : VNET_MAXPACKET;
 161                 localmtu = min(pkt->mtu, localmtu);
 162                 pkt->mtu = localmtu;
 163         } else { /* v1.3 */
 164                 localmtu = ETH_FRAME_LEN + VLAN_HLEN;
 165         }
 166         port->rmtu = localmtu;
 167
 168         /* for version >= 1.6, ACK packet mode we support */
 169         if (vio_version_after_eq(vio, 1, 6)) {
 170                 pkt->xfer_mode = VIO_NEW_DRING_MODE;
 171                 pkt->options = VIO_TX_DRING;
 172         }
 173
 174         if (!(xfer_mode | VIO_NEW_DRING_MODE) ||
 175             pkt->addr_type != VNET_ADDR_ETHERMAC ||
 176             pkt->mtu != localmtu) {
 177                 viodbg(HS, "SEND NET ATTR NACK\n");
 178
 179                 pkt->tag.stype = VIO_SUBTYPE_NACK;
 180
 181                 (void) vio_ldc_send(vio, pkt, sizeof(*pkt));
 182
 183                 return -ECONNRESET;
 184         } else {
 185                 viodbg(HS, "SEND NET ATTR ACK xmode[0x%x] atype[0x%x] "
 186                        "addr[%llx] ackfreq[%u] plnk_updt[0x%02x] opts[0x%02x] "
 187                        "mtu[%llu] (rmtu[%llu]) cflags[0x%04x] lso_max[%u]\n",
 188                        pkt->xfer_mode, pkt->addr_type,
 189                        (unsigned long long)pkt->addr,
 190                        pkt->ack_freq, pkt->plnk_updt, pkt->options,
 191                        (unsigned long long)pkt->mtu, port->rmtu, pkt->cflags,
 192                        pkt->ipv4_lso_maxlen);
 193
 194                 pkt->tag.stype = VIO_SUBTYPE_ACK;
 195
 196                 return vio_ldc_send(vio, pkt, sizeof(*pkt));
 197         }
 198
 199 }
 200
 201 static int handle_attr_ack(struct vio_driver_state *vio,
 202                            struct vio_net_attr_info *pkt)
 203 {
 204         viodbg(HS, "GOT NET ATTR ACK\n");
 205
 206         return 0;
 207 }
 208
 209 static int handle_attr_nack(struct vio_driver_state *vio,
 210                             struct vio_net_attr_info *pkt)
 211 {
 212         viodbg(HS, "GOT NET ATTR NACK\n");
 213
 214         return -ECONNRESET;
 215 }
 216
 217 static int vnet_handle_attr(struct vio_driver_state *vio, void *arg)
 218 {
 219         struct vio_net_attr_info *pkt = arg;
 220
 221         switch (pkt->tag.stype) {
 222         case VIO_SUBTYPE_INFO:
 223                 return handle_attr_info(vio, pkt);
 224
 225         case VIO_SUBTYPE_ACK:
 226                 return handle_attr_ack(vio, pkt);
 227
 228         case VIO_SUBTYPE_NACK:
 229                 return handle_attr_nack(vio, pkt);
 230
 231         default:
 232                 return -ECONNRESET;
 233         }
 234 }
 235
 236 static void vnet_handshake_complete(struct vio_driver_state *vio)
 237 {
 238         struct vio_dring_state *dr;
 239
 240         dr = &vio->drings[VIO_DRIVER_RX_RING];
 241         dr->snd_nxt = dr->rcv_nxt = 1;
 242
 243         dr = &vio->drings[VIO_DRIVER_TX_RING];
 244         dr->snd_nxt = dr->rcv_nxt = 1;
 245 }
 246
 247 /* The hypervisor interface that implements copying to/from imported
 248  * memory from another domain requires that copies are done to 8-byte
 249  * aligned buffers, and that the lengths of such copies are also 8-byte
 250  * multiples.
 251  *
 252  * So we align skb->data to an 8-byte multiple and pad-out the data
 253  * area so we can round the copy length up to the next multiple of
 254  * 8 for the copy.
 255  *
 256  * The transmitter puts the actual start of the packet 6 bytes into
 257  * the buffer it sends over, so that the IP headers after the ethernet
 258  * header are aligned properly.  These 6 bytes are not in the descriptor
 259  * length, they are simply implied.  This offset is represented using
 260  * the VNET_PACKET_SKIP macro.
 261  */
 262 static struct sk_buff *alloc_and_align_skb(struct net_device *dev,
 263                                            unsigned int len)
 264 {
 265         struct sk_buff *skb = netdev_alloc_skb(dev, len+VNET_PACKET_SKIP+8+8);
 266         unsigned long addr, off;
 267
 268         if (unlikely(!skb))
 269                 return NULL;
 270
 271         addr = (unsigned long) skb->data;
 272         off = ((addr + 7UL) & ~7UL) - addr;
 273         if (off)
 274                 skb_reserve(skb, off);
 275
 276         return skb;
 277 }
 278
 279 static int vnet_rx_one(struct vnet_port *port, unsigned int len,
 280                        struct ldc_trans_cookie *cookies, int ncookies)
 281 {
 282         struct net_device *dev = port->vp->dev;
 283         unsigned int copy_len;
 284         struct sk_buff *skb;
 285         int err;
 286
 287         err = -EMSGSIZE;
 288         if (unlikely(len < ETH_ZLEN || len > port->rmtu)) {
 289                 dev->stats.rx_length_errors++;
 290                 goto out_dropped;
 291         }
 292
 293         skb = alloc_and_align_skb(dev, len);
 294         err = -ENOMEM;
 295         if (unlikely(!skb)) {
 296                 dev->stats.rx_missed_errors++;
 297                 goto out_dropped;
 298         }
 299
 300         copy_len = (len + VNET_PACKET_SKIP + 7U) & ~7U;
 301         skb_put(skb, copy_len);
 302         err = ldc_copy(port->vio.lp, LDC_COPY_IN,
 303                        skb->data, copy_len, 0,
 304                        cookies, ncookies);
 305         if (unlikely(err < 0)) {
 306                 dev->stats.rx_frame_errors++;
 307                 goto out_free_skb;
 308         }
 309
 310         skb_pull(skb, VNET_PACKET_SKIP);
 311         skb_trim(skb, len);
 312         skb->protocol = eth_type_trans(skb, dev);
 313
 314         dev->stats.rx_packets++;
 315         dev->stats.rx_bytes += len;
 316         napi_gro_receive(&port->napi, skb);
 317         return 0;
 318
 319 out_free_skb:
 320         kfree_skb(skb);
 321
 322 out_dropped:
 323         dev->stats.rx_dropped++;
 324         return err;
 325 }
 326
 327 static int vnet_send_ack(struct vnet_port *port, struct vio_dring_state *dr,
 328                          u32 start, u32 end, u8 vio_dring_state)
 329 {
 330         struct vio_dring_data hdr = {
 331                 .tag = {
 332                         .type           = VIO_TYPE_DATA,
 333                         .stype          = VIO_SUBTYPE_ACK,
 334                         .stype_env      = VIO_DRING_DATA,
 335                         .sid            = vio_send_sid(&port->vio),
 336                 },
 337                 .dring_ident            = dr->ident,
 338                 .start_idx              = start,
 339                 .end_idx                = end,
 340                 .state                  = vio_dring_state,
 341         };
 342         int err, delay;
 343         int retries = 0;
 344
 345         hdr.seq = dr->snd_nxt;
 346         delay = 1;
 347         do {
 348                 err = vio_ldc_send(&port->vio, &hdr, sizeof(hdr));
 349                 if (err > 0) {
 350                         dr->snd_nxt++;
 351                         break;
 352                 }
 353                 udelay(delay);
 354                 if ((delay <<= 1) > 128)
 355                         delay = 128;
 356                 if (retries++ > VNET_MAX_RETRIES) {
 357                         pr_info("ECONNRESET %x:%x:%x:%x:%x:%x\n",
 358                                 port->raddr[0], port->raddr[1],
 359                                 port->raddr[2], port->raddr[3],
 360                                 port->raddr[4], port->raddr[5]);
 361                         break;
 362                 }
 363         } while (err == -EAGAIN);
 364
 365         if (err <= 0 && vio_dring_state == VIO_DRING_STOPPED) {
 366                 port->stop_rx_idx = end;
 367                 port->stop_rx = true;
 368         } else {
 369                 port->stop_rx_idx = 0;
 370                 port->stop_rx = false;
 371         }
 372
 373         return err;
 374 }
 375
 376 static u32 next_idx(u32 idx, struct vio_dring_state *dr)
 377 {
 378         if (++idx == dr->num_entries)
 379                 idx = 0;
 380         return idx;
 381 }
 382
 383 static u32 prev_idx(u32 idx, struct vio_dring_state *dr)
 384 {
 385         if (idx == 0)
 386                 idx = dr->num_entries - 1;
 387         else
 388                 idx--;
 389
 390         return idx;
 391 }
 392
 393 static struct vio_net_desc *get_rx_desc(struct vnet_port *port,
 394                                         struct vio_dring_state *dr,
 395                                         u32 index)
 396 {
 397         struct vio_net_desc *desc = port->vio.desc_buf;
 398         int err;
 399
 400         err = ldc_get_dring_entry(port->vio.lp, desc, dr->entry_size,
 401                                   (index * dr->entry_size),
 402                                   dr->cookies, dr->ncookies);
 403         if (err < 0)
 404                 return ERR_PTR(err);
 405
 406         return desc;
 407 }
 408
 409 static int put_rx_desc(struct vnet_port *port,
 410                        struct vio_dring_state *dr,
 411                        struct vio_net_desc *desc,
 412                        u32 index)
 413 {
 414         int err;
 415
 416         err = ldc_put_dring_entry(port->vio.lp, desc, dr->entry_size,
 417                                   (index * dr->entry_size),
 418                                   dr->cookies, dr->ncookies);
 419         if (err < 0)
 420                 return err;
 421
 422         return 0;
 423 }
 424
 425 static int vnet_walk_rx_one(struct vnet_port *port,
 426                             struct vio_dring_state *dr,
 427                             u32 index, int *needs_ack)
 428 {
 429         struct vio_net_desc *desc = get_rx_desc(port, dr, index);
 430         struct vio_driver_state *vio = &port->vio;
 431         int err;
 432
 433         BUG_ON(desc == NULL);
 434         if (IS_ERR(desc))
 435                 return PTR_ERR(desc);
 436
 437         if (desc->hdr.state != VIO_DESC_READY)
 438                 return 1;
 439
 440         rmb();
 441
 442         viodbg(DATA, "vio_walk_rx_one desc[%02x:%02x:%08x:%08x:%llx:%llx]\n",
 443                desc->hdr.state, desc->hdr.ack,
 444                desc->size, desc->ncookies,
 445                desc->cookies[0].cookie_addr,
 446                desc->cookies[0].cookie_size);
 447
 448         err = vnet_rx_one(port, desc->size, desc->cookies, desc->ncookies);
 449         if (err == -ECONNRESET)
 450                 return err;
 451         desc->hdr.state = VIO_DESC_DONE;
 452         err = put_rx_desc(port, dr, desc, index);
 453         if (err < 0)
 454                 return err;
 455         *needs_ack = desc->hdr.ack;
 456         return 0;
 457 }
 458
 459 static int vnet_walk_rx(struct vnet_port *port, struct vio_dring_state *dr,
 460                         u32 start, u32 end, int *npkts, int budget)
 461 {
 462         struct vio_driver_state *vio = &port->vio;
 463         int ack_start = -1, ack_end = -1;
 464         bool send_ack = true;
 465
 466         end = (end == (u32) -1) ? prev_idx(start, dr) : next_idx(end, dr);
 467
 468         viodbg(DATA, "vnet_walk_rx start[%08x] end[%08x]\n", start, end);
 469
 470         while (start != end) {
 471                 int ack = 0, err = vnet_walk_rx_one(port, dr, start, &ack);
 472                 if (err == -ECONNRESET)
 473                         return err;
 474                 if (err != 0)
 475                         break;
 476                 (*npkts)++;
 477                 if (ack_start == -1)
 478                         ack_start = start;
 479                 ack_end = start;
 480                 start = next_idx(start, dr);
 481                 if (ack && start != end) {
 482                         err = vnet_send_ack(port, dr, ack_start, ack_end,
 483                                             VIO_DRING_ACTIVE);
 484                         if (err == -ECONNRESET)
 485                                 return err;
 486                         ack_start = -1;
 487                 }
 488                 if ((*npkts) >= budget) {
 489                         send_ack = false;
 490                         break;
 491                 }
 492         }
 493         if (unlikely(ack_start == -1))
 494                 ack_start = ack_end = prev_idx(start, dr);
 495         if (send_ack) {
 496                 port->napi_resume = false;
 497                 return vnet_send_ack(port, dr, ack_start, ack_end,
 498                                      VIO_DRING_STOPPED);
 499         } else  {
 500                 port->napi_resume = true;
 501                 port->napi_stop_idx = ack_end;
 502                 return 1;
 503         }
 504 }
 505
 506 static int vnet_rx(struct vnet_port *port, void *msgbuf, int *npkts,
 507                    int budget)
 508 {
 509         struct vio_dring_data *pkt = msgbuf;
 510         struct vio_dring_state *dr = &port->vio.drings[VIO_DRIVER_RX_RING];
 511         struct vio_driver_state *vio = &port->vio;
 512
 513         viodbg(DATA, "vnet_rx stype_env[%04x] seq[%016llx] rcv_nxt[%016llx]\n",
 514                pkt->tag.stype_env, pkt->seq, dr->rcv_nxt);
 515
 516         if (unlikely(pkt->tag.stype_env != VIO_DRING_DATA))
 517                 return 0;
 518         if (unlikely(pkt->seq != dr->rcv_nxt)) {
 519                 pr_err("RX out of sequence seq[0x%llx] rcv_nxt[0x%llx]\n",
 520                        pkt->seq, dr->rcv_nxt);
 521                 return 0;
 522         }
 523
 524         if (!port->napi_resume)
 525                 dr->rcv_nxt++;
 526
 527         /* XXX Validate pkt->start_idx and pkt->end_idx XXX */
 528
 529         return vnet_walk_rx(port, dr, pkt->start_idx, pkt->end_idx,
 530                             npkts, budget);
 531 }
 532
 533 static int idx_is_pending(struct vio_dring_state *dr, u32 end)
 534 {
 535         u32 idx = dr->cons;
 536         int found = 0;
 537
 538         while (idx != dr->prod) {
 539                 if (idx == end) {
 540                         found = 1;
 541                         break;
 542                 }
 543                 idx = next_idx(idx, dr);
 544         }
 545         return found;
 546 }
 547
 548 static int vnet_ack(struct vnet_port *port, void *msgbuf)
 549 {
 550         struct vio_dring_state *dr = &port->vio.drings[VIO_DRIVER_TX_RING];
 551         struct vio_dring_data *pkt = msgbuf;
 552         struct net_device *dev;
 553         struct vnet *vp;
 554         u32 end;
 555         struct vio_net_desc *desc;
 556         struct netdev_queue *txq;
 557
 558         if (unlikely(pkt->tag.stype_env != VIO_DRING_DATA))
 559                 return 0;
 560
 561         end = pkt->end_idx;
 562         if (unlikely(!idx_is_pending(dr, end)))
 563                 return 0;
 564
 565         vp = port->vp;
 566         dev = vp->dev;
 567         /* sync for race conditions with vnet_start_xmit() and tell xmit it
 568          * is time to send a trigger.
 569          */
 570         netif_tx_lock(dev);
 571         dr->cons = next_idx(end, dr);
 572         desc = vio_dring_entry(dr, dr->cons);
 573         if (desc->hdr.state == VIO_DESC_READY && port->start_cons) {
 574                 /* vnet_start_xmit() just populated this dring but missed
 575                  * sending the "start" LDC message to the consumer.
 576                  * Send a "start" trigger on its behalf.
 577                  */
 578                 if (__vnet_tx_trigger(port, dr->cons) > 0)
 579                         port->start_cons = false;
 580                 else
 581                         port->start_cons = true;
 582         } else {
 583                 port->start_cons = true;
 584         }
 585         netif_tx_unlock(dev);
 586
 587         txq = netdev_get_tx_queue(dev, port->q_index);
 588         if (unlikely(netif_tx_queue_stopped(txq) &&
 589                      vnet_tx_dring_avail(dr) >= VNET_TX_WAKEUP_THRESH(dr)))
 590                 return 1;
 591
 592         return 0;
 593 }
 594
 595 static int vnet_nack(struct vnet_port *port, void *msgbuf)
 596 {
 597         /* XXX just reset or similar XXX */
 598         return 0;
 599 }
 600
 601 static int handle_mcast(struct vnet_port *port, void *msgbuf)
 602 {
 603         struct vio_net_mcast_info *pkt = msgbuf;
 604
 605         if (pkt->tag.stype != VIO_SUBTYPE_ACK)
 606                 pr_err("%s: Got unexpected MCAST reply [%02x:%02x:%04x:%08x]\n",
 607                        port->vp->dev->name,
 608                        pkt->tag.type,
 609                        pkt->tag.stype,
 610                        pkt->tag.stype_env,
 611                        pkt->tag.sid);
 612
 613         return 0;
 614 }
 615
 616 /* Got back a STOPPED LDC message on port. If the queue is stopped,
 617  * wake it up so that we'll send out another START message at the
 618  * next TX.
 619  */
 620 static void maybe_tx_wakeup(struct vnet_port *port)
 621 {
 622         struct netdev_queue *txq;
 623
 624         txq = netdev_get_tx_queue(port->vp->dev, port->q_index);
 625         __netif_tx_lock(txq, smp_processor_id());
 626         if (likely(netif_tx_queue_stopped(txq))) {
 627                 struct vio_dring_state *dr;
 628
 629                 dr = &port->vio.drings[VIO_DRIVER_TX_RING];
 630                 netif_tx_wake_queue(txq);
 631         }
 632         __netif_tx_unlock(txq);
 633 }
 634
 635 static inline bool port_is_up(struct vnet_port *vnet)
 636 {
 637         struct vio_driver_state *vio = &vnet->vio;
 638
 639         return !!(vio->hs_state & VIO_HS_COMPLETE);
 640 }
 641
 642 static int vnet_event_napi(struct vnet_port *port, int budget)
 643 {
 644         struct vio_driver_state *vio = &port->vio;
 645         int tx_wakeup, err;
 646         int npkts = 0;
 647         int event = (port->rx_event & LDC_EVENT_RESET);
 648
 649 ldc_ctrl:
 650         if (unlikely(event == LDC_EVENT_RESET ||
 651                      event == LDC_EVENT_UP)) {
 652                 vio_link_state_change(vio, event);
 653
 654                 if (event == LDC_EVENT_RESET) {
 655                         port->rmtu = 0;
 656                         vio_port_up(vio);
 657                 }
 658                 port->rx_event = 0;
 659                 return 0;
 660         }
 661         /* We may have multiple LDC events in rx_event. Unroll send_events() */
 662         event = (port->rx_event & LDC_EVENT_UP);
 663         port->rx_event &= ~(LDC_EVENT_RESET|LDC_EVENT_UP);
 664         if (event == LDC_EVENT_UP)
 665                 goto ldc_ctrl;
 666         event = port->rx_event;
 667         if (!(event & LDC_EVENT_DATA_READY))
 668                 return 0;
 669
 670         /* we dont expect any other bits than RESET, UP, DATA_READY */
 671         BUG_ON(event != LDC_EVENT_DATA_READY);
 672
 673         tx_wakeup = err = 0;
 674         while (1) {
 675                 union {
 676                         struct vio_msg_tag tag;
 677                         u64 raw[8];
 678                 } msgbuf;
 679
 680                 if (port->napi_resume) {
 681                         struct vio_dring_data *pkt =
 682                                 (struct vio_dring_data *)&msgbuf;
 683                         struct vio_dring_state *dr =
 684                                 &port->vio.drings[VIO_DRIVER_RX_RING];
 685
 686                         pkt->tag.type = VIO_TYPE_DATA;
 687                         pkt->tag.stype = VIO_SUBTYPE_INFO;
 688                         pkt->tag.stype_env = VIO_DRING_DATA;
 689                         pkt->seq = dr->rcv_nxt;
 690                         pkt->start_idx = next_idx(port->napi_stop_idx, dr);
 691                         pkt->end_idx = -1;
 692                         goto napi_resume;
 693                 }
 694 ldc_read:
 695                 err = ldc_read(vio->lp, &msgbuf, sizeof(msgbuf));
 696                 if (unlikely(err < 0)) {
 697                         if (err == -ECONNRESET)
 698                                 vio_conn_reset(vio);
 699                         break;
 700                 }
 701                 if (err == 0)
 702                         break;
 703                 viodbg(DATA, "TAG [%02x:%02x:%04x:%08x]\n",
 704                        msgbuf.tag.type,
 705                        msgbuf.tag.stype,
 706                        msgbuf.tag.stype_env,
 707                        msgbuf.tag.sid);
 708                 err = vio_validate_sid(vio, &msgbuf.tag);
 709                 if (err < 0)
 710                         break;
 711 napi_resume:
 712                 if (likely(msgbuf.tag.type == VIO_TYPE_DATA)) {
 713                         if (msgbuf.tag.stype == VIO_SUBTYPE_INFO) {
 714                                 if (!port_is_up(port)) {
 715                                         /* failures like handshake_failure()
 716                                          * may have cleaned up dring, but
 717                                          * NAPI polling may bring us here.
 718                                          */
 719                                         err = -ECONNRESET;
 720                                         break;
 721                                 }
 722                                 err = vnet_rx(port, &msgbuf, &npkts, budget);
 723                                 if (npkts >= budget)
 724                                         break;
 725                                 if (npkts == 0 && err != -ECONNRESET)
 726                                         goto ldc_read;
 727                         } else if (msgbuf.tag.stype == VIO_SUBTYPE_ACK) {
 728                                 err = vnet_ack(port, &msgbuf);
 729                                 if (err > 0)
 730                                         tx_wakeup |= err;
 731                         } else if (msgbuf.tag.stype == VIO_SUBTYPE_NACK) {
 732                                 err = vnet_nack(port, &msgbuf);
 733                         }
 734                 } else if (msgbuf.tag.type == VIO_TYPE_CTRL) {
 735                         if (msgbuf.tag.stype_env == VNET_MCAST_INFO)
 736                                 err = handle_mcast(port, &msgbuf);
 737                         else
 738                                 err = vio_control_pkt_engine(vio, &msgbuf);
 739                         if (err)
 740                                 break;
 741                 } else {
 742                         err = vnet_handle_unknown(port, &msgbuf);
 743                 }
 744                 if (err == -ECONNRESET)
 745                         break;
 746         }
 747         if (unlikely(tx_wakeup && err != -ECONNRESET))
 748                 maybe_tx_wakeup(port);
 749         return npkts;
 750 }
 751
 752 static int vnet_poll(struct napi_struct *napi, int budget)
 753 {
 754         struct vnet_port *port = container_of(napi, struct vnet_port, napi);
 755         struct vio_driver_state *vio = &port->vio;
 756         int processed = vnet_event_napi(port, budget);
 757
 758         if (processed < budget) {
 759                 napi_complete(napi);
 760                 port->rx_event &= ~LDC_EVENT_DATA_READY;
 761                 vio_set_intr(vio->vdev->rx_ino, HV_INTR_ENABLED);
 762         }
 763         return processed;
 764 }
 765
 766 static void vnet_event(void *arg, int event)
 767 {
 768         struct vnet_port *port = arg;
 769         struct vio_driver_state *vio = &port->vio;
 770
 771         port->rx_event |= event;
 772         vio_set_intr(vio->vdev->rx_ino, HV_INTR_DISABLED);
 773         napi_schedule(&port->napi);
 774
 775 }
 776
 777 static int __vnet_tx_trigger(struct vnet_port *port, u32 start)
 778 {
 779         struct vio_dring_state *dr = &port->vio.drings[VIO_DRIVER_TX_RING];
 780         struct vio_dring_data hdr = {
 781                 .tag = {
 782                         .type           = VIO_TYPE_DATA,
 783                         .stype          = VIO_SUBTYPE_INFO,
 784                         .stype_env      = VIO_DRING_DATA,
 785                         .sid            = vio_send_sid(&port->vio),
 786                 },
 787                 .dring_ident            = dr->ident,
 788                 .start_idx              = start,
 789                 .end_idx                = (u32) -1,
 790         };
 791         int err, delay;
 792         int retries = 0;
 793
 794         if (port->stop_rx) {
 795                 err = vnet_send_ack(port,
 796                                     &port->vio.drings[VIO_DRIVER_RX_RING],
 797                                     port->stop_rx_idx, -1,
 798                                     VIO_DRING_STOPPED);
 799                 if (err <= 0)
 800                         return err;
 801         }
 802
 803         hdr.seq = dr->snd_nxt;
 804         delay = 1;
 805         do {
 806                 err = vio_ldc_send(&port->vio, &hdr, sizeof(hdr));
 807                 if (err > 0) {
 808                         dr->snd_nxt++;
 809                         break;
 810                 }
 811                 udelay(delay);
 812                 if ((delay <<= 1) > 128)
 813                         delay = 128;
 814                 if (retries++ > VNET_MAX_RETRIES)
 815                         break;
 816         } while (err == -EAGAIN);
 817
 818         return err;
 819 }
 820
 821 struct vnet_port *__tx_port_find(struct vnet *vp, struct sk_buff *skb)
 822 {
 823         unsigned int hash = vnet_hashfn(skb->data);
 824         struct hlist_head *hp = &vp->port_hash[hash];
 825         struct vnet_port *port;
 826
 827         hlist_for_each_entry_rcu(port, hp, hash) {
 828                 if (!port_is_up(port))
 829                         continue;
 830                 if (ether_addr_equal(port->raddr, skb->data))
 831                         return port;
 832         }
 833         list_for_each_entry_rcu(port, &vp->port_list, list) {
 834                 if (!port->switch_port)
 835                         continue;
 836                 if (!port_is_up(port))
 837                         continue;
 838                 return port;
 839         }
 840         return NULL;
 841 }
 842
 843 static struct sk_buff *vnet_clean_tx_ring(struct vnet_port *port,
 844                                           unsigned *pending)
 845 {
 846         struct vio_dring_state *dr = &port->vio.drings[VIO_DRIVER_TX_RING];
 847         struct sk_buff *skb = NULL;
 848         int i, txi;
 849
 850         *pending = 0;
 851
 852         txi = dr->prod-1;
 853         if (txi < 0)
 854                 txi = VNET_TX_RING_SIZE-1;
 855
 856         for (i = 0; i < VNET_TX_RING_SIZE; ++i) {
 857                 struct vio_net_desc *d;
 858
 859                 d = vio_dring_entry(dr, txi);
 860
 861                 if (d->hdr.state == VIO_DESC_DONE) {
 862                         if (port->tx_bufs[txi].skb) {
 863                                 BUG_ON(port->tx_bufs[txi].skb->next);
 864
 865                                 port->tx_bufs[txi].skb->next = skb;
 866                                 skb = port->tx_bufs[txi].skb;
 867                                 port->tx_bufs[txi].skb = NULL;
 868
 869                                 ldc_unmap(port->vio.lp,
 870                                           port->tx_bufs[txi].cookies,
 871                                           port->tx_bufs[txi].ncookies);
 872                         }
 873                         d->hdr.state = VIO_DESC_FREE;
 874                 } else if (d->hdr.state == VIO_DESC_READY) {
 875                         (*pending)++;
 876                 } else if (d->hdr.state == VIO_DESC_FREE) {
 877                         break;
 878                 }
 879                 --txi;
 880                 if (txi < 0)
 881                         txi = VNET_TX_RING_SIZE-1;
 882         }
 883         return skb;
 884 }
 885
 886 static inline void vnet_free_skbs(struct sk_buff *skb)
 887 {
 888         struct sk_buff *next;
 889
 890         while (skb) {
 891                 next = skb->next;
 892                 skb->next = NULL;
 893                 dev_kfree_skb(skb);
 894                 skb = next;
 895         }
 896 }
 897
 898 static void vnet_clean_timer_expire(unsigned long port0)
 899 {
 900         struct vnet_port *port = (struct vnet_port *)port0;
 901         struct sk_buff *freeskbs;
 902         unsigned pending;
 903
 904         netif_tx_lock(port->vp->dev);
 905         freeskbs = vnet_clean_tx_ring(port, &pending);
 906         netif_tx_unlock(port->vp->dev);
 907
 908         vnet_free_skbs(freeskbs);
 909
 910         if (pending)
 911                 (void)mod_timer(&port->clean_timer,
 912                                 jiffies + VNET_CLEAN_TIMEOUT);
 913          else
 914                 del_timer(&port->clean_timer);
 915 }
 916
 917 static inline struct sk_buff *vnet_skb_shape(struct sk_buff *skb, void **pstart,
 918                                              int *plen)
 919 {
 920         struct sk_buff *nskb;
 921         int len, pad;
 922
 923         len = skb->len;
 924         pad = 0;
 925         if (len < ETH_ZLEN) {
 926                 pad += ETH_ZLEN - skb->len;
 927                 len += pad;
 928         }
 929         len += VNET_PACKET_SKIP;
 930         pad += 8 - (len & 7);
 931         len += 8 - (len & 7);
 932
 933         if (((unsigned long)skb->data & 7) != VNET_PACKET_SKIP ||
 934             skb_tailroom(skb) < pad ||
 935             skb_headroom(skb) < VNET_PACKET_SKIP) {
 936                 nskb = alloc_and_align_skb(skb->dev, skb->len);
 937                 skb_reserve(nskb, VNET_PACKET_SKIP);
 938                 if (skb_copy_bits(skb, 0, nskb->data, skb->len)) {
 939                         dev_kfree_skb(nskb);
 940                         dev_kfree_skb(skb);
 941                         return NULL;
 942                 }
 943                 (void)skb_put(nskb, skb->len);
 944                 dev_kfree_skb(skb);
 945                 skb = nskb;
 946         }
 947
 948         *pstart = skb->data - VNET_PACKET_SKIP;
 949         *plen = len;
 950         return skb;
 951 }
 952
 953 static u16
 954 vnet_select_queue(struct net_device *dev, struct sk_buff *skb,
 955                   void *accel_priv, select_queue_fallback_t fallback)
 956 {
 957         struct vnet *vp = netdev_priv(dev);
 958         struct vnet_port *port = __tx_port_find(vp, skb);
 959
 960         return port->q_index;
 961 }
 962
 963 static int vnet_start_xmit(struct sk_buff *skb, struct net_device *dev)
 964 {
 965         struct vnet *vp = netdev_priv(dev);
 966         struct vnet_port *port = NULL;
 967         struct vio_dring_state *dr;
 968         struct vio_net_desc *d;
 969         unsigned int len;
 970         struct sk_buff *freeskbs = NULL;
 971         int i, err, txi;
 972         void *start = NULL;
 973         int nlen = 0;
 974         unsigned pending = 0;
 975         struct netdev_queue *txq;
 976
 977         skb = vnet_skb_shape(skb, &start, &nlen);
 978         if (unlikely(!skb))
 979                 goto out_dropped;
 980
 981         rcu_read_lock();
 982         port = __tx_port_find(vp, skb);
 983         if (unlikely(!port))
 984                 goto out_dropped;
 985
 986         if (skb->len > port->rmtu) {
 987                 unsigned long localmtu = port->rmtu - ETH_HLEN;
 988
 989                 if (vio_version_after_eq(&port->vio, 1, 3))
 990                         localmtu -= VLAN_HLEN;
 991
 992                 if (skb->protocol == htons(ETH_P_IP)) {
 993                         struct flowi4 fl4;
 994                         struct rtable *rt = NULL;
 995
 996                         memset(&fl4, 0, sizeof(fl4));
 997                         fl4.flowi4_oif = dev->ifindex;
 998                         fl4.flowi4_tos = RT_TOS(ip_hdr(skb)->tos);
 999                         fl4.daddr = ip_hdr(skb)->daddr;
1000                         fl4.saddr = ip_hdr(skb)->saddr;
1001
1002                         rt = ip_route_output_key(dev_net(dev), &fl4);
1003                         rcu_read_unlock();
1004                         if (!IS_ERR(rt)) {
1005                                 skb_dst_set(skb, &rt->dst);
1006                                 icmp_send(skb, ICMP_DEST_UNREACH,
1007                                           ICMP_FRAG_NEEDED,
1008                                           htonl(localmtu));
1009                         }
1010                 }
1011 #if IS_ENABLED(CONFIG_IPV6)
1012                 else if (skb->protocol == htons(ETH_P_IPV6))
1013                         icmpv6_send(skb, ICMPV6_PKT_TOOBIG, 0, localmtu);
1014 #endif
1015                 goto out_dropped;
1016         }
1017
1018         dr = &port->vio.drings[VIO_DRIVER_TX_RING];
1019         i = skb_get_queue_mapping(skb);
1020         txq = netdev_get_tx_queue(dev, i);
1021         if (unlikely(vnet_tx_dring_avail(dr) < 1)) {
1022                 if (!netif_tx_queue_stopped(txq)) {
1023                         netif_tx_stop_queue(txq);
1024
1025                         /* This is a hard error, log it. */
1026                         netdev_err(dev, "BUG! Tx Ring full when queue awake!\n");
1027                         dev->stats.tx_errors++;
1028                 }
1029                 rcu_read_unlock();
1030                 return NETDEV_TX_BUSY;
1031         }
1032
1033         d = vio_dring_cur(dr);
1034
1035         txi = dr->prod;
1036
1037         freeskbs = vnet_clean_tx_ring(port, &pending);
1038
1039         BUG_ON(port->tx_bufs[txi].skb);
1040
1041         len = skb->len;
1042         if (len < ETH_ZLEN)
1043                 len = ETH_ZLEN;
1044
1045         port->tx_bufs[txi].skb = skb;
1046         skb = NULL;
1047
1048         err = ldc_map_single(port->vio.lp, start, nlen,
1049                              port->tx_bufs[txi].cookies, VNET_MAXCOOKIES,
1050                              (LDC_MAP_SHADOW | LDC_MAP_DIRECT | LDC_MAP_RW));
1051         if (err < 0) {
1052                 netdev_info(dev, "tx buffer map error %d\n", err);
1053                 goto out_dropped;
1054         }
1055         port->tx_bufs[txi].ncookies = err;
1056
1057         /* We don't rely on the ACKs to free the skb in vnet_start_xmit(),
1058          * thus it is safe to not set VIO_ACK_ENABLE for each transmission:
1059          * the protocol itself does not require it as long as the peer
1060          * sends a VIO_SUBTYPE_ACK for VIO_DRING_STOPPED.
1061          *
1062          * An ACK for every packet in the ring is expensive as the
1063          * sending of LDC messages is slow and affects performance.
1064          */
1065         d->hdr.ack = VIO_ACK_DISABLE;
1066         d->size = len;
1067         d->ncookies = port->tx_bufs[txi].ncookies;
1068         for (i = 0; i < d->ncookies; i++)
1069                 d->cookies[i] = port->tx_bufs[txi].cookies[i];
1070
1071         /* This has to be a non-SMP write barrier because we are writing
1072          * to memory which is shared with the peer LDOM.
1073          */
1074         wmb();
1075
1076         d->hdr.state = VIO_DESC_READY;
1077
1078         /* Exactly one ldc "start" trigger (for dr->cons) needs to be sent
1079          * to notify the consumer that some descriptors are READY.
1080          * After that "start" trigger, no additional triggers are needed until
1081          * a DRING_STOPPED is received from the consumer. The dr->cons field
1082          * (set up by vnet_ack()) has the value of the next dring index
1083          * that has not yet been ack-ed. We send a "start" trigger here
1084          * if, and only if, start_cons is true (reset it afterward). Conversely,
1085          * vnet_ack() should check if the dring corresponding to cons
1086          * is marked READY, but start_cons was false.
1087          * If so, vnet_ack() should send out the missed "start" trigger.
1088          *
1089          * Note that the wmb() above makes sure the cookies et al. are
1090          * not globally visible before the VIO_DESC_READY, and that the
1091          * stores are ordered correctly by the compiler. The consumer will
1092          * not proceed until the VIO_DESC_READY is visible assuring that
1093          * the consumer does not observe anything related to descriptors
1094          * out of order. The HV trap from the LDC start trigger is the
1095          * producer to consumer announcement that work is available to the
1096          * consumer
1097          */
1098         if (!port->start_cons)
1099                 goto ldc_start_done; /* previous trigger suffices */
1100
1101         err = __vnet_tx_trigger(port, dr->cons);
1102         if (unlikely(err < 0)) {
1103                 netdev_info(dev, "TX trigger error %d\n", err);
1104                 d->hdr.state = VIO_DESC_FREE;
1105                 dev->stats.tx_carrier_errors++;
1106                 goto out_dropped;
1107         }
1108
1109 ldc_start_done:
1110         port->start_cons = false;
1111
1112         dev->stats.tx_packets++;
1113         dev->stats.tx_bytes += port->tx_bufs[txi].skb->len;
1114
1115         dr->prod = (dr->prod + 1) & (VNET_TX_RING_SIZE - 1);
1116         if (unlikely(vnet_tx_dring_avail(dr) < 1)) {
1117                 netif_tx_stop_queue(txq);
1118                 if (vnet_tx_dring_avail(dr) > VNET_TX_WAKEUP_THRESH(dr))
1119                         netif_tx_wake_queue(txq);
1120         }
1121
1122         (void)mod_timer(&port->clean_timer, jiffies + VNET_CLEAN_TIMEOUT);
1123         rcu_read_unlock();
1124
1125         vnet_free_skbs(freeskbs);
1126
1127         return NETDEV_TX_OK;
1128
1129 out_dropped:
1130         if (pending)
1131                 (void)mod_timer(&port->clean_timer,
1132                                 jiffies + VNET_CLEAN_TIMEOUT);
1133         else if (port)
1134                 del_timer(&port->clean_timer);
1135         if (port)
1136                 rcu_read_unlock();
1137         if (skb)
1138                 dev_kfree_skb(skb);
1139         vnet_free_skbs(freeskbs);
1140         dev->stats.tx_dropped++;
1141         return NETDEV_TX_OK;
1142 }
1143
1144 static void vnet_tx_timeout(struct net_device *dev)
1145 {
1146         /* XXX Implement me XXX */
1147 }
1148
1149 static int vnet_open(struct net_device *dev)
1150 {
1151         netif_carrier_on(dev);
1152         netif_tx_start_all_queues(dev);
1153
1154         return 0;
1155 }
1156
1157 static int vnet_close(struct net_device *dev)
1158 {
1159         netif_tx_stop_all_queues(dev);
1160         netif_carrier_off(dev);
1161
1162         return 0;
1163 }
1164
1165 static struct vnet_mcast_entry *__vnet_mc_find(struct vnet *vp, u8 *addr)
1166 {
1167         struct vnet_mcast_entry *m;
1168
1169         for (m = vp->mcast_list; m; m = m->next) {
1170                 if (ether_addr_equal(m->addr, addr))
1171                         return m;
1172         }
1173         return NULL;
1174 }
1175
1176 static void __update_mc_list(struct vnet *vp, struct net_device *dev)
1177 {
1178         struct netdev_hw_addr *ha;
1179
1180         netdev_for_each_mc_addr(ha, dev) {
1181                 struct vnet_mcast_entry *m;
1182
1183                 m = __vnet_mc_find(vp, ha->addr);
1184                 if (m) {
1185                         m->hit = 1;
1186                         continue;
1187                 }
1188
1189                 if (!m) {
1190                         m = kzalloc(sizeof(*m), GFP_ATOMIC);
1191                         if (!m)
1192                                 continue;
1193                         memcpy(m->addr, ha->addr, ETH_ALEN);
1194                         m->hit = 1;
1195
1196                         m->next = vp->mcast_list;
1197                         vp->mcast_list = m;
1198                 }
1199         }
1200 }
1201
1202 static void __send_mc_list(struct vnet *vp, struct vnet_port *port)
1203 {
1204         struct vio_net_mcast_info info;
1205         struct vnet_mcast_entry *m, **pp;
1206         int n_addrs;
1207
1208         memset(&info, 0, sizeof(info));
1209
1210         info.tag.type = VIO_TYPE_CTRL;
1211         info.tag.stype = VIO_SUBTYPE_INFO;
1212         info.tag.stype_env = VNET_MCAST_INFO;
1213         info.tag.sid = vio_send_sid(&port->vio);
1214         info.set = 1;
1215
1216         n_addrs = 0;
1217         for (m = vp->mcast_list; m; m = m->next) {
1218                 if (m->sent)
1219                         continue;
1220                 m->sent = 1;
1221                 memcpy(&info.mcast_addr[n_addrs * ETH_ALEN],
1222                        m->addr, ETH_ALEN);
1223                 if (++n_addrs == VNET_NUM_MCAST) {
1224                         info.count = n_addrs;
1225
1226                         (void) vio_ldc_send(&port->vio, &info,
1227                                             sizeof(info));
1228                         n_addrs = 0;
1229                 }
1230         }
1231         if (n_addrs) {
1232                 info.count = n_addrs;
1233                 (void) vio_ldc_send(&port->vio, &info, sizeof(info));
1234         }
1235
1236         info.set = 0;
1237
1238         n_addrs = 0;
1239         pp = &vp->mcast_list;
1240         while ((m = *pp) != NULL) {
1241                 if (m->hit) {
1242                         m->hit = 0;
1243                         pp = &m->next;
1244                         continue;
1245                 }
1246
1247                 memcpy(&info.mcast_addr[n_addrs * ETH_ALEN],
1248                        m->addr, ETH_ALEN);
1249                 if (++n_addrs == VNET_NUM_MCAST) {
1250                         info.count = n_addrs;
1251                         (void) vio_ldc_send(&port->vio, &info,
1252                                             sizeof(info));
1253                         n_addrs = 0;
1254                 }
1255
1256                 *pp = m->next;
1257                 kfree(m);
1258         }
1259         if (n_addrs) {
1260                 info.count = n_addrs;
1261                 (void) vio_ldc_send(&port->vio, &info, sizeof(info));
1262         }
1263 }
1264
1265 static void vnet_set_rx_mode(struct net_device *dev)
1266 {
1267         struct vnet *vp = netdev_priv(dev);
1268         struct vnet_port *port;
1269
1270         rcu_read_lock();
1271         list_for_each_entry_rcu(port, &vp->port_list, list) {
1272
1273                 if (port->switch_port) {
1274                         __update_mc_list(vp, dev);
1275                         __send_mc_list(vp, port);
1276                         break;
1277                 }
1278         }
1279         rcu_read_unlock();
1280 }
1281
1282 static int vnet_change_mtu(struct net_device *dev, int new_mtu)
1283 {
1284         if (new_mtu < 68 || new_mtu > 65535)
1285                 return -EINVAL;
1286
1287         dev->mtu = new_mtu;
1288         return 0;
1289 }
1290
1291 static int vnet_set_mac_addr(struct net_device *dev, void *p)
1292 {
1293         return -EINVAL;
1294 }
1295
1296 static void vnet_get_drvinfo(struct net_device *dev,
1297                              struct ethtool_drvinfo *info)
1298 {
1299         strlcpy(info->driver, DRV_MODULE_NAME, sizeof(info->driver));
1300         strlcpy(info->version, DRV_MODULE_VERSION, sizeof(info->version));
1301 }
1302
1303 static u32 vnet_get_msglevel(struct net_device *dev)
1304 {
1305         struct vnet *vp = netdev_priv(dev);
1306         return vp->msg_enable;
1307 }
1308
1309 static void vnet_set_msglevel(struct net_device *dev, u32 value)
1310 {
1311         struct vnet *vp = netdev_priv(dev);
1312         vp->msg_enable = value;
1313 }
1314
1315 static const struct ethtool_ops vnet_ethtool_ops = {
1316         .get_drvinfo            = vnet_get_drvinfo,
1317         .get_msglevel           = vnet_get_msglevel,
1318         .set_msglevel           = vnet_set_msglevel,
1319         .get_link               = ethtool_op_get_link,
1320 };
1321
1322 static void vnet_port_free_tx_bufs(struct vnet_port *port)
1323 {
1324         struct vio_dring_state *dr;
1325         int i;
1326
1327         dr = &port->vio.drings[VIO_DRIVER_TX_RING];
1328         if (dr->base) {
1329                 ldc_free_exp_dring(port->vio.lp, dr->base,
1330                                    (dr->entry_size * dr->num_entries),
1331                                    dr->cookies, dr->ncookies);
1332                 dr->base = NULL;
1333                 dr->entry_size = 0;
1334                 dr->num_entries = 0;
1335                 dr->pending = 0;
1336                 dr->ncookies = 0;
1337         }
1338
1339         for (i = 0; i < VNET_TX_RING_SIZE; i++) {
1340                 struct vio_net_desc *d;
1341                 void *skb = port->tx_bufs[i].skb;
1342
1343                 if (!skb)
1344                         continue;
1345
1346                 d = vio_dring_entry(dr, i);
1347                 if (d->hdr.state == VIO_DESC_READY)
1348                         pr_warn("active transmit buffers freed\n");
1349
1350                 ldc_unmap(port->vio.lp,
1351                           port->tx_bufs[i].cookies,
1352                           port->tx_bufs[i].ncookies);
1353                 dev_kfree_skb(skb);
1354                 port->tx_bufs[i].skb = NULL;
1355                 d->hdr.state = VIO_DESC_FREE;
1356         }
1357 }
1358
1359 static int vnet_port_alloc_tx_bufs(struct vnet_port *port)
1360 {
1361         struct vio_dring_state *dr;
1362         unsigned long len;
1363         int i, err, ncookies;
1364         void *dring;
1365
1366         dr = &port->vio.drings[VIO_DRIVER_TX_RING];
1367
1368         len = (VNET_TX_RING_SIZE *
1369                (sizeof(struct vio_net_desc) +
1370                 (sizeof(struct ldc_trans_cookie) * 2)));
1371
1372         ncookies = VIO_MAX_RING_COOKIES;
1373         dring = ldc_alloc_exp_dring(port->vio.lp, len,
1374                                     dr->cookies, &ncookies,
1375                                     (LDC_MAP_SHADOW |
1376                                      LDC_MAP_DIRECT |
1377                                      LDC_MAP_RW));
1378         if (IS_ERR(dring)) {
1379                 err = PTR_ERR(dring);
1380                 goto err_out;
1381         }
1382
1383         dr->base = dring;
1384         dr->entry_size = (sizeof(struct vio_net_desc) +
1385                           (sizeof(struct ldc_trans_cookie) * 2));
1386         dr->num_entries = VNET_TX_RING_SIZE;
1387         dr->prod = dr->cons = 0;
1388         port->start_cons  = true; /* need an initial trigger */
1389         dr->pending = VNET_TX_RING_SIZE;
1390         dr->ncookies = ncookies;
1391
1392         for (i = 0; i < VNET_TX_RING_SIZE; ++i) {
1393                 struct vio_net_desc *d;
1394
1395                 d = vio_dring_entry(dr, i);
1396                 d->hdr.state = VIO_DESC_FREE;
1397         }
1398         return 0;
1399
1400 err_out:
1401         vnet_port_free_tx_bufs(port);
1402
1403         return err;
1404 }
1405
1406 #ifdef CONFIG_NET_POLL_CONTROLLER
1407 static void vnet_poll_controller(struct net_device *dev)
1408 {
1409         struct vnet *vp = netdev_priv(dev);
1410         struct vnet_port *port;
1411         unsigned long flags;
1412
1413         spin_lock_irqsave(&vp->lock, flags);
1414         if (!list_empty(&vp->port_list)) {
1415                 port = list_entry(vp->port_list.next, struct vnet_port, list);
1416                 napi_schedule(&port->napi);
1417         }
1418         spin_unlock_irqrestore(&vp->lock, flags);
1419 }
1420 #endif
1421 static LIST_HEAD(vnet_list);
1422 static DEFINE_MUTEX(vnet_list_mutex);
1423
1424 static const struct net_device_ops vnet_ops = {
1425         .ndo_open               = vnet_open,
1426         .ndo_stop               = vnet_close,
1427         .ndo_set_rx_mode        = vnet_set_rx_mode,
1428         .ndo_set_mac_address    = vnet_set_mac_addr,
1429         .ndo_validate_addr      = eth_validate_addr,
1430         .ndo_tx_timeout         = vnet_tx_timeout,
1431         .ndo_change_mtu         = vnet_change_mtu,
1432         .ndo_start_xmit         = vnet_start_xmit,
1433         .ndo_select_queue       = vnet_select_queue,
1434 #ifdef CONFIG_NET_POLL_CONTROLLER
1435         .ndo_poll_controller    = vnet_poll_controller,
1436 #endif
1437 };
1438
1439 static struct vnet *vnet_new(const u64 *local_mac)
1440 {
1441         struct net_device *dev;
1442         struct vnet *vp;
1443         int err, i;
1444
1445         dev = alloc_etherdev_mqs(sizeof(*vp), VNET_MAX_TXQS, 1);
1446         if (!dev)
1447                 return ERR_PTR(-ENOMEM);
1448         dev->needed_headroom = VNET_PACKET_SKIP + 8;
1449         dev->needed_tailroom = 8;
1450
1451         for (i = 0; i < ETH_ALEN; i++)
1452                 dev->dev_addr[i] = (*local_mac >> (5 - i) * 8) & 0xff;
1453
1454         vp = netdev_priv(dev);
1455
1456         spin_lock_init(&vp->lock);
1457         vp->dev = dev;
1458
1459         INIT_LIST_HEAD(&vp->port_list);
1460         for (i = 0; i < VNET_PORT_HASH_SIZE; i++)
1461                 INIT_HLIST_HEAD(&vp->port_hash[i]);
1462         INIT_LIST_HEAD(&vp->list);
1463         vp->local_mac = *local_mac;
1464
1465         dev->netdev_ops = &vnet_ops;
1466         dev->ethtool_ops = &vnet_ethtool_ops;
1467         dev->watchdog_timeo = VNET_TX_TIMEOUT;
1468
1469         err = register_netdev(dev);
1470         if (err) {
1471                 pr_err("Cannot register net device, aborting\n");
1472                 goto err_out_free_dev;
1473         }
1474
1475         netdev_info(dev, "Sun LDOM vnet %pM\n", dev->dev_addr);
1476
1477         list_add(&vp->list, &vnet_list);
1478
1479         return vp;
1480
1481 err_out_free_dev:
1482         free_netdev(dev);
1483
1484         return ERR_PTR(err);
1485 }
1486
1487 static struct vnet *vnet_find_or_create(const u64 *local_mac)
1488 {
1489         struct vnet *iter, *vp;
1490
1491         mutex_lock(&vnet_list_mutex);
1492         vp = NULL;
1493         list_for_each_entry(iter, &vnet_list, list) {
1494                 if (iter->local_mac == *local_mac) {
1495                         vp = iter;
1496                         break;
1497                 }
1498         }
1499         if (!vp)
1500                 vp = vnet_new(local_mac);
1501         mutex_unlock(&vnet_list_mutex);
1502
1503         return vp;
1504 }
1505
1506 static void vnet_cleanup(void)
1507 {
1508         struct vnet *vp;
1509         struct net_device *dev;
1510
1511         mutex_lock(&vnet_list_mutex);
1512         while (!list_empty(&vnet_list)) {
1513                 vp = list_first_entry(&vnet_list, struct vnet, list);
1514                 list_del(&vp->list);
1515                 dev = vp->dev;
1516                 /* vio_unregister_driver() should have cleaned up port_list */
1517                 BUG_ON(!list_empty(&vp->port_list));
1518                 unregister_netdev(dev);
1519                 free_netdev(dev);
1520         }
1521         mutex_unlock(&vnet_list_mutex);
1522 }
1523
1524 static const char *local_mac_prop = "local-mac-address";
1525
1526 static struct vnet *vnet_find_parent(struct mdesc_handle *hp,
1527                                                 u64 port_node)
1528 {
1529         const u64 *local_mac = NULL;
1530         u64 a;
1531
1532         mdesc_for_each_arc(a, hp, port_node, MDESC_ARC_TYPE_BACK) {
1533                 u64 target = mdesc_arc_target(hp, a);
1534                 const char *name;
1535
1536                 name = mdesc_get_property(hp, target, "name", NULL);
1537                 if (!name || strcmp(name, "network"))
1538                         continue;
1539
1540                 local_mac = mdesc_get_property(hp, target,
1541                                                local_mac_prop, NULL);
1542                 if (local_mac)
1543                         break;
1544         }
1545         if (!local_mac)
1546                 return ERR_PTR(-ENODEV);
1547
1548         return vnet_find_or_create(local_mac);
1549 }
1550
1551 static struct ldc_channel_config vnet_ldc_cfg = {
1552         .event          = vnet_event,
1553         .mtu            = 64,
1554         .mode           = LDC_MODE_UNRELIABLE,
1555 };
1556
1557 static struct vio_driver_ops vnet_vio_ops = {
1558         .send_attr              = vnet_send_attr,
1559         .handle_attr            = vnet_handle_attr,
1560         .handshake_complete     = vnet_handshake_complete,
1561 };
1562
1563 static void print_version(void)
1564 {
1565         printk_once(KERN_INFO "%s", version);
1566 }
1567
1568 const char *remote_macaddr_prop = "remote-mac-address";
1569
1570 static void
1571 vnet_port_add_txq(struct vnet_port *port)
1572 {
1573         struct vnet *vp = port->vp;
1574         int n;
1575
1576         n = vp->nports++;
1577         n = n & (VNET_MAX_TXQS - 1);
1578         port->q_index = n;
1579         netif_tx_wake_queue(netdev_get_tx_queue(vp->dev, port->q_index));
1580 }
1581
1582 static void
1583 vnet_port_rm_txq(struct vnet_port *port)
1584 {
1585         port->vp->nports--;
1586         netif_tx_stop_queue(netdev_get_tx_queue(port->vp->dev, port->q_index));
1587 }
1588
1589 static int vnet_port_probe(struct vio_dev *vdev, const struct vio_device_id *id)
1590 {
1591         struct mdesc_handle *hp;
1592         struct vnet_port *port;
1593         unsigned long flags;
1594         struct vnet *vp;
1595         const u64 *rmac;
1596         int len, i, err, switch_port;
1597
1598         print_version();
1599
1600         hp = mdesc_grab();
1601
1602         vp = vnet_find_parent(hp, vdev->mp);
1603         if (IS_ERR(vp)) {
1604                 pr_err("Cannot find port parent vnet\n");
1605                 err = PTR_ERR(vp);
1606                 goto err_out_put_mdesc;
1607         }
1608
1609         rmac = mdesc_get_property(hp, vdev->mp, remote_macaddr_prop, &len);
1610         err = -ENODEV;
1611         if (!rmac) {
1612                 pr_err("Port lacks %s property\n", remote_macaddr_prop);
1613                 goto err_out_put_mdesc;
1614         }
1615
1616         port = kzalloc(sizeof(*port), GFP_KERNEL);
1617         err = -ENOMEM;
1618         if (!port)
1619                 goto err_out_put_mdesc;
1620
1621         for (i = 0; i < ETH_ALEN; i++)
1622                 port->raddr[i] = (*rmac >> (5 - i) * 8) & 0xff;
1623
1624         port->vp = vp;
1625
1626         err = vio_driver_init(&port->vio, vdev, VDEV_NETWORK,
1627                               vnet_versions, ARRAY_SIZE(vnet_versions),
1628                               &vnet_vio_ops, vp->dev->name);
1629         if (err)
1630                 goto err_out_free_port;
1631
1632         err = vio_ldc_alloc(&port->vio, &vnet_ldc_cfg, port);
1633         if (err)
1634                 goto err_out_free_port;
1635
1636         netif_napi_add(port->vp->dev, &port->napi, vnet_poll, NAPI_POLL_WEIGHT);
1637
1638         err = vnet_port_alloc_tx_bufs(port);
1639         if (err)
1640                 goto err_out_free_ldc;
1641
1642         INIT_HLIST_NODE(&port->hash);
1643         INIT_LIST_HEAD(&port->list);
1644
1645         switch_port = 0;
1646         if (mdesc_get_property(hp, vdev->mp, "switch-port", NULL) != NULL)
1647                 switch_port = 1;
1648         port->switch_port = switch_port;
1649
1650         spin_lock_irqsave(&vp->lock, flags);
1651         if (switch_port)
1652                 list_add_rcu(&port->list, &vp->port_list);
1653         else
1654                 list_add_tail_rcu(&port->list, &vp->port_list);
1655         hlist_add_head_rcu(&port->hash,
1656                            &vp->port_hash[vnet_hashfn(port->raddr)]);
1657         vnet_port_add_txq(port);
1658         spin_unlock_irqrestore(&vp->lock, flags);
1659
1660         dev_set_drvdata(&vdev->dev, port);
1661
1662         pr_info("%s: PORT ( remote-mac %pM%s )\n",
1663                 vp->dev->name, port->raddr, switch_port ? " switch-port" : "");
1664
1665         setup_timer(&port->clean_timer, vnet_clean_timer_expire,
1666                     (unsigned long)port);
1667
1668         napi_enable(&port->napi);
1669         vio_port_up(&port->vio);
1670
1671         mdesc_release(hp);
1672
1673         return 0;
1674
1675 err_out_free_ldc:
1676         netif_napi_del(&port->napi);
1677         vio_ldc_free(&port->vio);
1678
1679 err_out_free_port:
1680         kfree(port);
1681
1682 err_out_put_mdesc:
1683         mdesc_release(hp);
1684         return err;
1685 }
1686
1687 static int vnet_port_remove(struct vio_dev *vdev)
1688 {
1689         struct vnet_port *port = dev_get_drvdata(&vdev->dev);
1690
1691         if (port) {
1692
1693                 del_timer_sync(&port->vio.timer);
1694
1695                 napi_disable(&port->napi);
1696
1697                 list_del_rcu(&port->list);
1698                 hlist_del_rcu(&port->hash);
1699
1700                 synchronize_rcu();
1701                 del_timer_sync(&port->clean_timer);
1702                 vnet_port_rm_txq(port);
1703                 netif_napi_del(&port->napi);
1704                 vnet_port_free_tx_bufs(port);
1705                 vio_ldc_free(&port->vio);
1706
1707                 dev_set_drvdata(&vdev->dev, NULL);
1708
1709                 kfree(port);
1710
1711         }
1712         return 0;
1713 }
1714
1715 static const struct vio_device_id vnet_port_match[] = {
1716         {
1717                 .type = "vnet-port",
1718         },
1719         {},
1720 };
1721 MODULE_DEVICE_TABLE(vio, vnet_port_match);
1722
1723 static struct vio_driver vnet_port_driver = {
1724         .id_table       = vnet_port_match,
1725         .probe          = vnet_port_probe,
1726         .remove         = vnet_port_remove,
1727         .name           = "vnet_port",
1728 };
1729
1730 static int __init vnet_init(void)
1731 {
1732         return vio_register_driver(&vnet_port_driver);
1733 }
1734
1735 static void __exit vnet_exit(void)
1736 {
1737         vio_unregister_driver(&vnet_port_driver);
1738         vnet_cleanup();
1739 }
1740
1741 module_init(vnet_init);
1742 module_exit(vnet_exit);