swim3.c

来自「linux 内核源代码」· C语言 代码 · 共 1,193 行 · 第 1/2 页

	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 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=%x, 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=%x, 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)
{
}
*/

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

	spin_lock_irqsave(&fs->lock, flags);
	if (fs->state != idle) {
		++fs->wanted;
		while (fs->state != available) {
			if (interruptible && signal_pending(current)) {
				--fs->wanted;
				spin_unlock_irqrestore(&fs->lock, flags);
				return -EINTR;
			}
			interruptible_sleep_on(&fs->wait);
		}
		--fs->wanted;
	}
	fs->state = state;
	spin_unlock_irqrestore(&fs->lock, flags);
	return 0;
}

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

	spin_lock_irqsave(&fs->lock, flags);
	fs->state = idle;
	start_request(fs);
	spin_unlock_irqrestore(&fs->lock, 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,
};

static int swim3_add_device(struct macio_dev *mdev, int index)
{
	struct device_node *swim = mdev->ofdev.node;
	struct device_node *mediabay;
	struct floppy_state *fs = &floppy_states[index];
	int rc = -EBUSY;

	/* Check & Request resources */
	if (macio_resource_count(mdev) < 2) {
		printk(KERN_WARNING "ifd%d: no address for %s\n",
		       index, swim->full_name);
		return -ENXIO;
	}
	if (macio_irq_count(mdev) < 2) {
		printk(KERN_WARNING "fd%d: no intrs for device %s\n",
			index, swim->full_name);
	}
	if (macio_request_resource(mdev, 0, "swim3 (mmio)")) {
		printk(KERN_ERR "fd%d: can't request mmio resource for %s\n",
		       index, swim->full_name);
		return -EBUSY;
	}
	if (macio_request_resource(mdev, 1, "swim3 (dma)")) {
		printk(KERN_ERR "fd%d: can't request dma resource for %s\n",
		       index, swim->full_name);
		macio_release_resource(mdev, 0);
		return -EBUSY;
	}
	dev_set_drvdata(&mdev->ofdev.dev, fs);

	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));
	spin_lock_init(&fs->lock);
	fs->state = idle;
	fs->swim3 = (struct swim3 __iomem *)
		ioremap(macio_resource_start(mdev, 0), 0x200);
	if (fs->swim3 == NULL) {
		printk("fd%d: couldn't map registers for %s\n",
		       index, swim->full_name);
		rc = -ENOMEM;
		goto out_release;
	}
	fs->dma = (struct dbdma_regs __iomem *)
		ioremap(macio_resource_start(mdev, 1), 0x200);
	if (fs->dma == NULL) {
		printk("fd%d: couldn't map DMA for %s\n",
		       index, swim->full_name);
		iounmap(fs->swim3);
		rc = -ENOMEM;
		goto out_release;
	}
	fs->swim3_intr = macio_irq(mdev, 0);
	fs->dma_intr = macio_irq(mdev, 1);;
	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 "fd%d: couldn't request irq %d for %s\n",
		       index, fs->swim3_intr, swim->full_name);
		pmac_call_feature(PMAC_FTR_SWIM3_ENABLE, swim, 0, 0);
		goto out_unmap;
		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);
		return -EBUSY;
	}
*/

	init_timer(&fs->timeout);

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

	return 0;

 out_unmap:
	iounmap(fs->dma);
	iounmap(fs->swim3);

 out_release:
	macio_release_resource(mdev, 0);
	macio_release_resource(mdev, 1);

	return rc;
}

static int __devinit swim3_attach(struct macio_dev *mdev, const struct of_device_id *match)
{
	int i, rc;
	struct gendisk *disk;

	/* Add the drive */
	rc = swim3_add_device(mdev, floppy_count);
	if (rc)
		return rc;

	/* Now create the queue if not there yet */
	if (swim3_queue == NULL) {
		/* If we failed, there isn't much we can do as the driver is still
		 * too dumb to remove the device, just bail out
		 */
		if (register_blkdev(FLOPPY_MAJOR, "fd"))
			return 0;
		swim3_queue = blk_init_queue(do_fd_request, &swim3_lock);
		if (swim3_queue == NULL) {
			unregister_blkdev(FLOPPY_MAJOR, "fd");
			return 0;
		}
	}

	/* Now register that disk. Same comment about failure handling */
	i = floppy_count++;
	disk = disks[i] = alloc_disk(1);
	if (disk == NULL)
		return 0;

	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);
	set_capacity(disk, 2880);
	add_disk(disk);

	return 0;
}

static struct of_device_id swim3_match[] =
{
	{
	.name		= "swim3",
	},
	{
	.compatible	= "ohare-swim3"
	},
	{
	.compatible	= "swim3"
	},
};

static struct macio_driver swim3_driver =
{
	.name 		= "swim3",
	.match_table	= swim3_match,
	.probe		= swim3_attach,
#if 0
	.suspend	= swim3_suspend,
	.resume		= swim3_resume,
#endif
};


int swim3_init(void)
{
	macio_register_driver(&swim3_driver);
	return 0;
}

module_init(swim3_init)

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