API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Jun 27th, 2017
82
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C++ 15.99 KB | None | 0 0
raw download clone embed print report
  1. #include <hal.h>
  2. #include <mmngr_phys.h>
  3.  
  4. #include <DebugDisplay.h>
  5.  
  6. #include "flpydsk.h"
  7.  
  8.  
  9.  
  10. #include <kybrd.h>
  11.  
  12.  
  13. //*************************************************************************
  14. //  Definitions
  15. //*************************************************************************
  16. //=========================================================================
  17. //  Enumerations
  18. //=========================================================================
  19.  
  20.  
  21. /*
  22.  *  Controller I/O Ports. Please see chapter for additional ports
  23.  */
  24.  
  25. enum FLPYDSK_IO {
  26.  
  27.     FLPYDSK_DOR     =   0x3f2,
  28.     FLPYDSK_MSR     =   0x3f4,
  29.     FLPYDSK_FIFO    =   0x3f5,
  30.     FLPYDSK_CTRL    =   0x3f7
  31. };
  32.  
  33. /*
  34.  *  Bits 0-4 of command byte. Please see chapter for additional commands
  35.  */
  36.  
  37. enum FLPYDSK_CMD {
  38.  
  39.     FDC_CMD_READ_TRACK  =   2,
  40.     FDC_CMD_SPECIFY     =   3,
  41.     FDC_CMD_CHECK_STAT  =   4,
  42.     FDC_CMD_WRITE_SECT  =   5,
  43.     FDC_CMD_READ_SECT   =   6,
  44.     FDC_CMD_CALIBRATE   =   7,
  45.     FDC_CMD_CHECK_INT   =   8,
  46.     FDC_CMD_FORMAT_TRACK=   0xd,
  47.     FDC_CMD_SEEK        =   0xf
  48. };
  49.  
  50. /*
  51.  *  Additional command masks. Can be masked with above commands
  52.  */
  53.  
  54. enum FLPYDSK_CMD_EXT {
  55.  
  56.     FDC_CMD_EXT_SKIP        =   0x20,   //00100000
  57.     FDC_CMD_EXT_DENSITY     =   0x40,   //01000000
  58.     FDC_CMD_EXT_MULTITRACK  =   0x80    //10000000
  59. };
  60.  
  61. /*
  62.  *  Digital Output Register
  63.  */
  64.  
  65. enum FLPYDSK_DOR_MASK {
  66.  
  67.     FLPYDSK_DOR_MASK_DRIVE0         =   0,  //00000000  = here for completeness sake
  68.     FLPYDSK_DOR_MASK_DRIVE1         =   1,  //00000001
  69.     FLPYDSK_DOR_MASK_DRIVE2         =   2,  //00000010
  70.     FLPYDSK_DOR_MASK_DRIVE3         =   3,  //00000011
  71.     FLPYDSK_DOR_MASK_RESET          =   4,  //00000100
  72.     FLPYDSK_DOR_MASK_DMA            =   8,  //00001000
  73.     FLPYDSK_DOR_MASK_DRIVE0_MOTOR   =   16, //00010000
  74.     FLPYDSK_DOR_MASK_DRIVE1_MOTOR   =   32, //00100000
  75.     FLPYDSK_DOR_MASK_DRIVE2_MOTOR   =   64, //01000000
  76.     FLPYDSK_DOR_MASK_DRIVE3_MOTOR   =   128 //10000000
  77. };
  78.  
  79. /*
  80.  *  Main Status Register
  81.  */
  82.  
  83. enum FLPYDSK_MSR_MASK {
  84.  
  85.     FLPYDSK_MSR_MASK_DRIVE1_POS_MODE    =   1,  //00000001
  86.     FLPYDSK_MSR_MASK_DRIVE2_POS_MODE    =   2,  //00000010
  87.     FLPYDSK_MSR_MASK_DRIVE3_POS_MODE    =   4,  //00000100
  88.     FLPYDSK_MSR_MASK_DRIVE4_POS_MODE    =   8,  //00001000
  89.     FLPYDSK_MSR_MASK_BUSY               =   16, //00010000
  90.     FLPYDSK_MSR_MASK_DMA                =   32, //00100000
  91.     FLPYDSK_MSR_MASK_DATAIO             =   64, //01000000
  92.     FLPYDSK_MSR_MASK_DATAREG            =   128 //10000000
  93. };
  94.  
  95. /*
  96.  *  Controller Status Port 0
  97.  */
  98.  
  99. enum FLPYDSK_ST0_MASK {
  100.  
  101.     FLPYDSK_ST0_MASK_DRIVE0     =   0,      //00000000  =   for completness sake
  102.     FLPYDSK_ST0_MASK_DRIVE1     =   1,      //00000001
  103.     FLPYDSK_ST0_MASK_DRIVE2     =   2,      //00000010
  104.     FLPYDSK_ST0_MASK_DRIVE3     =   3,      //00000011
  105.     FLPYDSK_ST0_MASK_HEADACTIVE =   4,      //00000100
  106.     FLPYDSK_ST0_MASK_NOTREADY   =   8,      //00001000
  107.     FLPYDSK_ST0_MASK_UNITCHECK  =   16,     //00010000
  108.     FLPYDSK_ST0_MASK_SEEKEND    =   32,     //00100000
  109.     FLPYDSK_ST0_MASK_INTCODE    =   64      //11000000
  110. };
  111.  
  112. /*
  113.  * LPYDSK_ST0_MASK_INTCODE types
  114.  */
  115.  
  116. enum FLPYDSK_ST0_INTCODE_TYP {
  117.  
  118.     FLPYDSK_ST0_TYP_NORMAL      =   0,
  119.     FLPYDSK_ST0_TYP_ABNORMAL_ERR=   1,
  120.     FLPYDSK_ST0_TYP_INVALID_ERR =   2,
  121.     FLPYDSK_ST0_TYP_NOTREADY    =   3
  122. };
  123.  
  124. /*
  125.  *  GAP 3 sizes
  126.  */
  127.  
  128. enum FLPYDSK_GAP3_LENGTH {
  129.  
  130.     FLPYDSK_GAP3_LENGTH_STD = 42,
  131.     FLPYDSK_GAP3_LENGTH_5_14= 32,
  132.     FLPYDSK_GAP3_LENGTH_3_5= 27
  133. };
  134.  
  135. /*
  136.  *  Formula: 2^sector_number * 128, where ^ denotes "to the power of"
  137.  */
  138.  
  139. enum FLPYDSK_SECTOR_DTL {
  140.  
  141.     FLPYDSK_SECTOR_DTL_128  =   0,
  142.     FLPYDSK_SECTOR_DTL_256  =   1,
  143.     FLPYDSK_SECTOR_DTL_512  =   2,
  144.     FLPYDSK_SECTOR_DTL_1024 =   4
  145. };
  146.  
  147. KEYCODE getch2 () {
  148.  
  149.     KEYCODE key = KEY_UNKNOWN;
  150.  
  151.     //! wait for a keypress
  152.     while (key==KEY_UNKNOWN)
  153.         key = kkybrd_get_last_key ();
  154.  
  155.     //! discard last keypress (we handled it) and return
  156.     kkybrd_discard_last_key ();
  157.     return key;
  158. }
  159.  
  160. //=========================================================================
  161. //  Constants
  162. //=========================================================================
  163.  
  164. //! floppy irq
  165. const int FLOPPY_IRQ = 6;
  166.  
  167. //! sectors per track
  168. const int FLPY_SECTORS_PER_TRACK = 18;
  169.  
  170. //! DMA tranfer buffer starts here and ends at 0x1000+64k
  171. //! this must be below 16MB and in idenitity mapped memory
  172. int DMA_BUFFER = 0x1000;        // 0x1000
  173.  
  174. //! FDC uses DMA channel 2
  175. const int FDC_DMA_CHANNEL = 2;
  176.  
  177. //=========================================================================
  178. //  Private Data
  179. //=========================================================================
  180.  
  181. //! Working Drive - Default: 0
  182. static uint8_t  _CurrentDrive = 0;
  183.  
  184. //! Set when IRQ fires
  185. static volatile uint8_t _FloppyDiskIRQ = 0;
  186.  
  187. //=========================================================================
  188. //  External References
  189. //=========================================================================
  190.  
  191. //! Sleep Function (in milliseconds)
  192. extern void sleep (int);
  193.  
  194. //=========================================================================
  195. //  Private Functions
  196. //=========================================================================
  197.  
  198. /*
  199.  *  DMA Routines.
  200.  */
  201.  
  202. bool _cdecl dma_initialise_floppy(uint8_t* buffer, unsigned length){
  203.     DebugPrintf("Debug : DMA intlizing\n");
  204.     getch2();
  205.    union{
  206.       uint8_t byte[4];//Lo[0], Mid[1], Hi[2]
  207.       unsigned long l;
  208.    }a, c;
  209.  
  210.    a.l=(unsigned)buffer;
  211.    c.l=(unsigned)length-1;
  212.  
  213.     DebugPrintf("Debug : Strt lil\n");
  214.     getch2();
  215.    //Check for buffer issues
  216.    if ((a.l >> 24) || (c.l >> 16) || (((a.l & 0xffff)+c.l) >> 16)){
  217. #ifdef _DEBUG
  218.       _asm{
  219.          mov      eax, 0x1337
  220.          cli
  221.          hlt
  222.       }
  223. #endif
  224.       DebugPrintf("\n\n\nDMA ERROR\n");
  225.       return false;
  226.    }
  227.     DebugPrintf("Debug : Tsk lil ok\n");
  228.     getch2();
  229.  
  230.    
  231.    dma_reset (1);
  232.  
  233.    DebugPrintf("Debug : Strt Dma Reset\n");
  234.    getch2();
  235.  
  236.    dma_mask_channel( FDC_DMA_CHANNEL );
  237.  
  238.     DebugPrintf("Debug : Mask channel 2 \n");
  239.     getch2();
  240.  
  241.    dma_reset_flipflop ( 1 );//Flipflop reset on DMA 1
  242.  
  243.     DebugPrintf("Debug : Flip flop 1\n");
  244.     getch2();
  245.  
  246.    dma_set_address( FDC_DMA_CHANNEL, a.byte[0],a.byte[1]);//Buffer address
  247.  
  248.     DebugPrintf("Debug : Buffer address\n");
  249.     getch2();
  250.  
  251.    dma_reset_flipflop( 1 );//Flipflop reset on DMA 1
  252.  
  253.     DebugPrintf("Debug : Flip Flop 1 ag\n");
  254.     getch2();
  255.  
  256.    dma_set_count( FDC_DMA_CHANNEL, c.byte[0],c.byte[1]);//Set count
  257.  
  258.     DebugPrintf("Debug : Set count\n");
  259.     getch2();
  260.  
  261.    dma_set_read ( FDC_DMA_CHANNEL );
  262.  
  263.     DebugPrintf("Debug : Set read\n");
  264.     getch2();
  265.  
  266.    dma_unmask_all( 1 );//Unmask channel 2
  267.  
  268.     DebugPrintf("Debug : un mask\n");
  269.     getch2();
  270.  
  271.    return true;
  272. }
  273.  
  274. /*
  275.  *  Basic Controller I/O Routines
  276.  */
  277.  
  278. //! return fdc status
  279. uint8_t flpydsk_read_status () {
  280.  
  281.     //! just return main status register
  282.     return inportb (FLPYDSK_MSR);
  283. }
  284.  
  285. //! write to the fdc dor
  286. void flpydsk_write_dor (uint8_t val ) {
  287.  
  288.     //! write the digital output register
  289.     outportb (FLPYDSK_DOR, val);
  290. }
  291.  
  292. //! send command byte to fdc
  293. void flpydsk_send_command (uint8_t cmd) {
  294.  
  295.     //! wait until data register is ready. We send commands to the data register
  296.     for (int i = 0; i < 500; i++ )
  297.         if ( flpydsk_read_status () & FLPYDSK_MSR_MASK_DATAREG )
  298.             return outportb (FLPYDSK_FIFO, cmd);
  299. }
  300.  
  301. //! get data from fdc
  302. uint8_t flpydsk_read_data () {
  303.  
  304.     //! same as above function but returns data register for reading
  305.     for (int i = 0; i < 500; i++ )
  306.         if ( flpydsk_read_status () & FLPYDSK_MSR_MASK_DATAREG )
  307.             return inportb (FLPYDSK_FIFO);
  308.  
  309.     return 0;
  310. }
  311.  
  312. //! write to the configuation control register
  313. void flpydsk_write_ccr (uint8_t val) {
  314.  
  315.     //! write the configuation control
  316.     outportb (FLPYDSK_CTRL, val);
  317. }
  318.  
  319. /*
  320.  *  Interrupt Handling Routines
  321.  */
  322.  
  323. //! wait for irq to fire
  324. inline void flpydsk_wait_irq () {
  325.  
  326.     //! wait for irq to fire
  327.     while (_FloppyDiskIRQ==0)
  328.         ;
  329.     _FloppyDiskIRQ = 0;
  330. }
  331.  
  332.  
  333. //! floppy disk irq handler
  334. void _cdecl i86_flpy_irq () {
  335.     _asm add esp, 12
  336.     _asm pushad
  337.     _asm cli
  338.  
  339.     //! irq fired
  340.     _FloppyDiskIRQ = 1;
  341.     //DebugPrintf("Interrupt");
  342.     //! tell hal we are done
  343.     interruptdone( FLOPPY_IRQ );
  344.  
  345.     _asm sti
  346.     _asm popad
  347.     _asm iretd
  348. }
  349.  
  350. /*
  351.  *  Controller Command Routines
  352.  */
  353.  
  354. //! check interrupt status command
  355. void flpydsk_check_int (uint32_t* st0, uint32_t* cyl) {
  356.  
  357.     flpydsk_send_command (FDC_CMD_CHECK_INT);
  358.  
  359.     *st0 = flpydsk_read_data ();
  360.     *cyl = flpydsk_read_data ();
  361.    
  362. }
  363.  
  364. //! turns the current floppy drives motor on/off
  365. void flpydsk_control_motor (bool b) {
  366.  
  367.     //! sanity check: invalid drive
  368.     if (_CurrentDrive > 3)
  369.         return;
  370.  
  371.     uint8_t motor = 0;
  372.  
  373.     //! select the correct mask based on current drive
  374.     switch (_CurrentDrive) {
  375.  
  376.         case 0:
  377.             motor = FLPYDSK_DOR_MASK_DRIVE0_MOTOR;
  378.             break;
  379.         case 1:
  380.             motor = FLPYDSK_DOR_MASK_DRIVE1_MOTOR;
  381.             break;
  382.         case 2:
  383.             motor = FLPYDSK_DOR_MASK_DRIVE2_MOTOR;
  384.             break;
  385.         case 3:
  386.             motor = FLPYDSK_DOR_MASK_DRIVE3_MOTOR;
  387.             break;
  388.     }
  389.  
  390.     //! turn on or off the motor of that drive
  391.     if (b)
  392.         flpydsk_write_dor (uint8_t(_CurrentDrive | motor | FLPYDSK_DOR_MASK_RESET | FLPYDSK_DOR_MASK_DMA));
  393.     else
  394.         flpydsk_write_dor (FLPYDSK_DOR_MASK_RESET);
  395.  
  396.     //! in all cases; wait a little bit for the motor to spin up/turn off
  397.     sleep (20);
  398. }
  399.  
  400. //! configure drive
  401. void flpydsk_drive_data (uint8_t stepr, uint8_t loadt, uint8_t unloadt, bool dma ) {
  402.  
  403.     uint8_t data = 0;
  404.  
  405.     //! send command
  406.     flpydsk_send_command (FDC_CMD_SPECIFY);
  407.     data = ( (stepr & 0xf) << 4) | (unloadt & 0xf);
  408.         flpydsk_send_command (data);
  409.     data = (( loadt << 1 ) | ( (dma) ? 0 : 1 ) );
  410.         flpydsk_send_command (data);
  411. }
  412.  
  413. //! calibrates the drive
  414. int flpydsk_calibrate (uint8_t drive) {
  415.  
  416.     uint32_t st0, cyl;
  417.  
  418.     if (drive >= 4)
  419.         return -2;
  420.  
  421.     //! turn on the motor
  422.     flpydsk_control_motor (true);
  423.  
  424.     for (int i = 0; i < 10; i++) {
  425.  
  426.         //! send command
  427.         flpydsk_send_command ( FDC_CMD_CALIBRATE );
  428.         flpydsk_send_command ( drive );
  429.         flpydsk_wait_irq ();
  430.         flpydsk_check_int ( &st0, &cyl);
  431.  
  432.         //! did we fine cylinder 0? if so, we are done
  433.         if (!cyl) {
  434.  
  435.             flpydsk_control_motor (false);
  436.             return 0;
  437.         }
  438.     }
  439.  
  440.     flpydsk_control_motor (false);
  441.     return -1;
  442. }
  443.  
  444. //! disable controller
  445. void flpydsk_disable_controller () {
  446.  
  447.     flpydsk_write_dor (0);
  448. }
  449.  
  450. //! enable controller
  451. void flpydsk_enable_controller () {
  452.  
  453.     flpydsk_write_dor ( FLPYDSK_DOR_MASK_RESET | FLPYDSK_DOR_MASK_DMA);
  454. }
  455.  
  456.  
  457.  
  458. //! reset controller
  459. void flpydsk_reset () {
  460.  
  461.     uint32_t st0, cyl;
  462.  
  463.     //! reset the controller
  464.     flpydsk_disable_controller ();
  465.     flpydsk_enable_controller ();
  466.     flpydsk_wait_irq ();
  467.  
  468.     //! send CHECK_INT/SENSE INTERRUPT command to all drives
  469.     for (int i=0; i<4; i++)
  470.         flpydsk_check_int (&st0,&cyl);
  471.  
  472.     //! transfer speed 500kb/s
  473.     flpydsk_write_ccr (0);
  474.  
  475.     //! pass mechanical drive info. steprate=3ms, unload time=240ms, load time=16ms
  476.     flpydsk_drive_data (3,16,240,true);
  477.  
  478.     //! calibrate the disk
  479.     flpydsk_calibrate ( _CurrentDrive );
  480. }
  481.  
  482. //! read a sector
  483. void flpydsk_read_sector_imp (uint8_t head, uint8_t track, uint8_t sector) {
  484.     DebugPrintf("Debug : Strt IMP\n");
  485.     getch2();
  486.  
  487.     uint32_t st0, cyl;
  488.  
  489.     //! initialise DMA
  490.     DebugPrintf("Debug : intl dma\n");
  491.     getch2();
  492.  
  493.     if (!dma_initialise_floppy ((uint8_t*) DMA_BUFFER, 512 ))
  494.     {
  495.         DebugPrintf("\n\nDMA BUFFER ERROR");
  496.         return;
  497.     }
  498.  
  499.     DebugPrintf("Debug : Dma complete \n");
  500.     getch2();
  501.  
  502.     //! set the DMA for read transfer
  503.     dma_set_read ( FDC_DMA_CHANNEL );
  504.  
  505.     DebugPrintf("Debug : Read 2\n");
  506.     getch2();
  507.  
  508.     //_FloppyDiskIRQ = 0;       ///////////////////////////
  509.  
  510.    
  511.     _asm sti                // Make sure we have interrupts.
  512.  
  513.     DebugPrintf("Debug : STi complete\n");
  514.     getch2();
  515.     //! read in a sector
  516.     flpydsk_send_command ( FDC_CMD_READ_SECT | FDC_CMD_EXT_MULTITRACK | FDC_CMD_EXT_SKIP | FDC_CMD_EXT_DENSITY );
  517.  
  518.     DebugPrintf("Debug : CMD 1\n");
  519.     getch2();
  520.  
  521.     flpydsk_send_command ( head << 2 | _CurrentDrive );
  522.  
  523.     DebugPrintf("Debug : CMD 2\n");
  524.     getch2();
  525.  
  526.     flpydsk_send_command ( track );
  527.  
  528.     DebugPrintf("Debug : CMD 3\n");
  529.     getch2();
  530.  
  531.     flpydsk_send_command ( head );
  532.  
  533.     DebugPrintf("Debug : CMD 4\n");
  534.     getch2();
  535.  
  536.     flpydsk_send_command ( sector );
  537.  
  538.     DebugPrintf("Debug : CMD 5\n");
  539.     getch2();
  540.  
  541.     flpydsk_send_command ( FLPYDSK_SECTOR_DTL_512 );
  542.  
  543.     DebugPrintf("Debug : CMD 6\n");
  544.     getch2();
  545.  
  546.     flpydsk_send_command ( ( ( sector + 1 ) >= FLPY_SECTORS_PER_TRACK ) ? FLPY_SECTORS_PER_TRACK : sector + 1 );
  547.  
  548.     DebugPrintf("Debug : CMD 7\n");
  549.     getch2();
  550.  
  551.     flpydsk_send_command ( FLPYDSK_GAP3_LENGTH_3_5 );
  552.  
  553.     DebugPrintf("Debug : CMD 8\n");
  554.     getch2();
  555.  
  556.     flpydsk_send_command ( 0xFF );
  557.  
  558.     DebugPrintf("Debug : CMD 9\n");
  559.     getch2();
  560.  
  561.     //! wait for irq
  562.     //flpydsk_wait_irq ();
  563.  
  564.     DebugPrintf("Debug :The stopped IRQ \n");
  565.     getch2();
  566.  
  567.     //! read status info
  568.     for (int j = 0; j < 7; j++)
  569.         flpydsk_read_data ();
  570.  
  571.     DebugPrintf("Debug : State\n");
  572.     getch2();
  573.  
  574.     //! let FDC know we handled interrupt
  575.     flpydsk_check_int (&st0, &cyl);
  576.  
  577.     DebugPrintf("Debug :INT\n");
  578.     getch2();
  579. }
  580.  
  581. //! seek to given track/cylinder
  582. int flpydsk_seek ( uint8_t cyl, uint8_t head ) {
  583.  
  584.     uint32_t st0, cyl0;
  585.  
  586.     DebugPrintf("Debug : Seeking\n");
  587.     getch2();
  588.  
  589.     if (_CurrentDrive >= 4)
  590.         return -1;
  591.  
  592.     for (int i = 0; i < 10; i++ ) {
  593.  
  594.         //! send the command
  595.         flpydsk_send_command (FDC_CMD_SEEK);
  596.  
  597.         DebugPrintf("Debug :seek CMD 1\n");
  598.          getch2();
  599.  
  600.         flpydsk_send_command ( (head) << 2 | _CurrentDrive);
  601.  
  602.         DebugPrintf("Debug :seek CMD 2\n");
  603.         getch2();
  604.  
  605.         flpydsk_send_command (cyl);
  606.  
  607.         DebugPrintf("Debug :seek CMD 3\n");
  608.         getch2();
  609.  
  610.         //! wait for the results phase IRQ
  611.         flpydsk_wait_irq ();
  612.  
  613.         DebugPrintf("Debug :seek irq\n");
  614.         getch2();
  615.  
  616.         flpydsk_check_int (&st0,&cyl0);
  617.  
  618.         DebugPrintf("Debug :check seek\n");
  619.         getch2();
  620.  
  621.         //! found the cylinder?
  622.         if ( cyl0 == cyl)
  623.         DebugPrintf("Debug : fnd cyl\n");
  624.         getch2();
  625.             return 0;
  626.     }
  627.  
  628.     DebugPrintf("Debug : Err cyl\n");
  629.     getch2();
  630.  
  631.     return -1;
  632. }
  633.  
  634.  
  635. //! sets DMA base address
  636. void flpydsk_set_dma (int addr) {
  637.  
  638.     DMA_BUFFER = addr;
  639. }
  640.  
  641. //=========================================================================
  642. //    INTERFACE FUNCTIONS
  643. //=========================================================================
  644.  
  645. //! convert LBA to CHS
  646. void flpydsk_lba_to_chs (int lba,int *head,int *track,int *sector) {
  647.  
  648.    *head = ( lba % ( FLPY_SECTORS_PER_TRACK * 2 ) ) / ( FLPY_SECTORS_PER_TRACK );
  649.    *track = lba / ( FLPY_SECTORS_PER_TRACK * 2 );
  650.    *sector = lba % FLPY_SECTORS_PER_TRACK + 1;
  651. }
  652.  
  653. extern void *memset (void*, char, size_t);      // Get memset from string.h
  654.  
  655. //! install floppy driver
  656. void flpydsk_install (int irq) {
  657.  
  658.     //! allocate the DMA buffer
  659.     //DMA_BUFFER = (int) flt_pmmngr_alloc_blocks (16);
  660.  
  661.     //! clear the DMA buffer
  662.     //memset ((void*) DMA_BUFFER, 0xFF, 16*0x1000);
  663.  
  664.     //! install irq handler
  665.     setvect (irq, i86_flpy_irq);
  666.  
  667.     //! reset the fdc
  668.     flpydsk_reset ();
  669.  
  670.     //! set drive information
  671.     flpydsk_drive_data (13, 1, 0xf, true);
  672. }
  673.  
  674. //! set current working drive
  675. void flpydsk_set_working_drive (uint8_t drive) {
  676.  
  677.     if (drive < 4)
  678.         _CurrentDrive = drive;
  679. }
  680.  
  681. //! get current working drive
  682. uint8_t flpydsk_get_working_drive () {
  683.  
  684.     return _CurrentDrive;
  685. }
  686.  
  687. //! read a sector
  688. uint8_t* flpydsk_read_sector (int sectorLBA) {
  689.     DebugPrintf("Debug : Start counting\n");
  690.     getch2();
  691.  
  692.     if (_CurrentDrive >= 4) {
  693.         DebugPrintf("\nFLPY: ERROR1\n");
  694.         return 0;}
  695.  
  696.     DebugPrintf("Debug : Completed The last task\n");
  697.     getch2();
  698.  
  699.     //! convert LBA sector to CHS
  700.  
  701.     DebugPrintf("Debug : Start Converting\n");
  702.     getch2();
  703.  
  704.     int head=0, track=0, sector=1;
  705.     flpydsk_lba_to_chs (sectorLBA, &head, &track, &sector);
  706.  
  707.     DebugPrintf("Debug : Tsk completed with code - \n");
  708.     DebugPrintf("Debug -----------[%i]--------, head);
  709.     DebugPrintf("Debug -----------[%i]--------, track);
  710.     DebugPrintf("Debug -----------[%i]--------, sector);
  711.     DebugPrintf("\n");
  712.     getch2();
  713.  
  714.     //! turn motor on and seek to track
  715.     flpydsk_control_motor (true);
  716.  
  717.     DebugPrintf("Debug : Seeking\n");
  718.     getch2();
  719.  
  720.     if (flpydsk_seek ((uint8_t)track, (uint8_t)head) != 0) {
  721.         DebugPrintf("\nFLPY: ERROR2\n");
  722.         return 0;}
  723.  
  724.     DebugPrintf("Debug : Cmd  seek ok \n");
  725.     getch2();
  726.  
  727.     //! read sector and turn motor off
  728.     flpydsk_read_sector_imp ((uint8_t)head, (uint8_t)track, (uint8_t)sector);
  729.  
  730.     DebugPrintf("Debug : Read sec cpm\n");
  731.     getch2();
  732.  
  733.     flpydsk_control_motor (false);
  734.  
  735.     DebugPrintf("Debug : Motor off\n");
  736.     getch2();
  737.  
  738.     return (uint8_t*) DMA_BUFFER;
  739.  
  740.     DebugPrintf("Debug : Buffer returned \n");
  741.     getch2();
  742. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!