Source of swim.c - linux-imx - PHYTEC BitBucket (Stash)

Ignore whitespace
Show whitespace characters
Hide e-diff
Unified diff
Side-by-side diff
Source viewDiff to previous
 
 * as published by the Free Software Foundation; either version
 * 2 of the License, or (at your option) any later version.
 *
 * 2004-08-21 (lv) - Initial implementation
 * 2008-10-30 (lv) - Port to 2.6
 */
​
#include <linux/module.h>
#include <linux/fd.h>
#include <linux/slab.h>
#include <linux/blkdev.h>
#include <linux/blk-mq.h>
#include <linux/mutex.h>
#include <linux/hdreg.h>
#include <linux/kernel.h>
#include <linux/delay.h>
#include <linux/platform_device.h>
​
#include <asm/mac_via.h>
​
#define CARDNAME "swim"
​
Show more
    int      total_secs;
    int      secpercyl;
    int      secpertrack;
​
    /* in-use information */
​
    int     track;
    int     ref_count;
​
    struct gendisk *disk;
    struct blk_mq_tag_set tag_set;
​
    /* parent controller */
​
    struct swim_priv *swd;
};
​
enum motor_action {
    OFF,
    ON,
};
Show more
enum head {
    LOWER_HEAD = 0,
    UPPER_HEAD = 1,
};
​
#define FD_MAX_UNIT 2
​
struct swim_priv {
    struct swim __iomem *base;
    spinlock_t lock;
    int fdc_queue;
    int floppy_count;
    struct floppy_state unit[FD_MAX_UNIT];
};
​
extern int swim_read_sector_header(struct swim __iomem *base,
                   struct sector_header *header);
extern int swim_read_sector_data(struct swim __iomem *base,
                 unsigned char *data);
​
static DEFINE_MUTEX(swim_mutex);
Show more
            if (try-- == 0)
                return BLK_STS_IOERR;
        } while (ret != 512);
​
        buffer += ret;
    }
​
    return 0;
}
​
static struct request *swim_next_request(struct swim_priv *swd)
static blk_status_t swim_queue_rq(struct blk_mq_hw_ctx *hctx,
                  const struct blk_mq_queue_data *bd)
{
    struct request_queue *q;
    struct request *rq;
    int old_pos = swd->fdc_queue;
    struct floppy_state *fs = hctx->queue->queuedata;
    struct swim_priv *swd = fs->swd;
    struct request *req = bd->rq;
    blk_status_t err;
​
    do {
        q = swd->unit[swd->fdc_queue].disk->queue;
        if (++swd->fdc_queue == swd->floppy_count)
            swd->fdc_queue = 0;
        if (q) {
            rq = blk_fetch_request(q);
            if (rq)
                return rq;
        }
    } while (swd->fdc_queue != old_pos);
    if (!spin_trylock_irq(&swd->lock))
        return BLK_STS_DEV_RESOURCE;
​
    return NULL;
}
    blk_mq_start_request(req);
​
static void do_fd_request(struct request_queue *q)
{
    struct swim_priv *swd = q->queuedata;
    struct request *req;
    struct floppy_state *fs;
    if (!fs->disk_in || rq_data_dir(req) == WRITE) {
        err = BLK_STS_IOERR;
        goto out;
    }
​
    req = swim_next_request(swd);
    while (req) {
        blk_status_t err = BLK_STS_IOERR;
    do {
        err = floppy_read_sectors(fs, blk_rq_pos(req),
                      blk_rq_cur_sectors(req),
                      bio_data(req->bio));
    } while (blk_update_request(req, err, blk_rq_cur_bytes(req)));
    __blk_mq_end_request(req, err);
​
        fs = req->rq_disk->private_data;
        if (blk_rq_pos(req) >= fs->total_secs)
            goto done;
        if (!fs->disk_in)
            goto done;
        if (rq_data_dir(req) == WRITE && fs->write_protected)
            goto done;
    err = BLK_STS_OK;
out:
    spin_unlock_irq(&swd->lock);
    return err;
​
        switch (rq_data_dir(req)) {
        case WRITE:
            /* NOT IMPLEMENTED */
            break;
        case READ:
            err = floppy_read_sectors(fs, blk_rq_pos(req),
                          blk_rq_cur_sectors(req),
                          bio_data(req->bio));
            break;
        }
    done:
        if (!__blk_end_request_cur(req, err))
            req = swim_next_request(swd);
    }
}
​
static struct floppy_struct floppy_type[4] = {
    {    0,  0, 0,  0, 0, 0x00, 0x00, 0x00, 0x00, NULL }, /* no testing   */
    {  720,  9, 1, 80, 0, 0x2A, 0x02, 0xDF, 0x50, NULL }, /* 360KB SS 3.5"*/
    { 1440,  9, 2, 80, 0, 0x2A, 0x02, 0xDF, 0x50, NULL }, /* 720KB 3.5"   */
    { 2880, 18, 2, 80, 0, 0x1B, 0x00, 0xCF, 0x6C, NULL }, /* 1.44MB 3.5"  */
};
​
static int get_floppy_geometry(struct floppy_state *fs, int type,
Show more
    fs->head_number = 2;
​
    fs->ref_count = 0;
    fs->ejected = 1;
​
    swd->floppy_count++;
​
    return 0;
}
​
static const struct blk_mq_ops swim_mq_ops = {
    .queue_rq = swim_queue_rq,
};
​
static int swim_floppy_init(struct swim_priv *swd)
{
    int err;
    int drive;
    struct swim __iomem *base = swd->base;
​
    /* scan floppy drives */
​
    swim_drive(base, INTERNAL_DRIVE);
    if (swim_readbit(base, DRIVE_PRESENT) &&
Show more
    err = register_blkdev(FLOPPY_MAJOR, "fd");
    if (err) {
        printk(KERN_ERR "Unable to get major %d for SWIM floppy\n",
               FLOPPY_MAJOR);
        return -EBUSY;
    }
​
    spin_lock_init(&swd->lock);
​
    for (drive = 0; drive < swd->floppy_count; drive++) {
        struct request_queue *q;
​
        swd->unit[drive].disk = alloc_disk(1);
        if (swd->unit[drive].disk == NULL) {
            err = -ENOMEM;
            goto exit_put_disks;
        }
        swd->unit[drive].disk->queue = blk_init_queue(do_fd_request,
                                  &swd->lock);
        if (!swd->unit[drive].disk->queue) {
            err = -ENOMEM;
​
        q = blk_mq_init_sq_queue(&swd->unit[drive].tag_set, &swim_mq_ops,
                        2, BLK_MQ_F_SHOULD_MERGE);
        if (IS_ERR(q)) {
            err = PTR_ERR(q);
            goto exit_put_disks;
        }
​
        swd->unit[drive].disk->queue = q;
        blk_queue_bounce_limit(swd->unit[drive].disk->queue,
                BLK_BOUNCE_HIGH);
        swd->unit[drive].disk->queue->queuedata = swd;
        swd->unit[drive].disk->queue->queuedata = &swd->unit[drive];
        swd->unit[drive].swd = swd;
    }
​
    for (drive = 0; drive < swd->floppy_count; drive++) {
        swd->unit[drive].disk->flags = GENHD_FL_REMOVABLE;
        swd->unit[drive].disk->major = FLOPPY_MAJOR;
        swd->unit[drive].disk->first_minor = drive;
        sprintf(swd->unit[drive].disk->disk_name, "fd%d", drive);
        swd->unit[drive].disk->fops = &floppy_fops;
        swd->unit[drive].disk->private_data = &swd->unit[drive];
Show more
exit_put_disks:
    unregister_blkdev(FLOPPY_MAJOR, "fd");
    do {
        struct gendisk *disk = swd->unit[drive].disk;
​
        if (disk) {
            if (disk->queue) {
                blk_cleanup_queue(disk->queue);
                disk->queue = NULL;
            }
            blk_mq_free_tag_set(&swd->unit[drive].tag_set);
            put_disk(disk);
        }
    } while (drive--);
    return err;
}
​
static int swim_probe(struct platform_device *dev)
{
    struct resource *res;
    struct swim __iomem *swim_base;
Show more
{
    struct swim_priv *swd = platform_get_drvdata(dev);
    int drive;
    struct resource *res;
​
    blk_unregister_region(MKDEV(FLOPPY_MAJOR, 0), 256);
​
    for (drive = 0; drive < swd->floppy_count; drive++) {
        del_gendisk(swd->unit[drive].disk);
        blk_cleanup_queue(swd->unit[drive].disk->queue);
        blk_mq_free_tag_set(&swd->unit[drive].tag_set);
        put_disk(swd->unit[drive].disk);
    }
​
    unregister_blkdev(FLOPPY_MAJOR, "fd");
​
    /* eject floppies */
​
    for (drive = 0; drive < swd->floppy_count; drive++)
        floppy_eject(&swd->unit[drive]);
​