+ if (likely(po->tx_ring.pg_vec)) {
+ ph = skb_shinfo(skb)->destructor_arg;
+ BUG_ON(__packet_get_status(po, ph) != TP_STATUS_SENDING);
+ BUG_ON(atomic_read(&po->tx_ring.pending) == 0);
+ atomic_dec(&po->tx_ring.pending);
+ __packet_set_status(po, ph, TP_STATUS_AVAILABLE);
+ }
+
+ sock_wfree(skb);
+}
+
+static int tpacket_fill_skb(struct packet_sock *po, struct sk_buff *skb,
+ void *frame, struct net_device *dev, int size_max,
+ __be16 proto, unsigned char *addr)
+{
+ union {
+ struct tpacket_hdr *h1;
+ struct tpacket2_hdr *h2;
+ void *raw;
+ } ph;
+ int to_write, offset, len, tp_len, nr_frags, len_max;
+ struct socket *sock = po->sk.sk_socket;
+ struct page *page;
+ void *data;
+ int err;
+
+ ph.raw = frame;
+
+ skb->protocol = proto;
+ skb->dev = dev;
+ skb->priority = po->sk.sk_priority;
+ skb->mark = po->sk.sk_mark;
+ skb_shinfo(skb)->destructor_arg = ph.raw;
+
+ switch (po->tp_version) {
+ case TPACKET_V2:
+ tp_len = ph.h2->tp_len;
+ break;
+ default:
+ tp_len = ph.h1->tp_len;
+ break;
+ }
+ if (unlikely(tp_len > size_max)) {
+ pr_err("packet size is too long (%d > %d)\n", tp_len, size_max);
+ return -EMSGSIZE;
+ }
+
+ skb_reserve(skb, LL_RESERVED_SPACE(dev));
+ skb_reset_network_header(skb);
+
+ data = ph.raw + po->tp_hdrlen - sizeof(struct sockaddr_ll);
+ to_write = tp_len;
+
+ if (sock->type == SOCK_DGRAM) {
+ err = dev_hard_header(skb, dev, ntohs(proto), addr,
+ NULL, tp_len);
+ if (unlikely(err < 0))
+ return -EINVAL;
+ } else if (dev->hard_header_len) {
+ /* net device doesn't like empty head */
+ if (unlikely(tp_len <= dev->hard_header_len)) {
+ pr_err("packet size is too short (%d < %d)\n",
+ tp_len, dev->hard_header_len);
+ return -EINVAL;
+ }
+
+ skb_push(skb, dev->hard_header_len);
+ err = skb_store_bits(skb, 0, data,
+ dev->hard_header_len);
+ if (unlikely(err))
+ return err;
+
+ data += dev->hard_header_len;
+ to_write -= dev->hard_header_len;
+ }
+
+ err = -EFAULT;
+ page = virt_to_page(data);
+ offset = offset_in_page(data);
+ len_max = PAGE_SIZE - offset;
+ len = ((to_write > len_max) ? len_max : to_write);
+
+ skb->data_len = to_write;
+ skb->len += to_write;
+ skb->truesize += to_write;
+ atomic_add(to_write, &po->sk.sk_wmem_alloc);
+
+ while (likely(to_write)) {
+ nr_frags = skb_shinfo(skb)->nr_frags;
+
+ if (unlikely(nr_frags >= MAX_SKB_FRAGS)) {
+ pr_err("Packet exceed the number of skb frags(%lu)\n",
+ MAX_SKB_FRAGS);
+ return -EFAULT;
+ }
+
+ flush_dcache_page(page);
+ get_page(page);
+ skb_fill_page_desc(skb,
+ nr_frags,
+ page++, offset, len);
+ to_write -= len;
+ offset = 0;
+ len_max = PAGE_SIZE;
+ len = ((to_write > len_max) ? len_max : to_write);
+ }
+
+ return tp_len;
+}
+
+static int tpacket_snd(struct packet_sock *po, struct msghdr *msg)
+{
+ struct socket *sock;
+ struct sk_buff *skb;
+ struct net_device *dev;
+ __be16 proto;
+ int ifindex, err, reserve = 0;
+ void *ph;
+ struct sockaddr_ll *saddr = (struct sockaddr_ll *)msg->msg_name;
+ int tp_len, size_max;
+ unsigned char *addr;
+ int len_sum = 0;
+ int status = 0;
+
+ sock = po->sk.sk_socket;
+
+ mutex_lock(&po->pg_vec_lock);
+
+ err = -EBUSY;
+ if (saddr == NULL) {
+ ifindex = po->ifindex;
+ proto = po->num;
+ addr = NULL;
+ } else {
+ err = -EINVAL;
+ if (msg->msg_namelen < sizeof(struct sockaddr_ll))
+ goto out;
+ if (msg->msg_namelen < (saddr->sll_halen
+ + offsetof(struct sockaddr_ll,
+ sll_addr)))
+ goto out;
+ ifindex = saddr->sll_ifindex;
+ proto = saddr->sll_protocol;
+ addr = saddr->sll_addr;
+ }
+
+ dev = dev_get_by_index(sock_net(&po->sk), ifindex);
+ err = -ENXIO;
+ if (unlikely(dev == NULL))
+ goto out;
+
+ reserve = dev->hard_header_len;
+
+ err = -ENETDOWN;
+ if (unlikely(!(dev->flags & IFF_UP)))
+ goto out_put;
+
+ size_max = po->tx_ring.frame_size
+ - (po->tp_hdrlen - sizeof(struct sockaddr_ll));
+
+ if (size_max > dev->mtu + reserve)
+ size_max = dev->mtu + reserve;
+
+ do {
+ ph = packet_current_frame(po, &po->tx_ring,
+ TP_STATUS_SEND_REQUEST);
+
+ if (unlikely(ph == NULL)) {
+ schedule();
+ continue;
+ }
+
+ status = TP_STATUS_SEND_REQUEST;
+ skb = sock_alloc_send_skb(&po->sk,
+ LL_ALLOCATED_SPACE(dev)
+ + sizeof(struct sockaddr_ll),
+ 0, &err);
+
+ if (unlikely(skb == NULL))
+ goto out_status;
+
+ tp_len = tpacket_fill_skb(po, skb, ph, dev, size_max, proto,
+ addr);
+
+ if (unlikely(tp_len < 0)) {
+ if (po->tp_loss) {
+ __packet_set_status(po, ph,
+ TP_STATUS_AVAILABLE);
+ packet_increment_head(&po->tx_ring);
+ kfree_skb(skb);
+ continue;
+ } else {
+ status = TP_STATUS_WRONG_FORMAT;
+ err = tp_len;
+ goto out_status;
+ }
+ }
+
+ skb->destructor = tpacket_destruct_skb;
+ __packet_set_status(po, ph, TP_STATUS_SENDING);
+ atomic_inc(&po->tx_ring.pending);
+
+ status = TP_STATUS_SEND_REQUEST;
+ err = dev_queue_xmit(skb);
+ if (unlikely(err > 0 && (err = net_xmit_errno(err)) != 0))
+ goto out_xmit;
+ packet_increment_head(&po->tx_ring);
+ len_sum += tp_len;
+ } while (likely((ph != NULL) ||
+ ((!(msg->msg_flags & MSG_DONTWAIT)) &&
+ (atomic_read(&po->tx_ring.pending))))
+ );
+
+ err = len_sum;
+ goto out_put;
+
+out_xmit:
+ skb->destructor = sock_wfree;
+ atomic_dec(&po->tx_ring.pending);
+out_status:
+ __packet_set_status(po, ph, status);
+ kfree_skb(skb);
+out_put:
+ dev_put(dev);
+out:
+ mutex_unlock(&po->pg_vec_lock);
+ return err;
+}
+#endif
+
+static int packet_snd(struct socket *sock,