swim3.c

Linux块设备驱动源码

		out_8(&sw->select, RELAX);
		fs->state = locating;
		act(fs);
		return;
	}
	out_8(&sw->select, RELAX);
	if (fs->settle_time < 2*HZ) {
		++fs->settle_time;
		set_timeout(fs, 1, settle_timeout);
		return;
	}
	printk(KERN_ERR "swim3: seek settle timeout\n");
	end_request(fd_req, 0);
	fs->state = idle;
	start_request(fs);
}

static void xfer_timeout(unsigned long data)
{
	struct floppy_state *fs = (struct floppy_state *) data;
	struct swim3 __iomem *sw = fs->swim3;
	struct dbdma_regs __iomem *dr = fs->dma;
	struct dbdma_cmd *cp = fs->dma_cmd;
	unsigned long s;
	int n;

	fs->timeout_pending = 0;
	out_le32(&dr->control, RUN << 16);
	/* We must wait a bit for dbdma to stop */
	for (n = 0; (in_le32(&dr->status) & ACTIVE) && n < 1000; n++)
		udelay(1);
	out_8(&sw->intr_enable, 0);
	out_8(&sw->control_bic, WRITE_SECTORS | DO_ACTION);
	out_8(&sw->select, RELAX);
	if (rq_data_dir(fd_req) == WRITE)
		++cp;
	if (ld_le16(&cp->xfer_status) != 0)
		s = fs->scount - ((ld_le16(&cp->res_count) + 511) >> 9);
	else
		s = 0;
	fd_req->sector += s;
	fd_req->current_nr_sectors -= s;
	printk(KERN_ERR "swim3: timeout %sing sector %ld\n",
	       (rq_data_dir(fd_req)==WRITE? "writ": "read"), (long)fd_req->sector);
	end_request(fd_req, 0);
	fs->state = idle;
	start_request(fs);
}

static irqreturn_t swim3_interrupt(int irq, void *dev_id, struct pt_regs *regs)
{
	struct floppy_state *fs = (struct floppy_state *) dev_id;
	struct swim3 __iomem *sw = fs->swim3;
	int intr, err, n;
	int stat, resid;
	struct dbdma_regs __iomem *dr;
	struct dbdma_cmd *cp;

	intr = in_8(&sw->intr);
	err = (intr & ERROR_INTR)? in_8(&sw->error): 0;
	if ((intr & ERROR_INTR) && fs->state != do_transfer)
		printk(KERN_ERR "swim3_interrupt, state=%d, dir=%lx, intr=%x, err=%x\n",
		       fs->state, rq_data_dir(fd_req), intr, err);
	switch (fs->state) {
	case locating:
		if (intr & SEEN_SECTOR) {
			out_8(&sw->control_bic, DO_ACTION | WRITE_SECTORS);
			out_8(&sw->select, RELAX);
			out_8(&sw->intr_enable, 0);
			del_timer(&fs->timeout);
			fs->timeout_pending = 0;
			if (sw->ctrack == 0xff) {
				printk(KERN_ERR "swim3: seen sector but cyl=ff?\n");
				fs->cur_cyl = -1;
				if (fs->retries > 5) {
					end_request(fd_req, 0);
					fs->state = idle;
					start_request(fs);
				} else {
					fs->state = jogging;
					act(fs);
				}
				break;
			}
			fs->cur_cyl = sw->ctrack;
			fs->cur_sector = sw->csect;
			if (fs->expect_cyl != -1 && fs->expect_cyl != fs->cur_cyl)
				printk(KERN_ERR "swim3: expected cyl %d, got %d\n",
				       fs->expect_cyl, fs->cur_cyl);
			fs->state = do_transfer;
			act(fs);
		}
		break;
	case seeking:
	case jogging:
		if (sw->nseek == 0) {
			out_8(&sw->control_bic, DO_SEEK);
			out_8(&sw->select, RELAX);
			out_8(&sw->intr_enable, 0);
			del_timer(&fs->timeout);
			fs->timeout_pending = 0;
			if (fs->state == seeking)
				++fs->retries;
			fs->state = settling;
			act(fs);
		}
		break;
	case settling:
		out_8(&sw->intr_enable, 0);
		del_timer(&fs->timeout);
		fs->timeout_pending = 0;
		act(fs);
		break;
	case do_transfer:
		if ((intr & (ERROR_INTR | TRANSFER_DONE)) == 0)
			break;
		out_8(&sw->intr_enable, 0);
		out_8(&sw->control_bic, WRITE_SECTORS | DO_ACTION);
		out_8(&sw->select, RELAX);
		del_timer(&fs->timeout);
		fs->timeout_pending = 0;
		dr = fs->dma;
		cp = fs->dma_cmd;
		if (rq_data_dir(fd_req) == WRITE)
			++cp;
		/*
		 * Check that the main data transfer has finished.
		 * On writing, the swim3 sometimes doesn't use
		 * up all the bytes of the postamble, so we can still
		 * see DMA active here.  That doesn't matter as long
		 * as all the sector data has been transferred.
		 */
		if ((intr & ERROR_INTR) == 0 && cp->xfer_status == 0) {
			/* wait a little while for DMA to complete */
			for (n = 0; n < 100; ++n) {
				if (cp->xfer_status != 0)
					break;
				udelay(1);
				barrier();
			}
		}
		/* turn off DMA */
		out_le32(&dr->control, (RUN | PAUSE) << 16);
		stat = ld_le16(&cp->xfer_status);
		resid = ld_le16(&cp->res_count);
		if (intr & ERROR_INTR) {
			n = fs->scount - 1 - resid / 512;
			if (n > 0) {
				fd_req->sector += n;
				fd_req->current_nr_sectors -= n;
				fd_req->buffer += n * 512;
				fs->req_sector += n;
			}
			if (fs->retries < 5) {
				++fs->retries;
				act(fs);
			} else {
				printk("swim3: error %sing block %ld (err=%x)\n",
				       rq_data_dir(fd_req) == WRITE? "writ": "read",
				       (long)fd_req->sector, err);
				end_request(fd_req, 0);
				fs->state = idle;
			}
		} else {
			if ((stat & ACTIVE) == 0 || resid != 0) {
				/* musta been an error */
				printk(KERN_ERR "swim3: fd dma: stat=%x resid=%d\n", stat, resid);
				printk(KERN_ERR "  state=%d, dir=%lx, intr=%x, err=%x\n",
				       fs->state, rq_data_dir(fd_req), intr, err);
				end_request(fd_req, 0);
				fs->state = idle;
				start_request(fs);
				break;
			}
			fd_req->sector += fs->scount;
			fd_req->current_nr_sectors -= fs->scount;
			fd_req->buffer += fs->scount * 512;
			if (fd_req->current_nr_sectors <= 0) {
				end_request(fd_req, 1);
				fs->state = idle;
			} else {
				fs->req_sector += fs->scount;
				if (fs->req_sector > fs->secpertrack) {
					fs->req_sector -= fs->secpertrack;
					if (++fs->head > 1) {
						fs->head = 0;
						++fs->req_cyl;
					}
				}
				act(fs);
			}
		}
		if (fs->state == idle)
			start_request(fs);
		break;
	default:
		printk(KERN_ERR "swim3: don't know what to do in state %d\n", fs->state);
	}
	return IRQ_HANDLED;
}

/*
static void fd_dma_interrupt(int irq, void *dev_id, struct pt_regs *regs)
{
}
*/

static int grab_drive(struct floppy_state *fs, enum swim_state state,
		      int interruptible)
{
	unsigned long flags;

	save_flags(flags);
	cli();
	if (fs->state != idle) {
		++fs->wanted;
		while (fs->state != available) {
			if (interruptible && signal_pending(current)) {
				--fs->wanted;
				restore_flags(flags);
				return -EINTR;
			}
			interruptible_sleep_on(&fs->wait);
		}
		--fs->wanted;
	}
	fs->state = state;
	restore_flags(flags);
	return 0;
}

static void release_drive(struct floppy_state *fs)
{
	unsigned long flags;

	save_flags(flags);
	cli();
	fs->state = idle;
	start_request(fs);
	restore_flags(flags);
}

static int fd_eject(struct floppy_state *fs)
{
	int err, n;

	err = grab_drive(fs, ejecting, 1);
	if (err)
		return err;
	swim3_action(fs, EJECT);
	for (n = 20; n > 0; --n) {
		if (signal_pending(current)) {
			err = -EINTR;
			break;
		}
		swim3_select(fs, RELAX);
		schedule_timeout_interruptible(1);
		if (swim3_readbit(fs, DISK_IN) == 0)
			break;
	}
	swim3_select(fs, RELAX);
	udelay(150);
	fs->ejected = 1;
	release_drive(fs);
	return err;
}

static struct floppy_struct floppy_type =
	{ 2880,18,2,80,0,0x1B,0x00,0xCF,0x6C,NULL };	/*  7 1.44MB 3.5"   */

static int floppy_ioctl(struct inode *inode, struct file *filp,
			unsigned int cmd, unsigned long param)
{
	struct floppy_state *fs = inode->i_bdev->bd_disk->private_data;
	int err;
		
	if ((cmd & 0x80) && !capable(CAP_SYS_ADMIN))
		return -EPERM;

#ifdef CONFIG_PMAC_MEDIABAY
	if (fs->media_bay && check_media_bay(fs->media_bay, MB_FD))
		return -ENXIO;
#endif

	switch (cmd) {
	case FDEJECT:
		if (fs->ref_count != 1)
			return -EBUSY;
		err = fd_eject(fs);
		return err;
	case FDGETPRM:
	        if (copy_to_user((void __user *) param, &floppy_type,
				 sizeof(struct floppy_struct)))
			return -EFAULT;
		return 0;
	}
	return -ENOTTY;
}

static int floppy_open(struct inode *inode, struct file *filp)
{
	struct floppy_state *fs = inode->i_bdev->bd_disk->private_data;
	struct swim3 __iomem *sw = fs->swim3;
	int n, err = 0;

	if (fs->ref_count == 0) {
#ifdef CONFIG_PMAC_MEDIABAY
		if (fs->media_bay && check_media_bay(fs->media_bay, MB_FD))
			return -ENXIO;
#endif
		out_8(&sw->setup, S_IBM_DRIVE | S_FCLK_DIV2);
		out_8(&sw->control_bic, 0xff);
		out_8(&sw->mode, 0x95);
		udelay(10);
		out_8(&sw->intr_enable, 0);
		out_8(&sw->control_bis, DRIVE_ENABLE | INTR_ENABLE);
		swim3_action(fs, MOTOR_ON);
		fs->write_prot = -1;
		fs->cur_cyl = -1;
		for (n = 0; n < 2 * HZ; ++n) {
			if (n >= HZ/30 && swim3_readbit(fs, SEEK_COMPLETE))
				break;
			if (signal_pending(current)) {
				err = -EINTR;
				break;
			}
			swim3_select(fs, RELAX);
			schedule_timeout_interruptible(1);
		}
		if (err == 0 && (swim3_readbit(fs, SEEK_COMPLETE) == 0
				 || swim3_readbit(fs, DISK_IN) == 0))
			err = -ENXIO;
		swim3_action(fs, SETMFM);
		swim3_select(fs, RELAX);

	} else if (fs->ref_count == -1 || filp->f_flags & O_EXCL)
		return -EBUSY;

	if (err == 0 && (filp->f_flags & O_NDELAY) == 0
	    && (filp->f_mode & 3)) {
		check_disk_change(inode->i_bdev);
		if (fs->ejected)
			err = -ENXIO;
	}

	if (err == 0 && (filp->f_mode & 2)) {
		if (fs->write_prot < 0)
			fs->write_prot = swim3_readbit(fs, WRITE_PROT);
		if (fs->write_prot)
			err = -EROFS;
	}

	if (err) {
		if (fs->ref_count == 0) {
			swim3_action(fs, MOTOR_OFF);
			out_8(&sw->control_bic, DRIVE_ENABLE | INTR_ENABLE);
			swim3_select(fs, RELAX);
		}
		return err;
	}

	if (filp->f_flags & O_EXCL)
		fs->ref_count = -1;
	else
		++fs->ref_count;

	return 0;
}

static int floppy_release(struct inode *inode, struct file *filp)
{
	struct floppy_state *fs = inode->i_bdev->bd_disk->private_data;
	struct swim3 __iomem *sw = fs->swim3;
	if (fs->ref_count > 0 && --fs->ref_count == 0) {
		swim3_action(fs, MOTOR_OFF);
		out_8(&sw->control_bic, 0xff);
		swim3_select(fs, RELAX);
	}
	return 0;
}

static int floppy_check_change(struct gendisk *disk)
{
	struct floppy_state *fs = disk->private_data;
	return fs->ejected;
}

static int floppy_revalidate(struct gendisk *disk)
{
	struct floppy_state *fs = disk->private_data;
	struct swim3 __iomem *sw;
	int ret, n;

#ifdef CONFIG_PMAC_MEDIABAY
	if (fs->media_bay && check_media_bay(fs->media_bay, MB_FD))
		return -ENXIO;
#endif

	sw = fs->swim3;
	grab_drive(fs, revalidating, 0);
	out_8(&sw->intr_enable, 0);
	out_8(&sw->control_bis, DRIVE_ENABLE);
	swim3_action(fs, MOTOR_ON);	/* necessary? */
	fs->write_prot = -1;
	fs->cur_cyl = -1;
	mdelay(1);
	for (n = HZ; n > 0; --n) {
		if (swim3_readbit(fs, SEEK_COMPLETE))
			break;
		if (signal_pending(current))
			break;
		swim3_select(fs, RELAX);
		schedule_timeout_interruptible(1);
	}
	ret = swim3_readbit(fs, SEEK_COMPLETE) == 0
		|| swim3_readbit(fs, DISK_IN) == 0;
	if (ret)
		swim3_action(fs, MOTOR_OFF);
	else {
		fs->ejected = 0;
		swim3_action(fs, SETMFM);
	}
	swim3_select(fs, RELAX);

	release_drive(fs);
	return ret;
}

static struct block_device_operations floppy_fops = {
	.open		= floppy_open,
	.release	= floppy_release,
	.ioctl		= floppy_ioctl,
	.media_changed	= floppy_check_change,
	.revalidate_disk= floppy_revalidate,
};

int swim3_init(void)
{
	struct device_node *swim;
	int err = -ENOMEM;
	int i;

	devfs_mk_dir("floppy");

	swim = find_devices("floppy");
	while (swim && (floppy_count < MAX_FLOPPIES))
	{
		swim3_add_device(swim);
		swim = swim->next;
	}

	swim = find_devices("swim3");
	while (swim && (floppy_count < MAX_FLOPPIES))
	{
		swim3_add_device(swim);
		swim = swim->next;
	}

	if (!floppy_count)
		return -ENODEV;

	for (i = 0; i < floppy_count; i++) {
		disks[i] = alloc_disk(1);
		if (!disks[i])
			goto out;
	}

	if (register_blkdev(FLOPPY_MAJOR, "fd")) {
		err = -EBUSY;
		goto out;
	}

	swim3_queue = blk_init_queue(do_fd_request, &swim3_lock);
	if (!swim3_queue) {
		err = -ENOMEM;
		goto out_queue;
	}

	for (i = 0; i < floppy_count; i++) {
		struct gendisk *disk = disks[i];
		disk->major = FLOPPY_MAJOR;
		disk->first_minor = i;
		disk->fops = &floppy_fops;
		disk->private_data = &floppy_states[i];
		disk->queue = swim3_queue;
		disk->flags |= GENHD_FL_REMOVABLE;
		sprintf(disk->disk_name, "fd%d", i);
		sprintf(disk->devfs_name, "floppy/%d", i);
		set_capacity(disk, 2880);
		add_disk(disk);
	}
	return 0;

out_queue:
	unregister_blkdev(FLOPPY_MAJOR, "fd");
out:
	while (i--)
		put_disk(disks[i]);
	/* shouldn't we do something with results of swim_add_device()? */
	return err;
}

static int swim3_add_device(struct device_node *swim)
{
	struct device_node *mediabay;
	struct floppy_state *fs = &floppy_states[floppy_count];

	if (swim->n_addrs < 2)
	{
		printk(KERN_INFO "swim3: expecting 2 addrs (n_addrs:%d, n_intrs:%d)\n",
		       swim->n_addrs, swim->n_intrs);
		return -EINVAL;
	}

	if (swim->n_intrs < 2)
	{
		printk(KERN_INFO "swim3: expecting 2 intrs (n_addrs:%d, n_intrs:%d)\n",
		       swim->n_addrs, swim->n_intrs);
		return -EINVAL;
	}

	if (!request_OF_resource(swim, 0, NULL)) {
		printk(KERN_INFO "swim3: can't request IO resource !\n");
		return -EINVAL;
	}

	mediabay = (strcasecmp(swim->parent->type, "media-bay") == 0) ? swim->parent : NULL;
	if (mediabay == NULL)
		pmac_call_feature(PMAC_FTR_SWIM3_ENABLE, swim, 0, 1);
	
	memset(fs, 0, sizeof(*fs));
	fs->state = idle;
	fs->swim3 = (struct swim3 __iomem *)
		ioremap(swim->addrs[0].address, 0x200);
	fs->dma = (struct dbdma_regs __iomem *)
		ioremap(swim->addrs[1].address, 0x200);
	fs->swim3_intr = swim->intrs[0].line;
	fs->dma_intr = swim->intrs[1].line;
	fs->cur_cyl = -1;
	fs->cur_sector = -1;
	fs->secpercyl = 36;
	fs->secpertrack = 18;
	fs->total_secs = 2880;
	fs->media_bay = mediabay;
	init_waitqueue_head(&fs->wait);

	fs->dma_cmd = (struct dbdma_cmd *) DBDMA_ALIGN(fs->dbdma_cmd_space);
	memset(fs->dma_cmd, 0, 2 * sizeof(struct dbdma_cmd));
	st_le16(&fs->dma_cmd[1].command, DBDMA_STOP);

	if (request_irq(fs->swim3_intr, swim3_interrupt, 0, "SWIM3", fs)) {
		printk(KERN_ERR "Couldn't get irq %d for SWIM3\n", fs->swim3_intr);
		pmac_call_feature(PMAC_FTR_SWIM3_ENABLE, swim, 0, 0);
		return -EBUSY;
	}
/*
	if (request_irq(fs->dma_intr, fd_dma_interrupt, 0, "SWIM3-dma", fs)) {
		printk(KERN_ERR "Couldn't get irq %d for SWIM3 DMA",
		       fs->dma_intr);
		pmac_call_feature(PMAC_FTR_SWIM3_ENABLE, swim, 0, 0);
		return -EBUSY;
	}
*/

	init_timer(&fs->timeout);

	printk(KERN_INFO "fd%d: SWIM3 floppy controller %s\n", floppy_count,
		mediabay ? "in media bay" : "");

	floppy_count++;
	
	return 0;
}

module_init(swim3_init)

MODULE_LICENSE("GPL");
MODULE_AUTHOR("Paul Mackerras");
MODULE_ALIAS_BLOCKDEV_MAJOR(FLOPPY_MAJOR);