Pileus Git - ~andy/linux/blob - net/packet/af_packet.c

   1 /*
   2  * INET         An implementation of the TCP/IP protocol suite for the LINUX
   3  *              operating system.  INET is implemented using the  BSD Socket
   4  *              interface as the means of communication with the user level.
   5  *
   6  *              PACKET - implements raw packet sockets.
   7  *
   8  * Authors:     Ross Biro
   9  *              Fred N. van Kempen, <waltje@uWalt.NL.Mugnet.ORG>
  10  *              Alan Cox, <gw4pts@gw4pts.ampr.org>
  11  *
  12  * Fixes:
  13  *              Alan Cox        :       verify_area() now used correctly
  14  *              Alan Cox        :       new skbuff lists, look ma no backlogs!
  15  *              Alan Cox        :       tidied skbuff lists.
  16  *              Alan Cox        :       Now uses generic datagram routines I
  17  *                                      added. Also fixed the peek/read crash
  18  *                                      from all old Linux datagram code.
  19  *              Alan Cox        :       Uses the improved datagram code.
  20  *              Alan Cox        :       Added NULL's for socket options.
  21  *              Alan Cox        :       Re-commented the code.
  22  *              Alan Cox        :       Use new kernel side addressing
  23  *              Rob Janssen     :       Correct MTU usage.
  24  *              Dave Platt      :       Counter leaks caused by incorrect
  25  *                                      interrupt locking and some slightly
  26  *                                      dubious gcc output. Can you read
  27  *                                      compiler: it said _VOLATILE_
  28  *      Richard Kooijman        :       Timestamp fixes.
  29  *              Alan Cox        :       New buffers. Use sk->mac.raw.
  30  *              Alan Cox        :       sendmsg/recvmsg support.
  31  *              Alan Cox        :       Protocol setting support
  32  *      Alexey Kuznetsov        :       Untied from IPv4 stack.
  33  *      Cyrus Durgin            :       Fixed kerneld for kmod.
  34  *      Michal Ostrowski        :       Module initialization cleanup.
  35  *         Ulises Alonso        :       Frame number limit removal and
  36  *                                      packet_set_ring memory leak.
  37  *              Eric Biederman  :       Allow for > 8 byte hardware addresses.
  38  *                                      The convention is that longer addresses
  39  *                                      will simply extend the hardware address
  40  *                                      byte arrays at the end of sockaddr_ll
  41  *                                      and packet_mreq.
  42  *              Johann Baudy    :       Added TX RING.
  43  *              Chetan Loke     :       Implemented TPACKET_V3 block abstraction
  44  *                                      layer.
  45  *                                      Copyright (C) 2011, <lokec@ccs.neu.edu>
  46  *
  47  *
  48  *              This program is free software; you can redistribute it and/or
  49  *              modify it under the terms of the GNU General Public License
  50  *              as published by the Free Software Foundation; either version
  51  *              2 of the License, or (at your option) any later version.
  52  *
  53  */
  54
  55 #include <linux/types.h>
  56 #include <linux/mm.h>
  57 #include <linux/capability.h>
  58 #include <linux/fcntl.h>
  59 #include <linux/socket.h>
  60 #include <linux/in.h>
  61 #include <linux/inet.h>
  62 #include <linux/netdevice.h>
  63 #include <linux/if_packet.h>
  64 #include <linux/wireless.h>
  65 #include <linux/kernel.h>
  66 #include <linux/kmod.h>
  67 #include <linux/slab.h>
  68 #include <linux/vmalloc.h>
  69 #include <net/net_namespace.h>
  70 #include <net/ip.h>
  71 #include <net/protocol.h>
  72 #include <linux/skbuff.h>
  73 #include <net/sock.h>
  74 #include <linux/errno.h>
  75 #include <linux/timer.h>
  76 #include <asm/uaccess.h>
  77 #include <asm/ioctls.h>
  78 #include <asm/page.h>
  79 #include <asm/cacheflush.h>
  80 #include <asm/io.h>
  81 #include <linux/proc_fs.h>
  82 #include <linux/seq_file.h>
  83 #include <linux/poll.h>
  84 #include <linux/module.h>
  85 #include <linux/init.h>
  86 #include <linux/mutex.h>
  87 #include <linux/if_vlan.h>
  88 #include <linux/virtio_net.h>
  89 #include <linux/errqueue.h>
  90 #include <linux/net_tstamp.h>
  91
  92 #ifdef CONFIG_INET
  93 #include <net/inet_common.h>
  94 #endif
  95
  96 /*
  97    Assumptions:
  98    - if device has no dev->hard_header routine, it adds and removes ll header
  99      inside itself. In this case ll header is invisible outside of device,
 100      but higher levels still should reserve dev->hard_header_len.
 101      Some devices are enough clever to reallocate skb, when header
 102      will not fit to reserved space (tunnel), another ones are silly
 103      (PPP).
 104    - packet socket receives packets with pulled ll header,
 105      so that SOCK_RAW should push it back.
 106
 107 On receive:
 108 -----------
 109
 110 Incoming, dev->hard_header!=NULL
 111    mac_header -> ll header
 112    data       -> data
 113
 114 Outgoing, dev->hard_header!=NULL
 115    mac_header -> ll header
 116    data       -> ll header
 117
 118 Incoming, dev->hard_header==NULL
 119    mac_header -> UNKNOWN position. It is very likely, that it points to ll
 120                  header.  PPP makes it, that is wrong, because introduce
 121                  assymetry between rx and tx paths.
 122    data       -> data
 123
 124 Outgoing, dev->hard_header==NULL
 125    mac_header -> data. ll header is still not built!
 126    data       -> data
 127
 128 Resume
 129   If dev->hard_header==NULL we are unlikely to restore sensible ll header.
 130
 131
 132 On transmit:
 133 ------------
 134
 135 dev->hard_header != NULL
 136    mac_header -> ll header
 137    data       -> ll header
 138
 139 dev->hard_header == NULL (ll header is added by device, we cannot control it)
 140    mac_header -> data
 141    data       -> data
 142
 143    We should set nh.raw on output to correct posistion,
 144    packet classifier depends on it.
 145  */
 146
 147 /* Private packet socket structures. */
 148
 149 struct packet_mclist {
 150         struct packet_mclist    *next;
 151         int                     ifindex;
 152         int                     count;
 153         unsigned short          type;
 154         unsigned short          alen;
 155         unsigned char           addr[MAX_ADDR_LEN];
 156 };
 157 /* identical to struct packet_mreq except it has
 158  * a longer address field.
 159  */
 160 struct packet_mreq_max {
 161         int             mr_ifindex;
 162         unsigned short  mr_type;
 163         unsigned short  mr_alen;
 164         unsigned char   mr_address[MAX_ADDR_LEN];
 165 };
 166
 167 static int packet_set_ring(struct sock *sk, union tpacket_req_u *req_u,
 168                 int closing, int tx_ring);
 169
 170
 171 #define V3_ALIGNMENT    (8)
 172
 173 #define BLK_HDR_LEN     (ALIGN(sizeof(struct tpacket_block_desc), V3_ALIGNMENT))
 174
 175 #define BLK_PLUS_PRIV(sz_of_priv) \
 176         (BLK_HDR_LEN + ALIGN((sz_of_priv), V3_ALIGNMENT))
 177
 178 /* kbdq - kernel block descriptor queue */
 179 struct tpacket_kbdq_core {
 180         struct pgv      *pkbdq;
 181         unsigned int    feature_req_word;
 182         unsigned int    hdrlen;
 183         unsigned char   reset_pending_on_curr_blk;
 184         unsigned char   delete_blk_timer;
 185         unsigned short  kactive_blk_num;
 186         unsigned short  blk_sizeof_priv;
 187
 188         /* last_kactive_blk_num:
 189          * trick to see if user-space has caught up
 190          * in order to avoid refreshing timer when every single pkt arrives.
 191          */
 192         unsigned short  last_kactive_blk_num;
 193
 194         char            *pkblk_start;
 195         char            *pkblk_end;
 196         int             kblk_size;
 197         unsigned int    knum_blocks;
 198         uint64_t        knxt_seq_num;
 199         char            *prev;
 200         char            *nxt_offset;
 201         struct sk_buff  *skb;
 202
 203         atomic_t        blk_fill_in_prog;
 204
 205         /* Default is set to 8ms */
 206 #define DEFAULT_PRB_RETIRE_TOV  (8)
 207
 208         unsigned short  retire_blk_tov;
 209         unsigned short  version;
 210         unsigned long   tov_in_jiffies;
 211
 212         /* timer to retire an outstanding block */
 213         struct timer_list retire_blk_timer;
 214 };
 215
 216 #define PGV_FROM_VMALLOC 1
 217 struct pgv {
 218         char *buffer;
 219 };
 220
 221 struct packet_ring_buffer {
 222         struct pgv              *pg_vec;
 223         unsigned int            head;
 224         unsigned int            frames_per_block;
 225         unsigned int            frame_size;
 226         unsigned int            frame_max;
 227
 228         unsigned int            pg_vec_order;
 229         unsigned int            pg_vec_pages;
 230         unsigned int            pg_vec_len;
 231
 232         struct tpacket_kbdq_core        prb_bdqc;
 233         atomic_t                pending;
 234 };
 235
 236 #define BLOCK_STATUS(x) ((x)->hdr.bh1.block_status)
 237 #define BLOCK_NUM_PKTS(x)       ((x)->hdr.bh1.num_pkts)
 238 #define BLOCK_O2FP(x)           ((x)->hdr.bh1.offset_to_first_pkt)
 239 #define BLOCK_LEN(x)            ((x)->hdr.bh1.blk_len)
 240 #define BLOCK_SNUM(x)           ((x)->hdr.bh1.seq_num)
 241 #define BLOCK_O2PRIV(x) ((x)->offset_to_priv)
 242 #define BLOCK_PRIV(x)           ((void *)((char *)(x) + BLOCK_O2PRIV(x)))
 243
 244 struct packet_sock;
 245 static int tpacket_snd(struct packet_sock *po, struct msghdr *msg);
 246
 247 static void *packet_previous_frame(struct packet_sock *po,
 248                 struct packet_ring_buffer *rb,
 249                 int status);
 250 static void packet_increment_head(struct packet_ring_buffer *buff);
 251 static int prb_curr_blk_in_use(struct tpacket_kbdq_core *,
 252                         struct tpacket_block_desc *);
 253 static void *prb_dispatch_next_block(struct tpacket_kbdq_core *,
 254                         struct packet_sock *);
 255 static void prb_retire_current_block(struct tpacket_kbdq_core *,
 256                 struct packet_sock *, unsigned int status);
 257 static int prb_queue_frozen(struct tpacket_kbdq_core *);
 258 static void prb_open_block(struct tpacket_kbdq_core *,
 259                 struct tpacket_block_desc *);
 260 static void prb_retire_rx_blk_timer_expired(unsigned long);
 261 static void _prb_refresh_rx_retire_blk_timer(struct tpacket_kbdq_core *);
 262 static void prb_init_blk_timer(struct packet_sock *,
 263                 struct tpacket_kbdq_core *,
 264                 void (*func) (unsigned long));
 265 static void prb_fill_rxhash(struct tpacket_kbdq_core *, struct tpacket3_hdr *);
 266 static void prb_clear_rxhash(struct tpacket_kbdq_core *,
 267                 struct tpacket3_hdr *);
 268 static void prb_fill_vlan_info(struct tpacket_kbdq_core *,
 269                 struct tpacket3_hdr *);
 270 static void packet_flush_mclist(struct sock *sk);
 271
 272 struct packet_fanout;
 273 struct packet_sock {
 274         /* struct sock has to be the first member of packet_sock */
 275         struct sock             sk;
 276         struct packet_fanout    *fanout;
 277         struct tpacket_stats    stats;
 278         union  tpacket_stats_u  stats_u;
 279         struct packet_ring_buffer       rx_ring;
 280         struct packet_ring_buffer       tx_ring;
 281         int                     copy_thresh;
 282         spinlock_t              bind_lock;
 283         struct mutex            pg_vec_lock;
 284         unsigned int            running:1,      /* prot_hook is attached*/
 285                                 auxdata:1,
 286                                 origdev:1,
 287                                 has_vnet_hdr:1;
 288         int                     ifindex;        /* bound device         */
 289         __be16                  num;
 290         struct packet_mclist    *mclist;
 291         atomic_t                mapped;
 292         enum tpacket_versions   tp_version;
 293         unsigned int            tp_hdrlen;
 294         unsigned int            tp_reserve;
 295         unsigned int            tp_loss:1;
 296         unsigned int            tp_tstamp;
 297         struct packet_type      prot_hook ____cacheline_aligned_in_smp;
 298 };
 299
 300 #define PACKET_FANOUT_MAX       256
 301
 302 struct packet_fanout {
 303 #ifdef CONFIG_NET_NS
 304         struct net              *net;
 305 #endif
 306         unsigned int            num_members;
 307         u16                     id;
 308         u8                      type;
 309         u8                      defrag;
 310         atomic_t                rr_cur;
 311         struct list_head        list;
 312         struct sock             *arr[PACKET_FANOUT_MAX];
 313         spinlock_t              lock;
 314         atomic_t                sk_ref;
 315         struct packet_type      prot_hook ____cacheline_aligned_in_smp;
 316 };
 317
 318 struct packet_skb_cb {
 319         unsigned int origlen;
 320         union {
 321                 struct sockaddr_pkt pkt;
 322                 struct sockaddr_ll ll;
 323         } sa;
 324 };
 325
 326 #define PACKET_SKB_CB(__skb)    ((struct packet_skb_cb *)((__skb)->cb))
 327
 328 #define GET_PBDQC_FROM_RB(x)    ((struct tpacket_kbdq_core *)(&(x)->prb_bdqc))
 329 #define GET_PBLOCK_DESC(x, bid) \
 330         ((struct tpacket_block_desc *)((x)->pkbdq[(bid)].buffer))
 331 #define GET_CURR_PBLOCK_DESC_FROM_CORE(x)       \
 332         ((struct tpacket_block_desc *)((x)->pkbdq[(x)->kactive_blk_num].buffer))
 333 #define GET_NEXT_PRB_BLK_NUM(x) \
 334         (((x)->kactive_blk_num < ((x)->knum_blocks-1)) ? \
 335         ((x)->kactive_blk_num+1) : 0)
 336
 337 static struct packet_sock *pkt_sk(struct sock *sk)
 338 {
 339         return (struct packet_sock *)sk;
 340 }
 341
 342 static void __fanout_unlink(struct sock *sk, struct packet_sock *po);
 343 static void __fanout_link(struct sock *sk, struct packet_sock *po);
 344
 345 /* register_prot_hook must be invoked with the po->bind_lock held,
 346  * or from a context in which asynchronous accesses to the packet
 347  * socket is not possible (packet_create()).
 348  */
 349 static void register_prot_hook(struct sock *sk)
 350 {
 351         struct packet_sock *po = pkt_sk(sk);
 352         if (!po->running) {
 353                 if (po->fanout)
 354                         __fanout_link(sk, po);
 355                 else
 356                         dev_add_pack(&po->prot_hook);
 357                 sock_hold(sk);
 358                 po->running = 1;
 359         }
 360 }
 361
 362 /* {,__}unregister_prot_hook() must be invoked with the po->bind_lock
 363  * held.   If the sync parameter is true, we will temporarily drop
 364  * the po->bind_lock and do a synchronize_net to make sure no
 365  * asynchronous packet processing paths still refer to the elements
 366  * of po->prot_hook.  If the sync parameter is false, it is the
 367  * callers responsibility to take care of this.
 368  */
 369 static void __unregister_prot_hook(struct sock *sk, bool sync)
 370 {
 371         struct packet_sock *po = pkt_sk(sk);
 372
 373         po->running = 0;
 374         if (po->fanout)
 375                 __fanout_unlink(sk, po);
 376         else
 377                 __dev_remove_pack(&po->prot_hook);
 378         __sock_put(sk);
 379
 380         if (sync) {
 381                 spin_unlock(&po->bind_lock);
 382                 synchronize_net();
 383                 spin_lock(&po->bind_lock);
 384         }
 385 }
 386
 387 static void unregister_prot_hook(struct sock *sk, bool sync)
 388 {
 389         struct packet_sock *po = pkt_sk(sk);
 390
 391         if (po->running)
 392                 __unregister_prot_hook(sk, sync);
 393 }
 394
 395 static inline __pure struct page *pgv_to_page(void *addr)
 396 {
 397         if (is_vmalloc_addr(addr))
 398                 return vmalloc_to_page(addr);
 399         return virt_to_page(addr);
 400 }
 401
 402 static void __packet_set_status(struct packet_sock *po, void *frame, int status)
 403 {
 404         union {
 405                 struct tpacket_hdr *h1;
 406                 struct tpacket2_hdr *h2;
 407                 void *raw;
 408         } h;
 409
 410         h.raw = frame;
 411         switch (po->tp_version) {
 412         case TPACKET_V1:
 413                 h.h1->tp_status = status;
 414                 flush_dcache_page(pgv_to_page(&h.h1->tp_status));
 415                 break;
 416         case TPACKET_V2:
 417                 h.h2->tp_status = status;
 418                 flush_dcache_page(pgv_to_page(&h.h2->tp_status));
 419                 break;
 420         case TPACKET_V3:
 421         default:
 422                 WARN(1, "TPACKET version not supported.\n");
 423                 BUG();
 424         }
 425
 426         smp_wmb();
 427 }
 428
 429 static int __packet_get_status(struct packet_sock *po, void *frame)
 430 {
 431         union {
 432                 struct tpacket_hdr *h1;
 433                 struct tpacket2_hdr *h2;
 434                 void *raw;
 435         } h;
 436
 437         smp_rmb();
 438
 439         h.raw = frame;
 440         switch (po->tp_version) {
 441         case TPACKET_V1:
 442                 flush_dcache_page(pgv_to_page(&h.h1->tp_status));
 443                 return h.h1->tp_status;
 444         case TPACKET_V2:
 445                 flush_dcache_page(pgv_to_page(&h.h2->tp_status));
 446                 return h.h2->tp_status;
 447         case TPACKET_V3:
 448         default:
 449                 WARN(1, "TPACKET version not supported.\n");
 450                 BUG();
 451                 return 0;
 452         }
 453 }
 454
 455 static void *packet_lookup_frame(struct packet_sock *po,
 456                 struct packet_ring_buffer *rb,
 457                 unsigned int position,
 458                 int status)
 459 {
 460         unsigned int pg_vec_pos, frame_offset;
 461         union {
 462                 struct tpacket_hdr *h1;
 463                 struct tpacket2_hdr *h2;
 464                 void *raw;
 465         } h;
 466
 467         pg_vec_pos = position / rb->frames_per_block;
 468         frame_offset = position % rb->frames_per_block;
 469
 470         h.raw = rb->pg_vec[pg_vec_pos].buffer +
 471                 (frame_offset * rb->frame_size);
 472
 473         if (status != __packet_get_status(po, h.raw))
 474                 return NULL;
 475
 476         return h.raw;
 477 }
 478
 479 static void *packet_current_frame(struct packet_sock *po,
 480                 struct packet_ring_buffer *rb,
 481                 int status)
 482 {
 483         return packet_lookup_frame(po, rb, rb->head, status);
 484 }
 485
 486 static void prb_del_retire_blk_timer(struct tpacket_kbdq_core *pkc)
 487 {
 488         del_timer_sync(&pkc->retire_blk_timer);
 489 }
 490
 491 static void prb_shutdown_retire_blk_timer(struct packet_sock *po,
 492                 int tx_ring,
 493                 struct sk_buff_head *rb_queue)
 494 {
 495         struct tpacket_kbdq_core *pkc;
 496
 497         pkc = tx_ring ? &po->tx_ring.prb_bdqc : &po->rx_ring.prb_bdqc;
 498
 499         spin_lock(&rb_queue->lock);
 500         pkc->delete_blk_timer = 1;
 501         spin_unlock(&rb_queue->lock);
 502
 503         prb_del_retire_blk_timer(pkc);
 504 }
 505
 506 static void prb_init_blk_timer(struct packet_sock *po,
 507                 struct tpacket_kbdq_core *pkc,
 508                 void (*func) (unsigned long))
 509 {
 510         init_timer(&pkc->retire_blk_timer);
 511         pkc->retire_blk_timer.data = (long)po;
 512         pkc->retire_blk_timer.function = func;
 513         pkc->retire_blk_timer.expires = jiffies;
 514 }
 515
 516 static void prb_setup_retire_blk_timer(struct packet_sock *po, int tx_ring)
 517 {
 518         struct tpacket_kbdq_core *pkc;
 519
 520         if (tx_ring)
 521                 BUG();
 522
 523         pkc = tx_ring ? &po->tx_ring.prb_bdqc : &po->rx_ring.prb_bdqc;
 524         prb_init_blk_timer(po, pkc, prb_retire_rx_blk_timer_expired);
 525 }
 526
 527 static int prb_calc_retire_blk_tmo(struct packet_sock *po,
 528                                 int blk_size_in_bytes)
 529 {
 530         struct net_device *dev;
 531         unsigned int mbits = 0, msec = 0, div = 0, tmo = 0;
 532         struct ethtool_cmd ecmd;
 533         int err;
 534
 535         rtnl_lock();
 536         dev = __dev_get_by_index(sock_net(&po->sk), po->ifindex);
 537         if (unlikely(!dev)) {
 538                 rtnl_unlock();
 539                 return DEFAULT_PRB_RETIRE_TOV;
 540         }
 541         err = __ethtool_get_settings(dev, &ecmd);
 542         rtnl_unlock();
 543         if (!err) {
 544                 switch (ecmd.speed) {
 545                 case SPEED_10000:
 546                         msec = 1;
 547                         div = 10000/1000;
 548                         break;
 549                 case SPEED_1000:
 550                         msec = 1;
 551                         div = 1000/1000;
 552                         break;
 553                 /*
 554                  * If the link speed is so slow you don't really
 555                  * need to worry about perf anyways
 556                  */
 557                 case SPEED_100:
 558                 case SPEED_10:
 559                 default:
 560                         return DEFAULT_PRB_RETIRE_TOV;
 561                 }
 562         }
 563
 564         mbits = (blk_size_in_bytes * 8) / (1024 * 1024);
 565
 566         if (div)
 567                 mbits /= div;
 568
 569         tmo = mbits * msec;
 570
 571         if (div)
 572                 return tmo+1;
 573         return tmo;
 574 }
 575
 576 static void prb_init_ft_ops(struct tpacket_kbdq_core *p1,
 577                         union tpacket_req_u *req_u)
 578 {
 579         p1->feature_req_word = req_u->req3.tp_feature_req_word;
 580 }
 581
 582 static void init_prb_bdqc(struct packet_sock *po,
 583                         struct packet_ring_buffer *rb,
 584                         struct pgv *pg_vec,
 585                         union tpacket_req_u *req_u, int tx_ring)
 586 {
 587         struct tpacket_kbdq_core *p1 = &rb->prb_bdqc;
 588         struct tpacket_block_desc *pbd;
 589
 590         memset(p1, 0x0, sizeof(*p1));
 591
 592         p1->knxt_seq_num = 1;
 593         p1->pkbdq = pg_vec;
 594         pbd = (struct tpacket_block_desc *)pg_vec[0].buffer;
 595         p1->pkblk_start = (char *)pg_vec[0].buffer;
 596         p1->kblk_size = req_u->req3.tp_block_size;
 597         p1->knum_blocks = req_u->req3.tp_block_nr;
 598         p1->hdrlen = po->tp_hdrlen;
 599         p1->version = po->tp_version;
 600         p1->last_kactive_blk_num = 0;
 601         po->stats_u.stats3.tp_freeze_q_cnt = 0;
 602         if (req_u->req3.tp_retire_blk_tov)
 603                 p1->retire_blk_tov = req_u->req3.tp_retire_blk_tov;
 604         else
 605                 p1->retire_blk_tov = prb_calc_retire_blk_tmo(po,
 606                                                 req_u->req3.tp_block_size);
 607         p1->tov_in_jiffies = msecs_to_jiffies(p1->retire_blk_tov);
 608         p1->blk_sizeof_priv = req_u->req3.tp_sizeof_priv;
 609
 610         prb_init_ft_ops(p1, req_u);
 611         prb_setup_retire_blk_timer(po, tx_ring);
 612         prb_open_block(p1, pbd);
 613 }
 614
 615 /*  Do NOT update the last_blk_num first.
 616  *  Assumes sk_buff_head lock is held.
 617  */
 618 static void _prb_refresh_rx_retire_blk_timer(struct tpacket_kbdq_core *pkc)
 619 {
 620         mod_timer(&pkc->retire_blk_timer,
 621                         jiffies + pkc->tov_in_jiffies);
 622         pkc->last_kactive_blk_num = pkc->kactive_blk_num;
 623 }
 624
 625 /*
 626  * Timer logic:
 627  * 1) We refresh the timer only when we open a block.
 628  *    By doing this we don't waste cycles refreshing the timer
 629  *        on packet-by-packet basis.
 630  *
 631  * With a 1MB block-size, on a 1Gbps line, it will take
 632  * i) ~8 ms to fill a block + ii) memcpy etc.
 633  * In this cut we are not accounting for the memcpy time.
 634  *
 635  * So, if the user sets the 'tmo' to 10ms then the timer
 636  * will never fire while the block is still getting filled
 637  * (which is what we want). However, the user could choose
 638  * to close a block early and that's fine.
 639  *
 640  * But when the timer does fire, we check whether or not to refresh it.
 641  * Since the tmo granularity is in msecs, it is not too expensive
 642  * to refresh the timer, lets say every '8' msecs.
 643  * Either the user can set the 'tmo' or we can derive it based on
 644  * a) line-speed and b) block-size.
 645  * prb_calc_retire_blk_tmo() calculates the tmo.
 646  *
 647  */
 648 static void prb_retire_rx_blk_timer_expired(unsigned long data)
 649 {
 650         struct packet_sock *po = (struct packet_sock *)data;
 651         struct tpacket_kbdq_core *pkc = &po->rx_ring.prb_bdqc;
 652         unsigned int frozen;
 653         struct tpacket_block_desc *pbd;
 654
 655         spin_lock(&po->sk.sk_receive_queue.lock);
 656
 657         frozen = prb_queue_frozen(pkc);
 658         pbd = GET_CURR_PBLOCK_DESC_FROM_CORE(pkc);
 659
 660         if (unlikely(pkc->delete_blk_timer))
 661                 goto out;
 662
 663         /* We only need to plug the race when the block is partially filled.
 664          * tpacket_rcv:
 665          *              lock(); increment BLOCK_NUM_PKTS; unlock()
 666          *              copy_bits() is in progress ...
 667          *              timer fires on other cpu:
 668          *              we can't retire the current block because copy_bits
 669          *              is in progress.
 670          *
 671          */
 672         if (BLOCK_NUM_PKTS(pbd)) {
 673                 while (atomic_read(&pkc->blk_fill_in_prog)) {
 674                         /* Waiting for skb_copy_bits to finish... */
 675                         cpu_relax();
 676                 }
 677         }
 678
 679         if (pkc->last_kactive_blk_num == pkc->kactive_blk_num) {
 680                 if (!frozen) {
 681                         prb_retire_current_block(pkc, po, TP_STATUS_BLK_TMO);
 682                         if (!prb_dispatch_next_block(pkc, po))
 683                                 goto refresh_timer;
 684                         else
 685                                 goto out;
 686                 } else {
 687                         /* Case 1. Queue was frozen because user-space was
 688                          *         lagging behind.
 689                          */
 690                         if (prb_curr_blk_in_use(pkc, pbd)) {
 691                                 /*
 692                                  * Ok, user-space is still behind.
 693                                  * So just refresh the timer.
 694                                  */
 695                                 goto refresh_timer;
 696                         } else {
 697                                /* Case 2. queue was frozen,user-space caught up,
 698                                 * now the link went idle && the timer fired.
 699                                 * We don't have a block to close.So we open this
 700                                 * block and restart the timer.
 701                                 * opening a block thaws the queue,restarts timer
 702                                 * Thawing/timer-refresh is a side effect.
 703                                 */
 704                                 prb_open_block(pkc, pbd);
 705                                 goto out;
 706                         }
 707                 }
 708         }
 709
 710 refresh_timer:
 711         _prb_refresh_rx_retire_blk_timer(pkc);
 712
 713 out:
 714         spin_unlock(&po->sk.sk_receive_queue.lock);
 715 }
 716
 717 static void prb_flush_block(struct tpacket_kbdq_core *pkc1,
 718                 struct tpacket_block_desc *pbd1, __u32 status)
 719 {
 720         /* Flush everything minus the block header */
 721
 722 #if ARCH_IMPLEMENTS_FLUSH_DCACHE_PAGE == 1
 723         u8 *start, *end;
 724
 725         start = (u8 *)pbd1;
 726
 727         /* Skip the block header(we know header WILL fit in 4K) */
 728         start += PAGE_SIZE;
 729
 730         end = (u8 *)PAGE_ALIGN((unsigned long)pkc1->pkblk_end);
 731         for (; start < end; start += PAGE_SIZE)
 732                 flush_dcache_page(pgv_to_page(start));
 733
 734         smp_wmb();
 735 #endif
 736
 737         /* Now update the block status. */
 738
 739         BLOCK_STATUS(pbd1) = status;
 740
 741         /* Flush the block header */
 742
 743 #if ARCH_IMPLEMENTS_FLUSH_DCACHE_PAGE == 1
 744         start = (u8 *)pbd1;
 745         flush_dcache_page(pgv_to_page(start));
 746
 747         smp_wmb();
 748 #endif
 749 }
 750
 751 /*
 752  * Side effect:
 753  *
 754  * 1) flush the block
 755  * 2) Increment active_blk_num
 756  *
 757  * Note:We DONT refresh the timer on purpose.
 758  *      Because almost always the next block will be opened.
 759  */
 760 static void prb_close_block(struct tpacket_kbdq_core *pkc1,
 761                 struct tpacket_block_desc *pbd1,
 762                 struct packet_sock *po, unsigned int stat)
 763 {
 764         __u32 status = TP_STATUS_USER | stat;
 765
 766         struct tpacket3_hdr *last_pkt;
 767         struct tpacket_hdr_v1 *h1 = &pbd1->hdr.bh1;
 768
 769         if (po->stats.tp_drops)
 770                 status |= TP_STATUS_LOSING;
 771
 772         last_pkt = (struct tpacket3_hdr *)pkc1->prev;
 773         last_pkt->tp_next_offset = 0;
 774
 775         /* Get the ts of the last pkt */
 776         if (BLOCK_NUM_PKTS(pbd1)) {
 777                 h1->ts_last_pkt.ts_sec = last_pkt->tp_sec;
 778                 h1->ts_last_pkt.ts_nsec = last_pkt->tp_nsec;
 779         } else {
 780                 /* Ok, we tmo'd - so get the current time */
 781                 struct timespec ts;
 782                 getnstimeofday(&ts);
 783                 h1->ts_last_pkt.ts_sec = ts.tv_sec;
 784                 h1->ts_last_pkt.ts_nsec = ts.tv_nsec;
 785         }
 786
 787         smp_wmb();
 788
 789         /* Flush the block */
 790         prb_flush_block(pkc1, pbd1, status);
 791
 792         pkc1->kactive_blk_num = GET_NEXT_PRB_BLK_NUM(pkc1);
 793 }
 794
 795 static void prb_thaw_queue(struct tpacket_kbdq_core *pkc)
 796 {
 797         pkc->reset_pending_on_curr_blk = 0;
 798 }
 799
 800 /*
 801  * Side effect of opening a block:
 802  *
 803  * 1) prb_queue is thawed.
 804  * 2) retire_blk_timer is refreshed.
 805  *
 806  */
 807 static void prb_open_block(struct tpacket_kbdq_core *pkc1,
 808         struct tpacket_block_desc *pbd1)
 809 {
 810         struct timespec ts;
 811         struct tpacket_hdr_v1 *h1 = &pbd1->hdr.bh1;
 812
 813         smp_rmb();
 814
 815         if (likely(TP_STATUS_KERNEL == BLOCK_STATUS(pbd1))) {
 816
 817                 /* We could have just memset this but we will lose the
 818                  * flexibility of making the priv area sticky
 819                  */
 820                 BLOCK_SNUM(pbd1) = pkc1->knxt_seq_num++;
 821                 BLOCK_NUM_PKTS(pbd1) = 0;
 822                 BLOCK_LEN(pbd1) = BLK_PLUS_PRIV(pkc1->blk_sizeof_priv);
 823                 getnstimeofday(&ts);
 824                 h1->ts_first_pkt.ts_sec = ts.tv_sec;
 825                 h1->ts_first_pkt.ts_nsec = ts.tv_nsec;
 826                 pkc1->pkblk_start = (char *)pbd1;
 827                 pkc1->nxt_offset = (char *)(pkc1->pkblk_start +
 828                 BLK_PLUS_PRIV(pkc1->blk_sizeof_priv));
 829                 BLOCK_O2FP(pbd1) = (__u32)BLK_PLUS_PRIV(pkc1->blk_sizeof_priv);
 830                 BLOCK_O2PRIV(pbd1) = BLK_HDR_LEN;
 831                 pbd1->version = pkc1->version;
 832                 pkc1->prev = pkc1->nxt_offset;
 833                 pkc1->pkblk_end = pkc1->pkblk_start + pkc1->kblk_size;
 834                 prb_thaw_queue(pkc1);
 835                 _prb_refresh_rx_retire_blk_timer(pkc1);
 836
 837                 smp_wmb();
 838
 839                 return;
 840         }
 841
 842         WARN(1, "ERROR block:%p is NOT FREE status:%d kactive_blk_num:%d\n",
 843                 pbd1, BLOCK_STATUS(pbd1), pkc1->kactive_blk_num);
 844         dump_stack();
 845         BUG();
 846 }
 847
 848 /*
 849  * Queue freeze logic:
 850  * 1) Assume tp_block_nr = 8 blocks.
 851  * 2) At time 't0', user opens Rx ring.
 852  * 3) Some time past 't0', kernel starts filling blocks starting from 0 .. 7
 853  * 4) user-space is either sleeping or processing block '0'.
 854  * 5) tpacket_rcv is currently filling block '7', since there is no space left,
 855  *    it will close block-7,loop around and try to fill block '0'.
 856  *    call-flow:
 857  *    __packet_lookup_frame_in_block
 858  *      prb_retire_current_block()
 859  *      prb_dispatch_next_block()
 860  *        |->(BLOCK_STATUS == USER) evaluates to true
 861  *    5.1) Since block-0 is currently in-use, we just freeze the queue.
 862  * 6) Now there are two cases:
 863  *    6.1) Link goes idle right after the queue is frozen.
 864  *         But remember, the last open_block() refreshed the timer.
 865  *         When this timer expires,it will refresh itself so that we can
 866  *         re-open block-0 in near future.
 867  *    6.2) Link is busy and keeps on receiving packets. This is a simple
 868  *         case and __packet_lookup_frame_in_block will check if block-0
 869  *         is free and can now be re-used.
 870  */
 871 static void prb_freeze_queue(struct tpacket_kbdq_core *pkc,
 872                                   struct packet_sock *po)
 873 {
 874         pkc->reset_pending_on_curr_blk = 1;
 875         po->stats_u.stats3.tp_freeze_q_cnt++;
 876 }
 877
 878 #define TOTAL_PKT_LEN_INCL_ALIGN(length) (ALIGN((length), V3_ALIGNMENT))
 879
 880 /*
 881  * If the next block is free then we will dispatch it
 882  * and return a good offset.
 883  * Else, we will freeze the queue.
 884  * So, caller must check the return value.
 885  */
 886 static void *prb_dispatch_next_block(struct tpacket_kbdq_core *pkc,
 887                 struct packet_sock *po)
 888 {
 889         struct tpacket_block_desc *pbd;
 890
 891         smp_rmb();
 892
 893         /* 1. Get current block num */
 894         pbd = GET_CURR_PBLOCK_DESC_FROM_CORE(pkc);
 895
 896         /* 2. If this block is currently in_use then freeze the queue */
 897         if (TP_STATUS_USER & BLOCK_STATUS(pbd)) {
 898                 prb_freeze_queue(pkc, po);
 899                 return NULL;
 900         }
 901
 902         /*
 903          * 3.
 904          * open this block and return the offset where the first packet
 905          * needs to get stored.
 906          */
 907         prb_open_block(pkc, pbd);
 908         return (void *)pkc->nxt_offset;
 909 }
 910
 911 static void prb_retire_current_block(struct tpacket_kbdq_core *pkc,
 912                 struct packet_sock *po, unsigned int status)
 913 {
 914         struct tpacket_block_desc *pbd = GET_CURR_PBLOCK_DESC_FROM_CORE(pkc);
 915
 916         /* retire/close the current block */
 917         if (likely(TP_STATUS_KERNEL == BLOCK_STATUS(pbd))) {
 918                 /*
 919                  * Plug the case where copy_bits() is in progress on
 920                  * cpu-0 and tpacket_rcv() got invoked on cpu-1, didn't
 921                  * have space to copy the pkt in the current block and
 922                  * called prb_retire_current_block()
 923                  *
 924                  * We don't need to worry about the TMO case because
 925                  * the timer-handler already handled this case.
 926                  */
 927                 if (!(status & TP_STATUS_BLK_TMO)) {
 928                         while (atomic_read(&pkc->blk_fill_in_prog)) {
 929                                 /* Waiting for skb_copy_bits to finish... */
 930                                 cpu_relax();
 931                         }
 932                 }
 933                 prb_close_block(pkc, pbd, po, status);
 934                 return;
 935         }
 936
 937         WARN(1, "ERROR-pbd[%d]:%p\n", pkc->kactive_blk_num, pbd);
 938         dump_stack();
 939         BUG();
 940 }
 941
 942 static int prb_curr_blk_in_use(struct tpacket_kbdq_core *pkc,
 943                                       struct tpacket_block_desc *pbd)
 944 {
 945         return TP_STATUS_USER & BLOCK_STATUS(pbd);
 946 }
 947
 948 static int prb_queue_frozen(struct tpacket_kbdq_core *pkc)
 949 {
 950         return pkc->reset_pending_on_curr_blk;
 951 }
 952
 953 static void prb_clear_blk_fill_status(struct packet_ring_buffer *rb)
 954 {
 955         struct tpacket_kbdq_core *pkc  = GET_PBDQC_FROM_RB(rb);
 956         atomic_dec(&pkc->blk_fill_in_prog);
 957 }
 958
 959 static void prb_fill_rxhash(struct tpacket_kbdq_core *pkc,
 960                         struct tpacket3_hdr *ppd)
 961 {
 962         ppd->hv1.tp_rxhash = skb_get_rxhash(pkc->skb);
 963 }
 964
 965 static void prb_clear_rxhash(struct tpacket_kbdq_core *pkc,
 966                         struct tpacket3_hdr *ppd)
 967 {
 968         ppd->hv1.tp_rxhash = 0;
 969 }
 970
 971 static void prb_fill_vlan_info(struct tpacket_kbdq_core *pkc,
 972                         struct tpacket3_hdr *ppd)
 973 {
 974         if (vlan_tx_tag_present(pkc->skb)) {
 975                 ppd->hv1.tp_vlan_tci = vlan_tx_tag_get(pkc->skb);
 976                 ppd->tp_status = TP_STATUS_VLAN_VALID;
 977         } else {
 978                 ppd->hv1.tp_vlan_tci = ppd->tp_status = 0;
 979         }
 980 }
 981
 982 static void prb_run_all_ft_ops(struct tpacket_kbdq_core *pkc,
 983                         struct tpacket3_hdr *ppd)
 984 {
 985         prb_fill_vlan_info(pkc, ppd);
 986
 987         if (pkc->feature_req_word & TP_FT_REQ_FILL_RXHASH)
 988                 prb_fill_rxhash(pkc, ppd);
 989         else
 990                 prb_clear_rxhash(pkc, ppd);
 991 }
 992
 993 static void prb_fill_curr_block(char *curr,
 994                                 struct tpacket_kbdq_core *pkc,
 995                                 struct tpacket_block_desc *pbd,
 996                                 unsigned int len)
 997 {
 998         struct tpacket3_hdr *ppd;
 999
1000         ppd  = (struct tpacket3_hdr *)curr;
1001         ppd->tp_next_offset = TOTAL_PKT_LEN_INCL_ALIGN(len);
1002         pkc->prev = curr;
1003         pkc->nxt_offset += TOTAL_PKT_LEN_INCL_ALIGN(len);
1004         BLOCK_LEN(pbd) += TOTAL_PKT_LEN_INCL_ALIGN(len);
1005         BLOCK_NUM_PKTS(pbd) += 1;
1006         atomic_inc(&pkc->blk_fill_in_prog);
1007         prb_run_all_ft_ops(pkc, ppd);
1008 }
1009
1010 /* Assumes caller has the sk->rx_queue.lock */
1011 static void *__packet_lookup_frame_in_block(struct packet_sock *po,
1012                                             struct sk_buff *skb,
1013                                                 int status,
1014                                             unsigned int len
1015                                             )
1016 {
1017         struct tpacket_kbdq_core *pkc;
1018         struct tpacket_block_desc *pbd;
1019         char *curr, *end;
1020
1021         pkc = GET_PBDQC_FROM_RB(((struct packet_ring_buffer *)&po->rx_ring));
1022         pbd = GET_CURR_PBLOCK_DESC_FROM_CORE(pkc);
1023
1024         /* Queue is frozen when user space is lagging behind */
1025         if (prb_queue_frozen(pkc)) {
1026                 /*
1027                  * Check if that last block which caused the queue to freeze,
1028                  * is still in_use by user-space.
1029                  */
1030                 if (prb_curr_blk_in_use(pkc, pbd)) {
1031                         /* Can't record this packet */
1032                         return NULL;
1033                 } else {
1034                         /*
1035                          * Ok, the block was released by user-space.
1036                          * Now let's open that block.
1037                          * opening a block also thaws the queue.
1038                          * Thawing is a side effect.
1039                          */
1040                         prb_open_block(pkc, pbd);
1041                 }
1042         }
1043
1044         smp_mb();
1045         curr = pkc->nxt_offset;
1046         pkc->skb = skb;
1047         end = (char *) ((char *)pbd + pkc->kblk_size);
1048
1049         /* first try the current block */
1050         if (curr+TOTAL_PKT_LEN_INCL_ALIGN(len) < end) {
1051                 prb_fill_curr_block(curr, pkc, pbd, len);
1052                 return (void *)curr;
1053         }
1054
1055         /* Ok, close the current block */
1056         prb_retire_current_block(pkc, po, 0);
1057
1058         /* Now, try to dispatch the next block */
1059         curr = (char *)prb_dispatch_next_block(pkc, po);
1060         if (curr) {
1061                 pbd = GET_CURR_PBLOCK_DESC_FROM_CORE(pkc);
1062                 prb_fill_curr_block(curr, pkc, pbd, len);
1063                 return (void *)curr;
1064         }
1065
1066         /*
1067          * No free blocks are available.user_space hasn't caught up yet.
1068          * Queue was just frozen and now this packet will get dropped.
1069          */
1070         return NULL;
1071 }
1072
1073 static void *packet_current_rx_frame(struct packet_sock *po,
1074                                             struct sk_buff *skb,
1075                                             int status, unsigned int len)
1076 {
1077         char *curr = NULL;
1078         switch (po->tp_version) {
1079         case TPACKET_V1:
1080         case TPACKET_V2:
1081                 curr = packet_lookup_frame(po, &po->rx_ring,
1082                                         po->rx_ring.head, status);
1083                 return curr;
1084         case TPACKET_V3:
1085                 return __packet_lookup_frame_in_block(po, skb, status, len);
1086         default:
1087                 WARN(1, "TPACKET version not supported\n");
1088                 BUG();
1089                 return 0;
1090         }
1091 }
1092
1093 static void *prb_lookup_block(struct packet_sock *po,
1094                                      struct packet_ring_buffer *rb,
1095                                      unsigned int previous,
1096                                      int status)
1097 {
1098         struct tpacket_kbdq_core *pkc  = GET_PBDQC_FROM_RB(rb);
1099         struct tpacket_block_desc *pbd = GET_PBLOCK_DESC(pkc, previous);
1100
1101         if (status != BLOCK_STATUS(pbd))
1102                 return NULL;
1103         return pbd;
1104 }
1105
1106 static int prb_previous_blk_num(struct packet_ring_buffer *rb)
1107 {
1108         unsigned int prev;
1109         if (rb->prb_bdqc.kactive_blk_num)
1110                 prev = rb->prb_bdqc.kactive_blk_num-1;
1111         else
1112                 prev = rb->prb_bdqc.knum_blocks-1;
1113         return prev;
1114 }
1115
1116 /* Assumes caller has held the rx_queue.lock */
1117 static void *__prb_previous_block(struct packet_sock *po,
1118                                          struct packet_ring_buffer *rb,
1119                                          int status)
1120 {
1121         unsigned int previous = prb_previous_blk_num(rb);
1122         return prb_lookup_block(po, rb, previous, status);
1123 }
1124
1125 static void *packet_previous_rx_frame(struct packet_sock *po,
1126                                              struct packet_ring_buffer *rb,
1127                                              int status)
1128 {
1129         if (po->tp_version <= TPACKET_V2)
1130                 return packet_previous_frame(po, rb, status);
1131
1132         return __prb_previous_block(po, rb, status);
1133 }
1134
1135 static void packet_increment_rx_head(struct packet_sock *po,
1136                                             struct packet_ring_buffer *rb)
1137 {
1138         switch (po->tp_version) {
1139         case TPACKET_V1:
1140         case TPACKET_V2:
1141                 return packet_increment_head(rb);
1142         case TPACKET_V3:
1143         default:
1144                 WARN(1, "TPACKET version not supported.\n");
1145                 BUG();
1146                 return;
1147         }
1148 }
1149
1150 static void *packet_previous_frame(struct packet_sock *po,
1151                 struct packet_ring_buffer *rb,
1152                 int status)
1153 {
1154         unsigned int previous = rb->head ? rb->head - 1 : rb->frame_max;
1155         return packet_lookup_frame(po, rb, previous, status);
1156 }
1157
1158 static void packet_increment_head(struct packet_ring_buffer *buff)
1159 {
1160         buff->head = buff->head != buff->frame_max ? buff->head+1 : 0;
1161 }
1162
1163 static void packet_sock_destruct(struct sock *sk)
1164 {
1165         skb_queue_purge(&sk->sk_error_queue);
1166
1167         WARN_ON(atomic_read(&sk->sk_rmem_alloc));
1168         WARN_ON(atomic_read(&sk->sk_wmem_alloc));
1169
1170         if (!sock_flag(sk, SOCK_DEAD)) {
1171                 pr_err("Attempt to release alive packet socket: %p\n", sk);
1172                 return;
1173         }
1174
1175         sk_refcnt_debug_dec(sk);
1176 }
1177
1178 static int fanout_rr_next(struct packet_fanout *f, unsigned int num)
1179 {
1180         int x = atomic_read(&f->rr_cur) + 1;
1181
1182         if (x >= num)
1183                 x = 0;
1184
1185         return x;
1186 }
1187
1188 static struct sock *fanout_demux_hash(struct packet_fanout *f, struct sk_buff *skb, unsigned int num)
1189 {
1190         u32 idx, hash = skb->rxhash;
1191
1192         idx = ((u64)hash * num) >> 32;
1193
1194         return f->arr[idx];
1195 }
1196
1197 static struct sock *fanout_demux_lb(struct packet_fanout *f, struct sk_buff *skb, unsigned int num)
1198 {
1199         int cur, old;
1200
1201         cur = atomic_read(&f->rr_cur);
1202         while ((old = atomic_cmpxchg(&f->rr_cur, cur,
1203                                      fanout_rr_next(f, num))) != cur)
1204                 cur = old;
1205         return f->arr[cur];
1206 }
1207
1208 static struct sock *fanout_demux_cpu(struct packet_fanout *f, struct sk_buff *skb, unsigned int num)
1209 {
1210         unsigned int cpu = smp_processor_id();
1211
1212         return f->arr[cpu % num];
1213 }
1214
1215 static int packet_rcv_fanout(struct sk_buff *skb, struct net_device *dev,
1216                              struct packet_type *pt, struct net_device *orig_dev)
1217 {
1218         struct packet_fanout *f = pt->af_packet_priv;
1219         unsigned int num = f->num_members;
1220         struct packet_sock *po;
1221         struct sock *sk;
1222
1223         if (!net_eq(dev_net(dev), read_pnet(&f->net)) ||
1224             !num) {
1225                 kfree_skb(skb);
1226                 return 0;
1227         }
1228
1229         switch (f->type) {
1230         case PACKET_FANOUT_HASH:
1231         default:
1232                 if (f->defrag) {
1233                         skb = ip_check_defrag(skb, IP_DEFRAG_AF_PACKET);
1234                         if (!skb)
1235                                 return 0;
1236                 }
1237                 skb_get_rxhash(skb);
1238                 sk = fanout_demux_hash(f, skb, num);
1239                 break;
1240         case PACKET_FANOUT_LB:
1241                 sk = fanout_demux_lb(f, skb, num);
1242                 break;
1243         case PACKET_FANOUT_CPU:
1244                 sk = fanout_demux_cpu(f, skb, num);
1245                 break;
1246         }
1247
1248         po = pkt_sk(sk);
1249
1250         return po->prot_hook.func(skb, dev, &po->prot_hook, orig_dev);
1251 }
1252
1253 static DEFINE_MUTEX(fanout_mutex);
1254 static LIST_HEAD(fanout_list);
1255
1256 static void __fanout_link(struct sock *sk, struct packet_sock *po)
1257 {
1258         struct packet_fanout *f = po->fanout;
1259
1260         spin_lock(&f->lock);
1261         f->arr[f->num_members] = sk;
1262         smp_wmb();
1263         f->num_members++;
1264         spin_unlock(&f->lock);
1265 }
1266
1267 static void __fanout_unlink(struct sock *sk, struct packet_sock *po)
1268 {
1269         struct packet_fanout *f = po->fanout;
1270         int i;
1271
1272         spin_lock(&f->lock);
1273         for (i = 0; i < f->num_members; i++) {
1274                 if (f->arr[i] == sk)
1275                         break;
1276         }
1277         BUG_ON(i >= f->num_members);
1278         f->arr[i] = f->arr[f->num_members - 1];
1279         f->num_members--;
1280         spin_unlock(&f->lock);
1281 }
1282
1283 static int fanout_add(struct sock *sk, u16 id, u16 type_flags)
1284 {
1285         struct packet_sock *po = pkt_sk(sk);
1286         struct packet_fanout *f, *match;
1287         u8 type = type_flags & 0xff;
1288         u8 defrag = (type_flags & PACKET_FANOUT_FLAG_DEFRAG) ? 1 : 0;
1289         int err;
1290
1291         switch (type) {
1292         case PACKET_FANOUT_HASH:
1293         case PACKET_FANOUT_LB:
1294         case PACKET_FANOUT_CPU:
1295                 break;
1296         default:
1297                 return -EINVAL;
1298         }
1299
1300         if (!po->running)
1301                 return -EINVAL;
1302
1303         if (po->fanout)
1304                 return -EALREADY;
1305
1306         mutex_lock(&fanout_mutex);
1307         match = NULL;
1308         list_for_each_entry(f, &fanout_list, list) {
1309                 if (f->id == id &&
1310                     read_pnet(&f->net) == sock_net(sk)) {
1311                         match = f;
1312                         break;
1313                 }
1314         }
1315         err = -EINVAL;
1316         if (match && match->defrag != defrag)
1317                 goto out;
1318         if (!match) {
1319                 err = -ENOMEM;
1320                 match = kzalloc(sizeof(*match), GFP_KERNEL);
1321                 if (!match)
1322                         goto out;
1323                 write_pnet(&match->net, sock_net(sk));
1324                 match->id = id;
1325                 match->type = type;
1326                 match->defrag = defrag;
1327                 atomic_set(&match->rr_cur, 0);
1328                 INIT_LIST_HEAD(&match->list);
1329                 spin_lock_init(&match->lock);
1330                 atomic_set(&match->sk_ref, 0);
1331                 match->prot_hook.type = po->prot_hook.type;
1332                 match->prot_hook.dev = po->prot_hook.dev;
1333                 match->prot_hook.func = packet_rcv_fanout;
1334                 match->prot_hook.af_packet_priv = match;
1335                 dev_add_pack(&match->prot_hook);
1336                 list_add(&match->list, &fanout_list);
1337         }
1338         err = -EINVAL;
1339         if (match->type == type &&
1340             match->prot_hook.type == po->prot_hook.type &&
1341             match->prot_hook.dev == po->prot_hook.dev) {
1342                 err = -ENOSPC;
1343                 if (atomic_read(&match->sk_ref) < PACKET_FANOUT_MAX) {
1344                         __dev_remove_pack(&po->prot_hook);
1345                         po->fanout = match;
1346                         atomic_inc(&match->sk_ref);
1347                         __fanout_link(sk, po);
1348                         err = 0;
1349                 }
1350         }
1351 out:
1352         mutex_unlock(&fanout_mutex);
1353         return err;
1354 }
1355
1356 static void fanout_release(struct sock *sk)
1357 {
1358         struct packet_sock *po = pkt_sk(sk);
1359         struct packet_fanout *f;
1360
1361         f = po->fanout;
1362         if (!f)
1363                 return;
1364
1365         po->fanout = NULL;
1366
1367         mutex_lock(&fanout_mutex);
1368         if (atomic_dec_and_test(&f->sk_ref)) {
1369                 list_del(&f->list);
1370                 dev_remove_pack(&f->prot_hook);
1371                 kfree(f);
1372         }
1373         mutex_unlock(&fanout_mutex);
1374 }
1375
1376 static const struct proto_ops packet_ops;
1377
1378 static const struct proto_ops packet_ops_spkt;
1379
1380 static int packet_rcv_spkt(struct sk_buff *skb, struct net_device *dev,
1381                            struct packet_type *pt, struct net_device *orig_dev)
1382 {
1383         struct sock *sk;
1384         struct sockaddr_pkt *spkt;
1385
1386         /*
1387          *      When we registered the protocol we saved the socket in the data
1388          *      field for just this event.
1389          */
1390
1391         sk = pt->af_packet_priv;
1392
1393         /*
1394          *      Yank back the headers [hope the device set this
1395          *      right or kerboom...]
1396          *
1397          *      Incoming packets have ll header pulled,
1398          *      push it back.
1399          *
1400          *      For outgoing ones skb->data == skb_mac_header(skb)
1401          *      so that this procedure is noop.
1402          */
1403
1404         if (skb->pkt_type == PACKET_LOOPBACK)
1405                 goto out;
1406
1407         if (!net_eq(dev_net(dev), sock_net(sk)))
1408                 goto out;
1409
1410         skb = skb_share_check(skb, GFP_ATOMIC);
1411         if (skb == NULL)
1412                 goto oom;
1413
1414         /* drop any routing info */
1415         skb_dst_drop(skb);
1416
1417         /* drop conntrack reference */
1418         nf_reset(skb);
1419
1420         spkt = &PACKET_SKB_CB(skb)->sa.pkt;
1421
1422         skb_push(skb, skb->data - skb_mac_header(skb));
1423
1424         /*
1425          *      The SOCK_PACKET socket receives _all_ frames.
1426          */
1427
1428         spkt->spkt_family = dev->type;
1429         strlcpy(spkt->spkt_device, dev->name, sizeof(spkt->spkt_device));
1430         spkt->spkt_protocol = skb->protocol;
1431
1432         /*
1433          *      Charge the memory to the socket. This is done specifically
1434          *      to prevent sockets using all the memory up.
1435          */
1436
1437         if (sock_queue_rcv_skb(sk, skb) == 0)
1438                 return 0;
1439
1440 out:
1441         kfree_skb(skb);
1442 oom:
1443         return 0;
1444 }
1445
1446
1447 /*
1448  *      Output a raw packet to a device layer. This bypasses all the other
1449  *      protocol layers and you must therefore supply it with a complete frame
1450  */
1451
1452 static int packet_sendmsg_spkt(struct kiocb *iocb, struct socket *sock,
1453                                struct msghdr *msg, size_t len)
1454 {
1455         struct sock *sk = sock->sk;
1456         struct sockaddr_pkt *saddr = (struct sockaddr_pkt *)msg->msg_name;
1457         struct sk_buff *skb = NULL;
1458         struct net_device *dev;
1459         __be16 proto = 0;
1460         int err;
1461         int extra_len = 0;
1462
1463         /*
1464          *      Get and verify the address.
1465          */
1466
1467         if (saddr) {
1468                 if (msg->msg_namelen < sizeof(struct sockaddr))
1469                         return -EINVAL;
1470                 if (msg->msg_namelen == sizeof(struct sockaddr_pkt))
1471                         proto = saddr->spkt_protocol;
1472         } else
1473                 return -ENOTCONN;       /* SOCK_PACKET must be sent giving an address */
1474
1475         /*
1476          *      Find the device first to size check it
1477          */
1478
1479         saddr->spkt_device[13] = 0;
1480 retry:
1481         rcu_read_lock();
1482         dev = dev_get_by_name_rcu(sock_net(sk), saddr->spkt_device);
1483         err = -ENODEV;
1484         if (dev == NULL)
1485                 goto out_unlock;
1486
1487         err = -ENETDOWN;
1488         if (!(dev->flags & IFF_UP))
1489                 goto out_unlock;
1490
1491         /*
1492          * You may not queue a frame bigger than the mtu. This is the lowest level
1493          * raw protocol and you must do your own fragmentation at this level.
1494          */
1495
1496         if (unlikely(sock_flag(sk, SOCK_NOFCS))) {
1497                 if (!netif_supports_nofcs(dev)) {
1498                         err = -EPROTONOSUPPORT;
1499                         goto out_unlock;
1500                 }
1501                 extra_len = 4; /* We're doing our own CRC */
1502         }
1503
1504         err = -EMSGSIZE;
1505         if (len > dev->mtu + dev->hard_header_len + VLAN_HLEN + extra_len)
1506                 goto out_unlock;
1507
1508         if (!skb) {
1509                 size_t reserved = LL_RESERVED_SPACE(dev);
1510                 int tlen = dev->needed_tailroom;
1511                 unsigned int hhlen = dev->header_ops ? dev->hard_header_len : 0;
1512
1513                 rcu_read_unlock();
1514                 skb = sock_wmalloc(sk, len + reserved + tlen, 0, GFP_KERNEL);
1515                 if (skb == NULL)
1516                         return -ENOBUFS;
1517                 /* FIXME: Save some space for broken drivers that write a hard
1518                  * header at transmission time by themselves. PPP is the notable
1519                  * one here. This should really be fixed at the driver level.
1520                  */
1521                 skb_reserve(skb, reserved);
1522                 skb_reset_network_header(skb);
1523
1524                 /* Try to align data part correctly */
1525                 if (hhlen) {
1526                         skb->data -= hhlen;
1527                         skb->tail -= hhlen;
1528                         if (len < hhlen)
1529                                 skb_reset_network_header(skb);
1530                 }
1531                 err = memcpy_fromiovec(skb_put(skb, len), msg->msg_iov, len);
1532                 if (err)
1533                         goto out_free;
1534                 goto retry;
1535         }
1536
1537         if (len > (dev->mtu + dev->hard_header_len + extra_len)) {
1538                 /* Earlier code assumed this would be a VLAN pkt,
1539                  * double-check this now that we have the actual
1540                  * packet in hand.
1541                  */
1542                 struct ethhdr *ehdr;
1543                 skb_reset_mac_header(skb);
1544                 ehdr = eth_hdr(skb);
1545                 if (ehdr->h_proto != htons(ETH_P_8021Q)) {
1546                         err = -EMSGSIZE;
1547                         goto out_unlock;
1548                 }
1549         }
1550
1551         skb->protocol = proto;
1552         skb->dev = dev;
1553         skb->priority = sk->sk_priority;
1554         skb->mark = sk->sk_mark;
1555         err = sock_tx_timestamp(sk, &skb_shinfo(skb)->tx_flags);
1556         if (err < 0)
1557                 goto out_unlock;
1558
1559         if (unlikely(extra_len == 4))
1560                 skb->no_fcs = 1;
1561
1562         dev_queue_xmit(skb);
1563         rcu_read_unlock();
1564         return len;
1565
1566 out_unlock:
1567         rcu_read_unlock();
1568 out_free:
1569         kfree_skb(skb);
1570         return err;
1571 }
1572
1573 static unsigned int run_filter(const struct sk_buff *skb,
1574                                       const struct sock *sk,
1575                                       unsigned int res)
1576 {
1577         struct sk_filter *filter;
1578
1579         rcu_read_lock();
1580         filter = rcu_dereference(sk->sk_filter);
1581         if (filter != NULL)
1582                 res = SK_RUN_FILTER(filter, skb);
1583         rcu_read_unlock();
1584
1585         return res;
1586 }
1587
1588 /*
1589  * This function makes lazy skb cloning in hope that most of packets
1590  * are discarded by BPF.
1591  *
1592  * Note tricky part: we DO mangle shared skb! skb->data, skb->len
1593  * and skb->cb are mangled. It works because (and until) packets
1594  * falling here are owned by current CPU. Output packets are cloned
1595  * by dev_queue_xmit_nit(), input packets are processed by net_bh
1596  * sequencially, so that if we return skb to original state on exit,
1597  * we will not harm anyone.
1598  */
1599
1600 static int packet_rcv(struct sk_buff *skb, struct net_device *dev,
1601                       struct packet_type *pt, struct net_device *orig_dev)
1602 {
1603         struct sock *sk;
1604         struct sockaddr_ll *sll;
1605         struct packet_sock *po;
1606         u8 *skb_head = skb->data;
1607         int skb_len = skb->len;
1608         unsigned int snaplen, res;
1609
1610         if (skb->pkt_type == PACKET_LOOPBACK)
1611                 goto drop;
1612
1613         sk = pt->af_packet_priv;
1614         po = pkt_sk(sk);
1615
1616         if (!net_eq(dev_net(dev), sock_net(sk)))
1617                 goto drop;
1618
1619         skb->dev = dev;
1620
1621         if (dev->header_ops) {
1622                 /* The device has an explicit notion of ll header,
1623                  * exported to higher levels.
1624                  *
1625                  * Otherwise, the device hides details of its frame
1626                  * structure, so that corresponding packet head is
1627                  * never delivered to user.
1628                  */
1629                 if (sk->sk_type != SOCK_DGRAM)
1630                         skb_push(skb, skb->data - skb_mac_header(skb));
1631                 else if (skb->pkt_type == PACKET_OUTGOING) {
1632                         /* Special case: outgoing packets have ll header at head */
1633                         skb_pull(skb, skb_network_offset(skb));
1634                 }
1635         }
1636
1637         snaplen = skb->len;
1638
1639         res = run_filter(skb, sk, snaplen);
1640         if (!res)
1641                 goto drop_n_restore;
1642         if (snaplen > res)
1643                 snaplen = res;
1644
1645         if (atomic_read(&sk->sk_rmem_alloc) >= sk->sk_rcvbuf)
1646                 goto drop_n_acct;
1647
1648         if (skb_shared(skb)) {
1649                 struct sk_buff *nskb = skb_clone(skb, GFP_ATOMIC);
1650                 if (nskb == NULL)
1651                         goto drop_n_acct;
1652
1653                 if (skb_head != skb->data) {
1654                         skb->data = skb_head;
1655                         skb->len = skb_len;
1656                 }
1657                 consume_skb(skb);
1658                 skb = nskb;
1659         }
1660
1661         BUILD_BUG_ON(sizeof(*PACKET_SKB_CB(skb)) + MAX_ADDR_LEN - 8 >
1662                      sizeof(skb->cb));
1663
1664         sll = &PACKET_SKB_CB(skb)->sa.ll;
1665         sll->sll_family = AF_PACKET;
1666         sll->sll_hatype = dev->type;
1667         sll->sll_protocol = skb->protocol;
1668         sll->sll_pkttype = skb->pkt_type;
1669         if (unlikely(po->origdev))
1670                 sll->sll_ifindex = orig_dev->ifindex;
1671         else
1672                 sll->sll_ifindex = dev->ifindex;
1673
1674         sll->sll_halen = dev_parse_header(skb, sll->sll_addr);
1675
1676         PACKET_SKB_CB(skb)->origlen = skb->len;
1677
1678         if (pskb_trim(skb, snaplen))
1679                 goto drop_n_acct;
1680
1681         skb_set_owner_r(skb, sk);
1682         skb->dev = NULL;
1683         skb_dst_drop(skb);
1684
1685         /* drop conntrack reference */
1686         nf_reset(skb);
1687
1688         spin_lock(&sk->sk_receive_queue.lock);
1689         po->stats.tp_packets++;
1690         skb->dropcount = atomic_read(&sk->sk_drops);
1691         __skb_queue_tail(&sk->sk_receive_queue, skb);
1692         spin_unlock(&sk->sk_receive_queue.lock);
1693         sk->sk_data_ready(sk, skb->len);
1694         return 0;
1695
1696 drop_n_acct:
1697         spin_lock(&sk->sk_receive_queue.lock);
1698         po->stats.tp_drops++;
1699         atomic_inc(&sk->sk_drops);
1700         spin_unlock(&sk->sk_receive_queue.lock);
1701
1702 drop_n_restore:
1703         if (skb_head != skb->data && skb_shared(skb)) {
1704                 skb->data = skb_head;
1705                 skb->len = skb_len;
1706         }
1707 drop:
1708         consume_skb(skb);
1709         return 0;
1710 }
1711
1712 static int tpacket_rcv(struct sk_buff *skb, struct net_device *dev,
1713                        struct packet_type *pt, struct net_device *orig_dev)
1714 {
1715         struct sock *sk;
1716         struct packet_sock *po;
1717         struct sockaddr_ll *sll;
1718         union {
1719                 struct tpacket_hdr *h1;
1720                 struct tpacket2_hdr *h2;
1721                 struct tpacket3_hdr *h3;
1722                 void *raw;
1723         } h;
1724         u8 *skb_head = skb->data;
1725         int skb_len = skb->len;
1726         unsigned int snaplen, res;
1727         unsigned long status = TP_STATUS_USER;
1728         unsigned short macoff, netoff, hdrlen;
1729         struct sk_buff *copy_skb = NULL;
1730         struct timeval tv;
1731         struct timespec ts;
1732         struct skb_shared_hwtstamps *shhwtstamps = skb_hwtstamps(skb);
1733
1734         if (skb->pkt_type == PACKET_LOOPBACK)
1735                 goto drop;
1736
1737         sk = pt->af_packet_priv;
1738         po = pkt_sk(sk);
1739
1740         if (!net_eq(dev_net(dev), sock_net(sk)))
1741                 goto drop;
1742
1743         if (dev->header_ops) {
1744                 if (sk->sk_type != SOCK_DGRAM)
1745                         skb_push(skb, skb->data - skb_mac_header(skb));
1746                 else if (skb->pkt_type == PACKET_OUTGOING) {
1747                         /* Special case: outgoing packets have ll header at head */
1748                         skb_pull(skb, skb_network_offset(skb));
1749                 }
1750         }
1751
1752         if (skb->ip_summed == CHECKSUM_PARTIAL)
1753                 status |= TP_STATUS_CSUMNOTREADY;
1754
1755         snaplen = skb->len;
1756
1757         res = run_filter(skb, sk, snaplen);
1758         if (!res)
1759                 goto drop_n_restore;
1760         if (snaplen > res)
1761                 snaplen = res;
1762
1763         if (sk->sk_type == SOCK_DGRAM) {
1764                 macoff = netoff = TPACKET_ALIGN(po->tp_hdrlen) + 16 +
1765                                   po->tp_reserve;
1766         } else {
1767                 unsigned int maclen = skb_network_offset(skb);
1768                 netoff = TPACKET_ALIGN(po->tp_hdrlen +
1769                                        (maclen < 16 ? 16 : maclen)) +
1770                         po->tp_reserve;
1771                 macoff = netoff - maclen;
1772         }
1773         if (po->tp_version <= TPACKET_V2) {
1774                 if (macoff + snaplen > po->rx_ring.frame_size) {
1775                         if (po->copy_thresh &&
1776                             atomic_read(&sk->sk_rmem_alloc) < sk->sk_rcvbuf) {
1777                                 if (skb_shared(skb)) {
1778                                         copy_skb = skb_clone(skb, GFP_ATOMIC);
1779                                 } else {
1780                                         copy_skb = skb_get(skb);
1781                                         skb_head = skb->data;
1782                                 }
1783                                 if (copy_skb)
1784                                         skb_set_owner_r(copy_skb, sk);
1785                         }
1786                         snaplen = po->rx_ring.frame_size - macoff;
1787                         if ((int)snaplen < 0)
1788                                 snaplen = 0;
1789                 }
1790         }
1791         spin_lock(&sk->sk_receive_queue.lock);
1792         h.raw = packet_current_rx_frame(po, skb,
1793                                         TP_STATUS_KERNEL, (macoff+snaplen));
1794         if (!h.raw)
1795                 goto ring_is_full;
1796         if (po->tp_version <= TPACKET_V2) {
1797                 packet_increment_rx_head(po, &po->rx_ring);
1798         /*
1799          * LOSING will be reported till you read the stats,
1800          * because it's COR - Clear On Read.
1801          * Anyways, moving it for V1/V2 only as V3 doesn't need this
1802          * at packet level.
1803          */
1804                 if (po->stats.tp_drops)
1805                         status |= TP_STATUS_LOSING;
1806         }
1807         po->stats.tp_packets++;
1808         if (copy_skb) {
1809                 status |= TP_STATUS_COPY;
1810                 __skb_queue_tail(&sk->sk_receive_queue, copy_skb);
1811         }
1812         spin_unlock(&sk->sk_receive_queue.lock);
1813
1814         skb_copy_bits(skb, 0, h.raw + macoff, snaplen);
1815
1816         switch (po->tp_version) {
1817         case TPACKET_V1:
1818                 h.h1->tp_len = skb->len;
1819                 h.h1->tp_snaplen = snaplen;
1820                 h.h1->tp_mac = macoff;
1821                 h.h1->tp_net = netoff;
1822                 if ((po->tp_tstamp & SOF_TIMESTAMPING_SYS_HARDWARE)
1823                                 && shhwtstamps->syststamp.tv64)
1824                         tv = ktime_to_timeval(shhwtstamps->syststamp);
1825                 else if ((po->tp_tstamp & SOF_TIMESTAMPING_RAW_HARDWARE)
1826                                 && shhwtstamps->hwtstamp.tv64)
1827                         tv = ktime_to_timeval(shhwtstamps->hwtstamp);
1828                 else if (skb->tstamp.tv64)
1829                         tv = ktime_to_timeval(skb->tstamp);
1830                 else
1831                         do_gettimeofday(&tv);
1832                 h.h1->tp_sec = tv.tv_sec;
1833                 h.h1->tp_usec = tv.tv_usec;
1834                 hdrlen = sizeof(*h.h1);
1835                 break;
1836         case TPACKET_V2:
1837                 h.h2->tp_len = skb->len;
1838                 h.h2->tp_snaplen = snaplen;
1839                 h.h2->tp_mac = macoff;
1840                 h.h2->tp_net = netoff;
1841                 if ((po->tp_tstamp & SOF_TIMESTAMPING_SYS_HARDWARE)
1842                                 && shhwtstamps->syststamp.tv64)
1843                         ts = ktime_to_timespec(shhwtstamps->syststamp);
1844                 else if ((po->tp_tstamp & SOF_TIMESTAMPING_RAW_HARDWARE)
1845                                 && shhwtstamps->hwtstamp.tv64)
1846                         ts = ktime_to_timespec(shhwtstamps->hwtstamp);
1847                 else if (skb->tstamp.tv64)
1848                         ts = ktime_to_timespec(skb->tstamp);
1849                 else
1850                         getnstimeofday(&ts);
1851                 h.h2->tp_sec = ts.tv_sec;
1852                 h.h2->tp_nsec = ts.tv_nsec;
1853                 if (vlan_tx_tag_present(skb)) {
1854                         h.h2->tp_vlan_tci = vlan_tx_tag_get(skb);
1855                         status |= TP_STATUS_VLAN_VALID;
1856                 } else {
1857                         h.h2->tp_vlan_tci = 0;
1858                 }
1859                 h.h2->tp_padding = 0;
1860                 hdrlen = sizeof(*h.h2);
1861                 break;
1862         case TPACKET_V3:
1863                 /* tp_nxt_offset,vlan are already populated above.
1864                  * So DONT clear those fields here
1865                  */
1866                 h.h3->tp_status |= status;
1867                 h.h3->tp_len = skb->len;
1868                 h.h3->tp_snaplen = snaplen;
1869                 h.h3->tp_mac = macoff;
1870                 h.h3->tp_net = netoff;
1871                 if ((po->tp_tstamp & SOF_TIMESTAMPING_SYS_HARDWARE)
1872                                 && shhwtstamps->syststamp.tv64)
1873                         ts = ktime_to_timespec(shhwtstamps->syststamp);
1874                 else if ((po->tp_tstamp & SOF_TIMESTAMPING_RAW_HARDWARE)
1875                                 && shhwtstamps->hwtstamp.tv64)
1876                         ts = ktime_to_timespec(shhwtstamps->hwtstamp);
1877                 else if (skb->tstamp.tv64)
1878                         ts = ktime_to_timespec(skb->tstamp);
1879                 else
1880                         getnstimeofday(&ts);
1881                 h.h3->tp_sec  = ts.tv_sec;
1882                 h.h3->tp_nsec = ts.tv_nsec;
1883                 hdrlen = sizeof(*h.h3);
1884                 break;
1885         default:
1886                 BUG();
1887         }
1888
1889         sll = h.raw + TPACKET_ALIGN(hdrlen);
1890         sll->sll_halen = dev_parse_header(skb, sll->sll_addr);
1891         sll->sll_family = AF_PACKET;
1892         sll->sll_hatype = dev->type;
1893         sll->sll_protocol = skb->protocol;
1894         sll->sll_pkttype = skb->pkt_type;
1895         if (unlikely(po->origdev))
1896                 sll->sll_ifindex = orig_dev->ifindex;
1897         else
1898                 sll->sll_ifindex = dev->ifindex;
1899
1900         smp_mb();
1901 #if ARCH_IMPLEMENTS_FLUSH_DCACHE_PAGE == 1
1902         {
1903                 u8 *start, *end;
1904
1905                 if (po->tp_version <= TPACKET_V2) {
1906                         end = (u8 *)PAGE_ALIGN((unsigned long)h.raw
1907                                 + macoff + snaplen);
1908                         for (start = h.raw; start < end; start += PAGE_SIZE)
1909                                 flush_dcache_page(pgv_to_page(start));
1910                 }
1911                 smp_wmb();
1912         }
1913 #endif
1914         if (po->tp_version <= TPACKET_V2)
1915                 __packet_set_status(po, h.raw, status);
1916         else
1917                 prb_clear_blk_fill_status(&po->rx_ring);
1918
1919         sk->sk_data_ready(sk, 0);
1920
1921 drop_n_restore:
1922         if (skb_head != skb->data && skb_shared(skb)) {
1923                 skb->data = skb_head;
1924                 skb->len = skb_len;
1925         }
1926 drop:
1927         kfree_skb(skb);
1928         return 0;
1929
1930 ring_is_full:
1931         po->stats.tp_drops++;
1932         spin_unlock(&sk->sk_receive_queue.lock);
1933
1934         sk->sk_data_ready(sk, 0);
1935         kfree_skb(copy_skb);
1936         goto drop_n_restore;
1937 }
1938
1939 static void tpacket_destruct_skb(struct sk_buff *skb)
1940 {
1941         struct packet_sock *po = pkt_sk(skb->sk);
1942         void *ph;
1943
1944         if (likely(po->tx_ring.pg_vec)) {
1945                 ph = skb_shinfo(skb)->destructor_arg;
1946                 BUG_ON(__packet_get_status(po, ph) != TP_STATUS_SENDING);
1947                 BUG_ON(atomic_read(&po->tx_ring.pending) == 0);
1948                 atomic_dec(&po->tx_ring.pending);
1949                 __packet_set_status(po, ph, TP_STATUS_AVAILABLE);
1950         }
1951
1952         sock_wfree(skb);
1953 }
1954
1955 static int tpacket_fill_skb(struct packet_sock *po, struct sk_buff *skb,
1956                 void *frame, struct net_device *dev, int size_max,
1957                 __be16 proto, unsigned char *addr, int hlen)
1958 {
1959         union {
1960                 struct tpacket_hdr *h1;
1961                 struct tpacket2_hdr *h2;
1962                 void *raw;
1963         } ph;
1964         int to_write, offset, len, tp_len, nr_frags, len_max;
1965         struct socket *sock = po->sk.sk_socket;
1966         struct page *page;
1967         void *data;
1968         int err;
1969
1970         ph.raw = frame;
1971
1972         skb->protocol = proto;
1973         skb->dev = dev;
1974         skb->priority = po->sk.sk_priority;
1975         skb->mark = po->sk.sk_mark;
1976         skb_shinfo(skb)->destructor_arg = ph.raw;
1977
1978         switch (po->tp_version) {
1979         case TPACKET_V2:
1980                 tp_len = ph.h2->tp_len;
1981                 break;
1982         default:
1983                 tp_len = ph.h1->tp_len;
1984                 break;
1985         }
1986         if (unlikely(tp_len > size_max)) {
1987                 pr_err("packet size is too long (%d > %d)\n", tp_len, size_max);
1988                 return -EMSGSIZE;
1989         }
1990
1991         skb_reserve(skb, hlen);
1992         skb_reset_network_header(skb);
1993
1994         data = ph.raw + po->tp_hdrlen - sizeof(struct sockaddr_ll);
1995         to_write = tp_len;
1996
1997         if (sock->type == SOCK_DGRAM) {
1998                 err = dev_hard_header(skb, dev, ntohs(proto), addr,
1999                                 NULL, tp_len);
2000                 if (unlikely(err < 0))
2001                         return -EINVAL;
2002         } else if (dev->hard_header_len) {
2003                 /* net device doesn't like empty head */
2004                 if (unlikely(tp_len <= dev->hard_header_len)) {
2005                         pr_err("packet size is too short (%d < %d)\n",
2006                                tp_len, dev->hard_header_len);
2007                         return -EINVAL;
2008                 }
2009
2010                 skb_push(skb, dev->hard_header_len);
2011                 err = skb_store_bits(skb, 0, data,
2012                                 dev->hard_header_len);
2013                 if (unlikely(err))
2014                         return err;
2015
2016                 data += dev->hard_header_len;
2017                 to_write -= dev->hard_header_len;
2018         }
2019
2020         err = -EFAULT;
2021         offset = offset_in_page(data);
2022         len_max = PAGE_SIZE - offset;
2023         len = ((to_write > len_max) ? len_max : to_write);
2024
2025         skb->data_len = to_write;
2026         skb->len += to_write;
2027         skb->truesize += to_write;
2028         atomic_add(to_write, &po->sk.sk_wmem_alloc);
2029
2030         while (likely(to_write)) {
2031                 nr_frags = skb_shinfo(skb)->nr_frags;
2032
2033                 if (unlikely(nr_frags >= MAX_SKB_FRAGS)) {
2034                         pr_err("Packet exceed the number of skb frags(%lu)\n",
2035                                MAX_SKB_FRAGS);
2036                         return -EFAULT;
2037                 }
2038
2039                 page = pgv_to_page(data);
2040                 data += len;
2041                 flush_dcache_page(page);
2042                 get_page(page);
2043                 skb_fill_page_desc(skb, nr_frags, page, offset, len);
2044                 to_write -= len;
2045                 offset = 0;
2046                 len_max = PAGE_SIZE;
2047                 len = ((to_write > len_max) ? len_max : to_write);
2048         }
2049
2050         return tp_len;
2051 }
2052
2053 static int tpacket_snd(struct packet_sock *po, struct msghdr *msg)
2054 {
2055         struct sk_buff *skb;
2056         struct net_device *dev;
2057         __be16 proto;
2058         bool need_rls_dev = false;
2059         int err, reserve = 0;
2060         void *ph;
2061         struct sockaddr_ll *saddr = (struct sockaddr_ll *)msg->msg_name;
2062         int tp_len, size_max;
2063         unsigned char *addr;
2064         int len_sum = 0;
2065         int status = 0;
2066         int hlen, tlen;
2067
2068         mutex_lock(&po->pg_vec_lock);
2069
2070         err = -EBUSY;
2071         if (saddr == NULL) {
2072                 dev = po->prot_hook.dev;
2073                 proto   = po->num;
2074                 addr    = NULL;
2075         } else {
2076                 err = -EINVAL;
2077                 if (msg->msg_namelen < sizeof(struct sockaddr_ll))
2078                         goto out;
2079                 if (msg->msg_namelen < (saddr->sll_halen
2080                                         + offsetof(struct sockaddr_ll,
2081                                                 sll_addr)))
2082                         goto out;
2083                 proto   = saddr->sll_protocol;
2084                 addr    = saddr->sll_addr;
2085                 dev = dev_get_by_index(sock_net(&po->sk), saddr->sll_ifindex);
2086                 need_rls_dev = true;
2087         }
2088
2089         err = -ENXIO;
2090         if (unlikely(dev == NULL))
2091                 goto out;
2092
2093         reserve = dev->hard_header_len;
2094
2095         err = -ENETDOWN;
2096         if (unlikely(!(dev->flags & IFF_UP)))
2097                 goto out_put;
2098
2099         size_max = po->tx_ring.frame_size
2100                 - (po->tp_hdrlen - sizeof(struct sockaddr_ll));
2101
2102         if (size_max > dev->mtu + reserve)
2103                 size_max = dev->mtu + reserve;
2104
2105         do {
2106                 ph = packet_current_frame(po, &po->tx_ring,
2107                                 TP_STATUS_SEND_REQUEST);
2108
2109                 if (unlikely(ph == NULL)) {
2110                         schedule();
2111                         continue;
2112                 }
2113
2114                 status = TP_STATUS_SEND_REQUEST;
2115                 hlen = LL_RESERVED_SPACE(dev);
2116                 tlen = dev->needed_tailroom;
2117                 skb = sock_alloc_send_skb(&po->sk,
2118                                 hlen + tlen + sizeof(struct sockaddr_ll),
2119                                 0, &err);
2120
2121                 if (unlikely(skb == NULL))
2122                         goto out_status;
2123
2124                 tp_len = tpacket_fill_skb(po, skb, ph, dev, size_max, proto,
2125                                 addr, hlen);
2126
2127                 if (unlikely(tp_len < 0)) {
2128                         if (po->tp_loss) {
2129                                 __packet_set_status(po, ph,
2130                                                 TP_STATUS_AVAILABLE);
2131                                 packet_increment_head(&po->tx_ring);
2132                                 kfree_skb(skb);
2133                                 continue;
2134                         } else {
2135                                 status = TP_STATUS_WRONG_FORMAT;
2136                                 err = tp_len;
2137                                 goto out_status;
2138                         }
2139                 }
2140
2141                 skb->destructor = tpacket_destruct_skb;
2142                 __packet_set_status(po, ph, TP_STATUS_SENDING);
2143                 atomic_inc(&po->tx_ring.pending);
2144
2145                 status = TP_STATUS_SEND_REQUEST;
2146                 err = dev_queue_xmit(skb);
2147                 if (unlikely(err > 0)) {
2148                         err = net_xmit_errno(err);
2149                         if (err && __packet_get_status(po, ph) ==
2150                                    TP_STATUS_AVAILABLE) {
2151                                 /* skb was destructed already */
2152                                 skb = NULL;
2153                                 goto out_status;
2154                         }
2155                         /*
2156                          * skb was dropped but not destructed yet;
2157                          * let's treat it like congestion or err < 0
2158                          */
2159                         err = 0;
2160                 }
2161                 packet_increment_head(&po->tx_ring);
2162                 len_sum += tp_len;
2163         } while (likely((ph != NULL) ||
2164                         ((!(msg->msg_flags & MSG_DONTWAIT)) &&
2165                          (atomic_read(&po->tx_ring.pending))))
2166                 );
2167
2168         err = len_sum;
2169         goto out_put;
2170
2171 out_status:
2172         __packet_set_status(po, ph, status);
2173         kfree_skb(skb);
2174 out_put:
2175         if (need_rls_dev)
2176                 dev_put(dev);
2177 out:
2178         mutex_unlock(&po->pg_vec_lock);
2179         return err;
2180 }
2181
2182 static struct sk_buff *packet_alloc_skb(struct sock *sk, size_t prepad,
2183                                         size_t reserve, size_t len,
2184                                         size_t linear, int noblock,
2185                                         int *err)
2186 {
2187         struct sk_buff *skb;
2188
2189         /* Under a page?  Don't bother with paged skb. */
2190         if (prepad + len < PAGE_SIZE || !linear)
2191                 linear = len;
2192
2193         skb = sock_alloc_send_pskb(sk, prepad + linear, len - linear, noblock,
2194                                    err);
2195         if (!skb)
2196                 return NULL;
2197
2198         skb_reserve(skb, reserve);
2199         skb_put(skb, linear);
2200         skb->data_len = len - linear;
2201         skb->len += len - linear;
2202
2203         return skb;
2204 }
2205
2206 static int packet_snd(struct socket *sock,
2207                           struct msghdr *msg, size_t len)
2208 {
2209         struct sock *sk = sock->sk;
2210         struct sockaddr_ll *saddr = (struct sockaddr_ll *)msg->msg_name;
2211         struct sk_buff *skb;
2212         struct net_device *dev;
2213         __be16 proto;
2214         bool need_rls_dev = false;
2215         unsigned char *addr;
2216         int err, reserve = 0;
2217         struct virtio_net_hdr vnet_hdr = { 0 };
2218         int offset = 0;
2219         int vnet_hdr_len;
2220         struct packet_sock *po = pkt_sk(sk);
2221         unsigned short gso_type = 0;
2222         int hlen, tlen;
2223         int extra_len = 0;
2224
2225         /*
2226          *      Get and verify the address.
2227          */
2228
2229         if (saddr == NULL) {
2230                 dev = po->prot_hook.dev;
2231                 proto   = po->num;
2232                 addr    = NULL;
2233         } else {
2234                 err = -EINVAL;
2235                 if (msg->msg_namelen < sizeof(struct sockaddr_ll))
2236                         goto out;
2237                 if (msg->msg_namelen < (saddr->sll_halen + offsetof(struct sockaddr_ll, sll_addr)))
2238                         goto out;
2239                 proto   = saddr->sll_protocol;
2240                 addr    = saddr->sll_addr;
2241                 dev = dev_get_by_index(sock_net(sk), saddr->sll_ifindex);
2242                 need_rls_dev = true;
2243         }
2244
2245         err = -ENXIO;
2246         if (dev == NULL)
2247                 goto out_unlock;
2248         if (sock->type == SOCK_RAW)
2249                 reserve = dev->hard_header_len;
2250
2251         err = -ENETDOWN;
2252         if (!(dev->flags & IFF_UP))
2253                 goto out_unlock;
2254
2255         if (po->has_vnet_hdr) {
2256                 vnet_hdr_len = sizeof(vnet_hdr);
2257
2258                 err = -EINVAL;
2259                 if (len < vnet_hdr_len)
2260                         goto out_unlock;
2261
2262                 len -= vnet_hdr_len;
2263
2264                 err = memcpy_fromiovec((void *)&vnet_hdr, msg->msg_iov,
2265                                        vnet_hdr_len);
2266                 if (err < 0)
2267                         goto out_unlock;
2268
2269                 if ((vnet_hdr.flags & VIRTIO_NET_HDR_F_NEEDS_CSUM) &&
2270                     (vnet_hdr.csum_start + vnet_hdr.csum_offset + 2 >
2271                       vnet_hdr.hdr_len))
2272                         vnet_hdr.hdr_len = vnet_hdr.csum_start +
2273                                                  vnet_hdr.csum_offset + 2;
2274
2275                 err = -EINVAL;
2276                 if (vnet_hdr.hdr_len > len)
2277                         goto out_unlock;
2278
2279                 if (vnet_hdr.gso_type != VIRTIO_NET_HDR_GSO_NONE) {
2280                         switch (vnet_hdr.gso_type & ~VIRTIO_NET_HDR_GSO_ECN) {
2281                         case VIRTIO_NET_HDR_GSO_TCPV4:
2282                                 gso_type = SKB_GSO_TCPV4;
2283                                 break;
2284                         case VIRTIO_NET_HDR_GSO_TCPV6:
2285                                 gso_type = SKB_GSO_TCPV6;
2286                                 break;
2287                         case VIRTIO_NET_HDR_GSO_UDP:
2288                                 gso_type = SKB_GSO_UDP;
2289                                 break;
2290                         default:
2291                                 goto out_unlock;
2292                         }
2293
2294                         if (vnet_hdr.gso_type & VIRTIO_NET_HDR_GSO_ECN)
2295                                 gso_type |= SKB_GSO_TCP_ECN;
2296
2297                         if (vnet_hdr.gso_size == 0)
2298                                 goto out_unlock;
2299
2300                 }
2301         }
2302
2303         if (unlikely(sock_flag(sk, SOCK_NOFCS))) {
2304                 if (!netif_supports_nofcs(dev)) {
2305                         err = -EPROTONOSUPPORT;
2306                         goto out_unlock;
2307                 }
2308                 extra_len = 4; /* We're doing our own CRC */
2309         }
2310
2311         err = -EMSGSIZE;
2312         if (!gso_type && (len > dev->mtu + reserve + VLAN_HLEN + extra_len))
2313                 goto out_unlock;
2314
2315         err = -ENOBUFS;
2316         hlen = LL_RESERVED_SPACE(dev);
2317         tlen = dev->needed_tailroom;
2318         skb = packet_alloc_skb(sk, hlen + tlen, hlen, len, vnet_hdr.hdr_len,
2319                                msg->msg_flags & MSG_DONTWAIT, &err);
2320         if (skb == NULL)
2321                 goto out_unlock;
2322
2323         skb_set_network_header(skb, reserve);
2324
2325         err = -EINVAL;
2326         if (sock->type == SOCK_DGRAM &&
2327             (offset = dev_hard_header(skb, dev, ntohs(proto), addr, NULL, len)) < 0)
2328                 goto out_free;
2329
2330         /* Returns -EFAULT on error */
2331         err = skb_copy_datagram_from_iovec(skb, offset, msg->msg_iov, 0, len);
2332         if (err)
2333                 goto out_free;
2334         err = sock_tx_timestamp(sk, &skb_shinfo(skb)->tx_flags);
2335         if (err < 0)
2336                 goto out_free;
2337
2338         if (!gso_type && (len > dev->mtu + reserve + extra_len)) {
2339                 /* Earlier code assumed this would be a VLAN pkt,
2340                  * double-check this now that we have the actual
2341                  * packet in hand.
2342                  */
2343                 struct ethhdr *ehdr;
2344                 skb_reset_mac_header(skb);
2345                 ehdr = eth_hdr(skb);
2346                 if (ehdr->h_proto != htons(ETH_P_8021Q)) {
2347                         err = -EMSGSIZE;
2348                         goto out_free;
2349                 }
2350         }
2351
2352         skb->protocol = proto;
2353         skb->dev = dev;
2354         skb->priority = sk->sk_priority;
2355         skb->mark = sk->sk_mark;
2356
2357         if (po->has_vnet_hdr) {
2358                 if (vnet_hdr.flags & VIRTIO_NET_HDR_F_NEEDS_CSUM) {
2359                         if (!skb_partial_csum_set(skb, vnet_hdr.csum_start,
2360                                                   vnet_hdr.csum_offset)) {
2361                                 err = -EINVAL;
2362                                 goto out_free;
2363                         }
2364                 }
2365
2366                 skb_shinfo(skb)->gso_size = vnet_hdr.gso_size;
2367                 skb_shinfo(skb)->gso_type = gso_type;
2368
2369                 /* Header must be checked, and gso_segs computed. */
2370                 skb_shinfo(skb)->gso_type |= SKB_GSO_DODGY;
2371                 skb_shinfo(skb)->gso_segs = 0;
2372
2373                 len += vnet_hdr_len;
2374         }
2375
2376         if (unlikely(extra_len == 4))
2377                 skb->no_fcs = 1;
2378
2379         /*
2380          *      Now send it
2381          */
2382
2383         err = dev_queue_xmit(skb);
2384         if (err > 0 && (err = net_xmit_errno(err)) != 0)
2385                 goto out_unlock;
2386
2387         if (need_rls_dev)
2388                 dev_put(dev);
2389
2390         return len;
2391
2392 out_free:
2393         kfree_skb(skb);
2394 out_unlock:
2395         if (dev && need_rls_dev)
2396                 dev_put(dev);
2397 out:
2398         return err;
2399 }
2400
2401 static int packet_sendmsg(struct kiocb *iocb, struct socket *sock,
2402                 struct msghdr *msg, size_t len)
2403 {
2404         struct sock *sk = sock->sk;
2405         struct packet_sock *po = pkt_sk(sk);
2406         if (po->tx_ring.pg_vec)
2407                 return tpacket_snd(po, msg);
2408         else
2409                 return packet_snd(sock, msg, len);
2410 }
2411
2412 /*
2413  *      Close a PACKET socket. This is fairly simple. We immediately go
2414  *      to 'closed' state and remove our protocol entry in the device list.
2415  */
2416
2417 static int packet_release(struct socket *sock)
2418 {
2419         struct sock *sk = sock->sk;
2420         struct packet_sock *po;
2421         struct net *net;
2422         union tpacket_req_u req_u;
2423
2424         if (!sk)
2425                 return 0;
2426
2427         net = sock_net(sk);
2428         po = pkt_sk(sk);
2429
2430         spin_lock_bh(&net->packet.sklist_lock);
2431         sk_del_node_init_rcu(sk);
2432         sock_prot_inuse_add(net, sk->sk_prot, -1);
2433         spin_unlock_bh(&net->packet.sklist_lock);
2434
2435         spin_lock(&po->bind_lock);
2436         unregister_prot_hook(sk, false);
2437         if (po->prot_hook.dev) {
2438                 dev_put(po->prot_hook.dev);
2439                 po->prot_hook.dev = NULL;
2440         }
2441         spin_unlock(&po->bind_lock);
2442
2443         packet_flush_mclist(sk);
2444
2445         memset(&req_u, 0, sizeof(req_u));
2446
2447         if (po->rx_ring.pg_vec)
2448                 packet_set_ring(sk, &req_u, 1, 0);
2449
2450         if (po->tx_ring.pg_vec)
2451                 packet_set_ring(sk, &req_u, 1, 1);
2452
2453         fanout_release(sk);
2454
2455         synchronize_net();
2456         /*
2457          *      Now the socket is dead. No more input will appear.
2458          */
2459         sock_orphan(sk);
2460         sock->sk = NULL;
2461
2462         /* Purge queues */
2463
2464         skb_queue_purge(&sk->sk_receive_queue);
2465         sk_refcnt_debug_release(sk);
2466
2467         sock_put(sk);
2468         return 0;
2469 }
2470
2471 /*
2472  *      Attach a packet hook.
2473  */
2474
2475 static int packet_do_bind(struct sock *sk, struct net_device *dev, __be16 protocol)
2476 {
2477         struct packet_sock *po = pkt_sk(sk);
2478
2479         if (po->fanout) {
2480                 if (dev)
2481                         dev_put(dev);
2482
2483                 return -EINVAL;
2484         }
2485
2486         lock_sock(sk);
2487
2488         spin_lock(&po->bind_lock);
2489         unregister_prot_hook(sk, true);
2490         po->num = protocol;
2491         po->prot_hook.type = protocol;
2492         if (po->prot_hook.dev)
2493                 dev_put(po->prot_hook.dev);
2494         po->prot_hook.dev = dev;
2495
2496         po->ifindex = dev ? dev->ifindex : 0;
2497
2498         if (protocol == 0)
2499                 goto out_unlock;
2500
2501         if (!dev || (dev->flags & IFF_UP)) {
2502                 register_prot_hook(sk);
2503         } else {
2504                 sk->sk_err = ENETDOWN;
2505                 if (!sock_flag(sk, SOCK_DEAD))
2506                         sk->sk_error_report(sk);
2507         }
2508
2509 out_unlock:
2510         spin_unlock(&po->bind_lock);
2511         release_sock(sk);
2512         return 0;
2513 }
2514
2515 /*
2516  *      Bind a packet socket to a device
2517  */
2518
2519 static int packet_bind_spkt(struct socket *sock, struct sockaddr *uaddr,
2520                             int addr_len)
2521 {
2522         struct sock *sk = sock->sk;
2523         char name[15];
2524         struct net_device *dev;
2525         int err = -ENODEV;
2526
2527         /*
2528          *      Check legality
2529          */
2530
2531         if (addr_len != sizeof(struct sockaddr))
2532                 return -EINVAL;
2533         strlcpy(name, uaddr->sa_data, sizeof(name));
2534
2535         dev = dev_get_by_name(sock_net(sk), name);
2536         if (dev)
2537                 err = packet_do_bind(sk, dev, pkt_sk(sk)->num);
2538         return err;
2539 }
2540
2541 static int packet_bind(struct socket *sock, struct sockaddr *uaddr, int addr_len)
2542 {
2543         struct sockaddr_ll *sll = (struct sockaddr_ll *)uaddr;
2544         struct sock *sk = sock->sk;
2545         struct net_device *dev = NULL;
2546         int err;
2547
2548
2549         /*
2550          *      Check legality
2551          */
2552
2553         if (addr_len < sizeof(struct sockaddr_ll))
2554                 return -EINVAL;
2555         if (sll->sll_family != AF_PACKET)
2556                 return -EINVAL;
2557
2558         if (sll->sll_ifindex) {
2559                 err = -ENODEV;
2560                 dev = dev_get_by_index(sock_net(sk), sll->sll_ifindex);
2561                 if (dev == NULL)
2562                         goto out;
2563         }
2564         err = packet_do_bind(sk, dev, sll->sll_protocol ? : pkt_sk(sk)->num);
2565
2566 out:
2567         return err;
2568 }
2569
2570 static struct proto packet_proto = {
2571         .name     = "PACKET",
2572         .owner    = THIS_MODULE,
2573         .obj_size = sizeof(struct packet_sock),
2574 };
2575
2576 /*
2577  *      Create a packet of type SOCK_PACKET.
2578  */
2579
2580 static int packet_create(struct net *net, struct socket *sock, int protocol,
2581                          int kern)
2582 {
2583         struct sock *sk;
2584         struct packet_sock *po;
2585         __be16 proto = (__force __be16)protocol; /* weird, but documented */
2586         int err;
2587
2588         if (!capable(CAP_NET_RAW))
2589                 return -EPERM;
2590         if (sock->type != SOCK_DGRAM && sock->type != SOCK_RAW &&
2591             sock->type != SOCK_PACKET)
2592                 return -ESOCKTNOSUPPORT;
2593
2594         sock->state = SS_UNCONNECTED;
2595
2596         err = -ENOBUFS;
2597         sk = sk_alloc(net, PF_PACKET, GFP_KERNEL, &packet_proto);
2598         if (sk == NULL)
2599                 goto out;
2600
2601         sock->ops = &packet_ops;
2602         if (sock->type == SOCK_PACKET)
2603                 sock->ops = &packet_ops_spkt;
2604
2605         sock_init_data(sock, sk);
2606
2607         po = pkt_sk(sk);
2608         sk->sk_family = PF_PACKET;
2609         po->num = proto;
2610
2611         sk->sk_destruct = packet_sock_destruct;
2612         sk_refcnt_debug_inc(sk);
2613
2614         /*
2615          *      Attach a protocol block
2616          */
2617
2618         spin_lock_init(&po->bind_lock);
2619         mutex_init(&po->pg_vec_lock);
2620         po->prot_hook.func = packet_rcv;
2621
2622         if (sock->type == SOCK_PACKET)
2623                 po->prot_hook.func = packet_rcv_spkt;
2624
2625         po->prot_hook.af_packet_priv = sk;
2626
2627         if (proto) {
2628                 po->prot_hook.type = proto;
2629                 register_prot_hook(sk);
2630         }
2631
2632         spin_lock_bh(&net->packet.sklist_lock);
2633         sk_add_node_rcu(sk, &net->packet.sklist);
2634         sock_prot_inuse_add(net, &packet_proto, 1);
2635         spin_unlock_bh(&net->packet.sklist_lock);
2636
2637         return 0;
2638 out:
2639         return err;
2640 }
2641
2642 static int packet_recv_error(struct sock *sk, struct msghdr *msg, int len)
2643 {
2644         struct sock_exterr_skb *serr;
2645         struct sk_buff *skb, *skb2;
2646         int copied, err;
2647
2648         err = -EAGAIN;
2649         skb = skb_dequeue(&sk->sk_error_queue);
2650         if (skb == NULL)
2651                 goto out;
2652
2653         copied = skb->len;
2654         if (copied > len) {
2655                 msg->msg_flags |= MSG_TRUNC;
2656                 copied = len;
2657         }
2658         err = skb_copy_datagram_iovec(skb, 0, msg->msg_iov, copied);
2659         if (err)
2660                 goto out_free_skb;
2661
2662         sock_recv_timestamp(msg, sk, skb);
2663
2664         serr = SKB_EXT_ERR(skb);
2665         put_cmsg(msg, SOL_PACKET, PACKET_TX_TIMESTAMP,
2666                  sizeof(serr->ee), &serr->ee);
2667
2668         msg->msg_flags |= MSG_ERRQUEUE;
2669         err = copied;
2670
2671         /* Reset and regenerate socket error */
2672         spin_lock_bh(&sk->sk_error_queue.lock);
2673         sk->sk_err = 0;
2674         if ((skb2 = skb_peek(&sk->sk_error_queue)) != NULL) {
2675                 sk->sk_err = SKB_EXT_ERR(skb2)->ee.ee_errno;
2676                 spin_unlock_bh(&sk->sk_error_queue.lock);
2677                 sk->sk_error_report(sk);
2678         } else
2679                 spin_unlock_bh(&sk->sk_error_queue.lock);
2680
2681 out_free_skb:
2682         kfree_skb(skb);
2683 out:
2684         return err;
2685 }
2686
2687 /*
2688  *      Pull a packet from our receive queue and hand it to the user.
2689  *      If necessary we block.
2690  */
2691
2692 static int packet_recvmsg(struct kiocb *iocb, struct socket *sock,
2693                           struct msghdr *msg, size_t len, int flags)
2694 {
2695         struct sock *sk = sock->sk;
2696         struct sk_buff *skb;
2697         int copied, err;
2698         struct sockaddr_ll *sll;
2699         int vnet_hdr_len = 0;
2700
2701         err = -EINVAL;
2702         if (flags & ~(MSG_PEEK|MSG_DONTWAIT|MSG_TRUNC|MSG_CMSG_COMPAT|MSG_ERRQUEUE))
2703                 goto out;
2704
2705 #if 0
2706         /* What error should we return now? EUNATTACH? */
2707         if (pkt_sk(sk)->ifindex < 0)
2708                 return -ENODEV;
2709 #endif
2710
2711         if (flags & MSG_ERRQUEUE) {
2712                 err = packet_recv_error(sk, msg, len);
2713                 goto out;
2714         }
2715
2716         /*
2717          *      Call the generic datagram receiver. This handles all sorts
2718          *      of horrible races and re-entrancy so we can forget about it
2719          *      in the protocol layers.
2720          *
2721          *      Now it will return ENETDOWN, if device have just gone down,
2722          *      but then it will block.
2723          */
2724
2725         skb = skb_recv_datagram(sk, flags, flags & MSG_DONTWAIT, &err);
2726
2727         /*
2728          *      An error occurred so return it. Because skb_recv_datagram()
2729          *      handles the blocking we don't see and worry about blocking
2730          *      retries.
2731          */
2732
2733         if (skb == NULL)
2734                 goto out;
2735
2736         if (pkt_sk(sk)->has_vnet_hdr) {
2737                 struct virtio_net_hdr vnet_hdr = { 0 };
2738
2739                 err = -EINVAL;
2740                 vnet_hdr_len = sizeof(vnet_hdr);
2741                 if (len < vnet_hdr_len)
2742                         goto out_free;
2743
2744                 len -= vnet_hdr_len;
2745
2746                 if (skb_is_gso(skb)) {
2747                         struct skb_shared_info *sinfo = skb_shinfo(skb);
2748
2749                         /* This is a hint as to how much should be linear. */
2750                         vnet_hdr.hdr_len = skb_headlen(skb);
2751                         vnet_hdr.gso_size = sinfo->gso_size;
2752                         if (sinfo->gso_type & SKB_GSO_TCPV4)
2753                                 vnet_hdr.gso_type = VIRTIO_NET_HDR_GSO_TCPV4;
2754                         else if (sinfo->gso_type & SKB_GSO_TCPV6)
2755                                 vnet_hdr.gso_type = VIRTIO_NET_HDR_GSO_TCPV6;
2756                         else if (sinfo->gso_type & SKB_GSO_UDP)
2757                                 vnet_hdr.gso_type = VIRTIO_NET_HDR_GSO_UDP;
2758                         else if (sinfo->gso_type & SKB_GSO_FCOE)
2759                                 goto out_free;
2760                         else
2761                                 BUG();
2762                         if (sinfo->gso_type & SKB_GSO_TCP_ECN)
2763                                 vnet_hdr.gso_type |= VIRTIO_NET_HDR_GSO_ECN;
2764                 } else
2765                         vnet_hdr.gso_type = VIRTIO_NET_HDR_GSO_NONE;
2766
2767                 if (skb->ip_summed == CHECKSUM_PARTIAL) {
2768                         vnet_hdr.flags = VIRTIO_NET_HDR_F_NEEDS_CSUM;
2769                         vnet_hdr.csum_start = skb_checksum_start_offset(skb);
2770                         vnet_hdr.csum_offset = skb->csum_offset;
2771                 } else if (skb->ip_summed == CHECKSUM_UNNECESSARY) {
2772                         vnet_hdr.flags = VIRTIO_NET_HDR_F_DATA_VALID;
2773                 } /* else everything is zero */
2774
2775                 err = memcpy_toiovec(msg->msg_iov, (void *)&vnet_hdr,
2776                                      vnet_hdr_len);
2777                 if (err < 0)
2778                         goto out_free;
2779         }
2780
2781         /*
2782          *      If the address length field is there to be filled in, we fill
2783          *      it in now.
2784          */
2785
2786         sll = &PACKET_SKB_CB(skb)->sa.ll;
2787         if (sock->type == SOCK_PACKET)
2788                 msg->msg_namelen = sizeof(struct sockaddr_pkt);
2789         else
2790                 msg->msg_namelen = sll->sll_halen + offsetof(struct sockaddr_ll, sll_addr);
2791
2792         /*
2793          *      You lose any data beyond the buffer you gave. If it worries a
2794          *      user program they can ask the device for its MTU anyway.
2795          */
2796
2797         copied = skb->len;
2798         if (copied > len) {
2799                 copied = len;
2800                 msg->msg_flags |= MSG_TRUNC;
2801         }
2802
2803         err = skb_copy_datagram_iovec(skb, 0, msg->msg_iov, copied);
2804         if (err)
2805                 goto out_free;
2806
2807         sock_recv_ts_and_drops(msg, sk, skb);
2808
2809         if (msg->msg_name)
2810                 memcpy(msg->msg_name, &PACKET_SKB_CB(skb)->sa,
2811                        msg->msg_namelen);
2812
2813         if (pkt_sk(sk)->auxdata) {
2814                 struct tpacket_auxdata aux;
2815
2816                 aux.tp_status = TP_STATUS_USER;
2817                 if (skb->ip_summed == CHECKSUM_PARTIAL)
2818                         aux.tp_status |= TP_STATUS_CSUMNOTREADY;
2819                 aux.tp_len = PACKET_SKB_CB(skb)->origlen;
2820                 aux.tp_snaplen = skb->len;
2821                 aux.tp_mac = 0;
2822                 aux.tp_net = skb_network_offset(skb);
2823                 if (vlan_tx_tag_present(skb)) {
2824                         aux.tp_vlan_tci = vlan_tx_tag_get(skb);
2825                         aux.tp_status |= TP_STATUS_VLAN_VALID;
2826                 } else {
2827                         aux.tp_vlan_tci = 0;
2828                 }
2829                 aux.tp_padding = 0;
2830                 put_cmsg(msg, SOL_PACKET, PACKET_AUXDATA, sizeof(aux), &aux);
2831         }
2832
2833         /*
2834          *      Free or return the buffer as appropriate. Again this
2835          *      hides all the races and re-entrancy issues from us.
2836          */
2837         err = vnet_hdr_len + ((flags&MSG_TRUNC) ? skb->len : copied);
2838
2839 out_free:
2840         skb_free_datagram(sk, skb);
2841 out:
2842         return err;
2843 }
2844
2845 static int packet_getname_spkt(struct socket *sock, struct sockaddr *uaddr,
2846                                int *uaddr_len, int peer)
2847 {
2848         struct net_device *dev;
2849         struct sock *sk = sock->sk;
2850
2851         if (peer)
2852                 return -EOPNOTSUPP;
2853
2854         uaddr->sa_family = AF_PACKET;
2855         rcu_read_lock();
2856         dev = dev_get_by_index_rcu(sock_net(sk), pkt_sk(sk)->ifindex);
2857         if (dev)
2858                 strncpy(uaddr->sa_data, dev->name, 14);
2859         else
2860                 memset(uaddr->sa_data, 0, 14);
2861         rcu_read_unlock();
2862         *uaddr_len = sizeof(*uaddr);
2863
2864         return 0;
2865 }
2866
2867 static int packet_getname(struct socket *sock, struct sockaddr *uaddr,
2868                           int *uaddr_len, int peer)
2869 {
2870         struct net_device *dev;
2871         struct sock *sk = sock->sk;
2872         struct packet_sock *po = pkt_sk(sk);
2873         DECLARE_SOCKADDR(struct sockaddr_ll *, sll, uaddr);
2874
2875         if (peer)
2876                 return -EOPNOTSUPP;
2877
2878         sll->sll_family = AF_PACKET;
2879         sll->sll_ifindex = po->ifindex;
2880         sll->sll_protocol = po->num;
2881         sll->sll_pkttype = 0;
2882         rcu_read_lock();
2883         dev = dev_get_by_index_rcu(sock_net(sk), po->ifindex);
2884         if (dev) {
2885                 sll->sll_hatype = dev->type;
2886                 sll->sll_halen = dev->addr_len;
2887                 memcpy(sll->sll_addr, dev->dev_addr, dev->addr_len);
2888         } else {
2889                 sll->sll_hatype = 0;    /* Bad: we have no ARPHRD_UNSPEC */
2890                 sll->sll_halen = 0;
2891         }
2892         rcu_read_unlock();
2893         *uaddr_len = offsetof(struct sockaddr_ll, sll_addr) + sll->sll_halen;
2894
2895         return 0;
2896 }
2897
2898 static int packet_dev_mc(struct net_device *dev, struct packet_mclist *i,
2899                          int what)
2900 {
2901         switch (i->type) {
2902         case PACKET_MR_MULTICAST:
2903                 if (i->alen != dev->addr_len)
2904                         return -EINVAL;
2905                 if (what > 0)
2906                         return dev_mc_add(dev, i->addr);
2907                 else
2908                         return dev_mc_del(dev, i->addr);
2909                 break;
2910         case PACKET_MR_PROMISC:
2911                 return dev_set_promiscuity(dev, what);
2912                 break;
2913         case PACKET_MR_ALLMULTI:
2914                 return dev_set_allmulti(dev, what);
2915                 break;
2916         case PACKET_MR_UNICAST:
2917                 if (i->alen != dev->addr_len)
2918                         return -EINVAL;
2919                 if (what > 0)
2920                         return dev_uc_add(dev, i->addr);
2921                 else
2922                         return dev_uc_del(dev, i->addr);
2923                 break;
2924         default:
2925                 break;
2926         }
2927         return 0;
2928 }
2929
2930 static void packet_dev_mclist(struct net_device *dev, struct packet_mclist *i, int what)
2931 {
2932         for ( ; i; i = i->next) {
2933                 if (i->ifindex == dev->ifindex)
2934                         packet_dev_mc(dev, i, what);
2935         }
2936 }
2937
2938 static int packet_mc_add(struct sock *sk, struct packet_mreq_max *mreq)
2939 {
2940         struct packet_sock *po = pkt_sk(sk);
2941         struct packet_mclist *ml, *i;
2942         struct net_device *dev;
2943         int err;
2944
2945         rtnl_lock();
2946
2947         err = -ENODEV;
2948         dev = __dev_get_by_index(sock_net(sk), mreq->mr_ifindex);
2949         if (!dev)
2950                 goto done;
2951
2952         err = -EINVAL;
2953         if (mreq->mr_alen > dev->addr_len)
2954                 goto done;
2955
2956         err = -ENOBUFS;
2957         i = kmalloc(sizeof(*i), GFP_KERNEL);
2958         if (i == NULL)
2959                 goto done;
2960
2961         err = 0;
2962         for (ml = po->mclist; ml; ml = ml->next) {
2963                 if (ml->ifindex == mreq->mr_ifindex &&
2964                     ml->type == mreq->mr_type &&
2965                     ml->alen == mreq->mr_alen &&
2966                     memcmp(ml->addr, mreq->mr_address, ml->alen) == 0) {
2967                         ml->count++;
2968                         /* Free the new element ... */
2969                         kfree(i);
2970                         goto done;
2971                 }
2972         }
2973
2974         i->type = mreq->mr_type;
2975         i->ifindex = mreq->mr_ifindex;
2976         i->alen = mreq->mr_alen;
2977         memcpy(i->addr, mreq->mr_address, i->alen);
2978         i->count = 1;
2979         i->next = po->mclist;
2980         po->mclist = i;
2981         err = packet_dev_mc(dev, i, 1);
2982         if (err) {
2983                 po->mclist = i->next;
2984                 kfree(i);
2985         }
2986
2987 done:
2988         rtnl_unlock();
2989         return err;
2990 }
2991
2992 static int packet_mc_drop(struct sock *sk, struct packet_mreq_max *mreq)
2993 {
2994         struct packet_mclist *ml, **mlp;
2995
2996         rtnl_lock();
2997
2998         for (mlp = &pkt_sk(sk)->mclist; (ml = *mlp) != NULL; mlp = &ml->next) {
2999                 if (ml->ifindex == mreq->mr_ifindex &&
3000                     ml->type == mreq->mr_type &&
3001                     ml->alen == mreq->mr_alen &&
3002                     memcmp(ml->addr, mreq->mr_address, ml->alen) == 0) {
3003                         if (--ml->count == 0) {
3004                                 struct net_device *dev;
3005                                 *mlp = ml->next;
3006                                 dev = __dev_get_by_index(sock_net(sk), ml->ifindex);
3007                                 if (dev)
3008                                         packet_dev_mc(dev, ml, -1);
3009                                 kfree(ml);
3010                         }
3011                         rtnl_unlock();
3012                         return 0;
3013                 }
3014         }
3015         rtnl_unlock();
3016         return -EADDRNOTAVAIL;
3017 }
3018
3019 static void packet_flush_mclist(struct sock *sk)
3020 {
3021         struct packet_sock *po = pkt_sk(sk);
3022         struct packet_mclist *ml;
3023
3024         if (!po->mclist)
3025                 return;
3026
3027         rtnl_lock();
3028         while ((ml = po->mclist) != NULL) {
3029                 struct net_device *dev;
3030
3031                 po->mclist = ml->next;
3032                 dev = __dev_get_by_index(sock_net(sk), ml->ifindex);
3033                 if (dev != NULL)
3034                         packet_dev_mc(dev, ml, -1);
3035                 kfree(ml);
3036         }
3037         rtnl_unlock();
3038 }
3039
3040 static int
3041 packet_setsockopt(struct socket *sock, int level, int optname, char __user *optval, unsigned int optlen)
3042 {
3043         struct sock *sk = sock->sk;
3044         struct packet_sock *po = pkt_sk(sk);
3045         int ret;
3046
3047         if (level != SOL_PACKET)
3048                 return -ENOPROTOOPT;
3049
3050         switch (optname) {
3051         case PACKET_ADD_MEMBERSHIP:
3052         case PACKET_DROP_MEMBERSHIP:
3053         {
3054                 struct packet_mreq_max mreq;
3055                 int len = optlen;
3056                 memset(&mreq, 0, sizeof(mreq));
3057                 if (len < sizeof(struct packet_mreq))
3058                         return -EINVAL;
3059                 if (len > sizeof(mreq))
3060                         len = sizeof(mreq);
3061                 if (copy_from_user(&mreq, optval, len))
3062                         return -EFAULT;
3063                 if (len < (mreq.mr_alen + offsetof(struct packet_mreq, mr_address)))
3064                         return -EINVAL;
3065                 if (optname == PACKET_ADD_MEMBERSHIP)
3066                         ret = packet_mc_add(sk, &mreq);
3067                 else
3068                         ret = packet_mc_drop(sk, &mreq);
3069                 return ret;
3070         }
3071
3072         case PACKET_RX_RING:
3073         case PACKET_TX_RING:
3074         {
3075                 union tpacket_req_u req_u;
3076                 int len;
3077
3078                 switch (po->tp_version) {
3079                 case TPACKET_V1:
3080                 case TPACKET_V2:
3081                         len = sizeof(req_u.req);
3082                         break;
3083                 case TPACKET_V3:
3084                 default:
3085                         len = sizeof(req_u.req3);
3086                         break;
3087                 }
3088                 if (optlen < len)
3089                         return -EINVAL;
3090                 if (pkt_sk(sk)->has_vnet_hdr)
3091                         return -EINVAL;
3092                 if (copy_from_user(&req_u.req, optval, len))
3093                         return -EFAULT;
3094                 return packet_set_ring(sk, &req_u, 0,
3095                         optname == PACKET_TX_RING);
3096         }
3097         case PACKET_COPY_THRESH:
3098         {
3099                 int val;
3100
3101                 if (optlen != sizeof(val))
3102                         return -EINVAL;
3103                 if (copy_from_user(&val, optval, sizeof(val)))
3104                         return -EFAULT;
3105
3106                 pkt_sk(sk)->copy_thresh = val;
3107                 return 0;
3108         }
3109         case PACKET_VERSION:
3110         {
3111                 int val;
3112
3113                 if (optlen != sizeof(val))
3114                         return -EINVAL;
3115                 if (po->rx_ring.pg_vec || po->tx_ring.pg_vec)
3116                         return -EBUSY;
3117                 if (copy_from_user(&val, optval, sizeof(val)))
3118                         return -EFAULT;
3119                 switch (val) {
3120                 case TPACKET_V1:
3121                 case TPACKET_V2:
3122                 case TPACKET_V3:
3123                         po->tp_version = val;
3124                         return 0;
3125                 default:
3126                         return -EINVAL;
3127                 }
3128         }
3129         case PACKET_RESERVE:
3130         {
3131                 unsigned int val;
3132
3133                 if (optlen != sizeof(val))
3134                         return -EINVAL;
3135                 if (po->rx_ring.pg_vec || po->tx_ring.pg_vec)
3136                         return -EBUSY;
3137                 if (copy_from_user(&val, optval, sizeof(val)))
3138                         return -EFAULT;
3139                 po->tp_reserve = val;
3140                 return 0;
3141         }
3142         case PACKET_LOSS:
3143         {
3144                 unsigned int val;
3145
3146                 if (optlen != sizeof(val))
3147                         return -EINVAL;
3148                 if (po->rx_ring.pg_vec || po->tx_ring.pg_vec)
3149                         return -EBUSY;
3150                 if (copy_from_user(&val, optval, sizeof(val)))
3151                         return -EFAULT;
3152                 po->tp_loss = !!val;
3153                 return 0;
3154         }
3155         case PACKET_AUXDATA:
3156         {
3157                 int val;
3158
3159                 if (optlen < sizeof(val))
3160                         return -EINVAL;
3161                 if (copy_from_user(&val, optval, sizeof(val)))
3162                         return -EFAULT;
3163
3164                 po->auxdata = !!val;
3165                 return 0;
3166         }
3167         case PACKET_ORIGDEV:
3168         {
3169                 int val;
3170
3171                 if (optlen < sizeof(val))
3172                         return -EINVAL;
3173                 if (copy_from_user(&val, optval, sizeof(val)))
3174                         return -EFAULT;
3175
3176                 po->origdev = !!val;
3177                 return 0;
3178         }
3179         case PACKET_VNET_HDR:
3180         {
3181                 int val;
3182
3183                 if (sock->type != SOCK_RAW)
3184                         return -EINVAL;
3185                 if (po->rx_ring.pg_vec || po->tx_ring.pg_vec)
3186                         return -EBUSY;
3187                 if (optlen < sizeof(val))
3188                         return -EINVAL;
3189                 if (copy_from_user(&val, optval, sizeof(val)))
3190                         return -EFAULT;
3191
3192                 po->has_vnet_hdr = !!val;
3193                 return 0;
3194         }
3195         case PACKET_TIMESTAMP:
3196         {
3197                 int val;
3198
3199                 if (optlen != sizeof(val))
3200                         return -EINVAL;
3201                 if (copy_from_user(&val, optval, sizeof(val)))
3202                         return -EFAULT;
3203
3204                 po->tp_tstamp = val;
3205                 return 0;
3206         }
3207         case PACKET_FANOUT:
3208         {
3209                 int val;
3210
3211                 if (optlen != sizeof(val))
3212                         return -EINVAL;
3213                 if (copy_from_user(&val, optval, sizeof(val)))
3214                         return -EFAULT;
3215
3216                 return fanout_add(sk, val & 0xffff, val >> 16);
3217         }
3218         default:
3219                 return -ENOPROTOOPT;
3220         }
3221 }
3222
3223 static int packet_getsockopt(struct socket *sock, int level, int optname,
3224                              char __user *optval, int __user *optlen)
3225 {
3226         int len;
3227         int val, lv = sizeof(val);
3228         struct sock *sk = sock->sk;
3229         struct packet_sock *po = pkt_sk(sk);
3230         void *data = &val;
3231         struct tpacket_stats st;
3232         union tpacket_stats_u st_u;
3233
3234         if (level != SOL_PACKET)
3235                 return -ENOPROTOOPT;
3236
3237         if (get_user(len, optlen))
3238                 return -EFAULT;
3239
3240         if (len < 0)
3241                 return -EINVAL;
3242
3243         switch (optname) {
3244         case PACKET_STATISTICS:
3245                 spin_lock_bh(&sk->sk_receive_queue.lock);
3246                 if (po->tp_version == TPACKET_V3) {
3247                         lv = sizeof(struct tpacket_stats_v3);
3248                         memcpy(&st_u.stats3, &po->stats,
3249                                sizeof(struct tpacket_stats));
3250                         st_u.stats3.tp_freeze_q_cnt =
3251                                         po->stats_u.stats3.tp_freeze_q_cnt;
3252                         st_u.stats3.tp_packets += po->stats.tp_drops;
3253                         data = &st_u.stats3;
3254                 } else {
3255                         lv = sizeof(struct tpacket_stats);
3256                         st = po->stats;
3257                         st.tp_packets += st.tp_drops;
3258                         data = &st;
3259                 }
3260                 memset(&po->stats, 0, sizeof(st));
3261                 spin_unlock_bh(&sk->sk_receive_queue.lock);
3262                 break;
3263         case PACKET_AUXDATA:
3264                 val = po->auxdata;
3265                 break;
3266         case PACKET_ORIGDEV:
3267                 val = po->origdev;
3268                 break;
3269         case PACKET_VNET_HDR:
3270                 val = po->has_vnet_hdr;
3271                 break;
3272         case PACKET_VERSION:
3273                 val = po->tp_version;
3274                 break;
3275         case PACKET_HDRLEN:
3276                 if (len > sizeof(int))
3277                         len = sizeof(int);
3278                 if (copy_from_user(&val, optval, len))
3279                         return -EFAULT;
3280                 switch (val) {
3281                 case TPACKET_V1:
3282                         val = sizeof(struct tpacket_hdr);
3283                         break;
3284                 case TPACKET_V2:
3285                         val = sizeof(struct tpacket2_hdr);
3286                         break;
3287                 case TPACKET_V3:
3288                         val = sizeof(struct tpacket3_hdr);
3289                         break;
3290                 default:
3291                         return -EINVAL;
3292                 }
3293                 break;
3294         case PACKET_RESERVE:
3295                 val = po->tp_reserve;
3296                 break;
3297         case PACKET_LOSS:
3298                 val = po->tp_loss;
3299                 break;
3300         case PACKET_TIMESTAMP:
3301                 val = po->tp_tstamp;
3302                 break;
3303         case PACKET_FANOUT:
3304                 val = (po->fanout ?
3305                        ((u32)po->fanout->id |
3306                         ((u32)po->fanout->type << 16)) :
3307                        0);
3308                 break;
3309         default:
3310                 return -ENOPROTOOPT;
3311         }
3312
3313         if (len > lv)
3314                 len = lv;
3315         if (put_user(len, optlen))
3316                 return -EFAULT;
3317         if (copy_to_user(optval, data, len))
3318                 return -EFAULT;
3319         return 0;
3320 }
3321
3322
3323 static int packet_notifier(struct notifier_block *this, unsigned long msg, void *data)
3324 {
3325         struct sock *sk;
3326         struct hlist_node *node;
3327         struct net_device *dev = data;
3328         struct net *net = dev_net(dev);
3329
3330         rcu_read_lock();
3331         sk_for_each_rcu(sk, node, &net->packet.sklist) {
3332                 struct packet_sock *po = pkt_sk(sk);
3333
3334                 switch (msg) {
3335                 case NETDEV_UNREGISTER:
3336                         if (po->mclist)
3337                                 packet_dev_mclist(dev, po->mclist, -1);
3338                         /* fallthrough */
3339
3340                 case NETDEV_DOWN:
3341                         if (dev->ifindex == po->ifindex) {
3342                                 spin_lock(&po->bind_lock);
3343                                 if (po->running) {
3344                                         __unregister_prot_hook(sk, false);
3345                                         sk->sk_err = ENETDOWN;
3346                                         if (!sock_flag(sk, SOCK_DEAD))
3347                                                 sk->sk_error_report(sk);
3348                                 }
3349                                 if (msg == NETDEV_UNREGISTER) {
3350                                         po->ifindex = -1;
3351                                         if (po->prot_hook.dev)
3352                                                 dev_put(po->prot_hook.dev);
3353                                         po->prot_hook.dev = NULL;
3354                                 }
3355                                 spin_unlock(&po->bind_lock);
3356                         }
3357                         break;
3358                 case NETDEV_UP:
3359                         if (dev->ifindex == po->ifindex) {
3360                                 spin_lock(&po->bind_lock);
3361                                 if (po->num)
3362                                         register_prot_hook(sk);
3363                                 spin_unlock(&po->bind_lock);
3364                         }
3365                         break;
3366                 }
3367         }
3368         rcu_read_unlock();
3369         return NOTIFY_DONE;
3370 }
3371
3372
3373 static int packet_ioctl(struct socket *sock, unsigned int cmd,
3374                         unsigned long arg)
3375 {
3376         struct sock *sk = sock->sk;
3377
3378         switch (cmd) {
3379         case SIOCOUTQ:
3380         {
3381                 int amount = sk_wmem_alloc_get(sk);
3382
3383                 return put_user(amount, (int __user *)arg);
3384         }
3385         case SIOCINQ:
3386         {
3387                 struct sk_buff *skb;
3388                 int amount = 0;
3389
3390                 spin_lock_bh(&sk->sk_receive_queue.lock);
3391                 skb = skb_peek(&sk->sk_receive_queue);
3392                 if (skb)
3393                         amount = skb->len;
3394                 spin_unlock_bh(&sk->sk_receive_queue.lock);
3395                 return put_user(amount, (int __user *)arg);
3396         }
3397         case SIOCGSTAMP:
3398                 return sock_get_timestamp(sk, (struct timeval __user *)arg);
3399         case SIOCGSTAMPNS:
3400                 return sock_get_timestampns(sk, (struct timespec __user *)arg);
3401
3402 #ifdef CONFIG_INET
3403         case SIOCADDRT:
3404         case SIOCDELRT:
3405         case SIOCDARP:
3406         case SIOCGARP:
3407         case SIOCSARP:
3408         case SIOCGIFADDR:
3409         case SIOCSIFADDR:
3410         case SIOCGIFBRDADDR:
3411         case SIOCSIFBRDADDR:
3412         case SIOCGIFNETMASK:
3413         case SIOCSIFNETMASK:
3414         case SIOCGIFDSTADDR:
3415         case SIOCSIFDSTADDR:
3416         case SIOCSIFFLAGS:
3417                 return inet_dgram_ops.ioctl(sock, cmd, arg);
3418 #endif
3419
3420         default:
3421                 return -ENOIOCTLCMD;
3422         }
3423         return 0;
3424 }
3425
3426 static unsigned int packet_poll(struct file *file, struct socket *sock,
3427                                 poll_table *wait)
3428 {
3429         struct sock *sk = sock->sk;
3430         struct packet_sock *po = pkt_sk(sk);
3431         unsigned int mask = datagram_poll(file, sock, wait);
3432
3433         spin_lock_bh(&sk->sk_receive_queue.lock);
3434         if (po->rx_ring.pg_vec) {
3435                 if (!packet_previous_rx_frame(po, &po->rx_ring,
3436                         TP_STATUS_KERNEL))
3437                         mask |= POLLIN | POLLRDNORM;
3438         }
3439         spin_unlock_bh(&sk->sk_receive_queue.lock);
3440         spin_lock_bh(&sk->sk_write_queue.lock);
3441         if (po->tx_ring.pg_vec) {
3442                 if (packet_current_frame(po, &po->tx_ring, TP_STATUS_AVAILABLE))
3443                         mask |= POLLOUT | POLLWRNORM;
3444         }
3445         spin_unlock_bh(&sk->sk_write_queue.lock);
3446         return mask;
3447 }
3448
3449
3450 /* Dirty? Well, I still did not learn better way to account
3451  * for user mmaps.
3452  */
3453
3454 static void packet_mm_open(struct vm_area_struct *vma)
3455 {
3456         struct file *file = vma->vm_file;
3457         struct socket *sock = file->private_data;
3458         struct sock *sk = sock->sk;
3459
3460         if (sk)
3461                 atomic_inc(&pkt_sk(sk)->mapped);
3462 }
3463
3464 static void packet_mm_close(struct vm_area_struct *vma)
3465 {
3466         struct file *file = vma->vm_file;
3467         struct socket *sock = file->private_data;
3468         struct sock *sk = sock->sk;
3469
3470         if (sk)
3471                 atomic_dec(&pkt_sk(sk)->mapped);
3472 }
3473
3474 static const struct vm_operations_struct packet_mmap_ops = {
3475         .open   =       packet_mm_open,
3476         .close  =       packet_mm_close,
3477 };
3478
3479 static void free_pg_vec(struct pgv *pg_vec, unsigned int order,
3480                         unsigned int len)
3481 {
3482         int i;
3483
3484         for (i = 0; i < len; i++) {
3485                 if (likely(pg_vec[i].buffer)) {
3486                         if (is_vmalloc_addr(pg_vec[i].buffer))
3487                                 vfree(pg_vec[i].buffer);
3488                         else
3489                                 free_pages((unsigned long)pg_vec[i].buffer,
3490                                            order);
3491                         pg_vec[i].buffer = NULL;
3492                 }
3493         }
3494         kfree(pg_vec);
3495 }
3496
3497 static char *alloc_one_pg_vec_page(unsigned long order)
3498 {
3499         char *buffer = NULL;
3500         gfp_t gfp_flags = GFP_KERNEL | __GFP_COMP |
3501                           __GFP_ZERO | __GFP_NOWARN | __GFP_NORETRY;
3502
3503         buffer = (char *) __get_free_pages(gfp_flags, order);
3504
3505         if (buffer)
3506                 return buffer;
3507
3508         /*
3509          * __get_free_pages failed, fall back to vmalloc
3510          */
3511         buffer = vzalloc((1 << order) * PAGE_SIZE);
3512
3513         if (buffer)
3514                 return buffer;
3515
3516         /*
3517          * vmalloc failed, lets dig into swap here
3518          */
3519         gfp_flags &= ~__GFP_NORETRY;
3520         buffer = (char *)__get_free_pages(gfp_flags, order);
3521         if (buffer)
3522                 return buffer;
3523
3524         /*
3525          * complete and utter failure
3526          */
3527         return NULL;
3528 }
3529
3530 static struct pgv *alloc_pg_vec(struct tpacket_req *req, int order)
3531 {
3532         unsigned int block_nr = req->tp_block_nr;
3533         struct pgv *pg_vec;
3534         int i;
3535
3536         pg_vec = kcalloc(block_nr, sizeof(struct pgv), GFP_KERNEL);
3537         if (unlikely(!pg_vec))
3538                 goto out;
3539
3540         for (i = 0; i < block_nr; i++) {
3541                 pg_vec[i].buffer = alloc_one_pg_vec_page(order);
3542                 if (unlikely(!pg_vec[i].buffer))
3543                         goto out_free_pgvec;
3544         }
3545
3546 out:
3547         return pg_vec;
3548
3549 out_free_pgvec:
3550         free_pg_vec(pg_vec, order, block_nr);
3551         pg_vec = NULL;
3552         goto out;
3553 }
3554
3555 static int packet_set_ring(struct sock *sk, union tpacket_req_u *req_u,
3556                 int closing, int tx_ring)
3557 {
3558         struct pgv *pg_vec = NULL;
3559         struct packet_sock *po = pkt_sk(sk);
3560         int was_running, order = 0;
3561         struct packet_ring_buffer *rb;
3562         struct sk_buff_head *rb_queue;
3563         __be16 num;
3564         int err = -EINVAL;
3565         /* Added to avoid minimal code churn */
3566         struct tpacket_req *req = &req_u->req;
3567
3568         /* Opening a Tx-ring is NOT supported in TPACKET_V3 */
3569         if (!closing && tx_ring && (po->tp_version > TPACKET_V2)) {
3570                 WARN(1, "Tx-ring is not supported.\n");
3571                 goto out;
3572         }
3573
3574         rb = tx_ring ? &po->tx_ring : &po->rx_ring;
3575         rb_queue = tx_ring ? &sk->sk_write_queue : &sk->sk_receive_queue;
3576
3577         err = -EBUSY;
3578         if (!closing) {
3579                 if (atomic_read(&po->mapped))
3580                         goto out;
3581                 if (atomic_read(&rb->pending))
3582                         goto out;
3583         }
3584
3585         if (req->tp_block_nr) {
3586                 /* Sanity tests and some calculations */
3587                 err = -EBUSY;
3588                 if (unlikely(rb->pg_vec))
3589                         goto out;
3590
3591                 switch (po->tp_version) {
3592                 case TPACKET_V1:
3593                         po->tp_hdrlen = TPACKET_HDRLEN;
3594                         break;
3595                 case TPACKET_V2:
3596                         po->tp_hdrlen = TPACKET2_HDRLEN;
3597                         break;
3598                 case TPACKET_V3:
3599                         po->tp_hdrlen = TPACKET3_HDRLEN;
3600                         break;
3601                 }
3602
3603                 err = -EINVAL;
3604                 if (unlikely((int)req->tp_block_size <= 0))
3605                         goto out;
3606                 if (unlikely(req->tp_block_size & (PAGE_SIZE - 1)))
3607                         goto out;
3608                 if (unlikely(req->tp_frame_size < po->tp_hdrlen +
3609                                         po->tp_reserve))
3610                         goto out;
3611                 if (unlikely(req->tp_frame_size & (TPACKET_ALIGNMENT - 1)))
3612                         goto out;
3613
3614                 rb->frames_per_block = req->tp_block_size/req->tp_frame_size;
3615                 if (unlikely(rb->frames_per_block <= 0))
3616                         goto out;
3617                 if (unlikely((rb->frames_per_block * req->tp_block_nr) !=
3618                                         req->tp_frame_nr))
3619                         goto out;
3620
3621                 err = -ENOMEM;
3622                 order = get_order(req->tp_block_size);
3623                 pg_vec = alloc_pg_vec(req, order);
3624                 if (unlikely(!pg_vec))
3625                         goto out;
3626                 switch (po->tp_version) {
3627                 case TPACKET_V3:
3628                 /* Transmit path is not supported. We checked
3629                  * it above but just being paranoid
3630                  */
3631                         if (!tx_ring)
3632                                 init_prb_bdqc(po, rb, pg_vec, req_u, tx_ring);
3633                                 break;
3634                 default:
3635                         break;
3636                 }
3637         }
3638         /* Done */
3639         else {
3640                 err = -EINVAL;
3641                 if (unlikely(req->tp_frame_nr))
3642                         goto out;
3643         }
3644
3645         lock_sock(sk);
3646
3647         /* Detach socket from network */
3648         spin_lock(&po->bind_lock);
3649         was_running = po->running;
3650         num = po->num;
3651         if (was_running) {
3652                 po->num = 0;
3653                 __unregister_prot_hook(sk, false);
3654         }
3655         spin_unlock(&po->bind_lock);
3656
3657         synchronize_net();
3658
3659         err = -EBUSY;
3660         mutex_lock(&po->pg_vec_lock);
3661         if (closing || atomic_read(&po->mapped) == 0) {
3662                 err = 0;
3663                 spin_lock_bh(&rb_queue->lock);
3664                 swap(rb->pg_vec, pg_vec);
3665                 rb->frame_max = (req->tp_frame_nr - 1);
3666                 rb->head = 0;
3667                 rb->frame_size = req->tp_frame_size;
3668                 spin_unlock_bh(&rb_queue->lock);
3669
3670                 swap(rb->pg_vec_order, order);
3671                 swap(rb->pg_vec_len, req->tp_block_nr);
3672
3673                 rb->pg_vec_pages = req->tp_block_size/PAGE_SIZE;
3674                 po->prot_hook.func = (po->rx_ring.pg_vec) ?
3675                                                 tpacket_rcv : packet_rcv;
3676                 skb_queue_purge(rb_queue);
3677                 if (atomic_read(&po->mapped))
3678                         pr_err("packet_mmap: vma is busy: %d\n",
3679                                atomic_read(&po->mapped));
3680         }
3681         mutex_unlock(&po->pg_vec_lock);
3682
3683         spin_lock(&po->bind_lock);
3684         if (was_running) {
3685                 po->num = num;
3686                 register_prot_hook(sk);
3687         }
3688         spin_unlock(&po->bind_lock);
3689         if (closing && (po->tp_version > TPACKET_V2)) {
3690                 /* Because we don't support block-based V3 on tx-ring */
3691                 if (!tx_ring)
3692                         prb_shutdown_retire_blk_timer(po, tx_ring, rb_queue);
3693         }
3694         release_sock(sk);
3695
3696         if (pg_vec)
3697                 free_pg_vec(pg_vec, order, req->tp_block_nr);
3698 out:
3699         return err;
3700 }
3701
3702 static int packet_mmap(struct file *file, struct socket *sock,
3703                 struct vm_area_struct *vma)
3704 {
3705         struct sock *sk = sock->sk;
3706         struct packet_sock *po = pkt_sk(sk);
3707         unsigned long size, expected_size;
3708         struct packet_ring_buffer *rb;
3709         unsigned long start;
3710         int err = -EINVAL;
3711         int i;
3712
3713         if (vma->vm_pgoff)
3714                 return -EINVAL;
3715
3716         mutex_lock(&po->pg_vec_lock);
3717
3718         expected_size = 0;
3719         for (rb = &po->rx_ring; rb <= &po->tx_ring; rb++) {
3720                 if (rb->pg_vec) {
3721                         expected_size += rb->pg_vec_len
3722                                                 * rb->pg_vec_pages
3723                                                 * PAGE_SIZE;
3724                 }
3725         }
3726
3727         if (expected_size == 0)
3728                 goto out;
3729
3730         size = vma->vm_end - vma->vm_start;
3731         if (size != expected_size)
3732                 goto out;
3733
3734         start = vma->vm_start;
3735         for (rb = &po->rx_ring; rb <= &po->tx_ring; rb++) {
3736                 if (rb->pg_vec == NULL)
3737                         continue;
3738
3739                 for (i = 0; i < rb->pg_vec_len; i++) {
3740                         struct page *page;
3741                         void *kaddr = rb->pg_vec[i].buffer;
3742                         int pg_num;
3743
3744                         for (pg_num = 0; pg_num < rb->pg_vec_pages; pg_num++) {
3745                                 page = pgv_to_page(kaddr);
3746                                 err = vm_insert_page(vma, start, page);
3747                                 if (unlikely(err))
3748                                         goto out;
3749                                 start += PAGE_SIZE;
3750                                 kaddr += PAGE_SIZE;
3751                         }
3752                 }
3753         }
3754
3755         atomic_inc(&po->mapped);
3756         vma->vm_ops = &packet_mmap_ops;
3757         err = 0;
3758
3759 out:
3760         mutex_unlock(&po->pg_vec_lock);
3761         return err;
3762 }
3763
3764 static const struct proto_ops packet_ops_spkt = {
3765         .family =       PF_PACKET,
3766         .owner =        THIS_MODULE,
3767         .release =      packet_release,
3768         .bind =         packet_bind_spkt,
3769         .connect =      sock_no_connect,
3770         .socketpair =   sock_no_socketpair,
3771         .accept =       sock_no_accept,
3772         .getname =      packet_getname_spkt,
3773         .poll =         datagram_poll,
3774         .ioctl =        packet_ioctl,
3775         .listen =       sock_no_listen,
3776         .shutdown =     sock_no_shutdown,
3777         .setsockopt =   sock_no_setsockopt,
3778         .getsockopt =   sock_no_getsockopt,
3779         .sendmsg =      packet_sendmsg_spkt,
3780         .recvmsg =      packet_recvmsg,
3781         .mmap =         sock_no_mmap,
3782         .sendpage =     sock_no_sendpage,
3783 };
3784
3785 static const struct proto_ops packet_ops = {
3786         .family =       PF_PACKET,
3787         .owner =        THIS_MODULE,
3788         .release =      packet_release,
3789         .bind =         packet_bind,
3790         .connect =      sock_no_connect,
3791         .socketpair =   sock_no_socketpair,
3792         .accept =       sock_no_accept,
3793         .getname =      packet_getname,
3794         .poll =         packet_poll,
3795         .ioctl =        packet_ioctl,
3796         .listen =       sock_no_listen,
3797         .shutdown =     sock_no_shutdown,
3798         .setsockopt =   packet_setsockopt,
3799         .getsockopt =   packet_getsockopt,
3800         .sendmsg =      packet_sendmsg,
3801         .recvmsg =      packet_recvmsg,
3802         .mmap =         packet_mmap,
3803         .sendpage =     sock_no_sendpage,
3804 };
3805
3806 static const struct net_proto_family packet_family_ops = {
3807         .family =       PF_PACKET,
3808         .create =       packet_create,
3809         .owner  =       THIS_MODULE,
3810 };
3811
3812 static struct notifier_block packet_netdev_notifier = {
3813         .notifier_call =        packet_notifier,
3814 };
3815
3816 #ifdef CONFIG_PROC_FS
3817
3818 static void *packet_seq_start(struct seq_file *seq, loff_t *pos)
3819         __acquires(RCU)
3820 {
3821         struct net *net = seq_file_net(seq);
3822
3823         rcu_read_lock();
3824         return seq_hlist_start_head_rcu(&net->packet.sklist, *pos);
3825 }
3826
3827 static void *packet_seq_next(struct seq_file *seq, void *v, loff_t *pos)
3828 {
3829         struct net *net = seq_file_net(seq);
3830         return seq_hlist_next_rcu(v, &net->packet.sklist, pos);
3831 }
3832
3833 static void packet_seq_stop(struct seq_file *seq, void *v)
3834         __releases(RCU)
3835 {
3836         rcu_read_unlock();
3837 }
3838
3839 static int packet_seq_show(struct seq_file *seq, void *v)
3840 {
3841         if (v == SEQ_START_TOKEN)
3842                 seq_puts(seq, "sk       RefCnt Type Proto  Iface R Rmem   User   Inode\n");
3843         else {
3844                 struct sock *s = sk_entry(v);
3845                 const struct packet_sock *po = pkt_sk(s);
3846
3847                 seq_printf(seq,
3848                            "%pK %-6d %-4d %04x   %-5d %1d %-6u %-6u %-6lu\n",
3849                            s,
3850                            atomic_read(&s->sk_refcnt),
3851                            s->sk_type,
3852                            ntohs(po->num),
3853                            po->ifindex,
3854                            po->running,
3855                            atomic_read(&s->sk_rmem_alloc),
3856                            sock_i_uid(s),
3857                            sock_i_ino(s));
3858         }
3859
3860         return 0;
3861 }
3862
3863 static const struct seq_operations packet_seq_ops = {
3864         .start  = packet_seq_start,
3865         .next   = packet_seq_next,
3866         .stop   = packet_seq_stop,
3867         .show   = packet_seq_show,
3868 };
3869
3870 static int packet_seq_open(struct inode *inode, struct file *file)
3871 {
3872         return seq_open_net(inode, file, &packet_seq_ops,
3873                             sizeof(struct seq_net_private));
3874 }
3875
3876 static const struct file_operations packet_seq_fops = {
3877         .owner          = THIS_MODULE,
3878         .open           = packet_seq_open,
3879         .read           = seq_read,
3880         .llseek         = seq_lseek,
3881         .release        = seq_release_net,
3882 };
3883
3884 #endif
3885
3886 static int __net_init packet_net_init(struct net *net)
3887 {
3888         spin_lock_init(&net->packet.sklist_lock);
3889         INIT_HLIST_HEAD(&net->packet.sklist);
3890
3891         if (!proc_net_fops_create(net, "packet", 0, &packet_seq_fops))
3892                 return -ENOMEM;
3893
3894         return 0;
3895 }
3896
3897 static void __net_exit packet_net_exit(struct net *net)
3898 {
3899         proc_net_remove(net, "packet");
3900 }
3901
3902 static struct pernet_operations packet_net_ops = {
3903         .init = packet_net_init,
3904         .exit = packet_net_exit,
3905 };
3906
3907
3908 static void __exit packet_exit(void)
3909 {
3910         unregister_netdevice_notifier(&packet_netdev_notifier);
3911         unregister_pernet_subsys(&packet_net_ops);
3912         sock_unregister(PF_PACKET);
3913         proto_unregister(&packet_proto);
3914 }
3915
3916 static int __init packet_init(void)
3917 {
3918         int rc = proto_register(&packet_proto, 0);
3919
3920         if (rc != 0)
3921                 goto out;
3922
3923         sock_register(&packet_family_ops);
3924         register_pernet_subsys(&packet_net_ops);
3925         register_netdevice_notifier(&packet_netdev_notifier);
3926 out:
3927         return rc;
3928 }
3929
3930 module_init(packet_init);
3931 module_exit(packet_exit);
3932 MODULE_LICENSE("GPL");
3933 MODULE_ALIAS_NETPROTO(PF_PACKET);