OSDev.org

Code: Select all

volatile BYTE fd_flag = 0;

void init_fd(void)
{   
   k_printfc(VGA_YELLOW, "================================================================================");

   // register interrupt-handler for IRQ6
   irq_register_handler(0x06, fd_handler);
   
   // reset controller
   fd_reset();
   fd_motoron();   
   fd_wait_for_irq();
   
   // determine controller version
   fd_send(0x10);
   if(fd_read() == 0x80) {
      k_printf("fd: NEC765 controller found\n");
   }
   else {
      k_printf("fd: enhanced controller found\n");
   }

   // set data rate
   outportb(FD_BASE + FD_CONF, 0x00);   

   // calibrate drive   
   fd_send(0x07);
   fd_send(0x00);
   fd_wait_for_irq();
   
   ////////////////////
   // TODO TODO TODO //
   ////////////////////   
   dma_block_t dma;
   dma.page    = 1;
   dma.offset    = 0;
   dma.count    = 511;
   dma_start(2, &dma, DMA_MODE_WRITE | DMA_MODE_NOAUTOINIT | DMA_MODE_SINGLE);
   
   fd_send(0x66);
   fd_send(0);
   fd_send(0); // cyl
   fd_send(0); // head
   fd_send(1); // sec number
   fd_send(2); // sec size
   fd_send(0);
   fd_send(0);
   fd_send(0xFF);
   fd_wait_for_irq();
   
   k_printf("ST0: 0x%x\n", fd_read());
   k_printf("ST1: 0x%x\n", fd_read());
   k_printf("ST2: 0x%x\n", fd_read());
   k_printf("Cyl: 0x%x\n", fd_read());
   k_printf("Head: 0x%x\n", fd_read());
   k_printf("Sec num: 0x%x\n", fd_read());
   k_printf("Sec size: 0x%x\n", fd_read());
   
   fd_dump_msr();
   ////////////////////
   // TODO TODO TODO //
   ////////////////////
   
   // turn drive motor off
   fd_motoroff();
   
   // successful
   k_printf("init_fd: done!\n");
   for(;;);
}

void fd_handler(void)
{
   // set interrupt flag
   fd_flag = 1;
   // check interrupt status
   fd_send(0x08);   
   k_printfc(VGA_YELLOW, "fd_handler\n");   
   k_printf("ST0: 0x%x\n", fd_read());
   k_printf("Cyl: 0x%x\n", fd_read());   
}

void fd_reset(void)
{   outportb(FD_BASE + FD_DOR, 0x00);   }

void fd_motoron(void)
{   outportb(FD_BASE + FD_DOR, FD_DOR_RESET | FD_DOR_DMA | FD_DOR_MOTA);   }

void fd_motoroff(void)
{   outportb(FD_BASE + FD_DOR, FD_DOR_RESET | FD_DOR_DMA);   }

void fd_wait_for_irq(void)
{
   fd_flag = 0;
   while(!fd_flag);
   fd_flag = 0;
}

void fd_send(BYTE byte) {
   volatile int msr;
   int tmo;   
   for(tmo=0; tmo<128; tmo++) {
      msr = inportb(FD_BASE + FD_MSR);
      if((msr&0xC0) == 0x80) { // data register ready + CPU to controller
         outportb(FD_BASE + FD_DATA, byte);
         return;
      }
      inportb(0x80); // delay
   }
   k_printfc(VGA_RED, "fd: timeout (send)!\n");
}

BYTE fd_read() {
   volatile int msr;
   int tmo;
    for(tmo=0; tmo<128; tmo++) {
      msr = inportb(FD_BASE + FD_MSR);
      if((msr & 0xD0) == 0xD0) { // data register ready + controller to CPU + busy
         return inportb(FD_BASE + FD_DATA);
      }
      inportb(0x80); // delay
   }
   k_printfc(VGA_RED, "fd: timeout (read)!\n");
   return -1;
}

int dma_start(int channel, dma_block_t *block, BYTE mode)
{
   if(channel>=8) return -1;

   // disable hardware interrupts
   disable();
   
   // mask channel
   outportb(dma_mask[channel], DMA_MASKBIT | channel);
   
   // clear channel
   outportb(dma_clear[channel], 0);
   
   // write mode
   outportb(dma_mode[channel], mode | channel);

   // write page
   outportb(dma_page[channel], block->page);      
   
   // write offset
   outportb(dma_addr[channel], (block->offset&0x00FF));
   outportb(dma_addr[channel], (block->offset&0xFF00)>>8);

   // write count
   outportb(dma_count[channel], (block->count&0x00FF));
   outportb(dma_count[channel], (block->count&0xFF00)>>8);
   
   // unmask
   outportb(dma_mask[channel], channel);

   // enable hardware interrupts
   enable();

   return 0;
}