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         vp = port->vp;
 563         dev = vp->dev;
 564         netif_tx_lock(dev);
 565         if (unlikely(!idx_is_pending(dr, end))) {
 566                 netif_tx_unlock(dev);
 567                 return 0;
 568         }
 569
 570         /* sync for race conditions with vnet_start_xmit() and tell xmit it
 571          * is time to send a trigger.
 572          */
 573         dr->cons = next_idx(end, dr);
 574         desc = vio_dring_entry(dr, dr->cons);
 575         if (desc->hdr.state == VIO_DESC_READY && port->start_cons) {
 576                 /* vnet_start_xmit() just populated this dring but missed
 577                  * sending the "start" LDC message to the consumer.
 578                  * Send a "start" trigger on its behalf.
 579                  */
 580                 if (__vnet_tx_trigger(port, dr->cons) > 0)
 581                         port->start_cons = false;
 582                 else
 583                         port->start_cons = true;
 584         } else {
 585                 port->start_cons = true;
 586         }
 587         netif_tx_unlock(dev);
 588
 589         txq = netdev_get_tx_queue(dev, port->q_index);
 590         if (unlikely(netif_tx_queue_stopped(txq) &&
 591                      vnet_tx_dring_avail(dr) >= VNET_TX_WAKEUP_THRESH(dr)))
 592                 return 1;
 593
 594         return 0;
 595 }
 596
 597 static int vnet_nack(struct vnet_port *port, void *msgbuf)
 598 {
 599         /* XXX just reset or similar XXX */
 600         return 0;
 601 }
 602
 603 static int handle_mcast(struct vnet_port *port, void *msgbuf)
 604 {
 605         struct vio_net_mcast_info *pkt = msgbuf;
 606
 607         if (pkt->tag.stype != VIO_SUBTYPE_ACK)
 608                 pr_err("%s: Got unexpected MCAST reply [%02x:%02x:%04x:%08x]\n",
 609                        port->vp->dev->name,
 610                        pkt->tag.type,
 611                        pkt->tag.stype,
 612                        pkt->tag.stype_env,
 613                        pkt->tag.sid);
 614
 615         return 0;
 616 }
 617
 618 /* Got back a STOPPED LDC message on port. If the queue is stopped,
 619  * wake it up so that we'll send out another START message at the
 620  * next TX.
 621  */
 622 static void maybe_tx_wakeup(struct vnet_port *port)
 623 {
 624         struct netdev_queue *txq;
 625
 626         txq = netdev_get_tx_queue(port->vp->dev, port->q_index);
 627         __netif_tx_lock(txq, smp_processor_id());
 628         if (likely(netif_tx_queue_stopped(txq))) {
 629                 struct vio_dring_state *dr;
 630
 631                 dr = &port->vio.drings[VIO_DRIVER_TX_RING];
 632                 netif_tx_wake_queue(txq);
 633         }
 634         __netif_tx_unlock(txq);
 635 }
 636
 637 static inline bool port_is_up(struct vnet_port *vnet)
 638 {
 639         struct vio_driver_state *vio = &vnet->vio;
 640
 641         return !!(vio->hs_state & VIO_HS_COMPLETE);
 642 }
 643
 644 static int vnet_event_napi(struct vnet_port *port, int budget)
 645 {
 646         struct vio_driver_state *vio = &port->vio;
 647         int tx_wakeup, err;
 648         int npkts = 0;
 649         int event = (port->rx_event & LDC_EVENT_RESET);
 650
 651 ldc_ctrl:
 652         if (unlikely(event == LDC_EVENT_RESET ||
 653                      event == LDC_EVENT_UP)) {
 654                 vio_link_state_change(vio, event);
 655
 656                 if (event == LDC_EVENT_RESET) {
 657                         port->rmtu = 0;
 658                         vio_port_up(vio);
 659                 }
 660                 port->rx_event = 0;
 661                 return 0;
 662         }
 663         /* We may have multiple LDC events in rx_event. Unroll send_events() */
 664         event = (port->rx_event & LDC_EVENT_UP);
 665         port->rx_event &= ~(LDC_EVENT_RESET|LDC_EVENT_UP);
 666         if (event == LDC_EVENT_UP)
 667                 goto ldc_ctrl;
 668         event = port->rx_event;
 669         if (!(event & LDC_EVENT_DATA_READY))
 670                 return 0;
 671
 672         /* we dont expect any other bits than RESET, UP, DATA_READY */
 673         BUG_ON(event != LDC_EVENT_DATA_READY);
 674
 675         tx_wakeup = err = 0;
 676         while (1) {
 677                 union {
 678                         struct vio_msg_tag tag;
 679                         u64 raw[8];
 680                 } msgbuf;
 681
 682                 if (port->napi_resume) {
 683                         struct vio_dring_data *pkt =
 684                                 (struct vio_dring_data *)&msgbuf;
 685                         struct vio_dring_state *dr =
 686                                 &port->vio.drings[VIO_DRIVER_RX_RING];
 687
 688                         pkt->tag.type = VIO_TYPE_DATA;
 689                         pkt->tag.stype = VIO_SUBTYPE_INFO;
 690                         pkt->tag.stype_env = VIO_DRING_DATA;
 691                         pkt->seq = dr->rcv_nxt;
 692                         pkt->start_idx = next_idx(port->napi_stop_idx, dr);
 693                         pkt->end_idx = -1;
 694                         goto napi_resume;
 695                 }
 696                 err = ldc_read(vio->lp, &msgbuf, sizeof(msgbuf));
 697                 if (unlikely(err < 0)) {
 698                         if (err == -ECONNRESET)
 699                                 vio_conn_reset(vio);
 700                         break;
 701                 }
 702                 if (err == 0)
 703                         break;
 704                 viodbg(DATA, "TAG [%02x:%02x:%04x:%08x]\n",
 705                        msgbuf.tag.type,
 706                        msgbuf.tag.stype,
 707                        msgbuf.tag.stype_env,
 708                        msgbuf.tag.sid);
 709                 err = vio_validate_sid(vio, &msgbuf.tag);
 710                 if (err < 0)
 711                         break;
 712 napi_resume:
 713                 if (likely(msgbuf.tag.type == VIO_TYPE_DATA)) {
 714                         if (msgbuf.tag.stype == VIO_SUBTYPE_INFO) {
 715                                 if (!port_is_up(port)) {
 716                                         /* failures like handshake_failure()
 717                                          * may have cleaned up dring, but
 718                                          * NAPI polling may bring us here.
 719                                          */
 720                                         err = -ECONNRESET;
 721                                         break;
 722                                 }
 723                                 err = vnet_rx(port, &msgbuf, &npkts, budget);
 724                                 if (npkts >= budget)
 725                                         break;
 726                                 if (npkts == 0)
 727                                         break;
 728                         } else if (msgbuf.tag.stype == VIO_SUBTYPE_ACK) {
 729                                 err = vnet_ack(port, &msgbuf);
 730                                 if (err > 0)
 731                                         tx_wakeup |= err;
 732                         } else if (msgbuf.tag.stype == VIO_SUBTYPE_NACK) {
 733                                 err = vnet_nack(port, &msgbuf);
 734                         }
 735                 } else if (msgbuf.tag.type == VIO_TYPE_CTRL) {
 736                         if (msgbuf.tag.stype_env == VNET_MCAST_INFO)
 737                                 err = handle_mcast(port, &msgbuf);
 738                         else
 739                                 err = vio_control_pkt_engine(vio, &msgbuf);
 740                         if (err)
 741                                 break;
 742                 } else {
 743                         err = vnet_handle_unknown(port, &msgbuf);
 744                 }
 745                 if (err == -ECONNRESET)
 746                         break;
 747         }
 748         if (unlikely(tx_wakeup && err != -ECONNRESET))
 749                 maybe_tx_wakeup(port);
 750         return npkts;
 751 }
 752
 753 static int vnet_poll(struct napi_struct *napi, int budget)
 754 {
 755         struct vnet_port *port = container_of(napi, struct vnet_port, napi);
 756         struct vio_driver_state *vio = &port->vio;
 757         int processed = vnet_event_napi(port, budget);
 758
 759         if (processed < budget) {
 760                 napi_complete(napi);
 761                 port->rx_event &= ~LDC_EVENT_DATA_READY;
 762                 vio_set_intr(vio->vdev->rx_ino, HV_INTR_ENABLED);
 763         }
 764         return processed;
 765 }
 766
 767 static void vnet_event(void *arg, int event)
 768 {
 769         struct vnet_port *port = arg;
 770         struct vio_driver_state *vio = &port->vio;
 771
 772         port->rx_event |= event;
 773         vio_set_intr(vio->vdev->rx_ino, HV_INTR_DISABLED);
 774         napi_schedule(&port->napi);
 775
 776 }
 777
 778 static int __vnet_tx_trigger(struct vnet_port *port, u32 start)
 779 {
 780         struct vio_dring_state *dr = &port->vio.drings[VIO_DRIVER_TX_RING];
 781         struct vio_dring_data hdr = {
 782                 .tag = {
 783                         .type           = VIO_TYPE_DATA,
 784                         .stype          = VIO_SUBTYPE_INFO,
 785                         .stype_env      = VIO_DRING_DATA,
 786                         .sid            = vio_send_sid(&port->vio),
 787                 },
 788                 .dring_ident            = dr->ident,
 789                 .start_idx              = start,
 790                 .end_idx                = (u32) -1,
 791         };
 792         int err, delay;
 793         int retries = 0;
 794
 795         if (port->stop_rx) {
 796                 err = vnet_send_ack(port,
 797                                     &port->vio.drings[VIO_DRIVER_RX_RING],
 798                                     port->stop_rx_idx, -1,
 799                                     VIO_DRING_STOPPED);
 800                 if (err <= 0)
 801                         return err;
 802         }
 803
 804         hdr.seq = dr->snd_nxt;
 805         delay = 1;
 806         do {
 807                 err = vio_ldc_send(&port->vio, &hdr, sizeof(hdr));
 808                 if (err > 0) {
 809                         dr->snd_nxt++;
 810                         break;
 811                 }
 812                 udelay(delay);
 813                 if ((delay <<= 1) > 128)
 814                         delay = 128;
 815                 if (retries++ > VNET_MAX_RETRIES)
 816                         break;
 817         } while (err == -EAGAIN);
 818
 819         return err;
 820 }
 821
 822 struct vnet_port *__tx_port_find(struct vnet *vp, struct sk_buff *skb)
 823 {
 824         unsigned int hash = vnet_hashfn(skb->data);
 825         struct hlist_head *hp = &vp->port_hash[hash];
 826         struct vnet_port *port;
 827
 828         hlist_for_each_entry_rcu(port, hp, hash) {
 829                 if (!port_is_up(port))
 830                         continue;
 831                 if (ether_addr_equal(port->raddr, skb->data))
 832                         return port;
 833         }
 834         list_for_each_entry_rcu(port, &vp->port_list, list) {
 835                 if (!port->switch_port)
 836                         continue;
 837                 if (!port_is_up(port))
 838                         continue;
 839                 return port;
 840         }
 841         return NULL;
 842 }
 843
 844 static struct sk_buff *vnet_clean_tx_ring(struct vnet_port *port,
 845                                           unsigned *pending)
 846 {
 847         struct vio_dring_state *dr = &port->vio.drings[VIO_DRIVER_TX_RING];
 848         struct sk_buff *skb = NULL;
 849         int i, txi;
 850
 851         *pending = 0;
 852
 853         txi = dr->prod-1;
 854         if (txi < 0)
 855                 txi = VNET_TX_RING_SIZE-1;
 856
 857         for (i = 0; i < VNET_TX_RING_SIZE; ++i) {
 858                 struct vio_net_desc *d;
 859
 860                 d = vio_dring_entry(dr, txi);
 861
 862                 if (d->hdr.state == VIO_DESC_DONE) {
 863                         if (port->tx_bufs[txi].skb) {
 864                                 BUG_ON(port->tx_bufs[txi].skb->next);
 865
 866                                 port->tx_bufs[txi].skb->next = skb;
 867                                 skb = port->tx_bufs[txi].skb;
 868                                 port->tx_bufs[txi].skb = NULL;
 869
 870                                 ldc_unmap(port->vio.lp,
 871                                           port->tx_bufs[txi].cookies,
 872                                           port->tx_bufs[txi].ncookies);
 873                         }
 874                         d->hdr.state = VIO_DESC_FREE;
 875                 } else if (d->hdr.state == VIO_DESC_READY) {
 876                         (*pending)++;
 877                 } else if (d->hdr.state == VIO_DESC_FREE) {
 878                         break;
 879                 }
 880                 --txi;
 881                 if (txi < 0)
 882                         txi = VNET_TX_RING_SIZE-1;
 883         }
 884         return skb;
 885 }
 886
 887 static inline void vnet_free_skbs(struct sk_buff *skb)
 888 {
 889         struct sk_buff *next;
 890
 891         while (skb) {
 892                 next = skb->next;
 893                 skb->next = NULL;
 894                 dev_kfree_skb(skb);
 895                 skb = next;
 896         }
 897 }
 898
 899 static void vnet_clean_timer_expire(unsigned long port0)
 900 {
 901         struct vnet_port *port = (struct vnet_port *)port0;
 902         struct sk_buff *freeskbs;
 903         unsigned pending;
 904
 905         netif_tx_lock(port->vp->dev);
 906         freeskbs = vnet_clean_tx_ring(port, &pending);
 907         netif_tx_unlock(port->vp->dev);
 908
 909         vnet_free_skbs(freeskbs);
 910
 911         if (pending)
 912                 (void)mod_timer(&port->clean_timer,
 913                                 jiffies + VNET_CLEAN_TIMEOUT);
 914          else
 915                 del_timer(&port->clean_timer);
 916 }
 917
 918 static inline struct sk_buff *vnet_skb_shape(struct sk_buff *skb, void **pstart,
 919                                              int *plen)
 920 {
 921         struct sk_buff *nskb;
 922         int len, pad;
 923
 924         len = skb->len;
 925         pad = 0;
 926         if (len < ETH_ZLEN) {
 927                 pad += ETH_ZLEN - skb->len;
 928                 len += pad;
 929         }
 930         len += VNET_PACKET_SKIP;
 931         pad += 8 - (len & 7);
 932         len += 8 - (len & 7);
 933
 934         if (((unsigned long)skb->data & 7) != VNET_PACKET_SKIP ||
 935             skb_tailroom(skb) < pad ||
 936             skb_headroom(skb) < VNET_PACKET_SKIP) {
 937                 nskb = alloc_and_align_skb(skb->dev, skb->len);
 938                 skb_reserve(nskb, VNET_PACKET_SKIP);
 939                 if (skb_copy_bits(skb, 0, nskb->data, skb->len)) {
 940                         dev_kfree_skb(nskb);
 941                         dev_kfree_skb(skb);
 942                         return NULL;
 943                 }
 944                 (void)skb_put(nskb, skb->len);
 945                 dev_kfree_skb(skb);
 946                 skb = nskb;
 947         }
 948
 949         *pstart = skb->data - VNET_PACKET_SKIP;
 950         *plen = len;
 951         return skb;
 952 }
 953
 954 static u16
 955 vnet_select_queue(struct net_device *dev, struct sk_buff *skb,
 956                   void *accel_priv, select_queue_fallback_t fallback)
 957 {
 958         struct vnet *vp = netdev_priv(dev);
 959         struct vnet_port *port = __tx_port_find(vp, skb);
 960
 961         return port->q_index;
 962 }
 963
 964 static int vnet_start_xmit(struct sk_buff *skb, struct net_device *dev)
 965 {
 966         struct vnet *vp = netdev_priv(dev);
 967         struct vnet_port *port = NULL;
 968         struct vio_dring_state *dr;
 969         struct vio_net_desc *d;
 970         unsigned int len;
 971         struct sk_buff *freeskbs = NULL;
 972         int i, err, txi;
 973         void *start = NULL;
 974         int nlen = 0;
 975         unsigned pending = 0;
 976         struct netdev_queue *txq;
 977
 978         skb = vnet_skb_shape(skb, &start, &nlen);
 979         if (unlikely(!skb))
 980                 goto out_dropped;
 981
 982         rcu_read_lock();
 983         port = __tx_port_find(vp, skb);
 984         if (unlikely(!port))
 985                 goto out_dropped;
 986
 987         if (skb->len > port->rmtu) {
 988                 unsigned long localmtu = port->rmtu - ETH_HLEN;
 989
 990                 if (vio_version_after_eq(&port->vio, 1, 3))
 991                         localmtu -= VLAN_HLEN;
 992
 993                 if (skb->protocol == htons(ETH_P_IP)) {
 994                         struct flowi4 fl4;
 995                         struct rtable *rt = NULL;
 996
 997                         memset(&fl4, 0, sizeof(fl4));
 998                         fl4.flowi4_oif = dev->ifindex;
 999                         fl4.flowi4_tos = RT_TOS(ip_hdr(skb)->tos);
1000                         fl4.daddr = ip_hdr(skb)->daddr;
1001                         fl4.saddr = ip_hdr(skb)->saddr;
1002
1003                         rt = ip_route_output_key(dev_net(dev), &fl4);
1004                         rcu_read_unlock();
1005                         if (!IS_ERR(rt)) {
1006                                 skb_dst_set(skb, &rt->dst);
1007                                 icmp_send(skb, ICMP_DEST_UNREACH,
1008                                           ICMP_FRAG_NEEDED,
1009                                           htonl(localmtu));
1010                         }
1011                 }
1012 #if IS_ENABLED(CONFIG_IPV6)
1013                 else if (skb->protocol == htons(ETH_P_IPV6))
1014                         icmpv6_send(skb, ICMPV6_PKT_TOOBIG, 0, localmtu);
1015 #endif
1016                 goto out_dropped;
1017         }
1018
1019         dr = &port->vio.drings[VIO_DRIVER_TX_RING];
1020         i = skb_get_queue_mapping(skb);
1021         txq = netdev_get_tx_queue(dev, i);
1022         if (unlikely(vnet_tx_dring_avail(dr) < 1)) {
1023                 if (!netif_tx_queue_stopped(txq)) {
1024                         netif_tx_stop_queue(txq);
1025
1026                         /* This is a hard error, log it. */
1027                         netdev_err(dev, "BUG! Tx Ring full when queue awake!\n");
1028                         dev->stats.tx_errors++;
1029                 }
1030                 rcu_read_unlock();
1031                 return NETDEV_TX_BUSY;
1032         }
1033
1034         d = vio_dring_cur(dr);
1035
1036         txi = dr->prod;
1037
1038         freeskbs = vnet_clean_tx_ring(port, &pending);
1039
1040         BUG_ON(port->tx_bufs[txi].skb);
1041
1042         len = skb->len;
1043         if (len < ETH_ZLEN)
1044                 len = ETH_ZLEN;
1045
1046         port->tx_bufs[txi].skb = skb;
1047         skb = NULL;
1048
1049         err = ldc_map_single(port->vio.lp, start, nlen,
1050                              port->tx_bufs[txi].cookies, VNET_MAXCOOKIES,
1051                              (LDC_MAP_SHADOW | LDC_MAP_DIRECT | LDC_MAP_RW));
1052         if (err < 0) {
1053                 netdev_info(dev, "tx buffer map error %d\n", err);
1054                 goto out_dropped;
1055         }
1056         port->tx_bufs[txi].ncookies = err;
1057
1058         /* We don't rely on the ACKs to free the skb in vnet_start_xmit(),
1059          * thus it is safe to not set VIO_ACK_ENABLE for each transmission:
1060          * the protocol itself does not require it as long as the peer
1061          * sends a VIO_SUBTYPE_ACK for VIO_DRING_STOPPED.
1062          *
1063          * An ACK for every packet in the ring is expensive as the
1064          * sending of LDC messages is slow and affects performance.
1065          */
1066         d->hdr.ack = VIO_ACK_DISABLE;
1067         d->size = len;
1068         d->ncookies = port->tx_bufs[txi].ncookies;
1069         for (i = 0; i < d->ncookies; i++)
1070                 d->cookies[i] = port->tx_bufs[txi].cookies[i];
1071
1072         /* This has to be a non-SMP write barrier because we are writing
1073          * to memory which is shared with the peer LDOM.
1074          */
1075         wmb();
1076
1077         d->hdr.state = VIO_DESC_READY;
1078
1079         /* Exactly one ldc "start" trigger (for dr->cons) needs to be sent
1080          * to notify the consumer that some descriptors are READY.
1081          * After that "start" trigger, no additional triggers are needed until
1082          * a DRING_STOPPED is received from the consumer. The dr->cons field
1083          * (set up by vnet_ack()) has the value of the next dring index
1084          * that has not yet been ack-ed. We send a "start" trigger here
1085          * if, and only if, start_cons is true (reset it afterward). Conversely,
1086          * vnet_ack() should check if the dring corresponding to cons
1087          * is marked READY, but start_cons was false.
1088          * If so, vnet_ack() should send out the missed "start" trigger.
1089          *
1090          * Note that the wmb() above makes sure the cookies et al. are
1091          * not globally visible before the VIO_DESC_READY, and that the
1092          * stores are ordered correctly by the compiler. The consumer will
1093          * not proceed until the VIO_DESC_READY is visible assuring that
1094          * the consumer does not observe anything related to descriptors
1095          * out of order. The HV trap from the LDC start trigger is the
1096          * producer to consumer announcement that work is available to the
1097          * consumer
1098          */
1099         if (!port->start_cons)
1100                 goto ldc_start_done; /* previous trigger suffices */
1101
1102         err = __vnet_tx_trigger(port, dr->cons);
1103         if (unlikely(err < 0)) {
1104                 netdev_info(dev, "TX trigger error %d\n", err);
1105                 d->hdr.state = VIO_DESC_FREE;
1106                 dev->stats.tx_carrier_errors++;
1107                 goto out_dropped;
1108         }
1109
1110 ldc_start_done:
1111         port->start_cons = false;
1112
1113         dev->stats.tx_packets++;
1114         dev->stats.tx_bytes += port->tx_bufs[txi].skb->len;
1115
1116         dr->prod = (dr->prod + 1) & (VNET_TX_RING_SIZE - 1);
1117         if (unlikely(vnet_tx_dring_avail(dr) < 1)) {
1118                 netif_tx_stop_queue(txq);
1119                 if (vnet_tx_dring_avail(dr) > VNET_TX_WAKEUP_THRESH(dr))
1120                         netif_tx_wake_queue(txq);
1121         }
1122
1123         (void)mod_timer(&port->clean_timer, jiffies + VNET_CLEAN_TIMEOUT);
1124         rcu_read_unlock();
1125
1126         vnet_free_skbs(freeskbs);
1127
1128         return NETDEV_TX_OK;
1129
1130 out_dropped:
1131         if (pending)
1132                 (void)mod_timer(&port->clean_timer,
1133                                 jiffies + VNET_CLEAN_TIMEOUT);
1134         else if (port)
1135                 del_timer(&port->clean_timer);
1136         if (port)
1137                 rcu_read_unlock();
1138         if (skb)
1139                 dev_kfree_skb(skb);
1140         vnet_free_skbs(freeskbs);
1141         dev->stats.tx_dropped++;
1142         return NETDEV_TX_OK;
1143 }
1144
1145 static void vnet_tx_timeout(struct net_device *dev)
1146 {
1147         /* XXX Implement me XXX */
1148 }
1149
1150 static int vnet_open(struct net_device *dev)
1151 {
1152         netif_carrier_on(dev);
1153         netif_tx_start_all_queues(dev);
1154
1155         return 0;
1156 }
1157
1158 static int vnet_close(struct net_device *dev)
1159 {
1160         netif_tx_stop_all_queues(dev);
1161         netif_carrier_off(dev);
1162
1163         return 0;
1164 }
1165
1166 static struct vnet_mcast_entry *__vnet_mc_find(struct vnet *vp, u8 *addr)
1167 {
1168         struct vnet_mcast_entry *m;
1169
1170         for (m = vp->mcast_list; m; m = m->next) {
1171                 if (ether_addr_equal(m->addr, addr))
1172                         return m;
1173         }
1174         return NULL;
1175 }
1176
1177 static void __update_mc_list(struct vnet *vp, struct net_device *dev)
1178 {
1179         struct netdev_hw_addr *ha;
1180
1181         netdev_for_each_mc_addr(ha, dev) {
1182                 struct vnet_mcast_entry *m;
1183
1184                 m = __vnet_mc_find(vp, ha->addr);
1185                 if (m) {
1186                         m->hit = 1;
1187                         continue;
1188                 }
1189
1190                 if (!m) {
1191                         m = kzalloc(sizeof(*m), GFP_ATOMIC);
1192                         if (!m)
1193                                 continue;
1194                         memcpy(m->addr, ha->addr, ETH_ALEN);
1195                         m->hit = 1;
1196
1197                         m->next = vp->mcast_list;
1198                         vp->mcast_list = m;
1199                 }
1200         }
1201 }
1202
1203 static void __send_mc_list(struct vnet *vp, struct vnet_port *port)
1204 {
1205         struct vio_net_mcast_info info;
1206         struct vnet_mcast_entry *m, **pp;
1207         int n_addrs;
1208
1209         memset(&info, 0, sizeof(info));
1210
1211         info.tag.type = VIO_TYPE_CTRL;
1212         info.tag.stype = VIO_SUBTYPE_INFO;
1213         info.tag.stype_env = VNET_MCAST_INFO;
1214         info.tag.sid = vio_send_sid(&port->vio);
1215         info.set = 1;
1216
1217         n_addrs = 0;
1218         for (m = vp->mcast_list; m; m = m->next) {
1219                 if (m->sent)
1220                         continue;
1221                 m->sent = 1;
1222                 memcpy(&info.mcast_addr[n_addrs * ETH_ALEN],
1223                        m->addr, ETH_ALEN);
1224                 if (++n_addrs == VNET_NUM_MCAST) {
1225                         info.count = n_addrs;
1226
1227                         (void) vio_ldc_send(&port->vio, &info,
1228                                             sizeof(info));
1229                         n_addrs = 0;
1230                 }
1231         }
1232         if (n_addrs) {
1233                 info.count = n_addrs;
1234                 (void) vio_ldc_send(&port->vio, &info, sizeof(info));
1235         }
1236
1237         info.set = 0;
1238
1239         n_addrs = 0;
1240         pp = &vp->mcast_list;
1241         while ((m = *pp) != NULL) {
1242                 if (m->hit) {
1243                         m->hit = 0;
1244                         pp = &m->next;
1245                         continue;
1246                 }
1247
1248                 memcpy(&info.mcast_addr[n_addrs * ETH_ALEN],
1249                        m->addr, ETH_ALEN);
1250                 if (++n_addrs == VNET_NUM_MCAST) {
1251                         info.count = n_addrs;
1252                         (void) vio_ldc_send(&port->vio, &info,
1253                                             sizeof(info));
1254                         n_addrs = 0;
1255                 }
1256
1257                 *pp = m->next;
1258                 kfree(m);
1259         }
1260         if (n_addrs) {
1261                 info.count = n_addrs;
1262                 (void) vio_ldc_send(&port->vio, &info, sizeof(info));
1263         }
1264 }
1265
1266 static void vnet_set_rx_mode(struct net_device *dev)
1267 {
1268         struct vnet *vp = netdev_priv(dev);
1269         struct vnet_port *port;
1270
1271         rcu_read_lock();
1272         list_for_each_entry_rcu(port, &vp->port_list, list) {
1273
1274                 if (port->switch_port) {
1275                         __update_mc_list(vp, dev);
1276                         __send_mc_list(vp, port);
1277                         break;
1278                 }
1279         }
1280         rcu_read_unlock();
1281 }
1282
1283 static int vnet_change_mtu(struct net_device *dev, int new_mtu)
1284 {
1285         if (new_mtu < 68 || new_mtu > 65535)
1286                 return -EINVAL;
1287
1288         dev->mtu = new_mtu;
1289         return 0;
1290 }
1291
1292 static int vnet_set_mac_addr(struct net_device *dev, void *p)
1293 {
1294         return -EINVAL;
1295 }
1296
1297 static void vnet_get_drvinfo(struct net_device *dev,
1298                              struct ethtool_drvinfo *info)
1299 {
1300         strlcpy(info->driver, DRV_MODULE_NAME, sizeof(info->driver));
1301         strlcpy(info->version, DRV_MODULE_VERSION, sizeof(info->version));
1302 }
1303
1304 static u32 vnet_get_msglevel(struct net_device *dev)
1305 {
1306         struct vnet *vp = netdev_priv(dev);
1307         return vp->msg_enable;
1308 }
1309
1310 static void vnet_set_msglevel(struct net_device *dev, u32 value)
1311 {
1312         struct vnet *vp = netdev_priv(dev);
1313         vp->msg_enable = value;
1314 }
1315
1316 static const struct ethtool_ops vnet_ethtool_ops = {
1317         .get_drvinfo            = vnet_get_drvinfo,
1318         .get_msglevel           = vnet_get_msglevel,
1319         .set_msglevel           = vnet_set_msglevel,
1320         .get_link               = ethtool_op_get_link,
1321 };
1322
1323 static void vnet_port_free_tx_bufs(struct vnet_port *port)
1324 {
1325         struct vio_dring_state *dr;
1326         int i;
1327
1328         dr = &port->vio.drings[VIO_DRIVER_TX_RING];
1329         if (dr->base) {
1330                 ldc_free_exp_dring(port->vio.lp, dr->base,
1331                                    (dr->entry_size * dr->num_entries),
1332                                    dr->cookies, dr->ncookies);
1333                 dr->base = NULL;
1334                 dr->entry_size = 0;
1335                 dr->num_entries = 0;
1336                 dr->pending = 0;
1337                 dr->ncookies = 0;
1338         }
1339
1340         for (i = 0; i < VNET_TX_RING_SIZE; i++) {
1341                 struct vio_net_desc *d;
1342                 void *skb = port->tx_bufs[i].skb;
1343
1344                 if (!skb)
1345                         continue;
1346
1347                 d = vio_dring_entry(dr, i);
1348                 if (d->hdr.state == VIO_DESC_READY)
1349                         pr_warn("active transmit buffers freed\n");
1350
1351                 ldc_unmap(port->vio.lp,
1352                           port->tx_bufs[i].cookies,
1353                           port->tx_bufs[i].ncookies);
1354                 dev_kfree_skb(skb);
1355                 port->tx_bufs[i].skb = NULL;
1356                 d->hdr.state = VIO_DESC_FREE;
1357         }
1358 }
1359
1360 static int vnet_port_alloc_tx_bufs(struct vnet_port *port)
1361 {
1362         struct vio_dring_state *dr;
1363         unsigned long len;
1364         int i, err, ncookies;
1365         void *dring;
1366
1367         dr = &port->vio.drings[VIO_DRIVER_TX_RING];
1368
1369         len = (VNET_TX_RING_SIZE *
1370                (sizeof(struct vio_net_desc) +
1371                 (sizeof(struct ldc_trans_cookie) * 2)));
1372
1373         ncookies = VIO_MAX_RING_COOKIES;
1374         dring = ldc_alloc_exp_dring(port->vio.lp, len,
1375                                     dr->cookies, &ncookies,
1376                                     (LDC_MAP_SHADOW |
1377                                      LDC_MAP_DIRECT |
1378                                      LDC_MAP_RW));
1379         if (IS_ERR(dring)) {
1380                 err = PTR_ERR(dring);
1381                 goto err_out;
1382         }
1383
1384         dr->base = dring;
1385         dr->entry_size = (sizeof(struct vio_net_desc) +
1386                           (sizeof(struct ldc_trans_cookie) * 2));
1387         dr->num_entries = VNET_TX_RING_SIZE;
1388         dr->prod = dr->cons = 0;
1389         port->start_cons  = true; /* need an initial trigger */
1390         dr->pending = VNET_TX_RING_SIZE;
1391         dr->ncookies = ncookies;
1392
1393         for (i = 0; i < VNET_TX_RING_SIZE; ++i) {
1394                 struct vio_net_desc *d;
1395
1396                 d = vio_dring_entry(dr, i);
1397                 d->hdr.state = VIO_DESC_FREE;
1398         }
1399         return 0;
1400
1401 err_out:
1402         vnet_port_free_tx_bufs(port);
1403
1404         return err;
1405 }
1406
1407 #ifdef CONFIG_NET_POLL_CONTROLLER
1408 static void vnet_poll_controller(struct net_device *dev)
1409 {
1410         struct vnet *vp = netdev_priv(dev);
1411         struct vnet_port *port;
1412         unsigned long flags;
1413
1414         spin_lock_irqsave(&vp->lock, flags);
1415         if (!list_empty(&vp->port_list)) {
1416                 port = list_entry(vp->port_list.next, struct vnet_port, list);
1417                 napi_schedule(&port->napi);
1418         }
1419         spin_unlock_irqrestore(&vp->lock, flags);
1420 }
1421 #endif
1422 static LIST_HEAD(vnet_list);
1423 static DEFINE_MUTEX(vnet_list_mutex);
1424
1425 static const struct net_device_ops vnet_ops = {
1426         .ndo_open               = vnet_open,
1427         .ndo_stop               = vnet_close,
1428         .ndo_set_rx_mode        = vnet_set_rx_mode,
1429         .ndo_set_mac_address    = vnet_set_mac_addr,
1430         .ndo_validate_addr      = eth_validate_addr,
1431         .ndo_tx_timeout         = vnet_tx_timeout,
1432         .ndo_change_mtu         = vnet_change_mtu,
1433         .ndo_start_xmit         = vnet_start_xmit,
1434         .ndo_select_queue       = vnet_select_queue,
1435 #ifdef CONFIG_NET_POLL_CONTROLLER
1436         .ndo_poll_controller    = vnet_poll_controller,
1437 #endif
1438 };
1439
1440 static struct vnet *vnet_new(const u64 *local_mac)
1441 {
1442         struct net_device *dev;
1443         struct vnet *vp;
1444         int err, i;
1445
1446         dev = alloc_etherdev_mqs(sizeof(*vp), VNET_MAX_TXQS, 1);
1447         if (!dev)
1448                 return ERR_PTR(-ENOMEM);
1449         dev->needed_headroom = VNET_PACKET_SKIP + 8;
1450         dev->needed_tailroom = 8;
1451
1452         for (i = 0; i < ETH_ALEN; i++)
1453                 dev->dev_addr[i] = (*local_mac >> (5 - i) * 8) & 0xff;
1454
1455         vp = netdev_priv(dev);
1456
1457         spin_lock_init(&vp->lock);
1458         vp->dev = dev;
1459
1460         INIT_LIST_HEAD(&vp->port_list);
1461         for (i = 0; i < VNET_PORT_HASH_SIZE; i++)
1462                 INIT_HLIST_HEAD(&vp->port_hash[i]);
1463         INIT_LIST_HEAD(&vp->list);
1464         vp->local_mac = *local_mac;
1465
1466         dev->netdev_ops = &vnet_ops;
1467         dev->ethtool_ops = &vnet_ethtool_ops;
1468         dev->watchdog_timeo = VNET_TX_TIMEOUT;
1469
1470         err = register_netdev(dev);
1471         if (err) {
1472                 pr_err("Cannot register net device, aborting\n");
1473                 goto err_out_free_dev;
1474         }
1475
1476         netdev_info(dev, "Sun LDOM vnet %pM\n", dev->dev_addr);
1477
1478         list_add(&vp->list, &vnet_list);
1479
1480         return vp;
1481
1482 err_out_free_dev:
1483         free_netdev(dev);
1484
1485         return ERR_PTR(err);
1486 }
1487
1488 static struct vnet *vnet_find_or_create(const u64 *local_mac)
1489 {
1490         struct vnet *iter, *vp;
1491
1492         mutex_lock(&vnet_list_mutex);
1493         vp = NULL;
1494         list_for_each_entry(iter, &vnet_list, list) {
1495                 if (iter->local_mac == *local_mac) {
1496                         vp = iter;
1497                         break;
1498                 }
1499         }
1500         if (!vp)
1501                 vp = vnet_new(local_mac);
1502         mutex_unlock(&vnet_list_mutex);
1503
1504         return vp;
1505 }
1506
1507 static void vnet_cleanup(void)
1508 {
1509         struct vnet *vp;
1510         struct net_device *dev;
1511
1512         mutex_lock(&vnet_list_mutex);
1513         while (!list_empty(&vnet_list)) {
1514                 vp = list_first_entry(&vnet_list, struct vnet, list);
1515                 list_del(&vp->list);
1516                 dev = vp->dev;
1517                 /* vio_unregister_driver() should have cleaned up port_list */
1518                 BUG_ON(!list_empty(&vp->port_list));
1519                 unregister_netdev(dev);
1520                 free_netdev(dev);
1521         }
1522         mutex_unlock(&vnet_list_mutex);
1523 }
1524
1525 static const char *local_mac_prop = "local-mac-address";
1526
1527 static struct vnet *vnet_find_parent(struct mdesc_handle *hp,
1528                                                 u64 port_node)
1529 {
1530         const u64 *local_mac = NULL;
1531         u64 a;
1532
1533         mdesc_for_each_arc(a, hp, port_node, MDESC_ARC_TYPE_BACK) {
1534                 u64 target = mdesc_arc_target(hp, a);
1535                 const char *name;
1536
1537                 name = mdesc_get_property(hp, target, "name", NULL);
1538                 if (!name || strcmp(name, "network"))
1539                         continue;
1540
1541                 local_mac = mdesc_get_property(hp, target,
1542                                                local_mac_prop, NULL);
1543                 if (local_mac)
1544                         break;
1545         }
1546         if (!local_mac)
1547                 return ERR_PTR(-ENODEV);
1548
1549         return vnet_find_or_create(local_mac);
1550 }
1551
1552 static struct ldc_channel_config vnet_ldc_cfg = {
1553         .event          = vnet_event,
1554         .mtu            = 64,
1555         .mode           = LDC_MODE_UNRELIABLE,
1556 };
1557
1558 static struct vio_driver_ops vnet_vio_ops = {
1559         .send_attr              = vnet_send_attr,
1560         .handle_attr            = vnet_handle_attr,
1561         .handshake_complete     = vnet_handshake_complete,
1562 };
1563
1564 static void print_version(void)
1565 {
1566         printk_once(KERN_INFO "%s", version);
1567 }
1568
1569 const char *remote_macaddr_prop = "remote-mac-address";
1570
1571 static void
1572 vnet_port_add_txq(struct vnet_port *port)
1573 {
1574         struct vnet *vp = port->vp;
1575         int n;
1576
1577         n = vp->nports++;
1578         n = n & (VNET_MAX_TXQS - 1);
1579         port->q_index = n;
1580         netif_tx_wake_queue(netdev_get_tx_queue(vp->dev, port->q_index));
1581 }
1582
1583 static void
1584 vnet_port_rm_txq(struct vnet_port *port)
1585 {
1586         port->vp->nports--;
1587         netif_tx_stop_queue(netdev_get_tx_queue(port->vp->dev, port->q_index));
1588 }
1589
1590 static int vnet_port_probe(struct vio_dev *vdev, const struct vio_device_id *id)
1591 {
1592         struct mdesc_handle *hp;
1593         struct vnet_port *port;
1594         unsigned long flags;
1595         struct vnet *vp;
1596         const u64 *rmac;
1597         int len, i, err, switch_port;
1598
1599         print_version();
1600
1601         hp = mdesc_grab();
1602
1603         vp = vnet_find_parent(hp, vdev->mp);
1604         if (IS_ERR(vp)) {
1605                 pr_err("Cannot find port parent vnet\n");
1606                 err = PTR_ERR(vp);
1607                 goto err_out_put_mdesc;
1608         }
1609
1610         rmac = mdesc_get_property(hp, vdev->mp, remote_macaddr_prop, &len);
1611         err = -ENODEV;
1612         if (!rmac) {
1613                 pr_err("Port lacks %s property\n", remote_macaddr_prop);
1614                 goto err_out_put_mdesc;
1615         }
1616
1617         port = kzalloc(sizeof(*port), GFP_KERNEL);
1618         err = -ENOMEM;
1619         if (!port)
1620                 goto err_out_put_mdesc;
1621
1622         for (i = 0; i < ETH_ALEN; i++)
1623                 port->raddr[i] = (*rmac >> (5 - i) * 8) & 0xff;
1624
1625         port->vp = vp;
1626
1627         err = vio_driver_init(&port->vio, vdev, VDEV_NETWORK,
1628                               vnet_versions, ARRAY_SIZE(vnet_versions),
1629                               &vnet_vio_ops, vp->dev->name);
1630         if (err)
1631                 goto err_out_free_port;
1632
1633         err = vio_ldc_alloc(&port->vio, &vnet_ldc_cfg, port);
1634         if (err)
1635                 goto err_out_free_port;
1636
1637         netif_napi_add(port->vp->dev, &port->napi, vnet_poll, NAPI_POLL_WEIGHT);
1638
1639         err = vnet_port_alloc_tx_bufs(port);
1640         if (err)
1641                 goto err_out_free_ldc;
1642
1643         INIT_HLIST_NODE(&port->hash);
1644         INIT_LIST_HEAD(&port->list);
1645
1646         switch_port = 0;
1647         if (mdesc_get_property(hp, vdev->mp, "switch-port", NULL) != NULL)
1648                 switch_port = 1;
1649         port->switch_port = switch_port;
1650
1651         spin_lock_irqsave(&vp->lock, flags);
1652         if (switch_port)
1653                 list_add_rcu(&port->list, &vp->port_list);
1654         else
1655                 list_add_tail_rcu(&port->list, &vp->port_list);
1656         hlist_add_head_rcu(&port->hash,
1657                            &vp->port_hash[vnet_hashfn(port->raddr)]);
1658         vnet_port_add_txq(port);
1659         spin_unlock_irqrestore(&vp->lock, flags);
1660
1661         dev_set_drvdata(&vdev->dev, port);
1662
1663         pr_info("%s: PORT ( remote-mac %pM%s )\n",
1664                 vp->dev->name, port->raddr, switch_port ? " switch-port" : "");
1665
1666         setup_timer(&port->clean_timer, vnet_clean_timer_expire,
1667                     (unsigned long)port);
1668
1669         napi_enable(&port->napi);
1670         vio_port_up(&port->vio);
1671
1672         mdesc_release(hp);
1673
1674         return 0;
1675
1676 err_out_free_ldc:
1677         netif_napi_del(&port->napi);
1678         vio_ldc_free(&port->vio);
1679
1680 err_out_free_port:
1681         kfree(port);
1682
1683 err_out_put_mdesc:
1684         mdesc_release(hp);
1685         return err;
1686 }
1687
1688 static int vnet_port_remove(struct vio_dev *vdev)
1689 {
1690         struct vnet_port *port = dev_get_drvdata(&vdev->dev);
1691
1692         if (port) {
1693
1694                 del_timer_sync(&port->vio.timer);
1695
1696                 napi_disable(&port->napi);
1697
1698                 list_del_rcu(&port->list);
1699                 hlist_del_rcu(&port->hash);
1700
1701                 synchronize_rcu();
1702                 del_timer_sync(&port->clean_timer);
1703                 vnet_port_rm_txq(port);
1704                 netif_napi_del(&port->napi);
1705                 vnet_port_free_tx_bufs(port);
1706                 vio_ldc_free(&port->vio);
1707
1708                 dev_set_drvdata(&vdev->dev, NULL);
1709
1710                 kfree(port);
1711
1712         }
1713         return 0;
1714 }
1715
1716 static const struct vio_device_id vnet_port_match[] = {
1717         {
1718                 .type = "vnet-port",
1719         },
1720         {},
1721 };
1722 MODULE_DEVICE_TABLE(vio, vnet_port_match);
1723
1724 static struct vio_driver vnet_port_driver = {
1725         .id_table       = vnet_port_match,
1726         .probe          = vnet_port_probe,
1727         .remove         = vnet_port_remove,
1728         .name           = "vnet_port",
1729 };
1730
1731 static int __init vnet_init(void)
1732 {
1733         return vio_register_driver(&vnet_port_driver);
1734 }
1735
1736 static void __exit vnet_exit(void)
1737 {
1738         vio_unregister_driver(&vnet_port_driver);
1739         vnet_cleanup();
1740 }
1741
1742 module_init(vnet_init);
1743 module_exit(vnet_exit);