nckernel  0.1
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Groups Pages
fdc_ctrl.c
Go to the documentation of this file.
1 #include <sys/types.h>
2 #include <sys/io.h>
3 #include <stddef.h>
4 #include <stdio.h>
5 #include <stdarg.h>
6 #include <time.h>
7 #include <errno.h>
8 #include <list.h>
9 #include <semaphore.h>
10 #include <device.h>
11 #include <pthread.h>
12 #include <assert.h>
13 
14 #include <isr.h>
15 #include <interrupt.h>
16 #include <fdc.h>
17 #include <fdc_ctrl.h>
18 #include <fdc_single.h>
19 
20 #include <debug.h>
21 
22 static inline void wait_execution(struct fdc_context *ctx)
23 {
24  volatile unsigned char reg;
25  do {
26  reg = inb(REG_MSR(fdc_single_ctrl(ctx->single)));
27  } while (DIR_OF_DATA(&reg) != WRITE_REQUIRED || IN_RESULT_PHASE(&reg));
28 }
29 
30 static inline int send_byte(struct fdc_context *ctx, unsigned char byte)
31 {
32  unsigned char reg;
33 
34  reg = inb(REG_MSR(fdc_single_ctrl(ctx->single)));
35  if (DIR_OF_DATA(&reg) != WRITE_REQUIRED) {
36  return -EINVAL;
37  }
38 
39  outb(REG_FIFO(fdc_single_ctrl(ctx->single)), byte);
40  return 0;
41 }
42 
43 static inline int read_byte(struct fdc_context *ctx, unsigned char *byte)
44 {
45  unsigned char reg;
46  reg = inb(REG_MSR(fdc_single_ctrl(ctx->single)));
47  if (DIR_OF_DATA(&reg) != READ_REQUIRED) {
48  return -EINVAL;
49  }
50 
51  *byte = inb(REG_FIFO(fdc_single_ctrl(ctx->single)));
52  return 0;
53 }
54 
55 static inline int configure(struct fdc_context *ctx)
56 {
57  int ret;
58 
59  ret = send_byte(ctx, CMD_CONFIGURE);
60  if (ret < 0) {
61  goto fault;
62  }
63 
64  ret = send_byte(ctx, 0);
65  if (ret < 0) {
66  goto fault;
67  }
68 
69  ret = send_byte(ctx, 0x20 | 0x10 | 0x01);
70  if (ret < 0) {
71  goto fault;
72  }
73 
74  ret = send_byte(ctx, 0);
75  if (ret < 0) {
76  goto fault;
77  }
78 
79  return 0;
80 
81 fault:
82  VALIDATE_RESULT(ctx);
83  return -EIO;
84 }
85 
86 static inline int sense_interrupt(struct fdc_context *ctx,
87  unsigned char *st0, unsigned char *cyl)
88 {
89  int ret;
90 
91  ret = send_byte(ctx, CMD_SENSE_INTERRUPT);
92  if (ret < 0) {
93  VALIDATE_RESULT(ctx);
94  return -EIO;
95  }
96 
97  ret = read_byte(ctx, st0);
98  if (ret < 0) {
99  return -EIO;
100  }
101 
109  ret = read_byte(ctx, cyl);
110  if (ret < 0) {
111  return -EIO;
112  }
113 
114  return 0;
115 }
116 
117 static inline int calibrate(struct fdc_context *ctx)
118 {
119  unsigned char st0;
120  unsigned char cyl;
121  int ret;
122  sem_t sem;
123 
124  ret = send_byte(ctx, CMD_RECALIBRATE);
125  if (ret < 0) {
126  goto fault;
127  }
128 
129  ret = send_byte(ctx, fdc_single_drive(ctx->single));
130  if (ret < 0) {
131  goto fault;
132  }
133 
134  sem = fdc_single_sem(ctx->single);
135  assert(sem && "Invalid sem");
136  sem_wait(&sem);
137 
138  ret = sense_interrupt(ctx, &st0, &cyl);
139  assert((st0 & 0x20) && "Failed to calibrate");
140  if (ret < 0 || cyl != 0) {
141  return -EIO;
142  }
143 
144  return 0;
145 
146 fault:
147  VALIDATE_RESULT(ctx);
148  return -EIO;
149 }
150 
151 static inline int specify(struct fdc_context *ctx)
152 {
153  int srt = 3; /* Step Rate Time: 3 ms */
154  int hut = 240; /* Head Unload Time: 240 ms */
155  int hlt = 16; /* Head Load Time: 16 ms */
156  int ret;
157 
158  ret = send_byte(ctx, CMD_SPECIFY);
159  if (ret < 0) {
160  goto fault;
161  }
162 
163  ret = send_byte(ctx, ((srt & 0x0F) << 4) | (hut & 0xf));
164  if (ret < 0) {
165  goto fault;
166  }
167 
168  ret = send_byte(ctx, (hlt << 1) | !DMA);
169  if (ret < 0) {
170  goto fault;
171  }
172 
173  return 0;
174 
175 fault:
176  VALIDATE_RESULT(ctx);
177  return -EIO;
178 }
179 
180 #if !defined(NDEBUG)
181 static inline int dump_reg(struct fdc_context *ctx)
182 {
183  if (send_byte(ctx, CMD_DUMPREG) < 0) {
184  VALIDATE_RESULT(ctx);
185  return -EIO;
186  }
187 
188  return 0;
189 }
190 
191 static inline void decode_status(unsigned long status)
192 {
193  static const char *interrupt_code[] = {
194  "Normal termination",
195  "Abnormal termination",
196  "Invalid command",
197  "Abnormal caused by polling",
198  };
199 
200  static const char *drive[] = {
201  "Drive 0",
202  "Drive 1",
203  "Drive 2",
204  "Drive 3",
205  };
206 
207  printf("IC: %s\n", interrupt_code[status >> 30]);
208  printf("Seek: %s\n", ((status >> 29) & 0x01) ? "End" : "None");
209  printf("Head: %d\n", ((status >> 26) & 0x01));
210  printf("Drive: %s\n", drive[(status >> 24) & 0x03]);
211  printf("End of cylinder: %s\n",
212  ((status >> 23) & 0x01) ? "True" : "False");
213 }
214 #else
215 #define dump_reg(ctx)
216 #define decode_status(status)
217 #endif
218 
219 static inline int fdc_reset(struct fdc_context *ctx)
220 {
221  unsigned char st0;
222  unsigned char cyl;
223  int i;
224  int ret;
225  sem_t sem;
226 
227  DOR_RESET(ctx->single);
228  DOR_ENABLE(ctx->single, fdc_single_drive(ctx->single), DMA);
229 
230  sem = fdc_single_sem(ctx->single);
231  assert(sem && "Invalid sem");
232  sem_wait(&sem);
233 
234  outb(REG_CCR(fdc_single_ctrl(ctx->single)), DRATE_500Kbps);
235 
236  for (i = 0; i < 4; i++) {
237  ret = sense_interrupt(ctx, &st0, &cyl);
238  if (ret < 0) {
239  }
240  }
241 
242  /*
243  unsigned char sra;
244  unsigned char srb;
245  do {
246  sra = inb(REG_SRA(fdc_single_ctrl(ctx->single)));
247  srb = inb(REG_SRB(fdc_single_ctrl(ctx->single)));
248 
249  DbgPrint("SRA = 0x%X, SRB = 0x%X\f", sra, srb);
250  } while (!SRA_RESET_DONE(&sra) || !SRB_RESET_DONE(&srb));
251  */
252 
253  // NOTE:
254  // IF PARAMTERS DIFFERENT FROM DEFALT
255  // - CONFIGURE COMMAND
256  // ELSE ..
257 
258  ret = configure(ctx);
259  if (ret < 0) {
260  goto out;
261  }
262 
263  ret = specify(ctx);
264  if (ret < 0) {
265  goto out;
266  }
267 
268  ret = calibrate(ctx);
269 out:
270  return ret;
271 }
272 
273 int fdc_seek(struct fdc_context *ctx, int cyl, int head)
274 {
275  unsigned char st0;
276  unsigned char _cyl;
277  sem_t sem;
278  int ret;
279 
280  ret = send_byte(ctx, CMD_SEEK);
281  if (ret < 0) {
282  goto fault;
283  }
284 
285  ret = send_byte(ctx, (head << 2) | fdc_single_drive(ctx->single));
286  if (ret < 0) {
287  goto fault;
288  }
289 
290  ret = send_byte(ctx, cyl);
291  if (ret < 0) {
292  goto fault;
293  }
294 
295  sem = fdc_single_sem(ctx->single);
296  assert(sem && "Invalid sem");
297  sem_wait(&sem);
298 
299  ret = sense_interrupt(ctx, &st0, &_cyl);
300  assert((st0 & 0x20) && "Failed to seek");
301  return ret;
302 fault:
303  VALIDATE_RESULT(ctx);
304  return -EIO;
305 }
306 
307 int fdc_lock(struct fdc_context *ctx)
308 {
309  unsigned char byte;
310  int ret;
311 
312  ret = send_byte(ctx, 0x80 | CMD_LOCK);
313  if (ret < 0) {
314  VALIDATE_RESULT(ctx);
315  return -EIO;
316  }
317 
318  ret = read_byte(ctx, &byte);
319  if (ret < 0) {
320  return -EIO;
321  }
322 
323  return (byte & 0x10) == 0x10 ? 0 : -EIO;
324 }
325 
326 int fdc_unlock(struct fdc_context *ctx)
327 {
328  unsigned char byte;
329  int ret;
330 
331  ret = send_byte(ctx, CMD_LOCK);
332  if (ret < 0) {
333  VALIDATE_RESULT(ctx);
334  return -EIO;
335  }
336 
337  ret = read_byte(ctx, &byte);
338  if (ret < 0) {
339  return -EIO;
340  }
341 
342  return (byte & 0x10) == 0x10 ? 0 : -EIO;
343 }
344 
345 int fdc_read_deleted_sector(struct fdc_context *ctx,
346  int c, int h, int r)
347 {
348  unsigned char byte;
349  int ret;
350  sem_t sem;
351 
352  ret = send_byte(ctx,
353  CMD_READ_DELETED_DATA | CMD_EXT_MULTITRACK |
354  CMD_EXT_DENSITY | CMD_EXT_SKIP);
355  if (ret < 0) {
356  goto fault;
357  }
358 
359  ret = send_byte(ctx, (h << 2) | fdc_single_drive(ctx->single));
360  if (ret < 0) {
361  goto fault;
362  }
363 
364  ret = send_byte(ctx, c);
365  if (ret < 0) {
366  goto fault;
367  }
368 
369  ret = send_byte(ctx, h);
370  if (ret < 0) {
371  goto fault;
372  }
373 
374  ret = send_byte(ctx, r);
375  if (ret < 0) {
376  goto fault;
377  }
378 
379  ret = send_byte(ctx, SECTOR_512);
380  if (ret < 0) {
381  goto fault;
382  }
383 
384  r = r < FD_SECTOR_PER_TRACK ? r + 1 : FD_SECTOR_PER_TRACK;
385  ret = send_byte(ctx, r);
386  if (ret < 0) {
387  goto fault;
388  }
389 
390  ret = send_byte(ctx, 27);
391  if (ret < 0) {
392  goto fault;
393  }
394 
395  ret = send_byte(ctx, 0xFF);
396  if (ret < 0) {
397  goto fault;
398  }
399 
400  sem = fdc_single_sem(ctx->single);
401  assert(sem && "Invalid sem");
402  sem_wait(&sem);
403 
404  ret = read_byte(ctx, &byte); /* ST0 */
405  if (ret < 0) {
406  goto out;
407  }
408 
409  ret = read_byte(ctx, &byte); /* ST1 */
410  if (ret < 0) {
411  goto out;
412  }
413 
414  ret = read_byte(ctx, &byte); /* ST2 */
415  if (ret < 0) {
416  goto out;
417  }
418 
419  ret = read_byte(ctx, &byte); /* C */
420  if (ret < 0) {
421  goto out;
422  }
423 
424  ret = read_byte(ctx, &byte); /* H */
425  if (ret < 0) {
426  goto out;
427  }
428 
429  ret = read_byte(ctx, &byte); /* R */
430  if (ret < 0) {
431  goto out;
432  }
433 
434  ret = read_byte(ctx, &byte); /* N */
435  goto out;
436 
437 fault:
438  VALIDATE_RESULT(ctx);
439 out:
440  return ret;
441 }
442 
443 int fdc_read_id(struct fdc_context *ctx, int head)
444 {
445  unsigned char byte;
446  sem_t sem;
447  int ret;
448 
449  ret = send_byte(ctx, CMD_READ_ID | CMD_EXT_DENSITY);
450  if (ret < 0) {
451  goto fault;
452  }
453 
454  ret = send_byte(ctx, (head << 2) | fdc_single_drive(ctx->single));
455  if (ret < 0) {
456  goto fault;
457  }
458 
459  sem = fdc_single_sem(ctx->single);
460  assert(sem && "Invalid sem");
461  sem_wait(&sem);
462 
463  ret = read_byte(ctx, &byte); /* ST0 */
464  if (ret < 0) {
465  goto out;
466  }
467 
468  ret = read_byte(ctx, &byte); /* ST1 */
469  if (ret < 0) {
470  goto out;
471  }
472 
473  ret = read_byte(ctx, &byte); /* ST2 */
474  if (ret < 0) {
475  goto out;
476  }
477 
478  ret = read_byte(ctx, &byte); /* C */
479  if (ret < 0) {
480  goto out;
481  }
482 
483  ret = read_byte(ctx, &byte); /* H */
484  if (ret < 0) {
485  goto out;
486  }
487 
488  ret = read_byte(ctx, &byte); /* R */
489  if (ret < 0) {
490  goto out;
491  }
492 
493  ret = read_byte(ctx, &byte); /* N */
494  goto out;
495 
496 fault:
497  VALIDATE_RESULT(ctx);
498 out:
499  return ret;
500 }
501 
502 int fdc_read_sector(struct fdc_context *ctx, int c, int h, int r)
503 {
504  unsigned char byte;
505  int ret;
506  sem_t sem;
507 
508  ret = send_byte(ctx,
509  CMD_READ_DATA | CMD_EXT_MULTITRACK |
510  CMD_EXT_DENSITY | CMD_EXT_SKIP);
511  if (ret < 0) {
512  goto fault;
513  }
514 
515  ret = send_byte(ctx, (h << 2) | fdc_single_drive(ctx->single));
516  if (ret < 0) {
517  goto fault;
518  }
519 
520  ret = send_byte(ctx, c);
521  if (ret < 0) {
522  goto fault;
523  }
524 
525  ret = send_byte(ctx, h);
526  if (ret < 0) {
527  goto fault;
528  }
529 
530  ret = send_byte(ctx, r);
531  if (ret < 0) {
532  goto fault;
533  }
534 
535  ret = send_byte(ctx, SECTOR_512);
536  if (ret < 0) {
537  goto fault;
538  }
539 
544  r = r < FD_SECTOR_PER_TRACK ? r + 1 : FD_SECTOR_PER_TRACK;
545  ret = send_byte(ctx, r);
546  if (ret < 0) {
547  goto fault;
548  }
549 
550  ret = send_byte(ctx, 0x1B); /* 0x1B == GAP1 default size */
551  if (ret < 0) {
552  goto fault;
553  }
554 
555  ret = send_byte(ctx, 0xFF); /* 512 bytes per sector */
556  if (ret < 0) {
557  goto fault;
558  }
559 
560  sem = fdc_single_sem(ctx->single);
561  assert(sem && "Invalid sem");
562  sem_wait(&sem);
563 
564  ret = read_byte(ctx, &byte); /* ST0 */
565  if (ret < 0) {
566  goto out;
567  }
568 
569  ret = read_byte(ctx, &byte); /* ST1 */
570  if (ret < 0) {
571  goto out;
572  }
573  if (byte) {
574  if (byte & 0x80) {
575  assert(!"Floppy had too few sectors");
576  }
577  if (byte & 0x10) {
578  assert(!"Too slow to get byte in or out of FIFO port");
579  }
580  }
581 
582  ret = read_byte(ctx, &byte); /* ST2 */
583  if (ret < 0) {
584  goto out;
585  }
586 
587  ret = read_byte(ctx, &byte); /* C */
588  if (ret < 0) {
589  goto out;
590  }
591 
592  ret = read_byte(ctx, &byte); /* H */
593  if (ret < 0) {
594  goto out;
595  }
596 
597  ret = read_byte(ctx, &byte); /* R */
598  if (ret < 0) {
599  goto out;
600  }
601 
602  ret = read_byte(ctx, &byte); /* N */
603  assert((byte == 2) && "result != 2");
604  goto out;
605 
606 fault:
607  VALIDATE_RESULT(ctx);
608 out:
609  return ret;
610 }
611 
612 int fdc_write_sector(struct fdc_context *ctx,
613  unsigned char c, unsigned char h, unsigned char r)
614 {
615  int ret;
616  unsigned char byte;
617  sem_t sem;
618 
619  ret = send_byte(ctx, CMD_WRITE_DATA | CMD_EXT_MULTITRACK |
620  CMD_EXT_DENSITY);
621  if (ret < 0) {
622  goto fault;
623  }
624 
625  ret = send_byte(ctx, (h << 2) | fdc_single_drive(ctx->single));
626  if (ret < 0) {
627  goto fault;
628  }
629 
630  ret = send_byte(ctx, c);
631  if (ret < 0) {
632  goto fault;
633  }
634 
635  ret = send_byte(ctx, h);
636  if (ret < 0) {
637  goto fault;
638  }
639 
640  ret = send_byte(ctx, r);
641  if (ret < 0) {
642  goto fault;
643  }
644 
645  ret = send_byte(ctx, SECTOR_512);
646  if (ret < 0) {
647  goto fault;
648  }
649 
650  r = r < FD_SECTOR_PER_TRACK ? r + 1 : FD_SECTOR_PER_TRACK;
651  ret = send_byte(ctx, r);
652  if (ret < 0) {
653  goto fault;
654  }
655 
656  ret = send_byte(ctx, 0x1B);
657  if (ret < 0) {
658  goto fault;
659  }
660 
661  ret = send_byte(ctx, 0xFF);
662  if (ret < 0) {
663  goto fault;
664  }
665 
666  sem = fdc_single_sem(ctx->single);
667  assert(sem && "Invalid sem");
668  sem_wait(&sem);
669 
670  ret = read_byte(ctx, &byte); /* ST0 */
671  if (ret < 0) {
672  goto out;
673  }
674 
675  ret = read_byte(ctx, &byte); /* ST1 */
676  if (ret < 0) {
677  goto out;
678  }
679  if (byte) {
680  if (byte & 0x80) {
681  assert(!"Floppy had too few sectors");
682  }
683  if (byte & 0x10) {
684  assert(!"Too slow to get byte in or out of FIFO port");
685  }
686  if (byte & 0x02) {
687  assert(!"Write protected");
688  }
689  }
690 
691  ret = read_byte(ctx, &byte); /* ST2 */
692  if (ret < 0) {
693  goto out;
694  }
695 
696  ret = read_byte(ctx, &byte); /* C */
697  if (ret < 0) {
698  goto out;
699  }
700 
701  ret = read_byte(ctx, &byte); /* H */
702  if (ret < 0) {
703  goto out;
704  }
705 
706  ret = read_byte(ctx, &byte); /* R */
707  if (ret < 0) {
708  goto out;
709  }
710 
711  ret = read_byte(ctx, &byte); /* N */
712  goto out;
713 
714 fault:
715  VALIDATE_RESULT(ctx);
716 out:
717  return ret;
718 }
719 
720 int fdc_write_deleted_sector(struct fdc_context *ctx,
721  unsigned char c, unsigned char h, unsigned char r)
722 {
723  int ret;
724  unsigned char byte;
725  sem_t sem;
726 
727  ret = send_byte(ctx, CMD_WRITE_DELETED_DATA | CMD_EXT_MULTITRACK |
728  CMD_EXT_DENSITY);
729  if (ret < 0) {
730  goto fault;
731  }
732 
733  ret = send_byte(ctx, (h << 2) | fdc_single_drive(ctx->single));
734  if (ret < 0) {
735  goto fault;
736  }
737 
738  ret = send_byte(ctx, c);
739  if (ret < 0) {
740  goto fault;
741  }
742 
743  ret = send_byte(ctx, h);
744  if (ret < 0) {
745  goto fault;
746  }
747 
748  ret = send_byte(ctx, r);
749  if (ret < 0) {
750  goto fault;
751  }
752 
753  ret = send_byte(ctx, SECTOR_512);
754  if (ret < 0) {
755  goto fault;
756  }
757 
758  r = r < FD_SECTOR_PER_TRACK ? r + 1 : FD_SECTOR_PER_TRACK;
759  ret = send_byte(ctx, r);
760  if (ret < 0) {
761  goto fault;
762  }
763 
764  ret = send_byte(ctx, 0x1B);
765  if (ret < 0) {
766  goto fault;
767  }
768 
769  ret = send_byte(ctx, 0xFF);
770  if (ret < 0) {
771  goto fault;
772  }
773 
774  sem = fdc_single_sem(ctx->single);
775  assert(sem && "Invalid sem");
776  sem_wait(&sem);
777 
778  ret = read_byte(ctx, &byte); /* ST0 */
779  if (ret < 0) {
780  goto out;
781  }
782 
783  ret = read_byte(ctx, &byte); /* ST1 */
784  if (ret < 0) {
785  goto out;
786  }
787 
788  ret = read_byte(ctx, &byte); /* ST2 */
789  if (ret < 0) {
790  goto out;
791  }
792 
793  ret = read_byte(ctx, &byte); /* C */
794  if (ret < 0) {
795  goto out;
796  }
797 
798  ret = read_byte(ctx, &byte); /* H */
799  if (ret < 0) {
800  goto out;
801  }
802 
803  ret = read_byte(ctx, &byte); /* R */
804  if (ret < 0) {
805  goto out;
806  }
807 
808  ret = read_byte(ctx, &byte); /* N */
809  goto out;
810 
811 fault:
812  VALIDATE_RESULT(ctx);
813 out:
814  return ret;
815 }
816 
817 int fdc_read_track(struct fdc_context *ctx,
818  unsigned char c, unsigned char h, unsigned char r)
819 {
820  int ret;
821  unsigned char byte;
822  sem_t sem;
823 
824  ret = send_byte(ctx, CMD_READ_TRACK | CMD_EXT_DENSITY);
825  if (ret < 0) {
826  goto fault;
827  }
828 
829  ret = send_byte(ctx, (h << 2) | fdc_single_drive(ctx->single));
830  if (ret < 0) {
831  goto fault;
832  }
833 
834  ret = send_byte(ctx, c);
835  if (ret < 0) {
836  goto fault;
837  }
838 
839  ret = send_byte(ctx, h);
840  if (ret < 0) {
841  goto fault;
842  }
843 
844  ret = send_byte(ctx, r);
845  if (ret < 0) {
846  goto fault;
847  }
848 
849  ret = send_byte(ctx, SECTOR_512);
850  if (ret < 0) {
851  goto fault;
852  }
853 
854  r = r < FD_SECTOR_PER_TRACK ? r + 1 : FD_SECTOR_PER_TRACK;
855  ret = send_byte(ctx, r);
856  if (ret < 0) {
857  goto fault;
858  }
859 
860  ret = send_byte(ctx, 0x1B);
861  if (ret < 0) {
862  goto fault;
863  }
864 
865  ret = send_byte(ctx, 0xFF);
866  if (ret < 0) {
867  goto fault;
868  }
869 
870  sem = fdc_single_sem(ctx->single);
871  assert(sem && "Invalid sem");
872  sem_wait(&sem);
873 
874  ret = read_byte(ctx, &byte); /* ST0 */
875  if (ret < 0) {
876  goto out;
877  }
878 
879  ret = read_byte(ctx, &byte); /* ST1 */
880  if (ret < 0) {
881  goto out;
882  }
883 
884  ret = read_byte(ctx, &byte); /* ST2 */
885  if (ret < 0) {
886  goto out;
887  }
888 
889  ret = read_byte(ctx, &byte); /* C */
890  if (ret < 0) {
891  goto out;
892  }
893 
894  ret = read_byte(ctx, &byte); /* H */
895  if (ret < 0) {
896  goto out;
897  }
898 
899  ret = read_byte(ctx, &byte); /* R */
900  if (ret < 0) {
901  goto out;
902  }
903 
904  ret = read_byte(ctx, &byte); /* N */
905  goto out;
906 
907 fault:
908  VALIDATE_RESULT(ctx);
909 out:
910  return ret;
911 }
912 
913 int fdc_verify(struct fdc_context *ctx,
914  unsigned char c, unsigned char h, unsigned char r)
915 {
916  int ret;
917  unsigned char byte;
918  sem_t sem;
919 
920  ret = send_byte(ctx, CMD_VERIFY | CMD_EXT_DENSITY |
921  CMD_EXT_MULTITRACK | CMD_EXT_SKIP);
922  if (ret < 0) {
923  goto fault;
924  }
925 
926  ret = send_byte(ctx, (h << 2) | fdc_single_drive(ctx->single));
927  if (ret < 0) {
928  goto fault;
929  }
930 
931  ret = send_byte(ctx, c);
932  if (ret < 0) {
933  goto fault;
934  }
935 
936  ret = send_byte(ctx, h);
937  if (ret < 0) {
938  goto fault;
939  }
940 
941  ret = send_byte(ctx, r);
942  if (ret < 0) {
943  goto fault;
944  }
945 
946  r = r < FD_SECTOR_PER_TRACK ? r + 1 : FD_SECTOR_PER_TRACK;
947  ret = send_byte(ctx, r);
948  if (ret < 0) {
949  goto fault;
950  }
951 
952  ret = send_byte(ctx, 0x1B);
953  if (ret < 0) {
954  goto fault;
955  }
956 
957  ret = send_byte(ctx, 0xFF);
958  if (ret < 0) {
959  goto fault;
960  }
961 
962  sem = fdc_single_sem(ctx->single);
963  assert(sem && "Invalid sem");
964  sem_wait(&sem);
965 
966  ret = read_byte(ctx, &byte); /* ST0 */
967  if (ret < 0) {
968  goto out;
969  }
970 
971  ret = read_byte(ctx, &byte); /* ST1 */
972  if (ret < 0) {
973  goto out;
974  }
975 
976  ret = read_byte(ctx, &byte); /* ST2 */
977  if (ret < 0) {
978  goto out;
979  }
980 
981  ret = read_byte(ctx, &byte); /* C */
982  if (ret < 0) {
983  goto out;
984  }
985 
986  ret = read_byte(ctx, &byte); /* H */
987  if (ret < 0) {
988  goto out;
989  }
990 
991  ret = read_byte(ctx, &byte); /* R */
992  if (ret < 0) {
993  goto out;
994  }
995 
996  ret = read_byte(ctx, &byte); /* N */
997  goto out;
998 
999 fault:
1000  VALIDATE_RESULT(ctx);
1001 out:
1002  return ret;
1003 }
1004 
1005 int fdc_version(struct fdc_context *ctx, unsigned char *ver)
1006 {
1007  int ret;
1008 
1009  ret = send_byte(ctx, CMD_VERSION);
1010  if (ret < 0) {
1011  VALIDATE_RESULT(ctx);
1012  goto out;
1013  }
1014 
1015  ret = read_byte(ctx, ver);
1016 
1017 out:
1018  return ret;
1019 }
1020 
1021 void fdc_motor_on(struct fdc_context *ctx)
1022 {
1023  unsigned char reg;
1024 
1025  reg = inb(REG_DOR(fdc_single_ctrl(ctx->single)));
1026  DOR_t *dor = (DOR_t*)&reg;
1027  dor->motor = 0x01 << fdc_single_drive(ctx->single);
1028  outb(REG_DOR(fdc_single_ctrl(ctx->single)), reg);
1029 }
1030 
1031 void fdc_motor_off(struct fdc_context *ctx)
1032 {
1033  unsigned char reg;
1034 
1035  reg = inb(REG_DOR(fdc_single_ctrl(ctx->single)));
1036  DOR_t *dor = (DOR_t*)&reg;
1037  dor->motor &= ~(0x01 << fdc_single_drive(ctx->single));
1038  outb(REG_DOR(fdc_single_ctrl(ctx->single)), reg);
1039 }
1040 
1041 int fdc_init_ctrl(struct fdc_context *ctx, int minor)
1042 {
1043  enum base_address ctrl;
1044  enum drive_letter drive;
1045 
1046  switch (minor) {
1047  case 0:
1048  case 1:
1049  ctrl = FDC_PRIMARY;
1050  drive = DRIVE_A + minor;
1051  break;
1052  default:
1053  return -EINVAL;
1054  }
1055 
1056  ctx->single = fdc_single_get(ctrl, drive);
1057  if (ctx->single) {
1058  if (!fdc_single_is_initiated(ctx->single)) {
1059  int ret;
1060  unsigned char ver;
1061 
1062  ret = fdc_version(ctx, &ver);
1063  if (ret < 0 || ver != 0x90) {
1064  assert(ver == 0x90 && "Not supported FDC");
1065  return -ENOTSUP;
1066  }
1067 
1068  fdc_reset(ctx);
1069  fdc_single_initiate(ctx->single);
1070  }
1071 
1072  return 0;
1073  }
1074 
1075  return -EFAULT;
1076 }
1077 
1078 int fdc_fini_ctrl(struct fdc_context *ctx)
1079 {
1080  fdc_single_put(ctx->single);
1081  ctx->single = NULL;
1082  return 0;
1083 }
1084 
1085 /* End of control functions */
Generated on Thu Nov 7 2013 02:45:25 for nckernel by   doxygen 1.8.4