mon_bin.c

linux 内核源代码

		return -EFAULT;
	}

	step_len = min(ep->len_cap, nbytes);
	if ((offset = rp->b_out + PKT_SIZE) >= rp->b_size) offset = 0;

	if (copy_from_buf(rp, offset, data, step_len)) {
		mutex_unlock(&rp->fetch_lock);
		return -EFAULT;
	}

	spin_lock_irqsave(&rp->b_lock, flags);
	mon_buff_area_free(rp, PKT_SIZE + ep->len_cap);
	spin_unlock_irqrestore(&rp->b_lock, flags);
	rp->b_read = 0;

	mutex_unlock(&rp->fetch_lock);
	return 0;
}

static int mon_bin_release(struct inode *inode, struct file *file)
{
	struct mon_reader_bin *rp = file->private_data;
	struct mon_bus* mbus = rp->r.m_bus;

	mutex_lock(&mon_lock);

	if (mbus->nreaders <= 0) {
		printk(KERN_ERR TAG ": consistency error on close\n");
		mutex_unlock(&mon_lock);
		return 0;
	}
	mon_reader_del(mbus, &rp->r);

	mon_free_buff(rp->b_vec, rp->b_size/CHUNK_SIZE);
	kfree(rp->b_vec);
	kfree(rp);

	mutex_unlock(&mon_lock);
	return 0;
}

static ssize_t mon_bin_read(struct file *file, char __user *buf,
    size_t nbytes, loff_t *ppos)
{
	struct mon_reader_bin *rp = file->private_data;
	unsigned long flags;
	struct mon_bin_hdr *ep;
	unsigned int offset;
	size_t step_len;
	char *ptr;
	ssize_t done = 0;
	int rc;

	mutex_lock(&rp->fetch_lock);

	if ((rc = mon_bin_wait_event(file, rp)) < 0) {
		mutex_unlock(&rp->fetch_lock);
		return rc;
	}

	ep = MON_OFF2HDR(rp, rp->b_out);

	if (rp->b_read < sizeof(struct mon_bin_hdr)) {
		step_len = min(nbytes, sizeof(struct mon_bin_hdr) - rp->b_read);
		ptr = ((char *)ep) + rp->b_read;
		if (step_len && copy_to_user(buf, ptr, step_len)) {
			mutex_unlock(&rp->fetch_lock);
			return -EFAULT;
		}
		nbytes -= step_len;
		buf += step_len;
		rp->b_read += step_len;
		done += step_len;
	}

	if (rp->b_read >= sizeof(struct mon_bin_hdr)) {
		step_len = min(nbytes, (size_t)ep->len_cap);
		offset = rp->b_out + PKT_SIZE;
		offset += rp->b_read - sizeof(struct mon_bin_hdr);
		if (offset >= rp->b_size)
			offset -= rp->b_size;
		if (copy_from_buf(rp, offset, buf, step_len)) {
			mutex_unlock(&rp->fetch_lock);
			return -EFAULT;
		}
		nbytes -= step_len;
		buf += step_len;
		rp->b_read += step_len;
		done += step_len;
	}

	/*
	 * Check if whole packet was read, and if so, jump to the next one.
	 */
	if (rp->b_read >= sizeof(struct mon_bin_hdr) + ep->len_cap) {
		spin_lock_irqsave(&rp->b_lock, flags);
		mon_buff_area_free(rp, PKT_SIZE + ep->len_cap);
		spin_unlock_irqrestore(&rp->b_lock, flags);
		rp->b_read = 0;
	}

	mutex_unlock(&rp->fetch_lock);
	return done;
}

/*
 * Remove at most nevents from chunked buffer.
 * Returns the number of removed events.
 */
static int mon_bin_flush(struct mon_reader_bin *rp, unsigned nevents)
{
	unsigned long flags;
	struct mon_bin_hdr *ep;
	int i;

	mutex_lock(&rp->fetch_lock);
	spin_lock_irqsave(&rp->b_lock, flags);
	for (i = 0; i < nevents; ++i) {
		if (MON_RING_EMPTY(rp))
			break;

		ep = MON_OFF2HDR(rp, rp->b_out);
		mon_buff_area_free(rp, PKT_SIZE + ep->len_cap);
	}
	spin_unlock_irqrestore(&rp->b_lock, flags);
	rp->b_read = 0;
	mutex_unlock(&rp->fetch_lock);
	return i;
}

/*
 * Fetch at most max event offsets into the buffer and put them into vec.
 * The events are usually freed later with mon_bin_flush.
 * Return the effective number of events fetched.
 */
static int mon_bin_fetch(struct file *file, struct mon_reader_bin *rp,
    u32 __user *vec, unsigned int max)
{
	unsigned int cur_out;
	unsigned int bytes, avail;
	unsigned int size;
	unsigned int nevents;
	struct mon_bin_hdr *ep;
	unsigned long flags;
	int rc;

	mutex_lock(&rp->fetch_lock);

	if ((rc = mon_bin_wait_event(file, rp)) < 0) {
		mutex_unlock(&rp->fetch_lock);
		return rc;
	}

	spin_lock_irqsave(&rp->b_lock, flags);
	avail = rp->b_cnt;
	spin_unlock_irqrestore(&rp->b_lock, flags);

	cur_out = rp->b_out;
	nevents = 0;
	bytes = 0;
	while (bytes < avail) {
		if (nevents >= max)
			break;

		ep = MON_OFF2HDR(rp, cur_out);
		if (put_user(cur_out, &vec[nevents])) {
			mutex_unlock(&rp->fetch_lock);
			return -EFAULT;
		}

		nevents++;
		size = ep->len_cap + PKT_SIZE;
		size = (size + PKT_ALIGN-1) & ~(PKT_ALIGN-1);
		if ((cur_out += size) >= rp->b_size)
			cur_out -= rp->b_size;
		bytes += size;
	}

	mutex_unlock(&rp->fetch_lock);
	return nevents;
}

/*
 * Count events. This is almost the same as the above mon_bin_fetch,
 * only we do not store offsets into user vector, and we have no limit.
 */
static int mon_bin_queued(struct mon_reader_bin *rp)
{
	unsigned int cur_out;
	unsigned int bytes, avail;
	unsigned int size;
	unsigned int nevents;
	struct mon_bin_hdr *ep;
	unsigned long flags;

	mutex_lock(&rp->fetch_lock);

	spin_lock_irqsave(&rp->b_lock, flags);
	avail = rp->b_cnt;
	spin_unlock_irqrestore(&rp->b_lock, flags);

	cur_out = rp->b_out;
	nevents = 0;
	bytes = 0;
	while (bytes < avail) {
		ep = MON_OFF2HDR(rp, cur_out);

		nevents++;
		size = ep->len_cap + PKT_SIZE;
		size = (size + PKT_ALIGN-1) & ~(PKT_ALIGN-1);
		if ((cur_out += size) >= rp->b_size)
			cur_out -= rp->b_size;
		bytes += size;
	}

	mutex_unlock(&rp->fetch_lock);
	return nevents;
}

/*
 */
static int mon_bin_ioctl(struct inode *inode, struct file *file,
    unsigned int cmd, unsigned long arg)
{
	struct mon_reader_bin *rp = file->private_data;
	// struct mon_bus* mbus = rp->r.m_bus;
	int ret = 0;
	struct mon_bin_hdr *ep;
	unsigned long flags;

	switch (cmd) {

	case MON_IOCQ_URB_LEN:
		/*
		 * N.B. This only returns the size of data, without the header.
		 */
		spin_lock_irqsave(&rp->b_lock, flags);
		if (!MON_RING_EMPTY(rp)) {
			ep = MON_OFF2HDR(rp, rp->b_out);
			ret = ep->len_cap;
		}
		spin_unlock_irqrestore(&rp->b_lock, flags);
		break;

	case MON_IOCQ_RING_SIZE:
		ret = rp->b_size;
		break;

	case MON_IOCT_RING_SIZE:
		/*
		 * Changing the buffer size will flush it's contents; the new
		 * buffer is allocated before releasing the old one to be sure
		 * the device will stay functional also in case of memory
		 * pressure.
		 */
		{
		int size;
		struct mon_pgmap *vec;

		if (arg < BUFF_MIN || arg > BUFF_MAX)
			return -EINVAL;

		size = CHUNK_ALIGN(arg);
		if ((vec = kzalloc(sizeof(struct mon_pgmap) * (size/CHUNK_SIZE),
		    GFP_KERNEL)) == NULL) {
			ret = -ENOMEM;
			break;
		}

		ret = mon_alloc_buff(vec, size/CHUNK_SIZE);
		if (ret < 0) {
			kfree(vec);
			break;
		}

		mutex_lock(&rp->fetch_lock);
		spin_lock_irqsave(&rp->b_lock, flags);
		mon_free_buff(rp->b_vec, size/CHUNK_SIZE);
		kfree(rp->b_vec);
		rp->b_vec  = vec;
		rp->b_size = size;
		rp->b_read = rp->b_in = rp->b_out = rp->b_cnt = 0;
		rp->cnt_lost = 0;
		spin_unlock_irqrestore(&rp->b_lock, flags);
		mutex_unlock(&rp->fetch_lock);
		}
		break;

	case MON_IOCH_MFLUSH:
		ret = mon_bin_flush(rp, arg);
		break;

	case MON_IOCX_GET:
		{
		struct mon_bin_get getb;

		if (copy_from_user(&getb, (void __user *)arg,
					    sizeof(struct mon_bin_get)))
			return -EFAULT;

		if (getb.alloc > 0x10000000)	/* Want to cast to u32 */
			return -EINVAL;
		ret = mon_bin_get_event(file, rp,
			  getb.hdr, getb.data, (unsigned int)getb.alloc);
		}
		break;

#ifdef CONFIG_COMPAT
	case MON_IOCX_GET32: {
		struct mon_bin_get32 getb;

		if (copy_from_user(&getb, (void __user *)arg,
					    sizeof(struct mon_bin_get32)))
			return -EFAULT;

		ret = mon_bin_get_event(file, rp,
		    compat_ptr(getb.hdr32), compat_ptr(getb.data32),
		    getb.alloc32);
		}
		break;
#endif

	case MON_IOCX_MFETCH:
		{
		struct mon_bin_mfetch mfetch;
		struct mon_bin_mfetch __user *uptr;

		uptr = (struct mon_bin_mfetch __user *)arg;

		if (copy_from_user(&mfetch, uptr, sizeof(mfetch)))
			return -EFAULT;

		if (mfetch.nflush) {
			ret = mon_bin_flush(rp, mfetch.nflush);
			if (ret < 0)
				return ret;
			if (put_user(ret, &uptr->nflush))
				return -EFAULT;
		}
		ret = mon_bin_fetch(file, rp, mfetch.offvec, mfetch.nfetch);
		if (ret < 0)
			return ret;
		if (put_user(ret, &uptr->nfetch))
			return -EFAULT;
		ret = 0;
		}
		break;

#ifdef CONFIG_COMPAT
	case MON_IOCX_MFETCH32:
		{
		struct mon_bin_mfetch32 mfetch;
		struct mon_bin_mfetch32 __user *uptr;

		uptr = (struct mon_bin_mfetch32 __user *) compat_ptr(arg);

		if (copy_from_user(&mfetch, uptr, sizeof(mfetch)))
			return -EFAULT;

		if (mfetch.nflush32) {
			ret = mon_bin_flush(rp, mfetch.nflush32);
			if (ret < 0)
				return ret;
			if (put_user(ret, &uptr->nflush32))
				return -EFAULT;
		}
		ret = mon_bin_fetch(file, rp, compat_ptr(mfetch.offvec32),
		    mfetch.nfetch32);
		if (ret < 0)
			return ret;
		if (put_user(ret, &uptr->nfetch32))
			return -EFAULT;
		ret = 0;
		}
		break;
#endif

	case MON_IOCG_STATS: {
		struct mon_bin_stats __user *sp;
		unsigned int nevents;
		unsigned int ndropped;

		spin_lock_irqsave(&rp->b_lock, flags);
		ndropped = rp->cnt_lost;
		rp->cnt_lost = 0;
		spin_unlock_irqrestore(&rp->b_lock, flags);
		nevents = mon_bin_queued(rp);

		sp = (struct mon_bin_stats __user *)arg;
		if (put_user(rp->cnt_lost, &sp->dropped))
			return -EFAULT;
		if (put_user(nevents, &sp->queued))
			return -EFAULT;

		}
		break;

	default:
		return -ENOTTY;
	}

	return ret;
}

static unsigned int
mon_bin_poll(struct file *file, struct poll_table_struct *wait)
{
	struct mon_reader_bin *rp = file->private_data;
	unsigned int mask = 0;
	unsigned long flags;

	if (file->f_mode & FMODE_READ)
		poll_wait(file, &rp->b_wait, wait);

	spin_lock_irqsave(&rp->b_lock, flags);
	if (!MON_RING_EMPTY(rp))
		mask |= POLLIN | POLLRDNORM;    /* readable */
	spin_unlock_irqrestore(&rp->b_lock, flags);
	return mask;
}

/*
 * open and close: just keep track of how many times the device is
 * mapped, to use the proper memory allocation function.
 */
static void mon_bin_vma_open(struct vm_area_struct *vma)
{
	struct mon_reader_bin *rp = vma->vm_private_data;
	rp->mmap_active++;
}

static void mon_bin_vma_close(struct vm_area_struct *vma)
{
	struct mon_reader_bin *rp = vma->vm_private_data;
	rp->mmap_active--;
}

/*
 * Map ring pages to user space.
 */
struct page *mon_bin_vma_nopage(struct vm_area_struct *vma,
                                unsigned long address, int *type)
{
	struct mon_reader_bin *rp = vma->vm_private_data;
	unsigned long offset, chunk_idx;
	struct page *pageptr;

	offset = (address - vma->vm_start) + (vma->vm_pgoff << PAGE_SHIFT);
	if (offset >= rp->b_size)
		return NOPAGE_SIGBUS;
	chunk_idx = offset / CHUNK_SIZE;
	pageptr = rp->b_vec[chunk_idx].pg;
	get_page(pageptr);
	if (type)
		*type = VM_FAULT_MINOR;
	return pageptr;
}

struct vm_operations_struct mon_bin_vm_ops = {
	.open =     mon_bin_vma_open,
	.close =    mon_bin_vma_close,
	.nopage =   mon_bin_vma_nopage,
};

int mon_bin_mmap(struct file *filp, struct vm_area_struct *vma)
{
	/* don't do anything here: "nopage" will set up page table entries */
	vma->vm_ops = &mon_bin_vm_ops;
	vma->vm_flags |= VM_RESERVED;
	vma->vm_private_data = filp->private_data;
	mon_bin_vma_open(vma);
	return 0;
}

struct file_operations mon_fops_binary = {
	.owner =	THIS_MODULE,
	.open =		mon_bin_open,
	.llseek =	no_llseek,
	.read =		mon_bin_read,
	/* .write =	mon_text_write, */
	.poll =		mon_bin_poll,
	.ioctl =	mon_bin_ioctl,
	.release =	mon_bin_release,
};

static int mon_bin_wait_event(struct file *file, struct mon_reader_bin *rp)
{
	DECLARE_WAITQUEUE(waita, current);
	unsigned long flags;

	add_wait_queue(&rp->b_wait, &waita);
	set_current_state(TASK_INTERRUPTIBLE);

	spin_lock_irqsave(&rp->b_lock, flags);
	while (MON_RING_EMPTY(rp)) {
		spin_unlock_irqrestore(&rp->b_lock, flags);

		if (file->f_flags & O_NONBLOCK) {
			set_current_state(TASK_RUNNING);
			remove_wait_queue(&rp->b_wait, &waita);
			return -EWOULDBLOCK; /* Same as EAGAIN in Linux */
		}
		schedule();
		if (signal_pending(current)) {
			remove_wait_queue(&rp->b_wait, &waita);
			return -EINTR;
		}
		set_current_state(TASK_INTERRUPTIBLE);

		spin_lock_irqsave(&rp->b_lock, flags);
	}
	spin_unlock_irqrestore(&rp->b_lock, flags);

	set_current_state(TASK_RUNNING);
	remove_wait_queue(&rp->b_wait, &waita);
	return 0;
}

static int mon_alloc_buff(struct mon_pgmap *map, int npages)
{
	int n;
	unsigned long vaddr;

	for (n = 0; n < npages; n++) {
		vaddr = get_zeroed_page(GFP_KERNEL);
		if (vaddr == 0) {
			while (n-- != 0)
				free_page((unsigned long) map[n].ptr);
			return -ENOMEM;
		}
		map[n].ptr = (unsigned char *) vaddr;
		map[n].pg = virt_to_page(vaddr);
	}
	return 0;
}

static void mon_free_buff(struct mon_pgmap *map, int npages)
{
	int n;

	for (n = 0; n < npages; n++)
		free_page((unsigned long) map[n].ptr);
}

int mon_bin_add(struct mon_bus *mbus, const struct usb_bus *ubus)
{
	struct device *dev;
	unsigned minor = ubus? ubus->busnum: 0;

	if (minor >= MON_BIN_MAX_MINOR)
		return 0;

	dev = device_create(mon_bin_class, ubus? ubus->controller: NULL,
			MKDEV(MAJOR(mon_bin_dev0), minor), "usbmon%d", minor);
	if (IS_ERR(dev))
		return 0;

	mbus->classdev = dev;
	return 1;
}

void mon_bin_del(struct mon_bus *mbus)
{
	device_destroy(mon_bin_class, mbus->classdev->devt);
}

int __init mon_bin_init(void)
{
	int rc;

	mon_bin_class = class_create(THIS_MODULE, "usbmon");
	if (IS_ERR(mon_bin_class)) {
		rc = PTR_ERR(mon_bin_class);
		goto err_class;
	}

	rc = alloc_chrdev_region(&mon_bin_dev0, 0, MON_BIN_MAX_MINOR, "usbmon");
	if (rc < 0)
		goto err_dev;

	cdev_init(&mon_bin_cdev, &mon_fops_binary);
	mon_bin_cdev.owner = THIS_MODULE;

	rc = cdev_add(&mon_bin_cdev, mon_bin_dev0, MON_BIN_MAX_MINOR);
	if (rc < 0)
		goto err_add;

	return 0;

err_add:
	unregister_chrdev_region(mon_bin_dev0, MON_BIN_MAX_MINOR);
err_dev:
	class_destroy(mon_bin_class);
err_class:
	return rc;
}

void mon_bin_exit(void)
{
	cdev_del(&mon_bin_cdev);
	unregister_chrdev_region(mon_bin_dev0, MON_BIN_MAX_MINOR);
	class_destroy(mon_bin_class);
}