pg.c

Linux块设备驱动源码

		p = read_reg(dev, 2) & 3;
		if (p == 0)
			pi_write_block(dev->pi, buf, n);
		if (p == 2)
			pi_read_block(dev->pi, buf, n);
		if (verbose > 1)
			printk("%s: %s %d bytes\n", dev->name,
			       p ? "Read" : "Write", n);
		dev->dlen += (1 - p) * d;
		buf += d;
		r = pg_wait(dev, STAT_BUSY, STAT_DRQ | STAT_READY | STAT_ERR,
			    tmo, "completion");
	}

	pi_disconnect(dev->pi);

	return r;
}

static int pg_reset(struct pg *dev)
{
	int i, k, err;
	int expect[5] = { 1, 1, 1, 0x14, 0xeb };
	int got[5];

	pi_connect(dev->pi);
	write_reg(dev, 6, DRIVE(dev));
	write_reg(dev, 7, 8);

	pg_sleep(20 * HZ / 1000);

	k = 0;
	while ((k++ < PG_RESET_TMO) && (status_reg(dev) & STAT_BUSY))
		pg_sleep(1);

	for (i = 0; i < 5; i++)
		got[i] = read_reg(dev, i + 1);

	err = memcmp(expect, got, sizeof(got)) ? -1 : 0;

	if (verbose) {
		printk("%s: Reset (%d) signature = ", dev->name, k);
		for (i = 0; i < 5; i++)
			printk("%3x", got[i]);
		if (err)
			printk(" (incorrect)");
		printk("\n");
	}

	pi_disconnect(dev->pi);
	return err;
}

static void xs(char *buf, char *targ, int len)
{
	char l = '\0';
	int k;

	for (k = 0; k < len; k++) {
		char c = *buf++;
		if (c != ' ' || c != l)
			l = *targ++ = c;
	}
	if (l == ' ')
		targ--;
	*targ = '\0';
}

static int pg_identify(struct pg *dev, int log)
{
	int s;
	char *ms[2] = { "master", "slave" };
	char mf[10], id[18];
	char id_cmd[12] = { ATAPI_IDENTIFY, 0, 0, 0, 36, 0, 0, 0, 0, 0, 0, 0 };
	char buf[36];

	s = pg_command(dev, id_cmd, 36, jiffies + PG_TMO);
	if (s)
		return -1;
	s = pg_completion(dev, buf, jiffies + PG_TMO);
	if (s)
		return -1;

	if (log) {
		xs(buf + 8, mf, 8);
		xs(buf + 16, id, 16);
		printk("%s: %s %s, %s\n", dev->name, mf, id, ms[dev->drive]);
	}

	return 0;
}

/*
 * returns  0, with id set if drive is detected
 *	   -1, if drive detection failed
 */
static int pg_probe(struct pg *dev)
{
	if (dev->drive == -1) {
		for (dev->drive = 0; dev->drive <= 1; dev->drive++)
			if (!pg_reset(dev))
				return pg_identify(dev, 1);
	} else {
		if (!pg_reset(dev))
			return pg_identify(dev, 1);
	}
	return -1;
}

static int pg_detect(void)
{
	struct pg *dev = &devices[0];
	int k, unit;

	printk("%s: %s version %s, major %d\n", name, name, PG_VERSION, major);

	k = 0;
	if (pg_drive_count == 0) {
		if (pi_init(dev->pi, 1, -1, -1, -1, -1, -1, pg_scratch,
			    PI_PG, verbose, dev->name)) {
			if (!pg_probe(dev)) {
				dev->present = 1;
				k++;
			} else
				pi_release(dev->pi);
		}

	} else
		for (unit = 0; unit < PG_UNITS; unit++, dev++) {
			int *parm = *drives[unit];
			if (!parm[D_PRT])
				continue;
			if (pi_init(dev->pi, 0, parm[D_PRT], parm[D_MOD],
				    parm[D_UNI], parm[D_PRO], parm[D_DLY],
				    pg_scratch, PI_PG, verbose, dev->name)) {
				if (!pg_probe(dev)) {
					dev->present = 1;
					k++;
				} else
					pi_release(dev->pi);
			}
		}

	if (k)
		return 0;

	printk("%s: No ATAPI device detected\n", name);
	return -1;
}

static int pg_open(struct inode *inode, struct file *file)
{
	int unit = iminor(inode) & 0x7f;
	struct pg *dev = &devices[unit];

	if ((unit >= PG_UNITS) || (!dev->present))
		return -ENODEV;

	if (test_and_set_bit(0, &dev->access))
		return -EBUSY;

	if (dev->busy) {
		pg_reset(dev);
		dev->busy = 0;
	}

	pg_identify(dev, (verbose > 1));

	dev->bufptr = kmalloc(PG_MAX_DATA, GFP_KERNEL);
	if (dev->bufptr == NULL) {
		clear_bit(0, &dev->access);
		printk("%s: buffer allocation failed\n", dev->name);
		return -ENOMEM;
	}

	file->private_data = dev;

	return 0;
}

static int pg_release(struct inode *inode, struct file *file)
{
	struct pg *dev = file->private_data;

	kfree(dev->bufptr);
	dev->bufptr = NULL;
	clear_bit(0, &dev->access);

	return 0;
}

static ssize_t pg_write(struct file *filp, const char __user *buf, size_t count, loff_t *ppos)
{
	struct pg *dev = filp->private_data;
	struct pg_write_hdr hdr;
	int hs = sizeof (hdr);

	if (dev->busy)
		return -EBUSY;
	if (count < hs)
		return -EINVAL;

	if (copy_from_user(&hdr, buf, hs))
		return -EFAULT;

	if (hdr.magic != PG_MAGIC)
		return -EINVAL;
	if (hdr.dlen > PG_MAX_DATA)
		return -EINVAL;
	if ((count - hs) > PG_MAX_DATA)
		return -EINVAL;

	if (hdr.func == PG_RESET) {
		if (count != hs)
			return -EINVAL;
		if (pg_reset(dev))
			return -EIO;
		return count;
	}

	if (hdr.func != PG_COMMAND)
		return -EINVAL;

	dev->start = jiffies;
	dev->timeout = hdr.timeout * HZ + HZ / 2 + jiffies;

	if (pg_command(dev, hdr.packet, hdr.dlen, jiffies + PG_TMO)) {
		if (dev->status & 0x10)
			return -ETIME;
		return -EIO;
	}

	dev->busy = 1;

	if (copy_from_user(dev->bufptr, buf + hs, count - hs))
		return -EFAULT;
	return count;
}

static ssize_t pg_read(struct file *filp, char __user *buf, size_t count, loff_t *ppos)
{
	struct pg *dev = filp->private_data;
	struct pg_read_hdr hdr;
	int hs = sizeof (hdr);
	int copy;

	if (!dev->busy)
		return -EINVAL;
	if (count < hs)
		return -EINVAL;

	dev->busy = 0;

	if (pg_completion(dev, dev->bufptr, dev->timeout))
		if (dev->status & 0x10)
			return -ETIME;

	hdr.magic = PG_MAGIC;
	hdr.dlen = dev->dlen;
	copy = 0;

	if (hdr.dlen < 0) {
		hdr.dlen = -1 * hdr.dlen;
		copy = hdr.dlen;
		if (copy > (count - hs))
			copy = count - hs;
	}

	hdr.duration = (jiffies - dev->start + HZ / 2) / HZ;
	hdr.scsi = dev->status & 0x0f;

	if (copy_to_user(buf, &hdr, hs))
		return -EFAULT;
	if (copy > 0)
		if (copy_to_user(buf + hs, dev->bufptr, copy))
			return -EFAULT;
	return copy + hs;
}

static int __init pg_init(void)
{
	int unit, err = 0;

	if (disable){
		err = -1;
		goto out;
	}

	pg_init_units();

	if (pg_detect()) {
		err = -1;
		goto out;
	}

	if (register_chrdev(major, name, &pg_fops)) {
		printk("pg_init: unable to get major number %d\n", major);
		for (unit = 0; unit < PG_UNITS; unit++) {
			struct pg *dev = &devices[unit];
			if (dev->present)
				pi_release(dev->pi);
		}
		err = -1;
		goto out;
	}
	pg_class = class_create(THIS_MODULE, "pg");
	if (IS_ERR(pg_class)) {
		err = PTR_ERR(pg_class);
		goto out_chrdev;
	}
	devfs_mk_dir("pg");
	for (unit = 0; unit < PG_UNITS; unit++) {
		struct pg *dev = &devices[unit];
		if (dev->present) {
			class_device_create(pg_class, MKDEV(major, unit),
					NULL, "pg%u", unit);
			err = devfs_mk_cdev(MKDEV(major, unit),
				      S_IFCHR | S_IRUSR | S_IWUSR, "pg/%u",
				      unit);
			if (err) 
				goto out_class;
		}
	}
	err = 0;
	goto out;

out_class:
	class_device_destroy(pg_class, MKDEV(major, unit));
	class_destroy(pg_class);
out_chrdev:
	unregister_chrdev(major, "pg");
out:
	return err;
}

static void __exit pg_exit(void)
{
	int unit;

	for (unit = 0; unit < PG_UNITS; unit++) {
		struct pg *dev = &devices[unit];
		if (dev->present) {
			class_device_destroy(pg_class, MKDEV(major, unit));
			devfs_remove("pg/%u", unit);
		}
	}
	class_destroy(pg_class);
	devfs_remove("pg");
	unregister_chrdev(major, name);

	for (unit = 0; unit < PG_UNITS; unit++) {
		struct pg *dev = &devices[unit];
		if (dev->present)
			pi_release(dev->pi);
	}
}

MODULE_LICENSE("GPL");
module_init(pg_init)
module_exit(pg_exit)