af_packet.c

嵌入式系统设计与实验教材二源码linux内核移植与编译

	}

	sk->data_ready(sk, 0);

drop_n_restore:
	if (skb_head != skb->data && skb_shared(skb)) {
		skb->data = skb_head;
		skb->len = skb_len;
	}
drop:
        kfree_skb(skb);
	return 0;

ring_is_full:
	po->stats.tp_drops++;
	spin_unlock(&sk->receive_queue.lock);

	sk->data_ready(sk, 0);
	if (copy_skb)
		kfree_skb(copy_skb);
	goto drop_n_restore;
}

#endif


static int packet_sendmsg(struct socket *sock, struct msghdr *msg, int len,
			  struct scm_cookie *scm)
{
	struct sock *sk = sock->sk;
	struct sockaddr_ll *saddr=(struct sockaddr_ll *)msg->msg_name;
	struct sk_buff *skb;
	struct net_device *dev;
	unsigned short proto;
	unsigned char *addr;
	int ifindex, err, reserve = 0;

	/*
	 *	Get and verify the address. 
	 */
	 
	if (saddr == NULL) {
		ifindex	= sk->protinfo.af_packet->ifindex;
		proto	= sk->num;
		addr	= NULL;
	} else {
		err = -EINVAL;
		if (msg->msg_namelen < sizeof(struct sockaddr_ll))
			goto out;
		ifindex	= saddr->sll_ifindex;
		proto	= saddr->sll_protocol;
		addr	= saddr->sll_addr;
	}


	dev = dev_get_by_index(ifindex);
	err = -ENXIO;
	if (dev == NULL)
		goto out_unlock;
	if (sock->type == SOCK_RAW)
		reserve = dev->hard_header_len;

	err = -EMSGSIZE;
	if (len > dev->mtu+reserve)
		goto out_unlock;

	skb = sock_alloc_send_skb(sk, len+dev->hard_header_len+15, 
				msg->msg_flags & MSG_DONTWAIT, &err);
	if (skb==NULL)
		goto out_unlock;

	skb_reserve(skb, (dev->hard_header_len+15)&~15);
	skb->nh.raw = skb->data;

	if (dev->hard_header) {
		int res;
		err = -EINVAL;
		res = dev->hard_header(skb, dev, ntohs(proto), addr, NULL, len);
		if (sock->type != SOCK_DGRAM) {
			skb->tail = skb->data;
			skb->len = 0;
		} else if (res < 0)
			goto out_free;
	}

	/* Returns -EFAULT on error */
	err = memcpy_fromiovec(skb_put(skb,len), msg->msg_iov, len);
	if (err)
		goto out_free;

	skb->protocol = proto;
	skb->dev = dev;
	skb->priority = sk->priority;

	err = -ENETDOWN;
	if (!(dev->flags & IFF_UP))
		goto out_free;

	/*
	 *	Now send it
	 */

	err = dev_queue_xmit(skb);
	if (err > 0 && (err = net_xmit_errno(err)) != 0)
		goto out_unlock;

	dev_put(dev);

	return(len);

out_free:
	kfree_skb(skb);
out_unlock:
	if (dev)
		dev_put(dev);
out:
	return err;
}

/*
 *	Close a PACKET socket. This is fairly simple. We immediately go
 *	to 'closed' state and remove our protocol entry in the device list.
 */

static int packet_release(struct socket *sock)
{
	struct sock *sk = sock->sk;
	struct sock **skp;

	if (!sk)
		return 0;

	write_lock_bh(&packet_sklist_lock);
	for (skp = &packet_sklist; *skp; skp = &(*skp)->next) {
		if (*skp == sk) {
			*skp = sk->next;
			__sock_put(sk);
			break;
		}
	}
	write_unlock_bh(&packet_sklist_lock);

	/*
	 *	Unhook packet receive handler.
	 */

	if (sk->protinfo.af_packet->running) {
		/*
		 *	Remove the protocol hook
		 */
		dev_remove_pack(&sk->protinfo.af_packet->prot_hook);
		sk->protinfo.af_packet->running = 0;
		__sock_put(sk);
	}

#ifdef CONFIG_PACKET_MULTICAST
	packet_flush_mclist(sk);
#endif

#ifdef CONFIG_PACKET_MMAP
	if (sk->protinfo.af_packet->pg_vec) {
		struct tpacket_req req;
		memset(&req, 0, sizeof(req));
		packet_set_ring(sk, &req, 1);
	}
#endif

	/*
	 *	Now the socket is dead. No more input will appear.
	 */

	sock_orphan(sk);
	sock->sk = NULL;

	/* Purge queues */

	skb_queue_purge(&sk->receive_queue);

	sock_put(sk);
	return 0;
}

/*
 *	Attach a packet hook.
 */

static int packet_do_bind(struct sock *sk, struct net_device *dev, int protocol)
{
	/*
	 *	Detach an existing hook if present.
	 */

	lock_sock(sk);

	spin_lock(&sk->protinfo.af_packet->bind_lock);
	if (sk->protinfo.af_packet->running) {
		dev_remove_pack(&sk->protinfo.af_packet->prot_hook);
		__sock_put(sk);
		sk->protinfo.af_packet->running = 0;
	}

	sk->num = protocol;
	sk->protinfo.af_packet->prot_hook.type = protocol;
	sk->protinfo.af_packet->prot_hook.dev = dev;

	sk->protinfo.af_packet->ifindex = dev ? dev->ifindex : 0;

	if (protocol == 0)
		goto out_unlock;

	if (dev) {
		if (dev->flags&IFF_UP) {
			dev_add_pack(&sk->protinfo.af_packet->prot_hook);
			sock_hold(sk);
			sk->protinfo.af_packet->running = 1;
		} else {
			sk->err = ENETDOWN;
			if (!sk->dead)
				sk->error_report(sk);
		}
	} else {
		dev_add_pack(&sk->protinfo.af_packet->prot_hook);
		sock_hold(sk);
		sk->protinfo.af_packet->running = 1;
	}

out_unlock:
	spin_unlock(&sk->protinfo.af_packet->bind_lock);
	release_sock(sk);
	return 0;
}

/*
 *	Bind a packet socket to a device
 */

#ifdef CONFIG_SOCK_PACKET

static int packet_bind_spkt(struct socket *sock, struct sockaddr *uaddr, int addr_len)
{
	struct sock *sk=sock->sk;
	char name[15];
	struct net_device *dev;
	int err = -ENODEV;
	
	/*
	 *	Check legality
	 */
	 
	if(addr_len!=sizeof(struct sockaddr))
		return -EINVAL;
	strncpy(name,uaddr->sa_data,14);
	name[14]=0;

	dev = dev_get_by_name(name);
	if (dev) {
		err = packet_do_bind(sk, dev, sk->num);
		dev_put(dev);
	}
	return err;
}
#endif

static int packet_bind(struct socket *sock, struct sockaddr *uaddr, int addr_len)
{
	struct sockaddr_ll *sll = (struct sockaddr_ll*)uaddr;
	struct sock *sk=sock->sk;
	struct net_device *dev = NULL;
	int err;


	/*
	 *	Check legality
	 */
	 
	if (addr_len < sizeof(struct sockaddr_ll))
		return -EINVAL;
	if (sll->sll_family != AF_PACKET)
		return -EINVAL;

	if (sll->sll_ifindex) {
		err = -ENODEV;
		dev = dev_get_by_index(sll->sll_ifindex);
		if (dev == NULL)
			goto out;
	}
	err = packet_do_bind(sk, dev, sll->sll_protocol ? : sk->num);
	if (dev)
		dev_put(dev);

out:
	return err;
}


/*
 *	Create a packet of type SOCK_PACKET. 
 */

static int packet_create(struct socket *sock, int protocol)
{
	struct sock *sk;
	int err;

	if (!capable(CAP_NET_RAW))
		return -EPERM;
	if (sock->type != SOCK_DGRAM && sock->type != SOCK_RAW
#ifdef CONFIG_SOCK_PACKET
	    && sock->type != SOCK_PACKET
#endif
	    )
		return -ESOCKTNOSUPPORT;

	sock->state = SS_UNCONNECTED;
	MOD_INC_USE_COUNT;

	err = -ENOBUFS;
	sk = sk_alloc(PF_PACKET, GFP_KERNEL, 1);
	if (sk == NULL)
		goto out;

	sock->ops = &packet_ops;
#ifdef CONFIG_SOCK_PACKET
	if (sock->type == SOCK_PACKET)
		sock->ops = &packet_ops_spkt;
#endif
	sock_init_data(sock,sk);

	sk->protinfo.af_packet = kmalloc(sizeof(struct packet_opt), GFP_KERNEL);
	if (sk->protinfo.af_packet == NULL)
		goto out_free;
	memset(sk->protinfo.af_packet, 0, sizeof(struct packet_opt));
	sk->family = PF_PACKET;
	sk->num = protocol;

	sk->destruct = packet_sock_destruct;
	atomic_inc(&packet_socks_nr);

	/*
	 *	Attach a protocol block
	 */

	spin_lock_init(&sk->protinfo.af_packet->bind_lock);
	sk->protinfo.af_packet->prot_hook.func = packet_rcv;
#ifdef CONFIG_SOCK_PACKET
	if (sock->type == SOCK_PACKET)
		sk->protinfo.af_packet->prot_hook.func = packet_rcv_spkt;
#endif
	sk->protinfo.af_packet->prot_hook.data = (void *)sk;

	if (protocol) {
		sk->protinfo.af_packet->prot_hook.type = protocol;
		dev_add_pack(&sk->protinfo.af_packet->prot_hook);
		sock_hold(sk);
		sk->protinfo.af_packet->running = 1;
	}

	write_lock_bh(&packet_sklist_lock);
	sk->next = packet_sklist;
	packet_sklist = sk;
	sock_hold(sk);
	write_unlock_bh(&packet_sklist_lock);
	return(0);

out_free:
	sk_free(sk);
out:
	MOD_DEC_USE_COUNT;
	return err;
}

/*
 *	Pull a packet from our receive queue and hand it to the user.
 *	If necessary we block.
 */

static int packet_recvmsg(struct socket *sock, struct msghdr *msg, int len,
			  int flags, struct scm_cookie *scm)
{
	struct sock *sk = sock->sk;
	struct sk_buff *skb;
	int copied, err;

	err = -EINVAL;
	if (flags & ~(MSG_PEEK|MSG_DONTWAIT|MSG_TRUNC))
		goto out;

#if 0
	/* What error should we return now? EUNATTACH? */
	if (sk->protinfo.af_packet->ifindex < 0)
		return -ENODEV;
#endif

	/*
	 *	If the address length field is there to be filled in, we fill
	 *	it in now.
	 */

	if (sock->type == SOCK_PACKET)
		msg->msg_namelen = sizeof(struct sockaddr_pkt);
	else
		msg->msg_namelen = sizeof(struct sockaddr_ll);

	/*
	 *	Call the generic datagram receiver. This handles all sorts
	 *	of horrible races and re-entrancy so we can forget about it
	 *	in the protocol layers.
	 *
	 *	Now it will return ENETDOWN, if device have just gone down,
	 *	but then it will block.
	 */

	skb=skb_recv_datagram(sk,flags,flags&MSG_DONTWAIT,&err);

	/*
	 *	An error occurred so return it. Because skb_recv_datagram() 
	 *	handles the blocking we don't see and worry about blocking
	 *	retries.
	 */

	if(skb==NULL)
		goto out;

	/*
	 *	You lose any data beyond the buffer you gave. If it worries a
	 *	user program they can ask the device for its MTU anyway.
	 */

	copied = skb->len;
	if (copied > len)
	{
		copied=len;
		msg->msg_flags|=MSG_TRUNC;
	}

	err = skb_copy_datagram_iovec(skb, 0, msg->msg_iov, copied);
	if (err)
		goto out_free;

	sock_recv_timestamp(msg, sk, skb);

	if (msg->msg_name)
		memcpy(msg->msg_name, skb->cb, msg->msg_namelen);

	/*
	 *	Free or return the buffer as appropriate. Again this
	 *	hides all the races and re-entrancy issues from us.
	 */
	err = (flags&MSG_TRUNC) ? skb->len : copied;

out_free:
	skb_free_datagram(sk, skb);
out:
	return err;
}

#ifdef CONFIG_SOCK_PACKET
static int packet_getname_spkt(struct socket *sock, struct sockaddr *uaddr,
			       int *uaddr_len, int peer)
{
	struct net_device *dev;
	struct sock *sk	= sock->sk;

	if (peer)
		return -EOPNOTSUPP;

	uaddr->sa_family = AF_PACKET;
	dev = dev_get_by_index(sk->protinfo.af_packet->ifindex);
	if (dev) {
		strncpy(uaddr->sa_data, dev->name, 15);
		dev_put(dev);
	} else
		memset(uaddr->sa_data, 0, 14);
	*uaddr_len = sizeof(*uaddr);

	return 0;
}
#endif

static int packet_getname(struct socket *sock, struct sockaddr *uaddr,
			  int *uaddr_len, int peer)
{
	struct net_device *dev;
	struct sock *sk = sock->sk;
	struct sockaddr_ll *sll = (struct sockaddr_ll*)uaddr;

	if (peer)
		return -EOPNOTSUPP;

	sll->sll_family = AF_PACKET;
	sll->sll_ifindex = sk->protinfo.af_packet->ifindex;
	sll->sll_protocol = sk->num;
	dev = dev_get_by_index(sk->protinfo.af_packet->ifindex);
	if (dev) {
		sll->sll_hatype = dev->type;
		sll->sll_halen = dev->addr_len;
		memcpy(sll->sll_addr, dev->dev_addr, dev->addr_len);
		dev_put(dev);
	} else {
		sll->sll_hatype = 0;	/* Bad: we have no ARPHRD_UNSPEC */
		sll->sll_halen = 0;
	}
	*uaddr_len = sizeof(*sll);

	return 0;
}

#ifdef CONFIG_PACKET_MULTICAST
static void packet_dev_mc(struct net_device *dev, struct packet_mclist *i, int what)
{
	switch (i->type) {
	case PACKET_MR_MULTICAST:
		if (what > 0)
			dev_mc_add(dev, i->addr, i->alen, 0);
		else
			dev_mc_delete(dev, i->addr, i->alen, 0);
		break;
	case PACKET_MR_PROMISC:
		dev_set_promiscuity(dev, what);
		break;
	case PACKET_MR_ALLMULTI:
		dev_set_allmulti(dev, what);
		break;
	default:;
	}
}

static void packet_dev_mclist(struct net_device *dev, struct packet_mclist *i, int what)
{
	for ( ; i; i=i->next) {
		if (i->ifindex == dev->ifindex)
			packet_dev_mc(dev, i, what);
	}
}

static int packet_mc_add(struct sock *sk, struct packet_mreq *mreq)
{
	struct packet_mclist *ml, *i;
	struct net_device *dev;
	int err;

	rtnl_lock();

	err = -ENODEV;
	dev = __dev_get_by_index(mreq->mr_ifindex);
	if (!dev)
		goto done;

	err = -EINVAL;
	if (mreq->mr_alen > dev->addr_len)
		goto done;

	err = -ENOBUFS;
	i = (struct packet_mclist *)kmalloc(sizeof(*i), GFP_KERNEL);
	if (i == NULL)
		goto done;

	err = 0;
	for (ml=sk->protinfo.af_packet->mclist; ml; ml=ml->next) {
		if (ml->ifindex == mreq->mr_ifindex &&
		    ml->type == mreq->mr_type &&
		    ml->alen == mreq->mr_alen &&
		    memcmp(ml->addr, mreq->mr_address, ml->alen) == 0) {
			ml->count++;
			/* Free the new element ... */
			kfree(i);
			goto done;
		}
	}

	i->type = mreq->mr_type;
	i->ifindex = mreq->mr_ifindex;
	i->alen = mreq->mr_alen;
	memcpy(i->addr, mreq->mr_address, i->alen);
	i->count = 1;
	i->next = sk->protinfo.af_packet->mclist;
	sk->protinfo.af_packet->mclist = i;
	packet_dev_mc(dev, i, +1);

done:
	rtnl_unlock();
	return err;
}

static int packet_mc_drop(struct sock *sk, struct packet_mreq *mreq)
{
	struct packet_mclist *ml, **mlp;

	rtnl_lock();

	for (mlp=&sk->protinfo.af_packet->mclist; (ml=*mlp)!=NULL; mlp=&ml->next) {
		if (ml->ifindex == mreq->mr_ifindex &&
		    ml->type == mreq->mr_type &&
		    ml->alen == mreq->mr_alen &&
		    memcmp(ml->addr, mreq->mr_address, ml->alen) == 0) {
			if (--ml->count == 0) {
				struct net_device *dev;
				*mlp = ml->next;
				dev = dev_get_by_index(ml->ifindex);
				if (dev) {
					packet_dev_mc(dev, ml, -1);
					dev_put(dev);
				}
				kfree(ml);
			}
			rtnl_unlock();
			return 0;
		}
	}
	rtnl_unlock();
	return -EADDRNOTAVAIL;
}

static void packet_flush_mclist(struct sock *sk)
{
	struct packet_mclist *ml;

	if (sk->protinfo.af_packet->mclist == NULL)
		return;

	rtnl_lock();
	while ((ml=sk->protinfo.af_packet->mclist) != NULL) {
		struct net_device *dev;
		sk->protinfo.af_packet->mclist = ml->next;
		if ((dev = dev_get_by_index(ml->ifindex)) != NULL) {
			packet_dev_mc(dev, ml, -1);
			dev_put(dev);
		}
		kfree(ml);
	}
	rtnl_unlock();
}
#endif

static int
packet_setsockopt(struct socket *sock, int level, int optname, char *optval, int optlen)