sbpcd.c.txt

Linux块设备驱动分析与模拟实现

402  */
403 static int sbpcd_dbg_ioctl(unsigned long arg, int level)
404 {
405   int val;
406 
407   val = get_fs_long((int *) arg);
408   switch(val)
409     {
410     case 0:     /* OFF */
411       sbpcd_debug = 0;
412       break;
413 
414     default:
415       if (val >= 128) sbpcd_debug &= ~(1 << (val - 128));
416       else sbpcd_debug |= (1 << val);
417     }
418   return(0);
419 }
420 
421 
422 /*==========================================================================*/
423 /*==========================================================================*/
424 /*
425  * Wait a little while (used for polling the drive).  If in initialization,
426  * setting a timeout doesn't work, so just loop for a while.
427  */
428 static inline void sbp_sleep(u_int jifs)
429 {
430    current->state = TASK_INTERRUPTIBLE;
431    current->timeout = jiffies + jifs;
432    schedule();
433 }
434 
435 /*==========================================================================*/
436 /*==========================================================================*/
437 /*
438  *  convert logical_block_address to m-s-f_number (3 bytes only)
439  */
440 static void lba2msf(int lba, u_char *msf)
441 {
442   lba += CD_BLOCK_OFFSET;
443   msf[0] = lba / (CD_SECS*CD_FRAMES);
444   lba %= CD_SECS*CD_FRAMES;
445   msf[1] = lba / CD_FRAMES;
446   msf[2] = lba % CD_FRAMES;
447 }
448 /*==========================================================================*/
449 /*==========================================================================*/
450 /*
451  *  convert msf-bin to msf-bcd
452  */
453 static void bin2bcdx(u_char *p)  /* must work only up to 75 or 99 */
454 {
455   *p=((*p/10)<<4)|(*p%10);
456 }
457 /*==========================================================================*/
458 static u_int blk2msf(u_int blk)
459 {
460   MSF msf;
461   u_int mm;
462 
463   msf.c[3] = 0;
464   msf.c[2] = (blk + CD_BLOCK_OFFSET) / (CD_SECS * CD_FRAMES);
465   mm = (blk + CD_BLOCK_OFFSET) % (CD_SECS * CD_FRAMES);
466   msf.c[1] = mm / CD_FRAMES;
467   msf.c[0] = mm % CD_FRAMES;
468   return (msf.n);
469 }
470 /*==========================================================================*/
471 static u_int make16(u_char rh, u_char rl)
472 {
473   return ((rh<<8)|rl);
474 }
475 /*==========================================================================*/
476 static u_int make32(u_int rh, u_int rl)
477 {
478   return ((rh<<16)|rl);
479 }
480 /*==========================================================================*/
481 static u_char swap_nibbles(u_char i)
482 {
483   return ((i<<4)|(i>>4));
484 }
485 /*==========================================================================*/
486 static u_char byt2bcd(u_char i)
487 {
488   return (((i/10)<<4)+i%10);
489 }
490 /*==========================================================================*/
491 static u_char bcd2bin(u_char bcd)
492 {
493   return ((bcd>>4)*10+(bcd&0x0F));
494 }
495 /*==========================================================================*/
496 static int msf2blk(int msfx)
497 {
498   MSF msf;
499   int i;
500 
501   msf.n=msfx;
502   i=(msf.c[2] * CD_SECS + msf.c[1]) * CD_FRAMES + msf.c[0] - CD_BLOCK_OFFSET;
503   if (i<0) return (0);
504   return (i);
505 }
506 /*==========================================================================*/
507 /* evaluate xx_ReadError code (still mysterious) */ 
508 static int sta2err(int sta)
509 {
510   if (sta<=2) return (sta);
511   if (sta==0x05) return (-4);
512   if (sta==0x06) return (-6);
513   if (sta==0x0d) return (-6);
514   if (sta==0x0e) return (-3);
515   if (sta==0x14) return (-3);
516   if (sta==0x0c) return (-11);
517   if (sta==0x0f) return (-11);
518   if (sta==0x10) return (-11);
519   if (sta>=0x16) return (-12);
520   DS[d].CD_changed=0xFF;
521   if (sta==0x11) return (-15);
522   return (-2);
523 }
524 /*==========================================================================*/
525 static void clr_cmdbuf(void)
526 {
527   int i;
528 
529   for (i=0;i<7;i++) drvcmd[i]=0;
530   cmd_type=0;
531 }
532 /*==========================================================================*/
533 static void mark_timeout(void)
534 {
535   timed_out=1;
536   DPRINTF((DBG_TIM,"SBPCD: timer stopped.\n"));
537 }
538 /*==========================================================================*/
539 static void flush_status(void)
540 {
541 #ifdef CDMKE
542   int i;
543 
544   if (current == task[0])
545     for (i=maxtim02;i!=0;i--) inb(CDi_status);
546   else 
547     {
548       sbp_sleep(150);
549       for (i=maxtim_data;i!=0;i--) inb(CDi_status);
550     }
551 #else
552   timed_out=0;
553   SET_TIMER(mark_timeout,150);
554   do { }
555   while (!timed_out);
556   CLEAR_TIMER;
557   inb(CDi_status);
558 #endif CDMKE
559 }
560 /*==========================================================================*/
561 static int CDi_stat_loop(void)
562 {
563   int i,j;
564   u_long timeout;
565   
566   if (current == task[0])
567     for(i=maxtim16;i!=0;i--)
568       {
569         j=inb(CDi_status);
570         if (!(j&s_not_data_ready)) return (j);
571         if (!(j&s_not_result_ready)) return (j);
572         if (!new_drive) if (j&s_attention) return (j);
573       }
574   else
575     for(timeout = jiffies + 1000, i=maxtim_data; timeout > jiffies; )
576       {
577         for ( ;i!=0;i--)
578           {
579             j=inb(CDi_status);
580             if (!(j&s_not_data_ready)) return (j);
581             if (!(j&s_not_result_ready)) return (j);
582             if (!new_drive) if (j&s_attention) return (j);
583           }
584         sbp_sleep(1);
585         i = 1;
586       }
587   return (-1);
588 }
589 /*==========================================================================*/
590 static int ResponseInfo(void)
591 {
592   int i,j, st=0;
593   u_long timeout;
594 
595   if (current == task[0])
596     for (i=0;i<response_count;i++)
597       {
598         for (j=maxtim_8;j!=0;j--)
599           {
600             st=inb(CDi_status);
601             if (!(st&s_not_result_ready)) break;
602           }
603         if (j==0) return (-1);
604         infobuf[i]=inb(CDi_info);
605       }
606   else 
607     {
608       for (i=0, timeout = jiffies + 100; i < response_count; i++) 
609         {
610           for (j=maxtim_data; ; )
611             {
612               for ( ;j!=0;j-- )
613                 {
614                   st=inb(CDi_status);
615                   if (!(st&s_not_result_ready)) break;
616                 }
617               if (j != 0 || timeout <= jiffies) break;
618               sbp_sleep(0);
619               j = 1;
620             }
621           if (timeout <= jiffies) return (-1);
622           infobuf[i]=inb(CDi_info);
623         }
624     }
625   return (0);
626 }
627 /*==========================================================================*/
628 static int EvaluateStatus(int st)
629 {
630   if (!new_drive)
631     {
632       DS[d].status_byte=0;
633       if (st&p_caddin_old) DS[d].status_byte |= p_door_closed|p_caddy_in;
634       if (st&p_spinning) DS[d].status_byte |= p_spinning;
635       if (st&p_check) DS[d].status_byte |= p_check;
636       if (st&p_busy_old) DS[d].status_byte |= p_busy_new;
637       if (st&p_disk_ok) DS[d].status_byte |= p_disk_ok;
638     }
639   else { DS[d].status_byte=st;
640          st=p_success_old; /* for new drives: fake "successful" bit of old drives */
641        }
642   return (st);
643 }
644 /*==========================================================================*/
645 static int ResponseStatus(void)
646 {
647   int i,j;
648   u_long timeout;
649 
650   DPRINTF((DBG_STA,"SBPCD: doing ResponseStatus...\n"));
651 
652   if (current == task[0])
653     {
654       if (flags_cmd_out & f_respo3) j = maxtim_8;
655       else if (flags_cmd_out&f_respo2) j=maxtim16;
656       else j=maxtim04;
657       for (;j!=0;j--)
658         {
659           i=inb(CDi_status);
660           if (!(i&s_not_result_ready)) break;
661         }
662     }
663   else
664     {
665       if (flags_cmd_out & f_respo3) timeout = jiffies;
666       else if (flags_cmd_out & f_respo2) timeout = jiffies + 1600;
667       else timeout = jiffies + 400;
668       j=maxtim_8;
669       do
670         {
671           for ( ;j!=0;j--)
672             { 
673               i=inb(CDi_status);
674               if (!(i&s_not_result_ready)) break;
675             }
676           if (j != 0 || timeout <= jiffies) break;
677           sbp_sleep(0);
678           j = 1;
679         }
680       while (1);
681     }
682   if (j==0) 
683     { if ((flags_cmd_out & f_respo3) == 0)
684         DPRINTF((DBG_STA,"SBPCD: ResponseStatus: timeout.\n"));
685       EvaluateStatus(0);
686       return (-1);
687     }
688   i=inb(CDi_info);
689   i=EvaluateStatus(i);
690   return (i);
691 }
692 /*==========================================================================*/
693 static void xx_ReadStatus(void)
694 {
695   int i;
696 
697   DPRINTF((DBG_STA,"SBPCD: giving xx_ReadStatus command\n"));
698 
699   if (!new_drive) OUT(CDo_command,0x81);
700   else
701     {
702 #if SBPCD_DIS_IRQ
703       cli();
704 #endif SBPCD_DIS_IRQ
705       OUT(CDo_command,0x05);
706       for (i=0;i<6;i++) OUT(CDo_command,0);
707 #if SBPCD_DIS_IRQ
708       sti();
709 #endif SBPCD_DIS_IRQ
710     }
711 }
712 /*==========================================================================*/
713 int xx_ReadError(void)
714 {
715   int cmd_out(void);
716   int i;
717 
718   clr_cmdbuf();
719   DPRINTF((DBG_ERR,"SBPCD: giving xx_ReadError command.\n"));
720   if (new_drive)
721     {
722       drvcmd[0]=0x82;
723       response_count=8;
724       flags_cmd_out=f_putcmd|f_ResponseStatus;
725     }
726   else
727     {
728       drvcmd[0]=0x82;
729       response_count=6;
730       flags_cmd_out=f_putcmd|f_getsta|f_ResponseStatus;
731     }
732   i=cmd_out();
733   DS[d].error_byte=0;
734   DPRINTF((DBG_ERR,"SBPCD: xx_ReadError: cmd_out(82) returns %d (%02X)\n",i,i));
735   if (i<0) return (i);
736   if (new_drive) i=2;
737   else i=1;
738   DS[d].error_byte=infobuf[i];
739   DPRINTF((DBG_ERR,"SBPCD: xx_ReadError: infobuf[%d] is %d (%02X)\n",i,DS[d].error_byte,DS[d].error_byte));
740   i=sta2err(infobuf[i]);
741   return (i);
742 }
743 /*==========================================================================*/
744 int cmd_out(void)
745 {
746   int i=0;
747 
748   if (flags_cmd_out&f_putcmd)
749     { 
750       DPRINTF((DBG_CMD,"SBPCD: cmd_out: put"));
751       for (i=0;i<7;i++) DPRINTF((DBG_CMD," %02X",drvcmd[i]));
752       DPRINTF((DBG_CMD,"\n"));
753 
754 #if SBPCD_DIS_IRQ
755       cli();
756 #endif SBPCD_DIS_IRQ
757       for (i=0;i<7;i++) OUT(CDo_command,drvcmd[i]);
758 #if SBPCD_DIS_IRQ
759       sti();
760 #endif SBPCD_DIS_IRQ
761     }
762   if (response_count!=0)
763     {
764       if (cmd_type!=0)
765         {
766           if (sbpro_type) OUT(CDo_sel_d_i,0x01);
767           DPRINTF((DBG_INF,"SBPCD: misleaded to try ResponseData.\n"));
768           if (sbpro_type) OUT(CDo_sel_d_i,0x00);
769         }
770       else i=ResponseInfo();
771       if (i<0) return (-9);
772     }
773   if (DS[d].in_SpinUp != 0) DPRINTF((DBG_SPI,"SBPCD: to CDi_stat_loop.\n"));
774   if (flags_cmd_out&f_lopsta)
775     {
776       i=CDi_stat_loop();
777       if ((i<0)||!(i&s_attention)) return (-9);
778     }
779   if (!(flags_cmd_out&f_getsta)) goto LOC_229;
780   
781 LOC_228:
782   if (DS[d].in_SpinUp != 0) DPRINTF((DBG_SPI,"SBPCD: to xx_ReadStatus.\n"));
783   xx_ReadStatus();
784 
785 LOC_229:
786   if (flags_cmd_out&f_ResponseStatus) 
787     {
788       if (DS[d].in_SpinUp != 0) DPRINTF((DBG_SPI,"SBPCD: to ResponseStatus.\n"));
789       i=ResponseStatus();
790                    /* builds status_byte, returns orig. status or p_busy_new */
791       if (i<0) return (-9);
792       if (flags_cmd_out&(f_bit1|f_wait_if_busy))
793         {
794           if (!st_check)
795             {
796               if (flags_cmd_out&f_bit1) if (i&p_success_old) goto LOC_232;
797               if (!(flags_cmd_out&f_wait_if_busy)) goto LOC_228;
798               if (!st_busy) goto LOC_228;
799             }
800         }
801     }
802 LOC_232: