af_packet.c

来自「Linux Kernel 2.6.9 for OMAP1710」· C语言 代码 · 共 1,901 行 · 第 1/3 页

		}
	}
	rtnl_unlock();
	return -EADDRNOTAVAIL;
}

static void packet_flush_mclist(struct sock *sk)
{
	struct packet_opt *po = pkt_sk(sk);
	struct packet_mclist *ml;

	if (!po->mclist)
		return;

	rtnl_lock();
	while ((ml = po->mclist) != NULL) {
		struct net_device *dev;

		po->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 __user *optval, int optlen)
{
	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;

		pkt_sk(sk)->copy_thresh = val;
		return 0;
	}
#endif
	default:
		return -ENOPROTOOPT;
	}
}

int packet_getsockopt(struct socket *sock, int level, int optname,
		      char __user *optval, int __user *optlen)
{
	int len;
	struct sock *sk = sock->sk;
	struct packet_opt *po = pkt_sk(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->sk_receive_queue.lock);
		st = po->stats;
		memset(&po->stats, 0, sizeof(st));
		spin_unlock_bh(&sk->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 hlist_node *node;
	struct net_device *dev = (struct net_device*)data;

	read_lock(&packet_sklist_lock);
	sk_for_each(sk, node, &packet_sklist) {
		struct packet_opt *po = pkt_sk(sk);

		switch (msg) {
		case NETDEV_UNREGISTER:
#ifdef CONFIG_PACKET_MULTICAST
			if (po->mclist)
				packet_dev_mclist(dev, po->mclist, -1);
			// fallthrough
#endif
		case NETDEV_DOWN:
			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->sk_err = ENETDOWN;
					if (!sock_flag(sk, SOCK_DEAD))
						sk->sk_error_report(sk);
				}
				if (msg == NETDEV_UNREGISTER) {
					po->ifindex = -1;
					po->prot_hook.dev = NULL;
				}
				spin_unlock(&po->bind_lock);
			}
			break;
		case NETDEV_UP:
			spin_lock(&po->bind_lock);
			if (dev->ifindex == po->ifindex && po->num &&
			    !po->running) {
				dev_add_pack(&po->prot_hook);
				sock_hold(sk);
				po->running = 1;
			}
			spin_unlock(&po->bind_lock);
			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->sk_wmem_alloc);
			return put_user(amount, (int __user *)arg);
		}
		case SIOCINQ:
		{
			struct sk_buff *skb;
			int amount = 0;

			spin_lock_bh(&sk->sk_receive_queue.lock);
			skb = skb_peek(&sk->sk_receive_queue);
			if (skb)
				amount = skb->len;
			spin_unlock_bh(&sk->sk_receive_queue.lock);
			return put_user(amount, (int __user *)arg);
		}
		case SIOCGSTAMP:
			return sock_get_timestamp(sk, (struct timeval __user *)arg);
			
#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:
			return inet_dgram_ops.ioctl(sock, cmd, arg);
#endif

		default:
			return dev_ioctl(cmd, (void __user *)arg);
	}
	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 = pkt_sk(sk);
	unsigned int mask = datagram_poll(file, sock, wait);

	spin_lock_bh(&sk->sk_receive_queue.lock);
	if (po->pg_vec) {
		unsigned last = po->head ? po->head-1 : po->frame_max;
		struct tpacket_hdr *h;

		h = (struct tpacket_hdr *)packet_lookup_frame(po, last);

		if (h->tp_status)
			mask |= POLLIN | POLLRDNORM;
	}
	spin_unlock_bh(&sk->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 = SOCKET_I(inode);
	struct sock *sk = sock->sk;
	
	if (sk)
		atomic_inc(&pkt_sk(sk)->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 = SOCKET_I(inode);
	struct sock *sk = sock->sk;
	
	if (sk)
		atomic_dec(&pkt_sk(sk)->mapped);
}

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

static inline struct page *pg_vec_endpage(char *one_pg_vec, unsigned int order)
{
	return virt_to_page(one_pg_vec + (PAGE_SIZE << order) - 1);
}

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

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

			pend = pg_vec_endpage(pg_vec[i], order);
			for (page = virt_to_page(pg_vec[i]); page <= pend; page++)
				ClearPageReserved(page);
			free_pages((unsigned long)pg_vec[i], order);
		}
	}
	kfree(pg_vec);
}


static int packet_set_ring(struct sock *sk, struct tpacket_req *req, int closing)
{
	char **pg_vec = NULL;
	struct packet_opt *po = pkt_sk(sk);
	int was_running, num, order = 0;
	int err = 0;
	
	if (req->tp_block_nr) {
		int i, l;

		/* Sanity tests and some calculations */

		if (po->pg_vec)
			return -EBUSY;

		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;

		po->frames_per_block = req->tp_block_size/req->tp_frame_size;
		if (po->frames_per_block <= 0)
			return -EINVAL;
		if (po->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(char *), GFP_KERNEL);
		if (pg_vec == NULL)
			goto out;
		memset(pg_vec, 0, req->tp_block_nr*sizeof(char **));

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

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

		l = 0;
		for (i=0; i<req->tp_block_nr; i++) {
			char *ptr = pg_vec[i];
			struct tpacket_hdr *header;
			int k;

			for (k=0; k<po->frames_per_block; k++) {
				
				header = (struct tpacket_hdr*)ptr;
				header->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);
	was_running = po->running;
	num = po->num;
	if (was_running) {
		__dev_remove_pack(&po->prot_hook);
		po->num = 0;
		po->running = 0;
		__sock_put(sk);
	}
	spin_unlock(&po->bind_lock);
		
	synchronize_net();

	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->sk_receive_queue.lock);
		pg_vec = XC(po->pg_vec, pg_vec);
		po->frame_max = req->tp_frame_nr-1;
		po->head = 0;
		po->frame_size = req->tp_frame_size;
		spin_unlock_bh(&sk->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->pg_vec ? tpacket_rcv : packet_rcv;
		skb_queue_purge(&sk->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 (was_running && !po->running) {
		sock_hold(sk);
		po->running = 1;
		po->num = num;
		dev_add_pack(&po->prot_hook);
	}
	spin_unlock(&po->bind_lock);

	release_sock(sk);

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 = pkt_sk(sk);
	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(vma, 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,
	.owner =	THIS_MODULE,
	.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,
	.owner =	THIS_MODULE,
	.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,
	.owner	=	THIS_MODULE,
};

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

#ifdef CONFIG_PROC_FS
static inline struct sock *packet_seq_idx(loff_t off)
{
	struct sock *s;
	struct hlist_node *node;

	sk_for_each(s, node, &packet_sklist) {
		if (!off--)
			return s;
	}
	return NULL;
}

static void *packet_seq_start(struct seq_file *seq, loff_t *pos)
{
	read_lock(&packet_sklist_lock);
	return *pos ? packet_seq_idx(*pos - 1) : SEQ_START_TOKEN;
}

static void *packet_seq_next(struct seq_file *seq, void *v, loff_t *pos)
{
	++*pos;
	return  (v == SEQ_START_TOKEN) 
		? sk_head(&packet_sklist) 
		: sk_next((struct sock*)v) ;
}

static void packet_seq_stop(struct seq_file *seq, void *v)
{
	read_unlock(&packet_sklist_lock);		
}

static int packet_seq_show(struct seq_file *seq, void *v) 
{
	if (v == SEQ_START_TOKEN)
		seq_puts(seq, "sk       RefCnt Type Proto  Iface R Rmem   User   Inode\n");
	else {
		struct sock *s = v;
		const struct packet_opt *po = pkt_sk(s);

		seq_printf(seq,
			   "%p %-6d %-4d %04x   %-5d %1d %-6u %-6u %-6lu\n",
			   s,
			   atomic_read(&s->sk_refcnt),
			   s->sk_type,
			   ntohs(po->num),
			   po->ifindex,
			   po->running,
			   atomic_read(&s->sk_rmem_alloc),
			   sock_i_uid(s),
			   sock_i_ino(s) );
	}

	return 0;
}

static struct seq_operations packet_seq_ops = {
	.start	= packet_seq_start,
	.next	= packet_seq_next,
	.stop	= packet_seq_stop,
	.show	= packet_seq_show,
};

static int packet_seq_open(struct inode *inode, struct file *file)
{
	return seq_open(file, &packet_seq_ops);
}

static struct file_operations packet_seq_fops = {
	.owner		= THIS_MODULE,
	.open		= packet_seq_open,
	.read		= seq_read,
	.llseek		= seq_lseek,
	.release	= seq_release,
};

#endif

static void __exit packet_exit(void)
{
	proc_net_remove("packet");
	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);
	proc_net_fops_create("packet", 0, &packet_seq_fops);

	return 0;
}

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