swim3.c 31 KB

12345678910111213141516171819202122232425262728293031323334353637383940414243444546474849505152535455565758596061626364656667686970717273747576777879808182838485868788899091929394959697989910010110210310410510610710810911011111211311411511611711811912012112212312412512612712812913013113213313413513613713813914014114214314414514614714814915015115215315415515615715815916016116216316416516616716816917017117217317417517617717817918018118218318418518618718818919019119219319419519619719819920020120220320420520620720820921021121221321421521621721821922022122222322422522622722822923023123223323423523623723823924024124224324424524624724824925025125225325425525625725825926026126226326426526626726826927027127227327427527627727827928028128228328428528628728828929029129229329429529629729829930030130230330430530630730830931031131231331431531631731831932032132232332432532632732832933033133233333433533633733833934034134234334434534634734834935035135235335435535635735835936036136236336436536636736836937037137237337437537637737837938038138238338438538638738838939039139239339439539639739839940040140240340440540640740840941041141241341441541641741841942042142242342442542642742842943043143243343443543643743843944044144244344444544644744844945045145245345445545645745845946046146246346446546646746846947047147247347447547647747847948048148248348448548648748848949049149249349449549649749849950050150250350450550650750850951051151251351451551651751851952052152252352452552652752852953053153253353453553653753853954054154254354454554654754854955055155255355455555655755855956056156256356456556656756856957057157257357457557657757857958058158258358458558658758858959059159259359459559659759859960060160260360460560660760860961061161261361461561661761861962062162262362462562662762862963063163263363463563663763863964064164264364464564664764864965065165265365465565665765865966066166266366466566666766866967067167267367467567667767867968068168268368468568668768868969069169269369469569669769869970070170270370470570670770870971071171271371471571671771871972072172272372472572672772872973073173273373473573673773873974074174274374474574674774874975075175275375475575675775875976076176276376476576676776876977077177277377477577677777877978078178278378478578678778878979079179279379479579679779879980080180280380480580680780880981081181281381481581681781881982082182282382482582682782882983083183283383483583683783883984084184284384484584684784884985085185285385485585685785885986086186286386486586686786886987087187287387487587687787887988088188288388488588688788888989089189289389489589689789889990090190290390490590690790890991091191291391491591691791891992092192292392492592692792892993093193293393493593693793893994094194294394494594694794894995095195295395495595695795895996096196296396496596696796896997097197297397497597697797897998098198298398498598698798898999099199299399499599699799899910001001100210031004100510061007100810091010101110121013101410151016101710181019102010211022102310241025102610271028102910301031103210331034103510361037103810391040104110421043104410451046104710481049105010511052105310541055105610571058105910601061106210631064106510661067106810691070107110721073107410751076107710781079108010811082108310841085108610871088108910901091109210931094109510961097109810991100110111021103110411051106110711081109111011111112111311141115111611171118111911201121112211231124112511261127112811291130113111321133113411351136113711381139114011411142114311441145114611471148114911501151115211531154115511561157115811591160116111621163116411651166116711681169117011711172117311741175117611771178117911801181118211831184118511861187118811891190119111921193119411951196119711981199120012011202120312041205120612071208120912101211121212131214121512161217121812191220122112221223122412251226122712281229123012311232123312341235123612371238123912401241124212431244124512461247124812491250125112521253125412551256125712581259126012611262126312641265126612671268126912701271127212731274127512761277
  1. // SPDX-License-Identifier: GPL-2.0-or-later
  2. /*
  3. * Driver for the SWIM3 (Super Woz Integrated Machine 3)
  4. * floppy controller found on Power Macintoshes.
  5. *
  6. * Copyright (C) 1996 Paul Mackerras.
  7. */
  8. /*
  9. * TODO:
  10. * handle 2 drives
  11. * handle GCR disks
  12. */
  13. #undef DEBUG
  14. #include <linux/stddef.h>
  15. #include <linux/kernel.h>
  16. #include <linux/sched/signal.h>
  17. #include <linux/timer.h>
  18. #include <linux/delay.h>
  19. #include <linux/fd.h>
  20. #include <linux/ioctl.h>
  21. #include <linux/blk-mq.h>
  22. #include <linux/interrupt.h>
  23. #include <linux/mutex.h>
  24. #include <linux/module.h>
  25. #include <linux/spinlock.h>
  26. #include <linux/wait.h>
  27. #include <asm/io.h>
  28. #include <asm/dbdma.h>
  29. #include <asm/prom.h>
  30. #include <linux/uaccess.h>
  31. #include <asm/mediabay.h>
  32. #include <asm/machdep.h>
  33. #include <asm/pmac_feature.h>
  34. #define MAX_FLOPPIES 2
  35. static DEFINE_MUTEX(swim3_mutex);
  36. static struct gendisk *disks[MAX_FLOPPIES];
  37. enum swim_state {
  38. idle,
  39. locating,
  40. seeking,
  41. settling,
  42. do_transfer,
  43. jogging,
  44. available,
  45. revalidating,
  46. ejecting
  47. };
  48. #define REG(x) unsigned char x; char x ## _pad[15];
  49. /*
  50. * The names for these registers mostly represent speculation on my part.
  51. * It will be interesting to see how close they are to the names Apple uses.
  52. */
  53. struct swim3 {
  54. REG(data);
  55. REG(timer); /* counts down at 1MHz */
  56. REG(error);
  57. REG(mode);
  58. REG(select); /* controls CA0, CA1, CA2 and LSTRB signals */
  59. REG(setup);
  60. REG(control); /* writing bits clears them */
  61. REG(status); /* writing bits sets them in control */
  62. REG(intr);
  63. REG(nseek); /* # tracks to seek */
  64. REG(ctrack); /* current track number */
  65. REG(csect); /* current sector number */
  66. REG(gap3); /* size of gap 3 in track format */
  67. REG(sector); /* sector # to read or write */
  68. REG(nsect); /* # sectors to read or write */
  69. REG(intr_enable);
  70. };
  71. #define control_bic control
  72. #define control_bis status
  73. /* Bits in select register */
  74. #define CA_MASK 7
  75. #define LSTRB 8
  76. /* Bits in control register */
  77. #define DO_SEEK 0x80
  78. #define FORMAT 0x40
  79. #define SELECT 0x20
  80. #define WRITE_SECTORS 0x10
  81. #define DO_ACTION 0x08
  82. #define DRIVE2_ENABLE 0x04
  83. #define DRIVE_ENABLE 0x02
  84. #define INTR_ENABLE 0x01
  85. /* Bits in status register */
  86. #define FIFO_1BYTE 0x80
  87. #define FIFO_2BYTE 0x40
  88. #define ERROR 0x20
  89. #define DATA 0x08
  90. #define RDDATA 0x04
  91. #define INTR_PENDING 0x02
  92. #define MARK_BYTE 0x01
  93. /* Bits in intr and intr_enable registers */
  94. #define ERROR_INTR 0x20
  95. #define DATA_CHANGED 0x10
  96. #define TRANSFER_DONE 0x08
  97. #define SEEN_SECTOR 0x04
  98. #define SEEK_DONE 0x02
  99. #define TIMER_DONE 0x01
  100. /* Bits in error register */
  101. #define ERR_DATA_CRC 0x80
  102. #define ERR_ADDR_CRC 0x40
  103. #define ERR_OVERRUN 0x04
  104. #define ERR_UNDERRUN 0x01
  105. /* Bits in setup register */
  106. #define S_SW_RESET 0x80
  107. #define S_GCR_WRITE 0x40
  108. #define S_IBM_DRIVE 0x20
  109. #define S_TEST_MODE 0x10
  110. #define S_FCLK_DIV2 0x08
  111. #define S_GCR 0x04
  112. #define S_COPY_PROT 0x02
  113. #define S_INV_WDATA 0x01
  114. /* Select values for swim3_action */
  115. #define SEEK_POSITIVE 0
  116. #define SEEK_NEGATIVE 4
  117. #define STEP 1
  118. #define MOTOR_ON 2
  119. #define MOTOR_OFF 6
  120. #define INDEX 3
  121. #define EJECT 7
  122. #define SETMFM 9
  123. #define SETGCR 13
  124. /* Select values for swim3_select and swim3_readbit */
  125. #define STEP_DIR 0
  126. #define STEPPING 1
  127. #define MOTOR_ON 2
  128. #define RELAX 3 /* also eject in progress */
  129. #define READ_DATA_0 4
  130. #define ONEMEG_DRIVE 5
  131. #define SINGLE_SIDED 6 /* drive or diskette is 4MB type? */
  132. #define DRIVE_PRESENT 7
  133. #define DISK_IN 8
  134. #define WRITE_PROT 9
  135. #define TRACK_ZERO 10
  136. #define TACHO 11
  137. #define READ_DATA_1 12
  138. #define GCR_MODE 13
  139. #define SEEK_COMPLETE 14
  140. #define TWOMEG_MEDIA 15
  141. /* Definitions of values used in writing and formatting */
  142. #define DATA_ESCAPE 0x99
  143. #define GCR_SYNC_EXC 0x3f
  144. #define GCR_SYNC_CONV 0x80
  145. #define GCR_FIRST_MARK 0xd5
  146. #define GCR_SECOND_MARK 0xaa
  147. #define GCR_ADDR_MARK "\xd5\xaa\x00"
  148. #define GCR_DATA_MARK "\xd5\xaa\x0b"
  149. #define GCR_SLIP_BYTE "\x27\xaa"
  150. #define GCR_SELF_SYNC "\x3f\xbf\x1e\x34\x3c\x3f"
  151. #define DATA_99 "\x99\x99"
  152. #define MFM_ADDR_MARK "\x99\xa1\x99\xa1\x99\xa1\x99\xfe"
  153. #define MFM_INDEX_MARK "\x99\xc2\x99\xc2\x99\xc2\x99\xfc"
  154. #define MFM_GAP_LEN 12
  155. struct floppy_state {
  156. enum swim_state state;
  157. struct swim3 __iomem *swim3; /* hardware registers */
  158. struct dbdma_regs __iomem *dma; /* DMA controller registers */
  159. int swim3_intr; /* interrupt number for SWIM3 */
  160. int dma_intr; /* interrupt number for DMA channel */
  161. int cur_cyl; /* cylinder head is on, or -1 */
  162. int cur_sector; /* last sector we saw go past */
  163. int req_cyl; /* the cylinder for the current r/w request */
  164. int head; /* head number ditto */
  165. int req_sector; /* sector number ditto */
  166. int scount; /* # sectors we're transferring at present */
  167. int retries;
  168. int settle_time;
  169. int secpercyl; /* disk geometry information */
  170. int secpertrack;
  171. int total_secs;
  172. int write_prot; /* 1 if write-protected, 0 if not, -1 dunno */
  173. struct dbdma_cmd *dma_cmd;
  174. int ref_count;
  175. int expect_cyl;
  176. struct timer_list timeout;
  177. int timeout_pending;
  178. int ejected;
  179. wait_queue_head_t wait;
  180. int wanted;
  181. struct macio_dev *mdev;
  182. char dbdma_cmd_space[5 * sizeof(struct dbdma_cmd)];
  183. int index;
  184. struct request *cur_req;
  185. struct blk_mq_tag_set tag_set;
  186. };
  187. #define swim3_err(fmt, arg...) dev_err(&fs->mdev->ofdev.dev, "[fd%d] " fmt, fs->index, arg)
  188. #define swim3_warn(fmt, arg...) dev_warn(&fs->mdev->ofdev.dev, "[fd%d] " fmt, fs->index, arg)
  189. #define swim3_info(fmt, arg...) dev_info(&fs->mdev->ofdev.dev, "[fd%d] " fmt, fs->index, arg)
  190. #ifdef DEBUG
  191. #define swim3_dbg(fmt, arg...) dev_dbg(&fs->mdev->ofdev.dev, "[fd%d] " fmt, fs->index, arg)
  192. #else
  193. #define swim3_dbg(fmt, arg...) do { } while(0)
  194. #endif
  195. static struct floppy_state floppy_states[MAX_FLOPPIES];
  196. static int floppy_count = 0;
  197. static DEFINE_SPINLOCK(swim3_lock);
  198. static unsigned short write_preamble[] = {
  199. 0x4e4e, 0x4e4e, 0x4e4e, 0x4e4e, 0x4e4e, /* gap field */
  200. 0, 0, 0, 0, 0, 0, /* sync field */
  201. 0x99a1, 0x99a1, 0x99a1, 0x99fb, /* data address mark */
  202. 0x990f /* no escape for 512 bytes */
  203. };
  204. static unsigned short write_postamble[] = {
  205. 0x9904, /* insert CRC */
  206. 0x4e4e, 0x4e4e,
  207. 0x9908, /* stop writing */
  208. 0, 0, 0, 0, 0, 0
  209. };
  210. static void seek_track(struct floppy_state *fs, int n);
  211. static void init_dma(struct dbdma_cmd *cp, int cmd, void *buf, int count);
  212. static void act(struct floppy_state *fs);
  213. static void scan_timeout(struct timer_list *t);
  214. static void seek_timeout(struct timer_list *t);
  215. static void settle_timeout(struct timer_list *t);
  216. static void xfer_timeout(struct timer_list *t);
  217. static irqreturn_t swim3_interrupt(int irq, void *dev_id);
  218. /*static void fd_dma_interrupt(int irq, void *dev_id);*/
  219. static int grab_drive(struct floppy_state *fs, enum swim_state state,
  220. int interruptible);
  221. static void release_drive(struct floppy_state *fs);
  222. static int fd_eject(struct floppy_state *fs);
  223. static int floppy_ioctl(struct block_device *bdev, fmode_t mode,
  224. unsigned int cmd, unsigned long param);
  225. static int floppy_open(struct block_device *bdev, fmode_t mode);
  226. static void floppy_release(struct gendisk *disk, fmode_t mode);
  227. static unsigned int floppy_check_events(struct gendisk *disk,
  228. unsigned int clearing);
  229. static int floppy_revalidate(struct gendisk *disk);
  230. static bool swim3_end_request(struct floppy_state *fs, blk_status_t err, unsigned int nr_bytes)
  231. {
  232. struct request *req = fs->cur_req;
  233. swim3_dbg(" end request, err=%d nr_bytes=%d, cur_req=%p\n",
  234. err, nr_bytes, req);
  235. if (err)
  236. nr_bytes = blk_rq_cur_bytes(req);
  237. if (blk_update_request(req, err, nr_bytes))
  238. return true;
  239. __blk_mq_end_request(req, err);
  240. fs->cur_req = NULL;
  241. return false;
  242. }
  243. static void swim3_select(struct floppy_state *fs, int sel)
  244. {
  245. struct swim3 __iomem *sw = fs->swim3;
  246. out_8(&sw->select, RELAX);
  247. if (sel & 8)
  248. out_8(&sw->control_bis, SELECT);
  249. else
  250. out_8(&sw->control_bic, SELECT);
  251. out_8(&sw->select, sel & CA_MASK);
  252. }
  253. static void swim3_action(struct floppy_state *fs, int action)
  254. {
  255. struct swim3 __iomem *sw = fs->swim3;
  256. swim3_select(fs, action);
  257. udelay(1);
  258. out_8(&sw->select, sw->select | LSTRB);
  259. udelay(2);
  260. out_8(&sw->select, sw->select & ~LSTRB);
  261. udelay(1);
  262. }
  263. static int swim3_readbit(struct floppy_state *fs, int bit)
  264. {
  265. struct swim3 __iomem *sw = fs->swim3;
  266. int stat;
  267. swim3_select(fs, bit);
  268. udelay(1);
  269. stat = in_8(&sw->status);
  270. return (stat & DATA) == 0;
  271. }
  272. static blk_status_t swim3_queue_rq(struct blk_mq_hw_ctx *hctx,
  273. const struct blk_mq_queue_data *bd)
  274. {
  275. struct floppy_state *fs = hctx->queue->queuedata;
  276. struct request *req = bd->rq;
  277. unsigned long x;
  278. spin_lock_irq(&swim3_lock);
  279. if (fs->cur_req || fs->state != idle) {
  280. spin_unlock_irq(&swim3_lock);
  281. return BLK_STS_DEV_RESOURCE;
  282. }
  283. blk_mq_start_request(req);
  284. fs->cur_req = req;
  285. if (fs->mdev->media_bay &&
  286. check_media_bay(fs->mdev->media_bay) != MB_FD) {
  287. swim3_dbg("%s", " media bay absent, dropping req\n");
  288. swim3_end_request(fs, BLK_STS_IOERR, 0);
  289. goto out;
  290. }
  291. if (fs->ejected) {
  292. swim3_dbg("%s", " disk ejected\n");
  293. swim3_end_request(fs, BLK_STS_IOERR, 0);
  294. goto out;
  295. }
  296. if (rq_data_dir(req) == WRITE) {
  297. if (fs->write_prot < 0)
  298. fs->write_prot = swim3_readbit(fs, WRITE_PROT);
  299. if (fs->write_prot) {
  300. swim3_dbg("%s", " try to write, disk write protected\n");
  301. swim3_end_request(fs, BLK_STS_IOERR, 0);
  302. goto out;
  303. }
  304. }
  305. /*
  306. * Do not remove the cast. blk_rq_pos(req) is now a sector_t and can be
  307. * 64 bits, but it will never go past 32 bits for this driver anyway, so
  308. * we can safely cast it down and not have to do a 64/32 division
  309. */
  310. fs->req_cyl = ((long)blk_rq_pos(req)) / fs->secpercyl;
  311. x = ((long)blk_rq_pos(req)) % fs->secpercyl;
  312. fs->head = x / fs->secpertrack;
  313. fs->req_sector = x % fs->secpertrack + 1;
  314. fs->state = do_transfer;
  315. fs->retries = 0;
  316. act(fs);
  317. out:
  318. spin_unlock_irq(&swim3_lock);
  319. return BLK_STS_OK;
  320. }
  321. static void set_timeout(struct floppy_state *fs, int nticks,
  322. void (*proc)(struct timer_list *t))
  323. {
  324. if (fs->timeout_pending)
  325. del_timer(&fs->timeout);
  326. fs->timeout.expires = jiffies + nticks;
  327. fs->timeout.function = proc;
  328. add_timer(&fs->timeout);
  329. fs->timeout_pending = 1;
  330. }
  331. static inline void scan_track(struct floppy_state *fs)
  332. {
  333. struct swim3 __iomem *sw = fs->swim3;
  334. swim3_select(fs, READ_DATA_0);
  335. in_8(&sw->intr); /* clear SEEN_SECTOR bit */
  336. in_8(&sw->error);
  337. out_8(&sw->intr_enable, SEEN_SECTOR);
  338. out_8(&sw->control_bis, DO_ACTION);
  339. /* enable intr when track found */
  340. set_timeout(fs, HZ, scan_timeout); /* enable timeout */
  341. }
  342. static inline void seek_track(struct floppy_state *fs, int n)
  343. {
  344. struct swim3 __iomem *sw = fs->swim3;
  345. if (n >= 0) {
  346. swim3_action(fs, SEEK_POSITIVE);
  347. sw->nseek = n;
  348. } else {
  349. swim3_action(fs, SEEK_NEGATIVE);
  350. sw->nseek = -n;
  351. }
  352. fs->expect_cyl = (fs->cur_cyl >= 0)? fs->cur_cyl + n: -1;
  353. swim3_select(fs, STEP);
  354. in_8(&sw->error);
  355. /* enable intr when seek finished */
  356. out_8(&sw->intr_enable, SEEK_DONE);
  357. out_8(&sw->control_bis, DO_SEEK);
  358. set_timeout(fs, 3*HZ, seek_timeout); /* enable timeout */
  359. fs->settle_time = 0;
  360. }
  361. static inline void init_dma(struct dbdma_cmd *cp, int cmd,
  362. void *buf, int count)
  363. {
  364. cp->req_count = cpu_to_le16(count);
  365. cp->command = cpu_to_le16(cmd);
  366. cp->phy_addr = cpu_to_le32(virt_to_bus(buf));
  367. cp->xfer_status = 0;
  368. }
  369. static inline void setup_transfer(struct floppy_state *fs)
  370. {
  371. int n;
  372. struct swim3 __iomem *sw = fs->swim3;
  373. struct dbdma_cmd *cp = fs->dma_cmd;
  374. struct dbdma_regs __iomem *dr = fs->dma;
  375. struct request *req = fs->cur_req;
  376. if (blk_rq_cur_sectors(req) <= 0) {
  377. swim3_warn("%s", "Transfer 0 sectors ?\n");
  378. return;
  379. }
  380. if (rq_data_dir(req) == WRITE)
  381. n = 1;
  382. else {
  383. n = fs->secpertrack - fs->req_sector + 1;
  384. if (n > blk_rq_cur_sectors(req))
  385. n = blk_rq_cur_sectors(req);
  386. }
  387. swim3_dbg(" setup xfer at sect %d (of %d) head %d for %d\n",
  388. fs->req_sector, fs->secpertrack, fs->head, n);
  389. fs->scount = n;
  390. swim3_select(fs, fs->head? READ_DATA_1: READ_DATA_0);
  391. out_8(&sw->sector, fs->req_sector);
  392. out_8(&sw->nsect, n);
  393. out_8(&sw->gap3, 0);
  394. out_le32(&dr->cmdptr, virt_to_bus(cp));
  395. if (rq_data_dir(req) == WRITE) {
  396. /* Set up 3 dma commands: write preamble, data, postamble */
  397. init_dma(cp, OUTPUT_MORE, write_preamble, sizeof(write_preamble));
  398. ++cp;
  399. init_dma(cp, OUTPUT_MORE, bio_data(req->bio), 512);
  400. ++cp;
  401. init_dma(cp, OUTPUT_LAST, write_postamble, sizeof(write_postamble));
  402. } else {
  403. init_dma(cp, INPUT_LAST, bio_data(req->bio), n * 512);
  404. }
  405. ++cp;
  406. out_le16(&cp->command, DBDMA_STOP);
  407. out_8(&sw->control_bic, DO_ACTION | WRITE_SECTORS);
  408. in_8(&sw->error);
  409. out_8(&sw->control_bic, DO_ACTION | WRITE_SECTORS);
  410. if (rq_data_dir(req) == WRITE)
  411. out_8(&sw->control_bis, WRITE_SECTORS);
  412. in_8(&sw->intr);
  413. out_le32(&dr->control, (RUN << 16) | RUN);
  414. /* enable intr when transfer complete */
  415. out_8(&sw->intr_enable, TRANSFER_DONE);
  416. out_8(&sw->control_bis, DO_ACTION);
  417. set_timeout(fs, 2*HZ, xfer_timeout); /* enable timeout */
  418. }
  419. static void act(struct floppy_state *fs)
  420. {
  421. for (;;) {
  422. swim3_dbg(" act loop, state=%d, req_cyl=%d, cur_cyl=%d\n",
  423. fs->state, fs->req_cyl, fs->cur_cyl);
  424. switch (fs->state) {
  425. case idle:
  426. return; /* XXX shouldn't get here */
  427. case locating:
  428. if (swim3_readbit(fs, TRACK_ZERO)) {
  429. swim3_dbg("%s", " locate track 0\n");
  430. fs->cur_cyl = 0;
  431. if (fs->req_cyl == 0)
  432. fs->state = do_transfer;
  433. else
  434. fs->state = seeking;
  435. break;
  436. }
  437. scan_track(fs);
  438. return;
  439. case seeking:
  440. if (fs->cur_cyl < 0) {
  441. fs->expect_cyl = -1;
  442. fs->state = locating;
  443. break;
  444. }
  445. if (fs->req_cyl == fs->cur_cyl) {
  446. swim3_warn("%s", "Whoops, seeking 0\n");
  447. fs->state = do_transfer;
  448. break;
  449. }
  450. seek_track(fs, fs->req_cyl - fs->cur_cyl);
  451. return;
  452. case settling:
  453. /* check for SEEK_COMPLETE after 30ms */
  454. fs->settle_time = (HZ + 32) / 33;
  455. set_timeout(fs, fs->settle_time, settle_timeout);
  456. return;
  457. case do_transfer:
  458. if (fs->cur_cyl != fs->req_cyl) {
  459. if (fs->retries > 5) {
  460. swim3_err("Wrong cylinder in transfer, want: %d got %d\n",
  461. fs->req_cyl, fs->cur_cyl);
  462. swim3_end_request(fs, BLK_STS_IOERR, 0);
  463. fs->state = idle;
  464. return;
  465. }
  466. fs->state = seeking;
  467. break;
  468. }
  469. setup_transfer(fs);
  470. return;
  471. case jogging:
  472. seek_track(fs, -5);
  473. return;
  474. default:
  475. swim3_err("Unknown state %d\n", fs->state);
  476. return;
  477. }
  478. }
  479. }
  480. static void scan_timeout(struct timer_list *t)
  481. {
  482. struct floppy_state *fs = from_timer(fs, t, timeout);
  483. struct swim3 __iomem *sw = fs->swim3;
  484. unsigned long flags;
  485. swim3_dbg("* scan timeout, state=%d\n", fs->state);
  486. spin_lock_irqsave(&swim3_lock, flags);
  487. fs->timeout_pending = 0;
  488. out_8(&sw->control_bic, DO_ACTION | WRITE_SECTORS);
  489. out_8(&sw->select, RELAX);
  490. out_8(&sw->intr_enable, 0);
  491. fs->cur_cyl = -1;
  492. if (fs->retries > 5) {
  493. swim3_end_request(fs, BLK_STS_IOERR, 0);
  494. fs->state = idle;
  495. } else {
  496. fs->state = jogging;
  497. act(fs);
  498. }
  499. spin_unlock_irqrestore(&swim3_lock, flags);
  500. }
  501. static void seek_timeout(struct timer_list *t)
  502. {
  503. struct floppy_state *fs = from_timer(fs, t, timeout);
  504. struct swim3 __iomem *sw = fs->swim3;
  505. unsigned long flags;
  506. swim3_dbg("* seek timeout, state=%d\n", fs->state);
  507. spin_lock_irqsave(&swim3_lock, flags);
  508. fs->timeout_pending = 0;
  509. out_8(&sw->control_bic, DO_SEEK);
  510. out_8(&sw->select, RELAX);
  511. out_8(&sw->intr_enable, 0);
  512. swim3_err("%s", "Seek timeout\n");
  513. swim3_end_request(fs, BLK_STS_IOERR, 0);
  514. fs->state = idle;
  515. spin_unlock_irqrestore(&swim3_lock, flags);
  516. }
  517. static void settle_timeout(struct timer_list *t)
  518. {
  519. struct floppy_state *fs = from_timer(fs, t, timeout);
  520. struct swim3 __iomem *sw = fs->swim3;
  521. unsigned long flags;
  522. swim3_dbg("* settle timeout, state=%d\n", fs->state);
  523. spin_lock_irqsave(&swim3_lock, flags);
  524. fs->timeout_pending = 0;
  525. if (swim3_readbit(fs, SEEK_COMPLETE)) {
  526. out_8(&sw->select, RELAX);
  527. fs->state = locating;
  528. act(fs);
  529. goto unlock;
  530. }
  531. out_8(&sw->select, RELAX);
  532. if (fs->settle_time < 2*HZ) {
  533. ++fs->settle_time;
  534. set_timeout(fs, 1, settle_timeout);
  535. goto unlock;
  536. }
  537. swim3_err("%s", "Seek settle timeout\n");
  538. swim3_end_request(fs, BLK_STS_IOERR, 0);
  539. fs->state = idle;
  540. unlock:
  541. spin_unlock_irqrestore(&swim3_lock, flags);
  542. }
  543. static void xfer_timeout(struct timer_list *t)
  544. {
  545. struct floppy_state *fs = from_timer(fs, t, timeout);
  546. struct swim3 __iomem *sw = fs->swim3;
  547. struct dbdma_regs __iomem *dr = fs->dma;
  548. unsigned long flags;
  549. int n;
  550. swim3_dbg("* xfer timeout, state=%d\n", fs->state);
  551. spin_lock_irqsave(&swim3_lock, flags);
  552. fs->timeout_pending = 0;
  553. out_le32(&dr->control, RUN << 16);
  554. /* We must wait a bit for dbdma to stop */
  555. for (n = 0; (in_le32(&dr->status) & ACTIVE) && n < 1000; n++)
  556. udelay(1);
  557. out_8(&sw->intr_enable, 0);
  558. out_8(&sw->control_bic, WRITE_SECTORS | DO_ACTION);
  559. out_8(&sw->select, RELAX);
  560. swim3_err("Timeout %sing sector %ld\n",
  561. (rq_data_dir(fs->cur_req)==WRITE? "writ": "read"),
  562. (long)blk_rq_pos(fs->cur_req));
  563. swim3_end_request(fs, BLK_STS_IOERR, 0);
  564. fs->state = idle;
  565. spin_unlock_irqrestore(&swim3_lock, flags);
  566. }
  567. static irqreturn_t swim3_interrupt(int irq, void *dev_id)
  568. {
  569. struct floppy_state *fs = (struct floppy_state *) dev_id;
  570. struct swim3 __iomem *sw = fs->swim3;
  571. int intr, err, n;
  572. int stat, resid;
  573. struct dbdma_regs __iomem *dr;
  574. struct dbdma_cmd *cp;
  575. unsigned long flags;
  576. struct request *req = fs->cur_req;
  577. swim3_dbg("* interrupt, state=%d\n", fs->state);
  578. spin_lock_irqsave(&swim3_lock, flags);
  579. intr = in_8(&sw->intr);
  580. err = (intr & ERROR_INTR)? in_8(&sw->error): 0;
  581. if ((intr & ERROR_INTR) && fs->state != do_transfer)
  582. swim3_err("Non-transfer error interrupt: state=%d, dir=%x, intr=%x, err=%x\n",
  583. fs->state, rq_data_dir(req), intr, err);
  584. switch (fs->state) {
  585. case locating:
  586. if (intr & SEEN_SECTOR) {
  587. out_8(&sw->control_bic, DO_ACTION | WRITE_SECTORS);
  588. out_8(&sw->select, RELAX);
  589. out_8(&sw->intr_enable, 0);
  590. del_timer(&fs->timeout);
  591. fs->timeout_pending = 0;
  592. if (sw->ctrack == 0xff) {
  593. swim3_err("%s", "Seen sector but cyl=ff?\n");
  594. fs->cur_cyl = -1;
  595. if (fs->retries > 5) {
  596. swim3_end_request(fs, BLK_STS_IOERR, 0);
  597. fs->state = idle;
  598. } else {
  599. fs->state = jogging;
  600. act(fs);
  601. }
  602. break;
  603. }
  604. fs->cur_cyl = sw->ctrack;
  605. fs->cur_sector = sw->csect;
  606. if (fs->expect_cyl != -1 && fs->expect_cyl != fs->cur_cyl)
  607. swim3_err("Expected cyl %d, got %d\n",
  608. fs->expect_cyl, fs->cur_cyl);
  609. fs->state = do_transfer;
  610. act(fs);
  611. }
  612. break;
  613. case seeking:
  614. case jogging:
  615. if (sw->nseek == 0) {
  616. out_8(&sw->control_bic, DO_SEEK);
  617. out_8(&sw->select, RELAX);
  618. out_8(&sw->intr_enable, 0);
  619. del_timer(&fs->timeout);
  620. fs->timeout_pending = 0;
  621. if (fs->state == seeking)
  622. ++fs->retries;
  623. fs->state = settling;
  624. act(fs);
  625. }
  626. break;
  627. case settling:
  628. out_8(&sw->intr_enable, 0);
  629. del_timer(&fs->timeout);
  630. fs->timeout_pending = 0;
  631. act(fs);
  632. break;
  633. case do_transfer:
  634. if ((intr & (ERROR_INTR | TRANSFER_DONE)) == 0)
  635. break;
  636. out_8(&sw->intr_enable, 0);
  637. out_8(&sw->control_bic, WRITE_SECTORS | DO_ACTION);
  638. out_8(&sw->select, RELAX);
  639. del_timer(&fs->timeout);
  640. fs->timeout_pending = 0;
  641. dr = fs->dma;
  642. cp = fs->dma_cmd;
  643. if (rq_data_dir(req) == WRITE)
  644. ++cp;
  645. /*
  646. * Check that the main data transfer has finished.
  647. * On writing, the swim3 sometimes doesn't use
  648. * up all the bytes of the postamble, so we can still
  649. * see DMA active here. That doesn't matter as long
  650. * as all the sector data has been transferred.
  651. */
  652. if ((intr & ERROR_INTR) == 0 && cp->xfer_status == 0) {
  653. /* wait a little while for DMA to complete */
  654. for (n = 0; n < 100; ++n) {
  655. if (cp->xfer_status != 0)
  656. break;
  657. udelay(1);
  658. barrier();
  659. }
  660. }
  661. /* turn off DMA */
  662. out_le32(&dr->control, (RUN | PAUSE) << 16);
  663. stat = le16_to_cpu(cp->xfer_status);
  664. resid = le16_to_cpu(cp->res_count);
  665. if (intr & ERROR_INTR) {
  666. n = fs->scount - 1 - resid / 512;
  667. if (n > 0) {
  668. blk_update_request(req, 0, n << 9);
  669. fs->req_sector += n;
  670. }
  671. if (fs->retries < 5) {
  672. ++fs->retries;
  673. act(fs);
  674. } else {
  675. swim3_err("Error %sing block %ld (err=%x)\n",
  676. rq_data_dir(req) == WRITE? "writ": "read",
  677. (long)blk_rq_pos(req), err);
  678. swim3_end_request(fs, BLK_STS_IOERR, 0);
  679. fs->state = idle;
  680. }
  681. } else {
  682. if ((stat & ACTIVE) == 0 || resid != 0) {
  683. /* musta been an error */
  684. swim3_err("fd dma error: stat=%x resid=%d\n", stat, resid);
  685. swim3_err(" state=%d, dir=%x, intr=%x, err=%x\n",
  686. fs->state, rq_data_dir(req), intr, err);
  687. swim3_end_request(fs, BLK_STS_IOERR, 0);
  688. fs->state = idle;
  689. break;
  690. }
  691. fs->retries = 0;
  692. if (swim3_end_request(fs, 0, fs->scount << 9)) {
  693. fs->req_sector += fs->scount;
  694. if (fs->req_sector > fs->secpertrack) {
  695. fs->req_sector -= fs->secpertrack;
  696. if (++fs->head > 1) {
  697. fs->head = 0;
  698. ++fs->req_cyl;
  699. }
  700. }
  701. act(fs);
  702. } else
  703. fs->state = idle;
  704. }
  705. break;
  706. default:
  707. swim3_err("Don't know what to do in state %d\n", fs->state);
  708. }
  709. spin_unlock_irqrestore(&swim3_lock, flags);
  710. return IRQ_HANDLED;
  711. }
  712. /*
  713. static void fd_dma_interrupt(int irq, void *dev_id)
  714. {
  715. }
  716. */
  717. /* Called under the mutex to grab exclusive access to a drive */
  718. static int grab_drive(struct floppy_state *fs, enum swim_state state,
  719. int interruptible)
  720. {
  721. unsigned long flags;
  722. swim3_dbg("%s", "-> grab drive\n");
  723. spin_lock_irqsave(&swim3_lock, flags);
  724. if (fs->state != idle && fs->state != available) {
  725. ++fs->wanted;
  726. /* this will enable irqs in order to sleep */
  727. if (!interruptible)
  728. wait_event_lock_irq(fs->wait,
  729. fs->state == available,
  730. swim3_lock);
  731. else if (wait_event_interruptible_lock_irq(fs->wait,
  732. fs->state == available,
  733. swim3_lock)) {
  734. --fs->wanted;
  735. spin_unlock_irqrestore(&swim3_lock, flags);
  736. return -EINTR;
  737. }
  738. --fs->wanted;
  739. }
  740. fs->state = state;
  741. spin_unlock_irqrestore(&swim3_lock, flags);
  742. return 0;
  743. }
  744. static void release_drive(struct floppy_state *fs)
  745. {
  746. struct request_queue *q = disks[fs->index]->queue;
  747. unsigned long flags;
  748. swim3_dbg("%s", "-> release drive\n");
  749. spin_lock_irqsave(&swim3_lock, flags);
  750. fs->state = idle;
  751. spin_unlock_irqrestore(&swim3_lock, flags);
  752. blk_mq_freeze_queue(q);
  753. blk_mq_quiesce_queue(q);
  754. blk_mq_unquiesce_queue(q);
  755. blk_mq_unfreeze_queue(q);
  756. }
  757. static int fd_eject(struct floppy_state *fs)
  758. {
  759. int err, n;
  760. err = grab_drive(fs, ejecting, 1);
  761. if (err)
  762. return err;
  763. swim3_action(fs, EJECT);
  764. for (n = 20; n > 0; --n) {
  765. if (signal_pending(current)) {
  766. err = -EINTR;
  767. break;
  768. }
  769. swim3_select(fs, RELAX);
  770. schedule_timeout_interruptible(1);
  771. if (swim3_readbit(fs, DISK_IN) == 0)
  772. break;
  773. }
  774. swim3_select(fs, RELAX);
  775. udelay(150);
  776. fs->ejected = 1;
  777. release_drive(fs);
  778. return err;
  779. }
  780. static struct floppy_struct floppy_type =
  781. { 2880,18,2,80,0,0x1B,0x00,0xCF,0x6C,NULL }; /* 7 1.44MB 3.5" */
  782. static int floppy_locked_ioctl(struct block_device *bdev, fmode_t mode,
  783. unsigned int cmd, unsigned long param)
  784. {
  785. struct floppy_state *fs = bdev->bd_disk->private_data;
  786. int err;
  787. if ((cmd & 0x80) && !capable(CAP_SYS_ADMIN))
  788. return -EPERM;
  789. if (fs->mdev->media_bay &&
  790. check_media_bay(fs->mdev->media_bay) != MB_FD)
  791. return -ENXIO;
  792. switch (cmd) {
  793. case FDEJECT:
  794. if (fs->ref_count != 1)
  795. return -EBUSY;
  796. err = fd_eject(fs);
  797. return err;
  798. case FDGETPRM:
  799. if (copy_to_user((void __user *) param, &floppy_type,
  800. sizeof(struct floppy_struct)))
  801. return -EFAULT;
  802. return 0;
  803. }
  804. return -ENOTTY;
  805. }
  806. static int floppy_ioctl(struct block_device *bdev, fmode_t mode,
  807. unsigned int cmd, unsigned long param)
  808. {
  809. int ret;
  810. mutex_lock(&swim3_mutex);
  811. ret = floppy_locked_ioctl(bdev, mode, cmd, param);
  812. mutex_unlock(&swim3_mutex);
  813. return ret;
  814. }
  815. static int floppy_open(struct block_device *bdev, fmode_t mode)
  816. {
  817. struct floppy_state *fs = bdev->bd_disk->private_data;
  818. struct swim3 __iomem *sw = fs->swim3;
  819. int n, err = 0;
  820. if (fs->ref_count == 0) {
  821. if (fs->mdev->media_bay &&
  822. check_media_bay(fs->mdev->media_bay) != MB_FD)
  823. return -ENXIO;
  824. out_8(&sw->setup, S_IBM_DRIVE | S_FCLK_DIV2);
  825. out_8(&sw->control_bic, 0xff);
  826. out_8(&sw->mode, 0x95);
  827. udelay(10);
  828. out_8(&sw->intr_enable, 0);
  829. out_8(&sw->control_bis, DRIVE_ENABLE | INTR_ENABLE);
  830. swim3_action(fs, MOTOR_ON);
  831. fs->write_prot = -1;
  832. fs->cur_cyl = -1;
  833. for (n = 0; n < 2 * HZ; ++n) {
  834. if (n >= HZ/30 && swim3_readbit(fs, SEEK_COMPLETE))
  835. break;
  836. if (signal_pending(current)) {
  837. err = -EINTR;
  838. break;
  839. }
  840. swim3_select(fs, RELAX);
  841. schedule_timeout_interruptible(1);
  842. }
  843. if (err == 0 && (swim3_readbit(fs, SEEK_COMPLETE) == 0
  844. || swim3_readbit(fs, DISK_IN) == 0))
  845. err = -ENXIO;
  846. swim3_action(fs, SETMFM);
  847. swim3_select(fs, RELAX);
  848. } else if (fs->ref_count == -1 || mode & FMODE_EXCL)
  849. return -EBUSY;
  850. if (err == 0 && (mode & FMODE_NDELAY) == 0
  851. && (mode & (FMODE_READ|FMODE_WRITE))) {
  852. if (bdev_check_media_change(bdev))
  853. floppy_revalidate(bdev->bd_disk);
  854. if (fs->ejected)
  855. err = -ENXIO;
  856. }
  857. if (err == 0 && (mode & FMODE_WRITE)) {
  858. if (fs->write_prot < 0)
  859. fs->write_prot = swim3_readbit(fs, WRITE_PROT);
  860. if (fs->write_prot)
  861. err = -EROFS;
  862. }
  863. if (err) {
  864. if (fs->ref_count == 0) {
  865. swim3_action(fs, MOTOR_OFF);
  866. out_8(&sw->control_bic, DRIVE_ENABLE | INTR_ENABLE);
  867. swim3_select(fs, RELAX);
  868. }
  869. return err;
  870. }
  871. if (mode & FMODE_EXCL)
  872. fs->ref_count = -1;
  873. else
  874. ++fs->ref_count;
  875. return 0;
  876. }
  877. static int floppy_unlocked_open(struct block_device *bdev, fmode_t mode)
  878. {
  879. int ret;
  880. mutex_lock(&swim3_mutex);
  881. ret = floppy_open(bdev, mode);
  882. mutex_unlock(&swim3_mutex);
  883. return ret;
  884. }
  885. static void floppy_release(struct gendisk *disk, fmode_t mode)
  886. {
  887. struct floppy_state *fs = disk->private_data;
  888. struct swim3 __iomem *sw = fs->swim3;
  889. mutex_lock(&swim3_mutex);
  890. if (fs->ref_count > 0)
  891. --fs->ref_count;
  892. else if (fs->ref_count == -1)
  893. fs->ref_count = 0;
  894. if (fs->ref_count == 0) {
  895. swim3_action(fs, MOTOR_OFF);
  896. out_8(&sw->control_bic, 0xff);
  897. swim3_select(fs, RELAX);
  898. }
  899. mutex_unlock(&swim3_mutex);
  900. }
  901. static unsigned int floppy_check_events(struct gendisk *disk,
  902. unsigned int clearing)
  903. {
  904. struct floppy_state *fs = disk->private_data;
  905. return fs->ejected ? DISK_EVENT_MEDIA_CHANGE : 0;
  906. }
  907. static int floppy_revalidate(struct gendisk *disk)
  908. {
  909. struct floppy_state *fs = disk->private_data;
  910. struct swim3 __iomem *sw;
  911. int ret, n;
  912. if (fs->mdev->media_bay &&
  913. check_media_bay(fs->mdev->media_bay) != MB_FD)
  914. return -ENXIO;
  915. sw = fs->swim3;
  916. grab_drive(fs, revalidating, 0);
  917. out_8(&sw->intr_enable, 0);
  918. out_8(&sw->control_bis, DRIVE_ENABLE);
  919. swim3_action(fs, MOTOR_ON); /* necessary? */
  920. fs->write_prot = -1;
  921. fs->cur_cyl = -1;
  922. mdelay(1);
  923. for (n = HZ; n > 0; --n) {
  924. if (swim3_readbit(fs, SEEK_COMPLETE))
  925. break;
  926. if (signal_pending(current))
  927. break;
  928. swim3_select(fs, RELAX);
  929. schedule_timeout_interruptible(1);
  930. }
  931. ret = swim3_readbit(fs, SEEK_COMPLETE) == 0
  932. || swim3_readbit(fs, DISK_IN) == 0;
  933. if (ret)
  934. swim3_action(fs, MOTOR_OFF);
  935. else {
  936. fs->ejected = 0;
  937. swim3_action(fs, SETMFM);
  938. }
  939. swim3_select(fs, RELAX);
  940. release_drive(fs);
  941. return ret;
  942. }
  943. static const struct block_device_operations floppy_fops = {
  944. .open = floppy_unlocked_open,
  945. .release = floppy_release,
  946. .ioctl = floppy_ioctl,
  947. .check_events = floppy_check_events,
  948. };
  949. static const struct blk_mq_ops swim3_mq_ops = {
  950. .queue_rq = swim3_queue_rq,
  951. };
  952. static void swim3_mb_event(struct macio_dev* mdev, int mb_state)
  953. {
  954. struct floppy_state *fs = macio_get_drvdata(mdev);
  955. struct swim3 __iomem *sw;
  956. if (!fs)
  957. return;
  958. sw = fs->swim3;
  959. if (mb_state != MB_FD)
  960. return;
  961. /* Clear state */
  962. out_8(&sw->intr_enable, 0);
  963. in_8(&sw->intr);
  964. in_8(&sw->error);
  965. }
  966. static int swim3_add_device(struct macio_dev *mdev, int index)
  967. {
  968. struct device_node *swim = mdev->ofdev.dev.of_node;
  969. struct floppy_state *fs = &floppy_states[index];
  970. int rc = -EBUSY;
  971. fs->mdev = mdev;
  972. fs->index = index;
  973. /* Check & Request resources */
  974. if (macio_resource_count(mdev) < 2) {
  975. swim3_err("%s", "No address in device-tree\n");
  976. return -ENXIO;
  977. }
  978. if (macio_irq_count(mdev) < 1) {
  979. swim3_err("%s", "No interrupt in device-tree\n");
  980. return -ENXIO;
  981. }
  982. if (macio_request_resource(mdev, 0, "swim3 (mmio)")) {
  983. swim3_err("%s", "Can't request mmio resource\n");
  984. return -EBUSY;
  985. }
  986. if (macio_request_resource(mdev, 1, "swim3 (dma)")) {
  987. swim3_err("%s", "Can't request dma resource\n");
  988. macio_release_resource(mdev, 0);
  989. return -EBUSY;
  990. }
  991. dev_set_drvdata(&mdev->ofdev.dev, fs);
  992. if (mdev->media_bay == NULL)
  993. pmac_call_feature(PMAC_FTR_SWIM3_ENABLE, swim, 0, 1);
  994. fs->state = idle;
  995. fs->swim3 = (struct swim3 __iomem *)
  996. ioremap(macio_resource_start(mdev, 0), 0x200);
  997. if (fs->swim3 == NULL) {
  998. swim3_err("%s", "Couldn't map mmio registers\n");
  999. rc = -ENOMEM;
  1000. goto out_release;
  1001. }
  1002. fs->dma = (struct dbdma_regs __iomem *)
  1003. ioremap(macio_resource_start(mdev, 1), 0x200);
  1004. if (fs->dma == NULL) {
  1005. swim3_err("%s", "Couldn't map dma registers\n");
  1006. iounmap(fs->swim3);
  1007. rc = -ENOMEM;
  1008. goto out_release;
  1009. }
  1010. fs->swim3_intr = macio_irq(mdev, 0);
  1011. fs->dma_intr = macio_irq(mdev, 1);
  1012. fs->cur_cyl = -1;
  1013. fs->cur_sector = -1;
  1014. fs->secpercyl = 36;
  1015. fs->secpertrack = 18;
  1016. fs->total_secs = 2880;
  1017. init_waitqueue_head(&fs->wait);
  1018. fs->dma_cmd = (struct dbdma_cmd *) DBDMA_ALIGN(fs->dbdma_cmd_space);
  1019. memset(fs->dma_cmd, 0, 2 * sizeof(struct dbdma_cmd));
  1020. fs->dma_cmd[1].command = cpu_to_le16(DBDMA_STOP);
  1021. if (mdev->media_bay == NULL || check_media_bay(mdev->media_bay) == MB_FD)
  1022. swim3_mb_event(mdev, MB_FD);
  1023. if (request_irq(fs->swim3_intr, swim3_interrupt, 0, "SWIM3", fs)) {
  1024. swim3_err("%s", "Couldn't request interrupt\n");
  1025. pmac_call_feature(PMAC_FTR_SWIM3_ENABLE, swim, 0, 0);
  1026. goto out_unmap;
  1027. }
  1028. timer_setup(&fs->timeout, NULL, 0);
  1029. swim3_info("SWIM3 floppy controller %s\n",
  1030. mdev->media_bay ? "in media bay" : "");
  1031. return 0;
  1032. out_unmap:
  1033. iounmap(fs->dma);
  1034. iounmap(fs->swim3);
  1035. out_release:
  1036. macio_release_resource(mdev, 0);
  1037. macio_release_resource(mdev, 1);
  1038. return rc;
  1039. }
  1040. static int swim3_attach(struct macio_dev *mdev,
  1041. const struct of_device_id *match)
  1042. {
  1043. struct floppy_state *fs;
  1044. struct gendisk *disk;
  1045. int rc;
  1046. if (floppy_count >= MAX_FLOPPIES)
  1047. return -ENXIO;
  1048. if (floppy_count == 0) {
  1049. rc = register_blkdev(FLOPPY_MAJOR, "fd");
  1050. if (rc)
  1051. return rc;
  1052. }
  1053. disk = alloc_disk(1);
  1054. if (disk == NULL) {
  1055. rc = -ENOMEM;
  1056. goto out_unregister;
  1057. }
  1058. fs = &floppy_states[floppy_count];
  1059. memset(fs, 0, sizeof(*fs));
  1060. disk->queue = blk_mq_init_sq_queue(&fs->tag_set, &swim3_mq_ops, 2,
  1061. BLK_MQ_F_SHOULD_MERGE);
  1062. if (IS_ERR(disk->queue)) {
  1063. rc = PTR_ERR(disk->queue);
  1064. disk->queue = NULL;
  1065. goto out_put_disk;
  1066. }
  1067. blk_queue_bounce_limit(disk->queue, BLK_BOUNCE_HIGH);
  1068. disk->queue->queuedata = fs;
  1069. rc = swim3_add_device(mdev, floppy_count);
  1070. if (rc)
  1071. goto out_cleanup_queue;
  1072. disk->major = FLOPPY_MAJOR;
  1073. disk->first_minor = floppy_count;
  1074. disk->fops = &floppy_fops;
  1075. disk->private_data = fs;
  1076. disk->events = DISK_EVENT_MEDIA_CHANGE;
  1077. disk->flags |= GENHD_FL_REMOVABLE;
  1078. sprintf(disk->disk_name, "fd%d", floppy_count);
  1079. set_capacity(disk, 2880);
  1080. add_disk(disk);
  1081. disks[floppy_count++] = disk;
  1082. return 0;
  1083. out_cleanup_queue:
  1084. blk_cleanup_queue(disk->queue);
  1085. disk->queue = NULL;
  1086. blk_mq_free_tag_set(&fs->tag_set);
  1087. out_put_disk:
  1088. put_disk(disk);
  1089. out_unregister:
  1090. if (floppy_count == 0)
  1091. unregister_blkdev(FLOPPY_MAJOR, "fd");
  1092. return rc;
  1093. }
  1094. static const struct of_device_id swim3_match[] =
  1095. {
  1096. {
  1097. .name = "swim3",
  1098. },
  1099. {
  1100. .compatible = "ohare-swim3"
  1101. },
  1102. {
  1103. .compatible = "swim3"
  1104. },
  1105. { /* end of list */ }
  1106. };
  1107. static struct macio_driver swim3_driver =
  1108. {
  1109. .driver = {
  1110. .name = "swim3",
  1111. .of_match_table = swim3_match,
  1112. },
  1113. .probe = swim3_attach,
  1114. #ifdef CONFIG_PMAC_MEDIABAY
  1115. .mediabay_event = swim3_mb_event,
  1116. #endif
  1117. #if 0
  1118. .suspend = swim3_suspend,
  1119. .resume = swim3_resume,
  1120. #endif
  1121. };
  1122. int swim3_init(void)
  1123. {
  1124. macio_register_driver(&swim3_driver);
  1125. return 0;
  1126. }
  1127. module_init(swim3_init)
  1128. MODULE_LICENSE("GPL");
  1129. MODULE_AUTHOR("Paul Mackerras");
  1130. MODULE_ALIAS_BLOCKDEV_MAJOR(FLOPPY_MAJOR);