af_packet.c

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

{
	struct sock *sk = sock->sk;
	int ret;

	if (level != SOL_PACKET)
		return -ENOPROTOOPT;

	switch(optname)	{
#ifdef CONFIG_PACKET_MULTICAST
	case PACKET_ADD_MEMBERSHIP:	
	case PACKET_DROP_MEMBERSHIP:
	{
		struct packet_mreq mreq;
		if (optlen<sizeof(mreq))
			return -EINVAL;
		if (copy_from_user(&mreq,optval,sizeof(mreq)))
			return -EFAULT;
		if (optname == PACKET_ADD_MEMBERSHIP)
			ret = packet_mc_add(sk, &mreq);
		else
			ret = packet_mc_drop(sk, &mreq);
		return ret;
	}
#endif
#ifdef CONFIG_PACKET_MMAP
	case PACKET_RX_RING:
	{
		struct tpacket_req req;

		if (optlen<sizeof(req))
			return -EINVAL;
		if (copy_from_user(&req,optval,sizeof(req)))
			return -EFAULT;
		return packet_set_ring(sk, &req, 0);
	}
	case PACKET_COPY_THRESH:
	{
		int val;

		if (optlen!=sizeof(val))
			return -EINVAL;
		if (copy_from_user(&val,optval,sizeof(val)))
			return -EFAULT;

		sk->protinfo.af_packet->copy_thresh = val;
		return 0;
	}
#endif
	default:
		return -ENOPROTOOPT;
	}
}

int packet_getsockopt(struct socket *sock, int level, int optname,
		      char *optval, int *optlen)
{
	int len;
	struct sock *sk = sock->sk;

	if (level != SOL_PACKET)
		return -ENOPROTOOPT;

  	if (get_user(len,optlen))
  		return -EFAULT;

	if (len < 0)
		return -EINVAL;
		
	switch(optname)	{
	case PACKET_STATISTICS:
	{
		struct tpacket_stats st;

		if (len > sizeof(struct tpacket_stats))
			len = sizeof(struct tpacket_stats);
		spin_lock_bh(&sk->receive_queue.lock);
		st = sk->protinfo.af_packet->stats;
		memset(&sk->protinfo.af_packet->stats, 0, sizeof(st));
		spin_unlock_bh(&sk->receive_queue.lock);
		st.tp_packets += st.tp_drops;

		if (copy_to_user(optval, &st, len))
			return -EFAULT;
		break;
	}
	default:
		return -ENOPROTOOPT;
	}

  	if (put_user(len, optlen))
  		return -EFAULT;
  	return 0;
}


static int packet_notifier(struct notifier_block *this, unsigned long msg, void *data)
{
	struct sock *sk;
	struct packet_opt *po;
	struct net_device *dev = (struct net_device*)data;

	read_lock(&packet_sklist_lock);
	for (sk = packet_sklist; sk; sk = sk->next) {
		po = sk->protinfo.af_packet;

		switch (msg) {
		case NETDEV_DOWN:
		case NETDEV_UNREGISTER:
			if (dev->ifindex == po->ifindex) {
				spin_lock(&po->bind_lock);
				if (po->running) {
					dev_remove_pack(&po->prot_hook);
					__sock_put(sk);
					po->running = 0;
					sk->err = ENETDOWN;
					if (!sk->dead)
						sk->error_report(sk);
				}
				if (msg == NETDEV_UNREGISTER) {
					po->ifindex = -1;
					po->prot_hook.dev = NULL;
				}
				spin_unlock(&po->bind_lock);
			}
#ifdef CONFIG_PACKET_MULTICAST
			if (po->mclist)
				packet_dev_mclist(dev, po->mclist, -1);
#endif
			break;
		case NETDEV_UP:
			spin_lock(&po->bind_lock);
			if (dev->ifindex == po->ifindex && sk->num && po->running==0) {
				dev_add_pack(&po->prot_hook);
				sock_hold(sk);
				po->running = 1;
			}
			spin_unlock(&po->bind_lock);
#ifdef CONFIG_PACKET_MULTICAST
			if (po->mclist)
				packet_dev_mclist(dev, po->mclist, +1);
#endif
			break;
		}
	}
	read_unlock(&packet_sklist_lock);
	return NOTIFY_DONE;
}


static int packet_ioctl(struct socket *sock, unsigned int cmd,
			unsigned long arg)
{
	struct sock *sk = sock->sk;

	switch(cmd) 
	{
		case SIOCOUTQ:
		{
			int amount = atomic_read(&sk->wmem_alloc);
			return put_user(amount, (int *)arg);
		}
		case SIOCINQ:
		{
			struct sk_buff *skb;
			int amount = 0;

			spin_lock_bh(&sk->receive_queue.lock);
			skb = skb_peek(&sk->receive_queue);
			if (skb)
				amount = skb->len;
			spin_unlock_bh(&sk->receive_queue.lock);
			return put_user(amount, (int *)arg);
		}
		case FIOSETOWN:
		case SIOCSPGRP: {
			int pid;
			if (get_user(pid, (int *) arg))
				return -EFAULT; 
			if (current->pid != pid && current->pgrp != -pid && 
			    !capable(CAP_NET_ADMIN))
				return -EPERM;
			sk->proc = pid;
			break;
		}
		case FIOGETOWN:
		case SIOCGPGRP:
			return put_user(sk->proc, (int *)arg);
		case SIOCGSTAMP:
			if(sk->stamp.tv_sec==0)
				return -ENOENT;
			if (copy_to_user((void *)arg, &sk->stamp,
					 sizeof(struct timeval)))
				return -EFAULT;
			break;
		case SIOCGIFFLAGS:
#ifndef CONFIG_INET
		case SIOCSIFFLAGS:
#endif
		case SIOCGIFCONF:
		case SIOCGIFMETRIC:
		case SIOCSIFMETRIC:
		case SIOCGIFMEM:
		case SIOCSIFMEM:
		case SIOCGIFMTU:
		case SIOCSIFMTU:
		case SIOCSIFLINK:
		case SIOCGIFHWADDR:
		case SIOCSIFHWADDR:
		case SIOCSIFMAP:
		case SIOCGIFMAP:
		case SIOCSIFSLAVE:
		case SIOCGIFSLAVE:
		case SIOCGIFINDEX:
		case SIOCGIFNAME:
		case SIOCGIFCOUNT:
		case SIOCSIFHWBROADCAST:
			return(dev_ioctl(cmd,(void *) arg));

		case SIOCGIFBR:
		case SIOCSIFBR:
#if defined(CONFIG_BRIDGE) || defined(CONFIG_BRIDGE_MODULE)
#ifdef CONFIG_INET
#ifdef CONFIG_KMOD
			if (br_ioctl_hook == NULL)
				request_module("bridge");
#endif
			if (br_ioctl_hook != NULL)
				return br_ioctl_hook(arg);
#endif
#endif				
			return -ENOPKG;

		case SIOCGIFDIVERT:
		case SIOCSIFDIVERT:
#ifdef CONFIG_NET_DIVERT
			return divert_ioctl(cmd, (struct divert_cf *) arg);
#else
			return -ENOPKG;
#endif /* CONFIG_NET_DIVERT */
			
#ifdef CONFIG_INET
		case SIOCADDRT:
		case SIOCDELRT:
		case SIOCDARP:
		case SIOCGARP:
		case SIOCSARP:
		case SIOCGIFADDR:
		case SIOCSIFADDR:
		case SIOCGIFBRDADDR:
		case SIOCSIFBRDADDR:
		case SIOCGIFNETMASK:
		case SIOCSIFNETMASK:
		case SIOCGIFDSTADDR:
		case SIOCSIFDSTADDR:
		case SIOCSIFFLAGS:
		case SIOCADDDLCI:
		case SIOCDELDLCI:
			return inet_dgram_ops.ioctl(sock, cmd, arg);
#endif

		default:
			if ((cmd >= SIOCDEVPRIVATE) &&
			    (cmd <= (SIOCDEVPRIVATE + 15)))
				return(dev_ioctl(cmd,(void *) arg));

#ifdef CONFIG_NET_RADIO
			if((cmd >= SIOCIWFIRST) && (cmd <= SIOCIWLAST))
				return(dev_ioctl(cmd,(void *) arg));
#endif
			return -EOPNOTSUPP;
	}
	return 0;
}

#ifndef CONFIG_PACKET_MMAP
#define packet_mmap sock_no_mmap
#define packet_poll datagram_poll
#else

unsigned int packet_poll(struct file * file, struct socket *sock, poll_table *wait)
{
	struct sock *sk = sock->sk;
	struct packet_opt *po = sk->protinfo.af_packet;
	unsigned int mask = datagram_poll(file, sock, wait);

	spin_lock_bh(&sk->receive_queue.lock);
	if (po->iovec) {
		unsigned last = po->head ? po->head-1 : po->iovmax;

		if (po->iovec[last]->tp_status)
			mask |= POLLIN | POLLRDNORM;
	}
	spin_unlock_bh(&sk->receive_queue.lock);
	return mask;
}


/* Dirty? Well, I still did not learn better way to account
 * for user mmaps.
 */

static void packet_mm_open(struct vm_area_struct *vma)
{
	struct file *file = vma->vm_file;
	struct inode *inode = file->f_dentry->d_inode;
	struct socket * sock = &inode->u.socket_i;
	struct sock *sk = sock->sk;
	
	if (sk)
		atomic_inc(&sk->protinfo.af_packet->mapped);
}

static void packet_mm_close(struct vm_area_struct *vma)
{
	struct file *file = vma->vm_file;
	struct inode *inode = file->f_dentry->d_inode;
	struct socket * sock = &inode->u.socket_i;
	struct sock *sk = sock->sk;
	
	if (sk)
		atomic_dec(&sk->protinfo.af_packet->mapped);
}

static struct vm_operations_struct packet_mmap_ops = {
	open:	packet_mm_open,
	close:	packet_mm_close,
};

static void free_pg_vec(unsigned long *pg_vec, unsigned order, unsigned len)
{
	int i;

	for (i=0; i<len; i++) {
		if (pg_vec[i]) {
			struct page *page, *pend;

			pend = virt_to_page(pg_vec[i] + (PAGE_SIZE << order) - 1);
			for (page = virt_to_page(pg_vec[i]); page <= pend; page++)
				ClearPageReserved(page);
			free_pages(pg_vec[i], order);
		}
	}
	kfree(pg_vec);
}


static int packet_set_ring(struct sock *sk, struct tpacket_req *req, int closing)
{
	unsigned long *pg_vec = NULL;
	struct tpacket_hdr **io_vec = NULL;
	struct packet_opt *po = sk->protinfo.af_packet;
	int order = 0;
	int err = 0;

	if (req->tp_block_nr) {
		int i, l;
		int frames_per_block;

		/* Sanity tests and some calculations */
		if ((int)req->tp_block_size <= 0)
			return -EINVAL;
		if (req->tp_block_size&(PAGE_SIZE-1))
			return -EINVAL;
		if (req->tp_frame_size < TPACKET_HDRLEN)
			return -EINVAL;
		if (req->tp_frame_size&(TPACKET_ALIGNMENT-1))
			return -EINVAL;
		frames_per_block = req->tp_block_size/req->tp_frame_size;
		if (frames_per_block <= 0)
			return -EINVAL;
		if (frames_per_block*req->tp_block_nr != req->tp_frame_nr)
			return -EINVAL;
		/* OK! */

		/* Allocate page vector */
		while ((PAGE_SIZE<<order) < req->tp_block_size)
			order++;

		err = -ENOMEM;

		pg_vec = kmalloc(req->tp_block_nr*sizeof(unsigned long*), GFP_KERNEL);
		if (pg_vec == NULL)
			goto out;
		memset(pg_vec, 0, req->tp_block_nr*sizeof(unsigned long*));

		for (i=0; i<req->tp_block_nr; i++) {
			struct page *page, *pend;
			pg_vec[i] = __get_free_pages(GFP_KERNEL, order);
			if (!pg_vec[i])
				goto out_free_pgvec;

			pend = virt_to_page(pg_vec[i] + (PAGE_SIZE << order) - 1);
			for (page = virt_to_page(pg_vec[i]); page <= pend; page++)
				SetPageReserved(page);
		}
		/* Page vector is allocated */

		/* Draw frames */
		io_vec = kmalloc(req->tp_frame_nr*sizeof(struct tpacket_hdr*), GFP_KERNEL);
		if (io_vec == NULL)
			goto out_free_pgvec;
		memset(io_vec, 0, req->tp_frame_nr*sizeof(struct tpacket_hdr*));

		l = 0;
		for (i=0; i<req->tp_block_nr; i++) {
			unsigned long ptr = pg_vec[i];
			int k;

			for (k=0; k<frames_per_block; k++, l++) {
				io_vec[l] = (struct tpacket_hdr*)ptr;
				io_vec[l]->tp_status = TP_STATUS_KERNEL;
				ptr += req->tp_frame_size;
			}
		}
		/* Done */
	} else {
		if (req->tp_frame_nr)
			return -EINVAL;
	}

	lock_sock(sk);

	/* Detach socket from network */
	spin_lock(&po->bind_lock);
	if (po->running)
		dev_remove_pack(&po->prot_hook);
	spin_unlock(&po->bind_lock);

	err = -EBUSY;
	if (closing || atomic_read(&po->mapped) == 0) {
		err = 0;
#define XC(a, b) ({ __typeof__ ((a)) __t; __t = (a); (a) = (b); __t; })

		spin_lock_bh(&sk->receive_queue.lock);
		pg_vec = XC(po->pg_vec, pg_vec);
		io_vec = XC(po->iovec, io_vec);
		po->iovmax = req->tp_frame_nr-1;
		po->head = 0;
		po->frame_size = req->tp_frame_size;
		spin_unlock_bh(&sk->receive_queue.lock);

		order = XC(po->pg_vec_order, order);
		req->tp_block_nr = XC(po->pg_vec_len, req->tp_block_nr);

		po->pg_vec_pages = req->tp_block_size/PAGE_SIZE;
		po->prot_hook.func = po->iovec ? tpacket_rcv : packet_rcv;
		skb_queue_purge(&sk->receive_queue);
#undef XC
		if (atomic_read(&po->mapped))
			printk(KERN_DEBUG "packet_mmap: vma is busy: %d\n", atomic_read(&po->mapped));
	}

	spin_lock(&po->bind_lock);
	if (po->running)
		dev_add_pack(&po->prot_hook);
	spin_unlock(&po->bind_lock);

	release_sock(sk);

	if (io_vec)
		kfree(io_vec);

out_free_pgvec:
	if (pg_vec)
		free_pg_vec(pg_vec, order, req->tp_block_nr);
out:
	return err;
}

static int packet_mmap(struct file *file, struct socket *sock, struct vm_area_struct *vma)
{
	struct sock *sk = sock->sk;
	struct packet_opt *po = sk->protinfo.af_packet;
	unsigned long size;
	unsigned long start;
	int err = -EINVAL;
	int i;

	if (vma->vm_pgoff)
		return -EINVAL;

	size = vma->vm_end - vma->vm_start;

	lock_sock(sk);
	if (po->pg_vec == NULL)
		goto out;
	if (size != po->pg_vec_len*po->pg_vec_pages*PAGE_SIZE)
		goto out;

	atomic_inc(&po->mapped);
	start = vma->vm_start;
	err = -EAGAIN;
	for (i=0; i<po->pg_vec_len; i++) {
		if (remap_page_range(start, __pa(po->pg_vec[i]),
				     po->pg_vec_pages*PAGE_SIZE,
				     vma->vm_page_prot))
			goto out;
		start += po->pg_vec_pages*PAGE_SIZE;
	}
	vma->vm_ops = &packet_mmap_ops;
	err = 0;

out:
	release_sock(sk);
	return err;
}
#endif


#ifdef CONFIG_SOCK_PACKET
struct proto_ops packet_ops_spkt = {
	family:		PF_PACKET,

	release:	packet_release,
	bind:		packet_bind_spkt,
	connect:	sock_no_connect,
	socketpair:	sock_no_socketpair,
	accept:		sock_no_accept,
	getname:	packet_getname_spkt,
	poll:		datagram_poll,
	ioctl:		packet_ioctl,
	listen:		sock_no_listen,
	shutdown:	sock_no_shutdown,
	setsockopt:	sock_no_setsockopt,
	getsockopt:	sock_no_getsockopt,
	sendmsg:	packet_sendmsg_spkt,
	recvmsg:	packet_recvmsg,
	mmap:		sock_no_mmap,
	sendpage:	sock_no_sendpage,
};
#endif

struct proto_ops packet_ops = {
	family:		PF_PACKET,

	release:	packet_release,
	bind:		packet_bind,
	connect:	sock_no_connect,
	socketpair:	sock_no_socketpair,
	accept:		sock_no_accept,
	getname:	packet_getname, 
	poll:		packet_poll,
	ioctl:		packet_ioctl,
	listen:		sock_no_listen,
	shutdown:	sock_no_shutdown,
	setsockopt:	packet_setsockopt,
	getsockopt:	packet_getsockopt,
	sendmsg:	packet_sendmsg,
	recvmsg:	packet_recvmsg,
	mmap:		packet_mmap,
	sendpage:	sock_no_sendpage,
};

static struct net_proto_family packet_family_ops = {
	family:		PF_PACKET,
	create:		packet_create,
};

static struct notifier_block packet_netdev_notifier = {
	notifier_call:	packet_notifier,
};

#ifdef CONFIG_PROC_FS
static int packet_read_proc(char *buffer, char **start, off_t offset,
			     int length, int *eof, void *data)
{
	off_t pos=0;
	off_t begin=0;
	int len=0;
	struct sock *s;
	
	len+= sprintf(buffer,"sk       RefCnt Type Proto  Iface R Rmem   User   Inode\n");

	read_lock(&packet_sklist_lock);

	for (s = packet_sklist; s; s = s->next) {
		len+=sprintf(buffer+len,"%p %-6d %-4d %04x   %-5d %1d %-6u %-6u %-6lu",
			     s,
			     atomic_read(&s->refcnt),
			     s->type,
			     ntohs(s->num),
			     s->protinfo.af_packet->ifindex,
			     s->protinfo.af_packet->running,
			     atomic_read(&s->rmem_alloc),
			     sock_i_uid(s),
			     sock_i_ino(s)
			     );

		buffer[len++]='\n';
		
		pos=begin+len;
		if(pos<offset) {
			len=0;
			begin=pos;
		}
		if(pos>offset+length)
			goto done;
	}
	*eof = 1;

done:
	read_unlock(&packet_sklist_lock);
	*start=buffer+(offset-begin);
	len-=(offset-begin);
	if(len>length)
		len=length;
	if(len<0)
		len=0;
	return len;
}
#endif

static void __exit packet_exit(void)
{
	remove_proc_entry("net/packet", 0);
	unregister_netdevice_notifier(&packet_netdev_notifier);
	sock_unregister(PF_PACKET);
	return;
}

static int __init packet_init(void)
{
	sock_register(&packet_family_ops);
	register_netdevice_notifier(&packet_netdev_notifier);
#ifdef CONFIG_PROC_FS
	create_proc_read_entry("net/packet", 0, 0, packet_read_proc, NULL);
#endif
	return 0;
}

module_init(packet_init);
module_exit(packet_exit);
MODULE_LICENSE("GPL");