Root/drivers/block/swim.c

1/*
2 * Driver for SWIM (Sander Woz Integrated Machine) floppy controller
3 *
4 * Copyright (C) 2004,2008 Laurent Vivier <Laurent@lvivier.info>
5 *
6 * based on Alastair Bridgewater SWIM analysis, 2001
7 * based on SWIM3 driver (c) Paul Mackerras, 1996
8 * based on netBSD IWM driver (c) 1997, 1998 Hauke Fath.
9 *
10 * This program is free software; you can redistribute it and/or
11 * modify it under the terms of the GNU General Public License
12 * as published by the Free Software Foundation; either version
13 * 2 of the License, or (at your option) any later version.
14 *
15 * 2004-08-21 (lv) - Initial implementation
16 * 2008-10-30 (lv) - Port to 2.6
17 */
18
19#include <linux/module.h>
20#include <linux/fd.h>
21#include <linux/slab.h>
22#include <linux/blkdev.h>
23#include <linux/mutex.h>
24#include <linux/hdreg.h>
25#include <linux/kernel.h>
26#include <linux/delay.h>
27#include <linux/platform_device.h>
28
29#include <asm/mac_via.h>
30
31#define CARDNAME "swim"
32
33struct sector_header {
34    unsigned char side;
35    unsigned char track;
36    unsigned char sector;
37    unsigned char size;
38    unsigned char crc0;
39    unsigned char crc1;
40} __attribute__((packed));
41
42#define DRIVER_VERSION "Version 0.2 (2008-10-30)"
43
44#define REG(x) unsigned char x, x ## _pad[0x200 - 1];
45
46struct swim {
47    REG(write_data)
48    REG(write_mark)
49    REG(write_CRC)
50    REG(write_parameter)
51    REG(write_phase)
52    REG(write_setup)
53    REG(write_mode0)
54    REG(write_mode1)
55
56    REG(read_data)
57    REG(read_mark)
58    REG(read_error)
59    REG(read_parameter)
60    REG(read_phase)
61    REG(read_setup)
62    REG(read_status)
63    REG(read_handshake)
64} __attribute__((packed));
65
66#define swim_write(base, reg, v) out_8(&(base)->write_##reg, (v))
67#define swim_read(base, reg) in_8(&(base)->read_##reg)
68
69/* IWM registers */
70
71struct iwm {
72    REG(ph0L)
73    REG(ph0H)
74    REG(ph1L)
75    REG(ph1H)
76    REG(ph2L)
77    REG(ph2H)
78    REG(ph3L)
79    REG(ph3H)
80    REG(mtrOff)
81    REG(mtrOn)
82    REG(intDrive)
83    REG(extDrive)
84    REG(q6L)
85    REG(q6H)
86    REG(q7L)
87    REG(q7H)
88} __attribute__((packed));
89
90#define iwm_write(base, reg, v) out_8(&(base)->reg, (v))
91#define iwm_read(base, reg) in_8(&(base)->reg)
92
93/* bits in phase register */
94
95#define SEEK_POSITIVE 0x070
96#define SEEK_NEGATIVE 0x074
97#define STEP 0x071
98#define MOTOR_ON 0x072
99#define MOTOR_OFF 0x076
100#define INDEX 0x073
101#define EJECT 0x077
102#define SETMFM 0x171
103#define SETGCR 0x175
104
105#define RELAX 0x033
106#define LSTRB 0x008
107
108#define CA_MASK 0x077
109
110/* Select values for swim_select and swim_readbit */
111
112#define READ_DATA_0 0x074
113#define TWOMEG_DRIVE 0x075
114#define SINGLE_SIDED 0x076
115#define DRIVE_PRESENT 0x077
116#define DISK_IN 0x170
117#define WRITE_PROT 0x171
118#define TRACK_ZERO 0x172
119#define TACHO 0x173
120#define READ_DATA_1 0x174
121#define MFM_MODE 0x175
122#define SEEK_COMPLETE 0x176
123#define ONEMEG_MEDIA 0x177
124
125/* Bits in handshake register */
126
127#define MARK_BYTE 0x01
128#define CRC_ZERO 0x02
129#define RDDATA 0x04
130#define SENSE 0x08
131#define MOTEN 0x10
132#define ERROR 0x20
133#define DAT2BYTE 0x40
134#define DAT1BYTE 0x80
135
136/* bits in setup register */
137
138#define S_INV_WDATA 0x01
139#define S_3_5_SELECT 0x02
140#define S_GCR 0x04
141#define S_FCLK_DIV2 0x08
142#define S_ERROR_CORR 0x10
143#define S_IBM_DRIVE 0x20
144#define S_GCR_WRITE 0x40
145#define S_TIMEOUT 0x80
146
147/* bits in mode register */
148
149#define CLFIFO 0x01
150#define ENBL1 0x02
151#define ENBL2 0x04
152#define ACTION 0x08
153#define WRITE_MODE 0x10
154#define HEDSEL 0x20
155#define MOTON 0x80
156
157/*----------------------------------------------------------------------------*/
158
159enum drive_location {
160    INTERNAL_DRIVE = 0x02,
161    EXTERNAL_DRIVE = 0x04,
162};
163
164enum media_type {
165    DD_MEDIA,
166    HD_MEDIA,
167};
168
169struct floppy_state {
170
171    /* physical properties */
172
173    enum drive_location location; /* internal or external drive */
174    int head_number; /* single- or double-sided drive */
175
176    /* media */
177
178    int disk_in;
179    int ejected;
180    enum media_type type;
181    int write_protected;
182
183    int total_secs;
184    int secpercyl;
185    int secpertrack;
186
187    /* in-use information */
188
189    int track;
190    int ref_count;
191
192    struct gendisk *disk;
193
194    /* parent controller */
195
196    struct swim_priv *swd;
197};
198
199enum motor_action {
200    OFF,
201    ON,
202};
203
204enum head {
205    LOWER_HEAD = 0,
206    UPPER_HEAD = 1,
207};
208
209#define FD_MAX_UNIT 2
210
211struct swim_priv {
212    struct swim __iomem *base;
213    spinlock_t lock;
214    struct request_queue *queue;
215    int floppy_count;
216    struct floppy_state unit[FD_MAX_UNIT];
217};
218
219extern int swim_read_sector_header(struct swim __iomem *base,
220                   struct sector_header *header);
221extern int swim_read_sector_data(struct swim __iomem *base,
222                 unsigned char *data);
223
224static DEFINE_MUTEX(swim_mutex);
225static inline void set_swim_mode(struct swim __iomem *base, int enable)
226{
227    struct iwm __iomem *iwm_base;
228    unsigned long flags;
229
230    if (!enable) {
231        swim_write(base, mode0, 0xf8);
232        return;
233    }
234
235    iwm_base = (struct iwm __iomem *)base;
236    local_irq_save(flags);
237
238    iwm_read(iwm_base, q7L);
239    iwm_read(iwm_base, mtrOff);
240    iwm_read(iwm_base, q6H);
241
242    iwm_write(iwm_base, q7H, 0x57);
243    iwm_write(iwm_base, q7H, 0x17);
244    iwm_write(iwm_base, q7H, 0x57);
245    iwm_write(iwm_base, q7H, 0x57);
246
247    local_irq_restore(flags);
248}
249
250static inline int get_swim_mode(struct swim __iomem *base)
251{
252    unsigned long flags;
253
254    local_irq_save(flags);
255
256    swim_write(base, phase, 0xf5);
257    if (swim_read(base, phase) != 0xf5)
258        goto is_iwm;
259    swim_write(base, phase, 0xf6);
260    if (swim_read(base, phase) != 0xf6)
261        goto is_iwm;
262    swim_write(base, phase, 0xf7);
263    if (swim_read(base, phase) != 0xf7)
264        goto is_iwm;
265    local_irq_restore(flags);
266    return 1;
267is_iwm:
268    local_irq_restore(flags);
269    return 0;
270}
271
272static inline void swim_select(struct swim __iomem *base, int sel)
273{
274    swim_write(base, phase, RELAX);
275
276    via1_set_head(sel & 0x100);
277
278    swim_write(base, phase, sel & CA_MASK);
279}
280
281static inline void swim_action(struct swim __iomem *base, int action)
282{
283    unsigned long flags;
284
285    local_irq_save(flags);
286
287    swim_select(base, action);
288    udelay(1);
289    swim_write(base, phase, (LSTRB<<4) | LSTRB);
290    udelay(1);
291    swim_write(base, phase, (LSTRB<<4) | ((~LSTRB) & 0x0F));
292    udelay(1);
293
294    local_irq_restore(flags);
295}
296
297static inline int swim_readbit(struct swim __iomem *base, int bit)
298{
299    int stat;
300
301    swim_select(base, bit);
302
303    udelay(10);
304
305    stat = swim_read(base, handshake);
306
307    return (stat & SENSE) == 0;
308}
309
310static inline void swim_drive(struct swim __iomem *base,
311                  enum drive_location location)
312{
313    if (location == INTERNAL_DRIVE) {
314        swim_write(base, mode0, EXTERNAL_DRIVE); /* clear drive 1 bit */
315        swim_write(base, mode1, INTERNAL_DRIVE); /* set drive 0 bit */
316    } else if (location == EXTERNAL_DRIVE) {
317        swim_write(base, mode0, INTERNAL_DRIVE); /* clear drive 0 bit */
318        swim_write(base, mode1, EXTERNAL_DRIVE); /* set drive 1 bit */
319    }
320}
321
322static inline void swim_motor(struct swim __iomem *base,
323                  enum motor_action action)
324{
325    if (action == ON) {
326        int i;
327
328        swim_action(base, MOTOR_ON);
329
330        for (i = 0; i < 2*HZ; i++) {
331            swim_select(base, RELAX);
332            if (swim_readbit(base, MOTOR_ON))
333                break;
334            current->state = TASK_INTERRUPTIBLE;
335            schedule_timeout(1);
336        }
337    } else if (action == OFF) {
338        swim_action(base, MOTOR_OFF);
339        swim_select(base, RELAX);
340    }
341}
342
343static inline void swim_eject(struct swim __iomem *base)
344{
345    int i;
346
347    swim_action(base, EJECT);
348
349    for (i = 0; i < 2*HZ; i++) {
350        swim_select(base, RELAX);
351        if (!swim_readbit(base, DISK_IN))
352            break;
353        current->state = TASK_INTERRUPTIBLE;
354        schedule_timeout(1);
355    }
356    swim_select(base, RELAX);
357}
358
359static inline void swim_head(struct swim __iomem *base, enum head head)
360{
361    /* wait drive is ready */
362
363    if (head == UPPER_HEAD)
364        swim_select(base, READ_DATA_1);
365    else if (head == LOWER_HEAD)
366        swim_select(base, READ_DATA_0);
367}
368
369static inline int swim_step(struct swim __iomem *base)
370{
371    int wait;
372
373    swim_action(base, STEP);
374
375    for (wait = 0; wait < HZ; wait++) {
376
377        current->state = TASK_INTERRUPTIBLE;
378        schedule_timeout(1);
379
380        swim_select(base, RELAX);
381        if (!swim_readbit(base, STEP))
382            return 0;
383    }
384    return -1;
385}
386
387static inline int swim_track00(struct swim __iomem *base)
388{
389    int try;
390
391    swim_action(base, SEEK_NEGATIVE);
392
393    for (try = 0; try < 100; try++) {
394
395        swim_select(base, RELAX);
396        if (swim_readbit(base, TRACK_ZERO))
397            break;
398
399        if (swim_step(base))
400            return -1;
401    }
402
403    if (swim_readbit(base, TRACK_ZERO))
404        return 0;
405
406    return -1;
407}
408
409static inline int swim_seek(struct swim __iomem *base, int step)
410{
411    if (step == 0)
412        return 0;
413
414    if (step < 0) {
415        swim_action(base, SEEK_NEGATIVE);
416        step = -step;
417    } else
418        swim_action(base, SEEK_POSITIVE);
419
420    for ( ; step > 0; step--) {
421        if (swim_step(base))
422            return -1;
423    }
424
425    return 0;
426}
427
428static inline int swim_track(struct floppy_state *fs, int track)
429{
430    struct swim __iomem *base = fs->swd->base;
431    int ret;
432
433    ret = swim_seek(base, track - fs->track);
434
435    if (ret == 0)
436        fs->track = track;
437    else {
438        swim_track00(base);
439        fs->track = 0;
440    }
441
442    return ret;
443}
444
445static int floppy_eject(struct floppy_state *fs)
446{
447    struct swim __iomem *base = fs->swd->base;
448
449    swim_drive(base, fs->location);
450    swim_motor(base, OFF);
451    swim_eject(base);
452
453    fs->disk_in = 0;
454    fs->ejected = 1;
455
456    return 0;
457}
458
459static inline int swim_read_sector(struct floppy_state *fs,
460                   int side, int track,
461                   int sector, unsigned char *buffer)
462{
463    struct swim __iomem *base = fs->swd->base;
464    unsigned long flags;
465    struct sector_header header;
466    int ret = -1;
467    short i;
468
469    swim_track(fs, track);
470
471    swim_write(base, mode1, MOTON);
472    swim_head(base, side);
473    swim_write(base, mode0, side);
474
475    local_irq_save(flags);
476    for (i = 0; i < 36; i++) {
477        ret = swim_read_sector_header(base, &header);
478        if (!ret && (header.sector == sector)) {
479            /* found */
480
481            ret = swim_read_sector_data(base, buffer);
482            break;
483        }
484    }
485    local_irq_restore(flags);
486
487    swim_write(base, mode0, MOTON);
488
489    if ((header.side != side) || (header.track != track) ||
490         (header.sector != sector))
491        return 0;
492
493    return ret;
494}
495
496static int floppy_read_sectors(struct floppy_state *fs,
497                   int req_sector, int sectors_nb,
498                   unsigned char *buffer)
499{
500    struct swim __iomem *base = fs->swd->base;
501    int ret;
502    int side, track, sector;
503    int i, try;
504
505
506    swim_drive(base, fs->location);
507    for (i = req_sector; i < req_sector + sectors_nb; i++) {
508        int x;
509        track = i / fs->secpercyl;
510        x = i % fs->secpercyl;
511        side = x / fs->secpertrack;
512        sector = x % fs->secpertrack + 1;
513
514        try = 5;
515        do {
516            ret = swim_read_sector(fs, side, track, sector,
517                        buffer);
518            if (try-- == 0)
519                return -EIO;
520        } while (ret != 512);
521
522        buffer += ret;
523    }
524
525    return 0;
526}
527
528static void redo_fd_request(struct request_queue *q)
529{
530    struct request *req;
531    struct floppy_state *fs;
532
533    req = blk_fetch_request(q);
534    while (req) {
535        int err = -EIO;
536
537        fs = req->rq_disk->private_data;
538        if (blk_rq_pos(req) >= fs->total_secs)
539            goto done;
540        if (!fs->disk_in)
541            goto done;
542        if (rq_data_dir(req) == WRITE && fs->write_protected)
543            goto done;
544
545        switch (rq_data_dir(req)) {
546        case WRITE:
547            /* NOT IMPLEMENTED */
548            break;
549        case READ:
550            err = floppy_read_sectors(fs, blk_rq_pos(req),
551                          blk_rq_cur_sectors(req),
552                          req->buffer);
553            break;
554        }
555    done:
556        if (!__blk_end_request_cur(req, err))
557            req = blk_fetch_request(q);
558    }
559}
560
561static void do_fd_request(struct request_queue *q)
562{
563    redo_fd_request(q);
564}
565
566static struct floppy_struct floppy_type[4] = {
567    { 0, 0, 0, 0, 0, 0x00, 0x00, 0x00, 0x00, NULL }, /* no testing */
568    { 720, 9, 1, 80, 0, 0x2A, 0x02, 0xDF, 0x50, NULL }, /* 360KB SS 3.5"*/
569    { 1440, 9, 2, 80, 0, 0x2A, 0x02, 0xDF, 0x50, NULL }, /* 720KB 3.5" */
570    { 2880, 18, 2, 80, 0, 0x1B, 0x00, 0xCF, 0x6C, NULL }, /* 1.44MB 3.5" */
571};
572
573static int get_floppy_geometry(struct floppy_state *fs, int type,
574                   struct floppy_struct **g)
575{
576    if (type >= ARRAY_SIZE(floppy_type))
577        return -EINVAL;
578
579    if (type)
580        *g = &floppy_type[type];
581    else if (fs->type == HD_MEDIA) /* High-Density media */
582        *g = &floppy_type[3];
583    else if (fs->head_number == 2) /* double-sided */
584        *g = &floppy_type[2];
585    else
586        *g = &floppy_type[1];
587
588    return 0;
589}
590
591static void setup_medium(struct floppy_state *fs)
592{
593    struct swim __iomem *base = fs->swd->base;
594
595    if (swim_readbit(base, DISK_IN)) {
596        struct floppy_struct *g;
597        fs->disk_in = 1;
598        fs->write_protected = swim_readbit(base, WRITE_PROT);
599        fs->type = swim_readbit(base, ONEMEG_MEDIA);
600
601        if (swim_track00(base))
602            printk(KERN_ERR
603                "SWIM: cannot move floppy head to track 0\n");
604
605        swim_track00(base);
606
607        get_floppy_geometry(fs, 0, &g);
608        fs->total_secs = g->size;
609        fs->secpercyl = g->head * g->sect;
610        fs->secpertrack = g->sect;
611        fs->track = 0;
612    } else {
613        fs->disk_in = 0;
614    }
615}
616
617static int floppy_open(struct block_device *bdev, fmode_t mode)
618{
619    struct floppy_state *fs = bdev->bd_disk->private_data;
620    struct swim __iomem *base = fs->swd->base;
621    int err;
622
623    if (fs->ref_count == -1 || (fs->ref_count && mode & FMODE_EXCL))
624        return -EBUSY;
625
626    if (mode & FMODE_EXCL)
627        fs->ref_count = -1;
628    else
629        fs->ref_count++;
630
631    swim_write(base, setup, S_IBM_DRIVE | S_FCLK_DIV2);
632    udelay(10);
633    swim_drive(base, INTERNAL_DRIVE);
634    swim_motor(base, ON);
635    swim_action(base, SETMFM);
636    if (fs->ejected)
637        setup_medium(fs);
638    if (!fs->disk_in) {
639        err = -ENXIO;
640        goto out;
641    }
642
643    if (mode & FMODE_NDELAY)
644        return 0;
645
646    if (mode & (FMODE_READ|FMODE_WRITE)) {
647        check_disk_change(bdev);
648        if ((mode & FMODE_WRITE) && fs->write_protected) {
649            err = -EROFS;
650            goto out;
651        }
652    }
653    return 0;
654out:
655    if (fs->ref_count < 0)
656        fs->ref_count = 0;
657    else if (fs->ref_count > 0)
658        --fs->ref_count;
659
660    if (fs->ref_count == 0)
661        swim_motor(base, OFF);
662    return err;
663}
664
665static int floppy_unlocked_open(struct block_device *bdev, fmode_t mode)
666{
667    int ret;
668
669    mutex_lock(&swim_mutex);
670    ret = floppy_open(bdev, mode);
671    mutex_unlock(&swim_mutex);
672
673    return ret;
674}
675
676static int floppy_release(struct gendisk *disk, fmode_t mode)
677{
678    struct floppy_state *fs = disk->private_data;
679    struct swim __iomem *base = fs->swd->base;
680
681    mutex_lock(&swim_mutex);
682    if (fs->ref_count < 0)
683        fs->ref_count = 0;
684    else if (fs->ref_count > 0)
685        --fs->ref_count;
686
687    if (fs->ref_count == 0)
688        swim_motor(base, OFF);
689    mutex_unlock(&swim_mutex);
690
691    return 0;
692}
693
694static int floppy_ioctl(struct block_device *bdev, fmode_t mode,
695            unsigned int cmd, unsigned long param)
696{
697    struct floppy_state *fs = bdev->bd_disk->private_data;
698    int err;
699
700    if ((cmd & 0x80) && !capable(CAP_SYS_ADMIN))
701            return -EPERM;
702
703    switch (cmd) {
704    case FDEJECT:
705        if (fs->ref_count != 1)
706            return -EBUSY;
707        mutex_lock(&swim_mutex);
708        err = floppy_eject(fs);
709        mutex_unlock(&swim_mutex);
710        return err;
711
712    case FDGETPRM:
713        if (copy_to_user((void __user *) param, (void *) &floppy_type,
714                 sizeof(struct floppy_struct)))
715            return -EFAULT;
716        break;
717
718    default:
719        printk(KERN_DEBUG "SWIM floppy_ioctl: unknown cmd %d\n",
720               cmd);
721        return -ENOSYS;
722    }
723    return 0;
724}
725
726static int floppy_getgeo(struct block_device *bdev, struct hd_geometry *geo)
727{
728    struct floppy_state *fs = bdev->bd_disk->private_data;
729    struct floppy_struct *g;
730    int ret;
731
732    ret = get_floppy_geometry(fs, 0, &g);
733    if (ret)
734        return ret;
735
736    geo->heads = g->head;
737    geo->sectors = g->sect;
738    geo->cylinders = g->track;
739
740    return 0;
741}
742
743static unsigned int floppy_check_events(struct gendisk *disk,
744                    unsigned int clearing)
745{
746    struct floppy_state *fs = disk->private_data;
747
748    return fs->ejected ? DISK_EVENT_MEDIA_CHANGE : 0;
749}
750
751static int floppy_revalidate(struct gendisk *disk)
752{
753    struct floppy_state *fs = disk->private_data;
754    struct swim __iomem *base = fs->swd->base;
755
756    swim_drive(base, fs->location);
757
758    if (fs->ejected)
759        setup_medium(fs);
760
761    if (!fs->disk_in)
762        swim_motor(base, OFF);
763    else
764        fs->ejected = 0;
765
766    return !fs->disk_in;
767}
768
769static const struct block_device_operations floppy_fops = {
770    .owner = THIS_MODULE,
771    .open = floppy_unlocked_open,
772    .release = floppy_release,
773    .ioctl = floppy_ioctl,
774    .getgeo = floppy_getgeo,
775    .check_events = floppy_check_events,
776    .revalidate_disk = floppy_revalidate,
777};
778
779static struct kobject *floppy_find(dev_t dev, int *part, void *data)
780{
781    struct swim_priv *swd = data;
782    int drive = (*part & 3);
783
784    if (drive > swd->floppy_count)
785        return NULL;
786
787    *part = 0;
788    return get_disk(swd->unit[drive].disk);
789}
790
791static int __devinit swim_add_floppy(struct swim_priv *swd,
792                     enum drive_location location)
793{
794    struct floppy_state *fs = &swd->unit[swd->floppy_count];
795    struct swim __iomem *base = swd->base;
796
797    fs->location = location;
798
799    swim_drive(base, location);
800
801    swim_motor(base, OFF);
802
803    if (swim_readbit(base, SINGLE_SIDED))
804        fs->head_number = 1;
805    else
806        fs->head_number = 2;
807    fs->ref_count = 0;
808    fs->ejected = 1;
809
810    swd->floppy_count++;
811
812    return 0;
813}
814
815static int __devinit swim_floppy_init(struct swim_priv *swd)
816{
817    int err;
818    int drive;
819    struct swim __iomem *base = swd->base;
820
821    /* scan floppy drives */
822
823    swim_drive(base, INTERNAL_DRIVE);
824    if (swim_readbit(base, DRIVE_PRESENT))
825        swim_add_floppy(swd, INTERNAL_DRIVE);
826    swim_drive(base, EXTERNAL_DRIVE);
827    if (swim_readbit(base, DRIVE_PRESENT))
828        swim_add_floppy(swd, EXTERNAL_DRIVE);
829
830    /* register floppy drives */
831
832    err = register_blkdev(FLOPPY_MAJOR, "fd");
833    if (err) {
834        printk(KERN_ERR "Unable to get major %d for SWIM floppy\n",
835               FLOPPY_MAJOR);
836        return -EBUSY;
837    }
838
839    for (drive = 0; drive < swd->floppy_count; drive++) {
840        swd->unit[drive].disk = alloc_disk(1);
841        if (swd->unit[drive].disk == NULL) {
842            err = -ENOMEM;
843            goto exit_put_disks;
844        }
845        swd->unit[drive].swd = swd;
846    }
847
848    swd->queue = blk_init_queue(do_fd_request, &swd->lock);
849    if (!swd->queue) {
850        err = -ENOMEM;
851        goto exit_put_disks;
852    }
853
854    for (drive = 0; drive < swd->floppy_count; drive++) {
855        swd->unit[drive].disk->flags = GENHD_FL_REMOVABLE;
856        swd->unit[drive].disk->major = FLOPPY_MAJOR;
857        swd->unit[drive].disk->first_minor = drive;
858        sprintf(swd->unit[drive].disk->disk_name, "fd%d", drive);
859        swd->unit[drive].disk->fops = &floppy_fops;
860        swd->unit[drive].disk->private_data = &swd->unit[drive];
861        swd->unit[drive].disk->queue = swd->queue;
862        set_capacity(swd->unit[drive].disk, 2880);
863        add_disk(swd->unit[drive].disk);
864    }
865
866    blk_register_region(MKDEV(FLOPPY_MAJOR, 0), 256, THIS_MODULE,
867                floppy_find, NULL, swd);
868
869    return 0;
870
871exit_put_disks:
872    unregister_blkdev(FLOPPY_MAJOR, "fd");
873    while (drive--)
874        put_disk(swd->unit[drive].disk);
875    return err;
876}
877
878static int __devinit swim_probe(struct platform_device *dev)
879{
880    struct resource *res;
881    struct swim __iomem *swim_base;
882    struct swim_priv *swd;
883    int ret;
884
885    res = platform_get_resource(dev, IORESOURCE_MEM, 0);
886    if (!res) {
887        ret = -ENODEV;
888        goto out;
889    }
890
891    if (!request_mem_region(res->start, resource_size(res), CARDNAME)) {
892        ret = -EBUSY;
893        goto out;
894    }
895
896    swim_base = ioremap(res->start, resource_size(res));
897    if (!swim_base) {
898        return -ENOMEM;
899        goto out_release_io;
900    }
901
902    /* probe device */
903
904    set_swim_mode(swim_base, 1);
905    if (!get_swim_mode(swim_base)) {
906        printk(KERN_INFO "SWIM device not found !\n");
907        ret = -ENODEV;
908        goto out_iounmap;
909    }
910
911    /* set platform driver data */
912
913    swd = kzalloc(sizeof(struct swim_priv), GFP_KERNEL);
914    if (!swd) {
915        ret = -ENOMEM;
916        goto out_iounmap;
917    }
918    platform_set_drvdata(dev, swd);
919
920    swd->base = swim_base;
921
922    ret = swim_floppy_init(swd);
923    if (ret)
924        goto out_kfree;
925
926    return 0;
927
928out_kfree:
929    platform_set_drvdata(dev, NULL);
930    kfree(swd);
931out_iounmap:
932    iounmap(swim_base);
933out_release_io:
934    release_mem_region(res->start, resource_size(res));
935out:
936    return ret;
937}
938
939static int __devexit swim_remove(struct platform_device *dev)
940{
941    struct swim_priv *swd = platform_get_drvdata(dev);
942    int drive;
943    struct resource *res;
944
945    blk_unregister_region(MKDEV(FLOPPY_MAJOR, 0), 256);
946
947    for (drive = 0; drive < swd->floppy_count; drive++) {
948        del_gendisk(swd->unit[drive].disk);
949        put_disk(swd->unit[drive].disk);
950    }
951
952    unregister_blkdev(FLOPPY_MAJOR, "fd");
953
954    blk_cleanup_queue(swd->queue);
955
956    /* eject floppies */
957
958    for (drive = 0; drive < swd->floppy_count; drive++)
959        floppy_eject(&swd->unit[drive]);
960
961    iounmap(swd->base);
962
963    res = platform_get_resource(dev, IORESOURCE_MEM, 0);
964    if (res)
965        release_mem_region(res->start, resource_size(res));
966
967    platform_set_drvdata(dev, NULL);
968    kfree(swd);
969
970    return 0;
971}
972
973static struct platform_driver swim_driver = {
974    .probe = swim_probe,
975    .remove = __devexit_p(swim_remove),
976    .driver = {
977        .name = CARDNAME,
978        .owner = THIS_MODULE,
979    },
980};
981
982static int __init swim_init(void)
983{
984    printk(KERN_INFO "SWIM floppy driver %s\n", DRIVER_VERSION);
985
986    return platform_driver_register(&swim_driver);
987}
988module_init(swim_init);
989
990static void __exit swim_exit(void)
991{
992    platform_driver_unregister(&swim_driver);
993}
994module_exit(swim_exit);
995
996MODULE_DESCRIPTION("Driver for SWIM floppy controller");
997MODULE_LICENSE("GPL");
998MODULE_AUTHOR("Laurent Vivier <laurent@lvivier.info>");
999MODULE_ALIAS_BLOCKDEV_MAJOR(FLOPPY_MAJOR);
1000

Archive Download this file

Branches:
ben-wpan
ben-wpan-stefan
javiroman/ks7010
jz-2.6.34
jz-2.6.34-rc5
jz-2.6.34-rc6
jz-2.6.34-rc7
jz-2.6.35
jz-2.6.36
jz-2.6.37
jz-2.6.38
jz-2.6.39
jz-3.0
jz-3.1
jz-3.11
jz-3.12
jz-3.13
jz-3.15
jz-3.16
jz-3.18-dt
jz-3.2
jz-3.3
jz-3.4
jz-3.5
jz-3.6
jz-3.6-rc2-pwm
jz-3.9
jz-3.9-clk
jz-3.9-rc8
jz47xx
jz47xx-2.6.38
master

Tags:
od-2011-09-04
od-2011-09-18
v2.6.34-rc5
v2.6.34-rc6
v2.6.34-rc7
v3.9



interactive