Logo Search packages:      
Sourcecode: faumachine version File versions  Download package

disk.c

/*
 * $Id: disk.c,v 1.227 2009-02-27 10:44:58 vrsieh Exp $ 
 *
 * Copyright (C) 2004-2009 FAUmachine Team <info@faumachine.org>.
 * This program is free software. You can redistribute it and/or modify it
 * under the terms of the GNU General Public License, either version 2 of
 * the License, or (at your option) any later version. See COPYING.
 */

#include "build_config.h"
#include "compiler.h"

/* ===================== RUNTIME || INIT ======================== */
#if defined(RUNTIME_RM) || defined(INIT_RM)

#define FD_CHANGELINE_SUPPORT 1

#define IDE_ERR_STAT    0x01
#define IDE_INDEX_STAT  0x02
#define IDE_ECC_STAT    0x04
#define IDE_DRQ_STAT    0x08
#define IDE_SEEK_STAT   0x10
#define IDE_WRERR_STAT  0x20
#define IDE_READY_STAT  0x40
#define IDE_BUSY_STAT   0x80

#define WIN_RESTORE     0x10 /* Recalibrate Device */
#define WIN_IDENTIFY    0xec /* Identify ATA Device */
#define WIN_PIDENTIFY   0xa1 /* Identify ATAPI Device */

struct diskette_param_table_t {
      /* FIXME VOSSI */
      uint8_t x0, x1, x2, x3, x4, x5, x6, x7, x8, x9, x10;
};

extern const uint16_t ide_port_table[];
extern const struct diskette_param_table_t diskette_param_table;

#endif /* RUNTIME_RM || INIT_RM */
/* ==================== RUNTIME_RM ==================== */
#ifdef RUNTIME_RM

CODE16;

#include "assert.h"
#include "fixme.h"
#include "stdio.h"
#include "string.h"
#include "in.h"

#include "io.h"
#include "const.h"
#include "cmos.h"
#include "var.h"
#include "el_torito.h"
#include "ptrace.h"
#include "disk.h"

#define ATAPI_TEST_UNIT_READY 0x00
#define ATAPI_REQUEST_SENSE   0x03
#define ATAPI_READ_SECTOR     0x28

#define ATAPI_NO_SENSE        0
#define ATAPI_NOT_READY       2
#define ATAPI_UNIT_ATTENTION  6

struct atapi_compacket {
      unsigned char opcode;
      unsigned char reserved0;
      uint32_t lba;
      unsigned char reserved1;
      union {
            unsigned short transfer_length;
            unsigned short paramlist_length;
            unsigned short allocation_length;
      } misc;
      unsigned char reserved2;
      unsigned short reserved3;
} __attribute__((__packed__));

struct atapi_request_sense {
      /* Byte 0: */
      /*    Bit 0-6: error code */
      /*    Bit 7: valid */
      unsigned char error_code;
      /* Byte 1: */
      unsigned char segment_number;
      /* Byte 2: */
      /*    Bit 0-3: sense_key */
      /*    Bit 4: reserved */
      /*    Bit 5: ili */
      /*    Bit 6-7: reserved */
      unsigned char sense_key;
      /* Byte 3-6: */
      unsigned char information[4];
      /* Byte 7: */
      unsigned char add_sense_len;
      /* Byte 8-11: */
      unsigned char command_info[4];
      /* Byte 12: */
      unsigned char asc;
      /* Byte 13: */
      unsigned char ascq;
      /* Byte 14: */
      unsigned char fruc;
      /* Byte 15-17: */
      unsigned char sks[3];
};

struct Int13DPT {
      unsigned short size;
      unsigned short flags;
      uint32_t cyls;
      uint32_t heads;
      uint32_t secs;
      uint32_t total_low;     /* Should be long long */
      uint32_t total_high;
      unsigned short bytes_per_sec;
      /* ... */
};


struct Int13Ext {
      unsigned char size;
      unsigned char reserved;
      unsigned short count;
      unsigned short offset;
      unsigned short segment;
#if 0 /* FIXME VOSSI */
      unsigned long long lba;
#else
      uint32_t lba;
      uint32_t lba2;
#endif
};

/*
 * Since no provisions are made for multiple drive types, most
 * values in this table are ignored.  I set parameters for 1.44M
 * floppy here
 */
/* Should be located at 0xefc7 in ROM. FIXME VOSSI */

const struct diskette_param_table_t diskette_param_table = {
      0xAF,
      0x02, /* Head load time 0000001, DMA used. */
      0x25,
      0x02,
      18,
      0x1B,
      0xFF,
      0x6C,
      0xF6,
      0x0F,
      0x08
};

CONST unsigned short ide_port_table[] = {
      /* hda/hdb */ 0x01f0,
      /* hdc/hdd */ 0x0170,
      /* hde/hdf */ 0x01e8,
      /* hdg/hdh */ 0x0168,
      /* hdi/hdj */ 0x01e0,
      /* hdk/hdl */ 0x0160,
};

static void
int13_fail(struct regs *regs, unsigned char status)
{
      if (DL < 2) {
            var_put(fd_res, status);
      } else {
            var_put(hd_res, status);
      }

      AH = status;
      F |= 1 << 0;      /* Set carry. */
}

static void
int13_success(struct regs *regs, unsigned char status)
{
      if (DL < 2) {
            var_put(fd_res, status);
      } else {
            var_put(hd_res, status);
      }

      AH = status;
      F &= ~(1 << 0);   /* Clear carry. */
}

unsigned char
ide_harddisk_read_chs(
      unsigned char device,
      unsigned char num, 
      unsigned short cylinder, 
      unsigned char head,
      unsigned char sector,
      unsigned short buffer_seg,
      unsigned short buffer_off)
{
      uint16_t port;
      uint8_t status;

      assert(/* 0 <= device / 2 && */ device / 2 < 6);
      assert(/* 0 <= device % 2 && */ device % 2 < 2);

      /* Get port addresses. */
      port = const_get(ide_port_table[device / 2]);

      /* Wait while busy. */
      do {
            status = inb(port + 0x206);
      } while (status & IDE_BUSY_STAT);

      /*
       * Send read command.
       */
      outb(num, port + 2);          /* number of sectors */
      outb(sector & 0x3f, port + 3);      /* sector */
      outb(cylinder & 0xff, port + 4);/* cylinder (low bits) */
      outb(cylinder >> 8, port + 5);      /* cylinder (high bits) */
      outb(0xa0 | ((device % 2) << 4) | head, port + 6);
                              /* unit / head */
      outb(0x20, port + 7);         /* command: read */

      /*
       * Get all sectors.
       */
      for ( ; 0 < num; num--) {
            unsigned int count;
            unsigned short data;

            /* Wait while busy. */
            do {
                  status = inb(port + 0x206);
            } while (status & IDE_BUSY_STAT);

            if (status & IDE_ERR_STAT) {
                  /* ERR bit set */
                  assert(0);
                  return 0xff;      /* FIXME VOSSI */
            }

            if (! (status & IDE_DRQ_STAT)) {
                  /* FIXME VOSSI */
                  assert(0);
                  return 0xff;
            }

            buffer_seg += buffer_off / 16;
            buffer_off %= 16;

            for (count = 0; count < 512 / 2; count++) {
                  data = inw(port);
                  put_word(buffer_seg, buffer_off + count * 2, data);
            }

            buffer_seg += 512 / 16;
      }

      return 0;
}

static unsigned char
ide_harddisk_write_chs(
      unsigned char device,
      unsigned char num, 
      unsigned short cylinder, 
      unsigned char head,
      unsigned char sector,
      unsigned short buffer_seg,
      unsigned short buffer_off
)
{
      uint16_t port;
      uint8_t status;

      assert(/* 0 <= device / 2 && */ device / 2 < 6);
      assert(/* 0 <= device % 2 && */ device % 2 < 2);

      /* Get port addresses. */
      port = const_get(ide_port_table[device / 2]);

      /* Wait while busy. */
      do {
            status = inb(port + 0x206);
      } while (status & IDE_BUSY_STAT);

      /*
       * Send write command.
       */
      outb(num, port + 2);          /* number of sectors */
      outb(sector & 0x3f, port + 3);      /* sector */
      outb(cylinder & 0xff, port + 4);/* cylinder (low bits) */
      outb(cylinder >> 8, port + 5);      /* cylinder (high bits) */
      outb(0xa0 | ((device % 2) << 4) | head, port + 6);
                              /* unit / head */
      outb(0x30, port + 7);         /* command: write */

      /* Wait while busy. */
      do {
            status = inb(port + 0x206);
      } while (status & IDE_BUSY_STAT);

      /*
       * Send all sectors.
       */
      for ( ; 0 < num; num--) {
            unsigned int count;
            unsigned short data;

            if (! (status & IDE_DRQ_STAT)) {
                  /* FIXME VOSSI */
                  assert(0);
                  return 0xff;
            }

            /* Send sector. */
            buffer_seg += buffer_off / 16;
            buffer_off %= 16;

            for (count = 0; count < 512 / 2; count++) {
                  data = get_word(buffer_seg, buffer_off + count * 2);
                  outw(data, port);
            }

            buffer_seg += 512 / 16;

            /* Wait while busy. */
            do {
                  status = inb(port + 0x206);
            } while (status & IDE_BUSY_STAT);

            if (status & IDE_ERR_STAT) {
                  /* ERR bit set */
                  assert(0);
                  return 0xff;      /* FIXME VOSSI */
            }
      }

      return 0;
}

static unsigned char
ide_harddisk_read_lba(
      unsigned char controller,
      unsigned char unit,
      unsigned char num, 
      uint32_t /* long long */ start,
      unsigned short buffer_seg,
      unsigned short buffer_off
) {
      uint16_t port;
      uint8_t status;

      assert(/* 0 <= controller && */ controller < 6);
      assert(/* 0 <= unit && */ unit < 2);

      /* Get port addresses. */
      port = const_get(ide_port_table[controller]);

      /* Wait while busy. */
      do {
            status = inb(port + 0x206);
      } while (status & IDE_BUSY_STAT);

      /*
       * Send read command.
       */
      outb(num, port + 2);                /* number of sectors */
      outb((start >> 0) & 0xff, port + 3);      /* sector */
      outb((start >> 8) & 0xff, port + 4);      /* cylinder (low bits) */
      outb((start >> 16) & 0xff, port + 5);     /* cylinder (high bits) */
      outb(0xe0 | (unit << 4) | ((start >> 24) & 0xf), port + 6);
                                    /* unit / head */
      outb(0x20, port + 7);               /* command: read */

      /*
       * Get all sectors.
       */
      for ( ; 0 < num; num--) {
            unsigned int count;
            unsigned short data;

            /* Wait while busy. */
            do {
                  status = inb(port + 0x206);
            } while (status & IDE_BUSY_STAT);

            if (status & IDE_ERR_STAT) {
                  /* ERR bit set */
                  return 0xff;    /* FIXME VOSSI */
            }

            /* Read result. */
            if (! (status & IDE_DRQ_STAT)) {
                  /* FIXME VOSSI */
                  assert(0);
                  return 0xff;
            }

            buffer_seg += buffer_off / 16;
            buffer_off %= 16;

            for (count = 0; count < 512 / 2; count++) {
                  data = inw(port);
                  put_word(buffer_seg, buffer_off + count * 2, data);
            }

            buffer_seg += 512 / 16;
      }

      return 0;
}

static unsigned char
ide_harddisk_write_lba(
      unsigned char controller,
      unsigned char unit,
      unsigned char num, 
      uint32_t /* long long */ start,
      unsigned short buffer_seg,
      unsigned short buffer_off)
{
      uint16_t port;
      uint8_t status;

      assert(/* 0 <= controller && */ controller < 6);
      assert(/* 0 <= unit && */ unit < 2);

      /* Get port addresses. */
      port = const_get(ide_port_table[controller]);

      /* Wait while busy. */
      do {
            status = inb(port + 0x206);
      } while (status & IDE_BUSY_STAT);

      /*
       * Send write command.
       */
      outb(num, port + 2);                /* number of sectors */
      outb((start >> 0) & 0xff, port + 3);      /* sector */
      outb((start >> 8) & 0xff, port + 4);      /* cylinder (low bits) */
      outb((start >> 16) & 0xff, port + 5);     /* cylinder (high bits) */
      outb(0xe0 | (unit << 4) | ((start >> 24) & 0xf), port + 6);
                                    /* unit / head */
      outb(0x30, port + 7);               /* command: write */

      /* Wait while busy. */
      do {
            status = inb(port + 0x206);
      } while (status & IDE_BUSY_STAT);

      /*
       * Send all sectors.
       */
      for ( ; 0 < num; num--) {
            unsigned int count;
            unsigned short data;

            if (! (status & IDE_DRQ_STAT)) {
                  /* FIXME VOSSI */
                  assert(0);
                  return 0xff;
            }

            /* Write sector. */
            buffer_seg += buffer_off / 16;
            buffer_off %= 16;

            for (count = 0; count < 512 / 2; count++) {
                  data = get_word(buffer_seg, buffer_off + count * 2);
                  outw(data, port);
            }

            buffer_seg += 512 / 16;

            /* Wait while busy. */
            do {
                  status = inb(port + 0x206);
            } while (status & IDE_BUSY_STAT);

            if (status & IDE_ERR_STAT) {
                  /* ERR bit set */
                  return 0xff;    /* FIXME VOSSI */
            }
      }

      return 0;
}

static void
floppy_poweron(unsigned short drive)
{
      var_put(fmot_tmout, 0x5a); /* About 5sec at 18Hz. */

      if (var_get(fmot_flag) & (1 << drive)) {
            /* Floppy already spinning... */
            return;
      }

      var_put(fmot_flag, var_get(fmot_flag) | (1 << drive));
      outb(0x0c | ((var_get(fmot_flag) & 0x0f) << 4) | drive, 0x3f2);

      /* Wait for floppy spinning up... */
      /* FIXME VOSSI */
}

static ALWAYS_INLINE void
floppy_poweroff(void)
{
      outb(0x0c, 0x3f2);
      var_put(fmot_flag, 0x00);
}

void
floppy_tick(void)
{
      unsigned char timeout;

      timeout = var_get(fmot_tmout);

      if (timeout == 0) {
            /* Nothing to do... */
            return;
      }

      timeout--;
      var_put(fmot_tmout, timeout);

      if (timeout != 0) {
            /* Nothing to do... */
            return;
      }

      /* Power-off motor. */
      floppy_poweroff();
}

static unsigned char
floppy_err(unsigned char st0, unsigned char st1, unsigned char st2)
{
      unsigned char ret;

      if (((st0 >> 6) & 3) == 0) { /* Interrupt code. */
            ret = 0; /* No error. */

      } else {
            /* Error. */
            if ((st1 >> 0) & 1) { /* Missing address mark. */
                  ret = 0x02; /* Missing address mark. */
            } else if ((st1 >> 1) & 1) { /* Not writable. */
                  ret = 0x03; /* Write protected. */
            } else if ((st1 >> 2) & 1) { /* No data. */
                  ret = 0x04; /* Sector not found / read error. */
            } else if ((st1 >> 4) & 1) { /* Overrun/underrun. */
                  ret = 0x08; /* DMA overrun/underrun. */
            } else if ((st1 >> 5) & 1) { /* Data error. */
                  ret = 0x10; /* CRC checksum error. */
            } else {
                  ret = 0x80; /* Unknown error. */ /* FIXME VOSSI */
            }
      }

#if 0
      dprintf("floppy_err: ret=0x%02x\n", ret);
#endif

      return ret;
}

static unsigned char
floppy_read_chs(
      unsigned char drive,
      unsigned char num,
      unsigned char cylinder,
      unsigned char head,
      unsigned char sector,
      unsigned short buffer_seg,
      unsigned short buffer_off
)
{
      unsigned char status;
      unsigned long size;
      unsigned long address;

      if (1 < drive) {
            return 0x01;      /* invalid parameter */
      }

      if (1 < head) {
            return 0x01;      /* invalid parameter */
      }

      size = num * 512 - 1;
      address = ((unsigned long) buffer_seg << 4) + buffer_off;
      if((address & 0xffff) + size > 0xffff) {
            return 0x09;      /* dma boundary */
      }

      /*
       * Start motor
       */
      floppy_poweron(drive);
      
      /*
       * Set up DMA
       */
      outb(0x06, 0x0a); /* Enable DMA 2. */
      
      outb(0x00, 0x0c); /* Clear byte pointer flip/flop. */
      outb((address >> 0) & 0xff, 0x04); /* Address low byte. */
      outb((address >> 8) & 0xff, 0x04); /* Address high byte. */
      
      outb(0x00, 0x0c); /* Clear byte pointer flip/flop. */
      outb((size >> 0) & 0xff, 0x05); /* Count low byte. */
      outb((size >> 8) & 0xff, 0x05); /* Count high byte. */
      
      /* Mode register: single block, inc addr, no autoinit, read, chan 2 */
      outb((1 << 6) | (0 << 5) | (0 << 4) | (1 << 2) | 2, 0x0b);

      outb((address >> 16) & 0xff, 0x81); /* Page register */

      /*
       * Wait until DR is ready
       */
      do {
            status = inb(0x3f4);
      } while ((status & 0xc0) != 0x80);

      /*
       * Send read command.
       */
      outb(0xc6, 0x3f5); /* READ with multitrack */
      outb((head << 2) | drive, 0x3f5);
      outb(cylinder, 0x3f5); /* cylinder */
      outb(head, 0x3f5); /* head */
      outb(sector, 0x3f5); /* sector */
      outb(0x02, 0x3f5); /* sectorsize = 0x02 >> 7 */
      outb(18, 0x3f5); /* sector per track/side */
      outb(0x1b, 0x3f5); /* length of GAP 3 */
      outb(0xff, 0x3f5); /* data length (0xff = unused) */
      
      /*
       * Wait until command is has been finished
       */
      var_put(f_recal, var_get(f_recal) & ~(1 << 7));
      while (! (var_get(f_recal) & (1 << 7))) {
            asm volatile (
                  "sti\n\t"
                  "hlt\n\t"
                  "cli\n\t"
            );
      }

      /*
       * Read result
       */
      var_put(f_stat[0], inb(0x3f5));
      var_put(f_stat[1], inb(0x3f5));
      var_put(f_stat[2], inb(0x3f5));
      var_put(f_stat[3], inb(0x3f5));
      var_put(f_stat[4], inb(0x3f5));
      var_put(f_stat[5], inb(0x3f5));
      var_put(f_stat[6], inb(0x3f5));

      /*
       * DMA
       */
      outb(0x02, 0x0a); /* Disable DMA 2. */

      return floppy_err(var_get(f_stat[0]),
                  var_get(f_stat[1]), var_get(f_stat[2]));
}

static unsigned char
floppy_write_chs(
      unsigned char drive,
      unsigned char num,
      unsigned char cylinder,
      unsigned char head,
      unsigned char sector,
      unsigned short buffer_seg,
      unsigned short buffer_off
)
{
      unsigned char status;
      unsigned long size;
      unsigned long address;

      if (1 < drive) {
            return 0x01;      /* invalid parameter */
      }

      if (1 < head) {
            return 0x01;      /* invalid parameter */
      }

      size = num * 512 - 1;
      address = ((unsigned long) buffer_seg << 4) + buffer_off;
      if((address & 0xffff) + size > 0xffff) {
            return 0x09;      /* dma boundary */
      }

      /*
       * Start motor
       */
      floppy_poweron(drive);
      
      /*
       * Set up DMA
       */
      outb(0x06, 0x0a); /* Enable DMA 2. */
      
      outb(0x00, 0x0c); /* Clear byte pointer flip/flop. */
      outb((address >> 0) & 0xff, 0x04); /* Address low byte. */
      outb((address >> 8) & 0xff, 0x04); /* Address high byte. */
      
      outb(0x00, 0x0c); /* Clear byte pointer flip/flop. */
      outb((size >> 0) & 0xff, 0x05); /* Count low byte. */
      outb((size >> 8) & 0xff, 0x05); /* Count high byte. */
      
      /* Mode register: single block, inc addr, no autoinit, write, chan 2 */
      outb((1 << 6) | (0 << 5) | (0 << 4) | (2 << 2) | 2, 0x0b);

      outb((address >> 16) & 0xff, 0x81); /* Page register */

      /*
       * Wait until DR is ready
       */
      do {
            status = inb(0x3f4);
      } while ((status & 0xc0) != 0x80);

      /*
       * Send write command.
       */
      outb(0xc5, 0x3f5); /* WRITE */
      outb((head << 2) | drive, 0x3f5);
      outb(cylinder, 0x3f5); /* cylinder */
      outb(head, 0x3f5); /* head */
      outb(sector, 0x3f5); /* sector */
      outb(0x02, 0x3f5); /* sectorsize = 0x02 >> 7 */
      outb(18, 0x3f5); /* sectors per track/side */
      outb(0x1b, 0x3f5); /* length of GAP 3 */
      outb(0xff, 0x3f5); /* data length (0xff = unused) */
      
      /*
       * Wait until command is has been finished
       */
      var_put(f_recal, var_get(f_recal) & ~(1 << 7));
      while (! (var_get(f_recal) & (1 << 7))) {
            asm volatile (
                  "sti\n\t"
                  "hlt\n\t"
                  "cli\n\t"
            );
      }

      /*
       * Read result
       */
      var_put(f_stat[0], inb(0x3f5));
      var_put(f_stat[1], inb(0x3f5));
      var_put(f_stat[2], inb(0x3f5));
      var_put(f_stat[3], inb(0x3f5));
      var_put(f_stat[4], inb(0x3f5));
      var_put(f_stat[5], inb(0x3f5));
      var_put(f_stat[6], inb(0x3f5));
      
      /*
       * DMA
       */
      outb(0x02, 0x0a); /* Disable DMA 2. */

      return floppy_err(var_get(f_stat[0]),
                  var_get(f_stat[1]), var_get(f_stat[2]));
}

static unsigned char
floppy_verify_chs(
      unsigned char drive,
      unsigned char num,
      unsigned char cylinder,
      unsigned char head,
      unsigned char sector,
      unsigned short buffer_seg,
      unsigned short buffer_off)
{
      return 0x01;
}

#if FD_CHANGELINE_SUPPORT
static unsigned char
floppy_check_changeline(unsigned char drive)
{
      unsigned char status;
      unsigned char result;

      if(drive >= 2) {
            return 0x80;      /* drive not ready or present */
      }

      /* FIXME sand:
       * not sure, whether this method to figure out
       * disk change is correct...
       */

      floppy_poweron(drive);

      if(inb(0x3f7) & 0x80) {
            /*
             * Wait until DR is ready
             */
            do {
                  status = inb(0x3f4);
            } while ((status & 0xc0) != 0x80);

            /*
             * Send recalibrate command.
             */
            outb(0x07, 0x3f5); /* RECALIBRATE */
            outb(drive, 0x3f5);

            /*
             * Wait until command has been finished
             */
            var_put(f_recal, var_get(f_recal) & ~(1 << 7));
            while (! (var_get(f_recal) & (1 << 7))) {
                  asm volatile (
                              "sti\n\t"
                              "hlt\n\t"
                              "cli\n\t"
                             );
            }

            /*
             * Wait until DR is ready
             */
            do {
                  status = inb(0x3f4);
            } while ((status & 0xc0) != 0x80);

            /*
             * Send sense interrupt status command.
             */
            outb(0x08, 0x3f5); /* SENSE INTERRUPT STATUS */
            outb(drive, 0x3f5);

            /*
             * Wait until DR is ready
             */
            do {
                  status = inb(0x3f4);
            } while ((status & 0xd0) != 0xd0);

            var_put(f_stat[0], inb(0x3f5));
            var_put(f_stat[1], inb(0x3f5));

            if(var_get(f_stat[1]) == 1 &&
                        (var_get(f_stat[0]) & 0xf0) == 0x20) {
                  if(inb(0x37f) & 0x80) {
                        result = 0x80;    /* drive not ready or present */
                  } else {
                        result = 0x06; /* change line active */
                  }
            } else {
                  result = 0x80; /* drive not ready or present */
            }
      } else {
            result = 0; /* disk not changed */
      }

      floppy_poweroff();

      return result;
}
#endif /* FD_CHANGELINE_SUPPORT */

      static void
outsw(unsigned short port, unsigned short *addr, uint32_t count)
{
      for ( ; 0 < count; count--) {
            outw(*addr++, port);
      }
}

static unsigned char
send_atapi_pc(
      unsigned short unit,
      unsigned short port,
      struct atapi_compacket *command
)
{
      uint8_t status;

      /* Select drive. */
      outb(unit << 4, port + 6);

      /* Wait while busy. */
      do {
            status = inb(port + 0x206);
      } while (status & IDE_BUSY_STAT);

      /* Send packet command. */
      outb(0x00, port + 1);         /* No DMA, no overlay */
      outb(2048 % 256, port + 4);   /* 2048 bytes transfer */
      outb(2048 / 256, port + 5);
      outb(0xa0, port + 7);         /* Packet command */

      /* Wait while busy. */
      do {
            status = inb(port + 0x206);
      } while (status & IDE_BUSY_STAT);
      if (status & IDE_ERR_STAT) {
            /* Use sense command - FIXME MARCEL */
            return 1;
      }
      assert(status & IDE_DRQ_STAT);

      assert(((inb(port + 2) >> 0) & 1) == 1); /* Command */
      assert(((inb(port + 2) >> 1) & 1) == 0); /* Out */
      assert(inb(port + 4) == 2048 % 256);
      assert(inb(port + 5) == 2048 / 256);

      /* Send command packet. */
      outsw(port, (void *) command, sizeof(struct atapi_compacket) / 2);

      /* Wait while busy. */
      do {
            status = inb(port + 0x206);
      } while (status & IDE_BUSY_STAT);
      if (status & IDE_ERR_STAT) {
            return 1;
      }

      return 0;
}

static unsigned char
atapi_request_sense(
      unsigned short unit,
      unsigned short port,
      struct atapi_request_sense *rs
)
{
      unsigned short *buf = (unsigned short *) rs;
      unsigned char command[12];
      unsigned char status;
      unsigned short count;

      for (count = 0; count < sizeof(command) / sizeof(command[0]); count++) {
            command[count] = 0;
      }
      command[0] = ATAPI_REQUEST_SENSE;
      command[4] = sizeof(*rs);

      /*
       * Send ATAPI command.
       */
      if (send_atapi_pc(unit, port, (struct atapi_compacket *) command)) {
            return 1;
      }

      /*
       * Wait for result.
       */
      do {
            status = inb(port + 0x206);
      } while (status & IDE_BUSY_STAT);
      if (status & IDE_ERR_STAT) {
            return 1;
      }

      /*
       * Get result.
       */
      for (count = 0; count < sizeof(*rs) / 2; count++) {
            assert(status & IDE_DRQ_STAT);
            *buf++ = inw(port);
      }

      return 0;
}

unsigned char
read_sector_lba_atapi(
      unsigned char device,
      unsigned short num,
      uint32_t /* long long */ block,
      unsigned short buffer_seg,
      unsigned short buffer_off,
      unsigned int sector_size
      )
{
      unsigned short port;
      unsigned short atapi_num;
      unsigned short start_dummy;
      unsigned short end_dummy;
      unsigned int ret;
      unsigned int count;
      unsigned int sector_count;
      unsigned short data;
      union {
            struct atapi_compacket command;
            struct atapi_request_sense rs;
      } u;
      unsigned char status;

again:      ;
      assert(/* 0 <= device / 2 && */ device / 2 < 6);
      assert(/* 0 <= device % 2 && */ device % 2 < 2);

      /* Get port addresses. */
      port = const_get(ide_port_table[device / 2]);

      /*
       * Send read command.
       */
      u.command.opcode = ATAPI_READ_SECTOR;
      u.command.reserved0 = 0;
      u.command.reserved1 = 0;
      u.command.reserved2 = 0;
      u.command.reserved3 = 0;
      if (sector_size == 2048) {
            /* CDROM mode */
            u.command.lba = htonl(block);
            start_dummy = 0;
            end_dummy = 0;
            atapi_num = num;

      } else {
            /* floppy emulation mode */
            unsigned long slba;
            unsigned long elba;

            assert(sector_size == 512);

            slba = block;
            elba = block + num;

            u.command.lba = htonl(slba / 4);
            start_dummy = slba % 4;
            if (elba % 4 == 0) {
                  end_dummy = 0;
            } else {
                  end_dummy = 4 - elba % 4;
            }
            atapi_num = elba / 4 - slba / 4;
            if (elba % 4 != 0) {
                  atapi_num++;
            }
      }
      u.command.misc.transfer_length = htons(atapi_num);

      ret = send_atapi_pc(device % 2, port, &u.command);
      if (ret != 0) {
            ret = atapi_request_sense(device % 2, port, &u.rs);
            if (ret != 0) {
                  return ret;
            }
            if ((u.rs.sense_key & 0xf) == ATAPI_UNIT_ATTENTION) {
                  goto again;
            }
            return 1;
      }

      /* read dummy data */
      for (sector_count = 0; sector_count < start_dummy; sector_count++) {
            do {
                  status = inb(port + 0x206);
            } while (status & IDE_BUSY_STAT);
            if (status & IDE_ERR_STAT) {
                  return 1;
            }
            assert(status & IDE_DRQ_STAT);
            for (count = 0; count < sector_size / 2; count++) {
                  data = inw(port);
            }
      }
      /*
       * Get wanted sectors.
       */
      for (sector_count = 0; sector_count < num; sector_count++) {
            do {
                  status = inb(port + 0x206);
            } while (status & IDE_BUSY_STAT);
            if (status & IDE_ERR_STAT) {
                  return 1;
            }
            assert(status & IDE_DRQ_STAT);
            for (count = 0; count < sector_size / 2; count++) {
                  data = inw(port);
                  put_word(buffer_seg, buffer_off, data);
                  buffer_off += 2;
                  if (16 <= buffer_off) {
                        buffer_seg++;
                        buffer_off -= 16;
                  }
            }
      }
      /* read dummy data */
      for (sector_count = 0; sector_count < end_dummy; sector_count++) {
            do {
                  status = inb(port + 0x206);
            } while (status & IDE_BUSY_STAT);
            if (status & IDE_ERR_STAT) {
                  return 1;
            }
            assert(status & IDE_DRQ_STAT);
            for (count = 0; count < sector_size / 2; count++) {
                  data = inw(port);
            }
      }

      return 0;
}

static void
int13_eltorito(struct regs *regs)
{
      struct info {
            unsigned char size;
            unsigned char media;
            unsigned char emulated_drive;
            unsigned char controller_index;
            unsigned long ilba;
            unsigned short device_spec;
            unsigned short buffer_segment;
            unsigned short load_segment;
            unsigned short sector_count;
            unsigned char cylinders;
            unsigned char spt;
            unsigned char heads;
      };

      if (AH == 0x4a          /* ElTorito - Initiate disk emu */
       || AH == 0x4c          /* ElTorito - Initiate disk emu and boot */
       || AH == 0x4d) { /* ElTorito - Return Boot catalog */
            /* FIXME VOSSI */
            assert(0);
            int13_fail(regs, 0x01);

      } else if (AH == 0x4b) {
            /*
             * ElTorito - Terminate disk emu
             */
            put_byte(DS, SI + offsetof(struct info, size),
                  0x13);
            put_byte(DS, SI + offsetof(struct info, media),
                  ebda_get(cdemu.media));
            put_byte(DS, SI + offsetof(struct info, emulated_drive),
                  ebda_get(cdemu.emulated_drive));
            put_byte(DS, SI + offsetof(struct info, controller_index),
                  ebda_get(cdemu.controller_index));
            put_long(DS, SI + offsetof(struct info, ilba),
                  ebda_get(cdemu.ilba));
            put_word(DS, SI + offsetof(struct info, device_spec),
                  ebda_get(cdemu.device_spec));
            put_word(DS, SI + offsetof(struct info, buffer_segment),
                  ebda_get(cdemu.buffer_segment));
            put_word(DS, SI + offsetof(struct info, load_segment),
                  ebda_get(cdemu.load_segment));
            put_word(DS, SI + offsetof(struct info, sector_count),
                  ebda_get(cdemu.sector_count));
            put_byte(DS, SI + offsetof(struct info, cylinders),
                  ebda_get(cdemu.vdevice.cylinders));
            put_byte(DS, SI + offsetof(struct info, spt),
                  ebda_get(cdemu.vdevice.spt));
            put_byte(DS, SI + offsetof(struct info, heads),
                  ebda_get(cdemu.vdevice.heads));

            if (AL == 0x00) {
                  /* Should be handled accordingly to spec */
                  /* FIXME VOSSI */
                  ebda_put(cdemu.active, 0);
            }

            var_put(hd_res, 0x00);
            int13_success(regs, 0x00);
      }
}

static void
int13_floppy(struct regs *regs)
{
      unsigned char ret;
#if 0
      assert(/* 0x00 <= DL && */ DL <= 0x01);
#endif

      if (AH == 0x00) {
            /*
             * Floppy controller reset.
             */
            unsigned char drive_type;

            if (2 <= DL) {
                  int13_fail(regs, 0x01); /* Invalid param. */
                  return;
            }

            drive_type = cmos_get(fd_config);
            if (DL == 0) {
                  drive_type >>= 4;
            } else { assert(DL == 1);
                  drive_type &= 0xf;
            }

            if (drive_type == 0) {
                  int13_fail(regs, 0x80); /* Not responding. */
                  return;
            }

            var_put(f_track[DL], 0);
            int13_success(regs, 0x00);

      } else if (AH == 0x01) {
            /*
             * Read floppy status.
             */
            if (var_get(fd_res) == 0) {
                  int13_success(regs, 0x00);
            } else {
                  int13_fail(regs, var_get(fd_res));
            }

      } else if (AH == 0x02) {
            /*
             * Read floppy sectors.
             */
            ret = floppy_read_chs(DL, AL, CH, DH, CL, ES, BX);
            if (ret == 0) {
                  int13_success(regs, 0x00);
            } else {
                  AL = 0;
                  int13_fail(regs, ret);
            }

      } else if (AH == 0x03) {
            /*
             * Write floppy sectors.
             */
            ret = floppy_write_chs(DL, AL, CH, DH, CL, ES, BX);
            if (ret == 0) {
                  int13_success(regs, 0x00);
            } else {
                  AL = 0;
                  int13_fail(regs, ret);
            }

      } else if (AH == 0x04) {
            /*
             * Verify floppy sectors.
             */
            ret = floppy_verify_chs(DL, AL, CH, DH, CL, ES, BX);
            if (ret == 0) {
                  int13_success(regs, 0x00);
            } else {
                  AL = 0;
                  int13_fail(regs, ret);
            }

      } else if (AH == 0x05) {
            /*
             * Format disk track.
             */
            dprintf("int13_floppy: Unknown function %02x\n", AH);
            assert(0);

      } else if (AH == 0x08) {
            /*
             * Read floppy drive parameters.
             */
            static CONST struct {
                  unsigned char max_track;
                  unsigned char sectors;
                  unsigned char max_head;

            } floppy_types[] = {
                  /* 0 none         */    { 0, 0, 0 },
                  /* 1 360KB, 5.25" */    { 40-1, 9, 2-1 },
                  /* 2 1.2MB, 5.25" */    { 80-1, 15, 2-1 },
                  /* 3 720KB, 3.5"  */    { 80-1, 9, 2-1 },
                  /* 4 1.44MB, 3.5" */    { 80-1, 18, 2-1 },
                  /* 5 2.88MB, 3.5" */    { 80-1, 36, 2-1 },
                  /* 6 160KB, 5.25" */    { 40-1, 8, 1-1 },
                  /* 7 180KB, 5.25" */    { 40-1, 9, 1-1 },
                  /* 8 320KB, 5.25" */    { 40-1, 8, 2-1 },
                  /* 9 unknown      */    { 0, 0, 0 },
                  /* a unknown      */    { 0, 0, 0 },
                  /* b unknown      */    { 0, 0, 0 },
                  /* c unknown      */    { 0, 0, 0 },
                  /* d unknown      */    { 0, 0, 0 },
                  /* e unknown      */    { 0, 0, 0 },
                  /* f unknown      */    { 0, 0, 0 }
            };

            unsigned char num_floppies;
            unsigned char drive_type;

            num_floppies = 0;
            drive_type = cmos_get(fd_config);
            if ((drive_type >> 0) & 0xf) {
                  num_floppies++;
            }
            if ((drive_type >> 4) & 0xf) {
                  num_floppies++;
            }

            if (2 <= DL) {
                  AL = 0;
                  BX = 0;
                  CX = 0;
                  DX = 0;
                  ES = 0;
                  DI = 0;
                  DL = num_floppies;
                  AH = 0x00;  /* no error! */
                  F &= ~(1 << 0);   /* Set carry. */
                  return;
            }

            if (DL == 0) {
                  drive_type >>= 4;
            } else { assert(DL == 1);
                  drive_type &= 0xf;
            }

            BH = 0;
            BL = drive_type;
            AL = 0;
            DL = num_floppies;
            CH = const_get(floppy_types[drive_type].max_track);
            CL = const_get(floppy_types[drive_type].sectors);
            DH = const_get(floppy_types[drive_type].max_head);

            /* Get diskette parameter table stored in INT vector 0x1e. */
            DI = get_word(0x0000, 0x1e * 4 + 0);
            ES = get_word(0x0000, 0x1e * 4 + 2);

            /* Disk status not changed upon success. */
            AH = 0x00;
            F &= ~(1 << 0); /* Clear carry. */

      } else if (AH == 0x15) {
            /*
             * Read floppy drive type.
             */
            unsigned char drive_type;

            if (2 <= DL) {
                  /* var_put(fmot_stat, ...);   FIXME VOSSI */
                  int13_fail(regs, 0x01);
                  return;
            }

            drive_type = cmos_get(fd_config);
            if (DL == 0) {
                  drive_type >>= 4;
            } else { assert(DL == 1);
                  drive_type &= 0xf;
            }
            if (drive_type == 0) {
                  int13_success(regs, 0x00);    /* Drive not present. */
            } else {
                                          /* Drive present. */
#if FD_CHANGELINE_SUPPORT
                  int13_success(regs, 0x02);    /* Does support */
                                          /* change line. */
#else
                  int13_success(regs, 0x01);    /* Does not support */
                                          /* change line. */
#endif
            }
      } else if (AH == 0x16) {
            /*
             * Get floppy change line status.
             */
#if FD_CHANGELINE_SUPPORT
            ret = floppy_check_changeline(DL);
            if(ret == 0) {
                  /* disk not changed: return successfully */
                  int13_success(regs,0x00);
            } else {
                  /* other cases:
                   *  - disk changed
                   *  - no disk present,
                   *  - drive not ready,
                   *  - drive not present,
                   *  - ...
                   * => return as if failed
                   */
                  int13_fail(regs,ret);
            }
#else
            dprintf("int13_floppy: Unknown function %02x\n", AH);
            assert(0);
#endif
      } else if (AH == 0x17) {
            /*
             * Set floppy type for format (old).
             */
            dprintf("int13_floppy: Unknown function %02x\n", AH);
            assert(0);

      } else if (AH == 0x18) {
            /*
             * Set floppy type for format (new).
             * AH:                  0x18
             * DL:                  drive
             * CH:                  cylinder number (bits 0-7)
             * CL (bit 6-7):  cylinder number (bits 8-9)
             * CL (bit 0-5):  sectors per track
             */
            dprintf("int13_floppy: Set floppy type called (%d,%d).\n",
                        (((CL >> 6) & 3) << 8) | CH, (CL >> 0) & 0x3f);
            int13_success(regs, 0x00);    /* FIXME VOSSI */

      } else if (AH == 0x41) {
            /*
             * BIOS Enhanced Disk Drive call:
             * "Check Extensions Present".
             */
            /* Not supported for floppy disks. FIXME VOSSI */
            int13_fail(regs, 0x01);

      } else {
            dprintf("int13_floppy: Drive 0x%02x: Unknown function %02x\n",
                        DL, AH);
            int13_fail(regs, 0x01);
      }
}

static void
int13_harddisk(struct regs *regs)
{
      unsigned char device;

      if (DL < 0x80 || 0x80 + MAX_BIOS_HDS <= DL) {
            var_put(hd_res, 0x01);
            int13_fail(regs, 0x01);
            return;
      }
      device = ebda_get(hdidmap[DL - 0x80]);
      if (MAX_BIOS_HDS <= device) {
            var_put(hd_res, 0x01);
            int13_fail(regs, 0x01);
            return;
      }

      /* Clear completion flag. */
      var_put(hd_irq, 0);

      if (AH == 0x00) {
            /*
             * Reset disk controller.
             */
            /* ata_reset(device); */      /* FIXME VOSSI */
            var_put(hd_res, 0x00);
            int13_success(regs, 0x00);

      } else if (AH == 0x02
            || AH == 0x03
            || AH == 0x04) {
            /*
             * Read disk sectors.
             * Write disk sectors.
             * Verify disk sectors.
             */
            unsigned char count;
            unsigned short cylinder;
            unsigned char sector;
            unsigned char head;
            unsigned short segment;
            unsigned short offset;
            unsigned char status;

            count = AL;
            cylinder = ((unsigned short) (CL & 0xc0) << 2) | CH;
            sector = CL & 0x3f;
            head = DH;

            segment = ES;
            offset = BX;

            /* Sanity check on count. */
            if (128 < count || count == 0) {
                  var_put(hd_res, 0x01);
                  int13_fail(regs, 0x01);
                  return;
            }

            /* Sanity check on c/h/s. */
            if (ebda_get(devices[device].lchs_cylinders) <= cylinder
             || ebda_get(devices[device].lchs_heads) <= head
             || ebda_get(devices[device].lchs_spt) < sector) {
                  var_put(hd_res, 0x01);
                  int13_fail(regs, 0x01);
                  return;
            }

            /* Verify */
            if (AH == 0x04) {
                  /* FIXME VOSSI */
                  var_put(hd_res, 0x00);
                  int13_success(regs, 0x00);
                  return;
            }

            if (AH == 0x02) {
                  status = ide_harddisk_read_chs(device,
                              count, cylinder, head, sector,
                              segment, offset);
            } else {
                  status = ide_harddisk_write_chs(device,
                              count, cylinder, head, sector,
                              segment, offset);
            }
            
            /* Set nb of sector transferred. */
            /* FIXME VOSSI */

            if (status != 0) {
                  var_put(hd_res, 0x0c);
                  int13_fail(regs, 0x0c);
                  return;
            }

            var_put(hd_res, 0x00);
            int13_success(regs, 0x00);

      } else if (AH == 0x05) {
            /*
             * Format disk track.
             */
            /* FIXME VOSSI */
            var_put(hd_res, 0x00);
            int13_success(regs, 0x00);

      } else if (AH == 0x08) {
            /*
             * Read disk drive parameters.
             */
            unsigned short nlc;
            unsigned short nlh;
            unsigned short nlspt;
            unsigned short count;

            nlc = ebda_get(devices[device].lchs_cylinders);
            if (1024 < nlc) {
                  nlc = 1024; /* FIXME VOSSI */
            }
            nlh = ebda_get(devices[device].lchs_heads);
            nlspt = ebda_get(devices[device].lchs_spt);
            count = ebda_get(harddrives);

            nlc = nlc - 2; /* 0 based , last sector not used */

            AL = 0;
            CH = nlc & 0xff;
            CL = ((nlc >> 8) << 6) | (nlspt & 0x3f);
            DH = nlh - 1;
            DL = count; /* FIXME returns 0, 1, or n hard drives */

            /* FIXME should set ES & DI */

            var_put(hd_res, 0x00);
            int13_success(regs, 0x00);

      } else if (AH == 0x10) {
            /*
             * Check drive ready.
             */
            /* FIXME MARCEL */
            F &= ~0x00000001;       /* CARRY = 0 */

      } else if (AH == 0x15) {
            /*
             * Read disk drive size.
             */
            unsigned short npc;
            unsigned short nph;
            unsigned short npspt;
            unsigned long lba;

            /* Get physical geometry from table. */
            /* FIXME VOSSI */
            npc = ebda_get(devices[device].pchs_cylinders);
            nph = ebda_get(devices[device].pchs_heads);
            npspt = ebda_get(devices[device].pchs_spt);

            /* Compute sector count seen by int13. */
            lba = (unsigned long) (npc - 1)
                  * (unsigned long) nph
                  * (unsigned long) npspt;

            CX = (lba >> 16) & 0xffff;
            DX = (lba >>  0) & 0xffff;

            var_put(hd_res, 0x00);
            int13_success(regs, 0x03); /* Type: 3 (Harddisk) */

      } else if (AH == 0x41) {
            /*
             * BIOS Enhanced Disk Drive call:
             * "Check Extensions Present".
             */
            BX = 0xaa55;      /* Install check. */
            CX = 0x0007;      /* Options supported. */
            var_put(hd_res, 0x00);
            int13_success(regs, 0x30);    /* Version 3.0 */

      } else if (AH == 0x42
            || AH == 0x43
            || AH == 0x44
            || AH == 0x47) {
            /*
             * Extended read.
             * Extended write.
             * Extended verify.
             * Extended seek.
             */
            unsigned short count;
            unsigned short segment;
            unsigned short offset;
#if 0
            unsigned long long lba;
#else
            unsigned long lba;
#endif
            unsigned char status;


            count = get_word(DS, SI + 2);
            segment = get_word(DS, SI + 6);
            offset = get_word(DS, SI + 4);
#if 0
            lba = get_qword(DS, SI + 8);
#else
            lba = get_long(DS, SI + 8);
#endif

            /* Sanity check on lba. */
            if (ebda_get(devices[device].sectors) <= lba) {
                  var_put(hd_res, 0x01);
                  int13_fail(regs, 0x01);
                  return;
            }

            if (AH == 0x44 || AH == 0x47) {
                  /* Verify or seek. */
                  var_put(hd_res, 0x00);
                  int13_success(regs, 0x00);
                  return;
            }

            if (AH == 0x42) {
                  /* Read */
                  status = ide_harddisk_read_lba(device / 2, device % 2,
                              count, lba, segment, offset);
            } else { assert(AH == 0x43);
                  /* Write */
                  status = ide_harddisk_write_lba(device / 2, device % 2,
                              count, lba, segment, offset);
            }

            if (status != 0) {
                  var_put(hd_res, 0x0c);
                  int13_fail(regs, 0x0c);
                  return;
            }

            /* FIXME VOSSI */
            put_word(DS, SI + 2, count);

            var_put(hd_res, 0x00);
            int13_success(regs, 0x00);

      } else if (AH == 0x45
            || AH == 0x49) {
            /*
             * IBM/MS lock/unlock drive.
             * IBM/MS extended media change.
             */
            /* All these functions return SUCCESS. */
            var_put(hd_res, 0x00);
            int13_fail(regs, 0x00);

      } else if (AH == 0x46) {
            /*
             * IBM/MS eject media.
             */
            /* This function return FAILURE. */
            var_put(hd_res, 0xb2);
            int13_fail(regs, 0xb2);

      } else if (AH == 0x48) {
            /*
             * BIOS Enhanced Disk Drive call:
             * "Get Device Parameters".
             */
            struct info {
                  unsigned short size;
                  unsigned short flags;
                  unsigned long cyls;
                  unsigned long heads;
                  unsigned long secs;
                  unsigned long total_low;      /* Should be long long */
                  unsigned long total_high;
                  unsigned short bytes_per_sec;
                  /* ... */
            };
            unsigned short size;

            size = get_word(DS, SI + offsetof(struct info, size));

            if (size < 0x1a) {
                  /* Buffer is too small. */
                  var_put(hd_res, 0x01);
                  int13_fail(regs, 0x01);
                  return;
            }

            if (0x1a <= size) {
                  /* EDD 1.x */
                  unsigned long cyls;
                  unsigned long heads;
                  unsigned long secs;
                  unsigned long /* long */ total;     /* FIXME MARCEL */

                  cyls = ebda_get(devices[device].pchs_cylinders);
                  heads = ebda_get(devices[device].pchs_heads);
                  secs = ebda_get(devices[device].pchs_spt);
                  total = cyls * heads * secs;

                  put_word(DS, SI + offsetof(struct info, size), 0x1a);
                  put_word(DS, SI + offsetof(struct info, flags), 0x02);
                  put_long(DS, SI + offsetof(struct info, cyls), cyls);
                  put_long(DS, SI + offsetof(struct info, heads), heads);
                  put_long(DS, SI + offsetof(struct info, secs), secs);
                  put_long(DS, SI + offsetof(struct info, total_low), total);
                  put_long(DS, SI + offsetof(struct info, total_high), 0L); /* FIXME MARCEL */
                  put_word(DS, SI + offsetof(struct info, bytes_per_sec), 512);
            }
            if (0x1e <= size) {
                  /* EDD 2.x */
                  /* put_word(DS, SI + offsetof(struct info, size), 0x1e); */
                  put_word(DS, SI + offsetof(struct info, size), 0x1a);

                  /* Not supported yet. FIXME VOSSI */
            }
            if (0x42 <= size) {
                  /* EDD 3.x */
                  /* Not supported yet. FIXME VOSSI */
            }

            var_put(hd_res, 0x00);
            int13_success(regs, 0x00);

      } else if (AH == 0x4e) {
            /*
             * Set hardware configuration.
             */
            assert(0);  /* FIXME VOSSI */

      } else if (AH == 0x09
            || AH == 0x0c
            || AH == 0x0d
            || AH == 0x11
            || AH == 0x14) {
            /*
             * Initialize drive parameters.
             * Seek to specified cylinder.
             * Alternate disk reset.
             * Recalibrate.
             * Controller internal diagnostic.
             */
            /* All these functions return SUCCESS. */
            var_put(hd_res, 0x00);
            int13_success(regs, 0x00);

      } else if (AH == 0x0a
            || AH == 0x0b
            || AH == 0x18
            || AH == 0x50) {
            /*
             * Read disk sectors with ECC.
             * Write disk sectors with ECC.
             * Set media type for format.
             * IBM/MS send packet command.
             */
            /* All these functions return UNSUPPORTED. */
            var_put(hd_res, 0x01);
            int13_fail(regs, 0x01);

      } else {
            dprintf("int13_harddisk: AH=0x%02x\n", AH);
            /* Return UNSUPPORTED. */
            var_put(hd_res, 0x01);
            int13_fail(regs, 0x01);
      }
}

static void
int13_cdrom(struct regs *regs)
{
      unsigned char device;

      if (DL < 0xe0 || 0xe0 + MAX_BIOS_HDS <= DL) {
            var_put(hd_res, 0x01);
            int13_fail(regs, 0x01);
            return;
      }
      device = ebda_get(cdidmap[DL - 0xe0]);
      if (device == MAX_BIOS_HDS) {
            var_put(hd_res, 0x01);
            int13_fail(regs, 0x01);
            return;
      }

      if (AH == 0x00
       || AH == 0x09
       || AH == 0x0c
       || AH == 0x0d
       || AH == 0x10
       || AH == 0x11
       || AH == 0x14
       || AH == 0x16) {
            /*
             * Disk controller reset.
             * Initialize drive parameters.
             * Seek to specified cylinder.
             * Alternate disk reset.
             * Check drive ready.
             * Recalibrate.
             * Controller internal diagnostic.
             * Detect disk change.
             */
            /* All these functions return SUCCESS. */
            var_put(hd_res, 0x00);
            int13_success(regs, 0x00);

      } else if (AH == 0x03
            || AH == 0x05
            || AH == 0x43) {
            /*
             * Write disk sectors.
             * Format disk track.
             * Extended write.
             */
            /* All these functions return DISK WRITE-PROTECTED. */
            var_put(hd_res, 0x03);
            int13_success(regs, 0x03);

      } else if (AH == 0x01) {
            /*
             * Read disk status.
             */
            unsigned char status;

            status = var_get(hd_res);
            var_put(hd_res, 0x00);
            if (status) {
                  int13_fail(regs, status);
            } else {
                  int13_success(regs, status);
            }

      } else if (AH == 0x015) {
            /*
             * Read disk drive size.
             */
            var_put(hd_res, 0x02);
            int13_fail(regs, 0x02);

      } else if (AH == 0x41) {
            /*
             * BIOS Enhanced Disk Drive call:
             * "Check Extensions Present".
             */
            BX = 0xaa55;            /* Install check. */
            CX = 0x0007;            /* Ext disk access, removable and EDD.*/
            var_put(hd_res, 0x00);
            int13_success(regs, 0x01);    /* Version 1.0 */

      } else if (AH == 0x42
            || AH == 0x44
            || AH == 0x47) {
            /*
             * Extended read.
             * Extended verify sectors.
             * Extended seek.
             */
            unsigned short count;
            unsigned short segment;
            unsigned short offset;
#if 0 /* FIXME VOSSI */
            unsigned long long lba;
#else
            unsigned long lba;
#endif
            unsigned char status;

            count = get_word(DS, SI + offsetof(struct Int13Ext, count));
            segment = get_word(DS, SI + offsetof(struct Int13Ext, segment));
            offset = get_word(DS, SI + offsetof(struct Int13Ext, offset));

#if 0 /* FIXME VOSSI */
            lba = get_longlong(DS, SI + offsetof(struct Int13Ext, lba));
#else
            lba = get_long(DS, SI + offsetof(struct Int13Ext, lba));
#endif

            if (AH == 0x44
             || AH == 0x47) {
                  /* Verify or seek. */
                  var_put(hd_res, 0x00);
                  int13_success(regs, 0x00);
                  return;
            }

            status = read_sector_lba_atapi(device,
                        count, lba, segment, offset, 2048);
            count = (status == 0) ? count : 0;  /* FIXME VOSSI */
            put_word(DS, SI + offsetof(struct Int13Ext, count), count);
            if (status != 0) {
                  var_put(hd_res, 0x0c);
                  int13_fail(regs, 0x0c);
                  return;
            }

            var_put(hd_res, 0x00);
            int13_success(regs, 0x00);

      } else if (AH == 0x45) {
            /*
             * Lock/unlock drive.
             */
            assert(0);  /* FIXME VOSSI */

      } else if (AH == 0x46) {
            /*
             * Eject media.
             */
            assert(0);  /* FIXME VOSSI */

      } else if (AH == 0x48) {
            /*
             * BIOS Enhanced Disk Drive call:
             * "Get Device Parameters".
             */
            unsigned short size;

            size = get_word(DS, SI + offsetof(struct Int13DPT, size));

            if (size < 0x1a) {
                  /* Buffer is too small. */
                  var_put(hd_res, 0x01);
                  int13_fail(regs, 0x01);
                  return;
            }
            if (0x1a <= size) {
                  unsigned short blksize;

                  blksize = 2048;   /* FIXME VOSSI */

                  put_word(DS, SI + offsetof(struct Int13DPT, size),
                              0x1a);

                  /* removable, media change, lockable, max values */
                  put_word(DS, SI + offsetof(struct Int13DPT, flags),
                              0x74);

                  put_long(DS, SI + offsetof(struct Int13DPT, cyls),
                              0xffffffff);
                  put_long(DS, SI + offsetof(struct Int13DPT, heads),
                              0xffffffff);
                  put_long(DS, SI + offsetof(struct Int13DPT, secs),
                              0xffffffff);
                  put_long(DS, SI + offsetof(struct Int13DPT, total_low),
                              0xffffffff);
                  put_long(DS, SI + offsetof(struct Int13DPT, total_high),
                              0xffffffff);
                  put_word(DS, SI + offsetof(struct Int13DPT, bytes_per_sec),
                              blksize);
            }
            if (0x1e <= size) {
                  /* EDD 2.x */
                  put_word(DS, SI + offsetof(struct Int13DPT, size), 0x1e);

                  /* FIXME VOSSI */
            }
            if (0x42 <= size) {
                  /* EDD 3.x */
                  /* Not supported yet. FIXME VOSSI */
            }

            var_put(hd_res, 0x00);
            int13_success(regs, 0x00);

      } else if (AH == 0x49) {
            /*
             * Extended media change.
             */
            /* Always send changed?? */
            var_put(hd_res, 0x06);
            int13_fail(regs, 0x06);

      } else if (AH == 0x4e) {
            /*
             * Set hardware configuration.
             */
            /* DMA, prefetch, PIO maximum not supported. */
            if (AL == 0x01
             || AL == 0x03
             || AL == 0x04
             || AL == 0x06) {
                  var_put(hd_res, 0x00);
                  int13_success(regs, 0x00);
            } else {
                  var_put(hd_res, 0x01);
                  int13_success(regs, 0x01);
            }

      } else if (AH == 0x02
            || AH == 0x04
            || AH == 0x08
            || AH == 0x0a
            || AH == 0x0b
            || AH == 0x18
            || AH == 0x50) {
            /*
             * Read sectors.
             * Verify sectors.
             * Read disk drive parameters.
             * Read disk sectors with ECC.
             * Write disk sectors with ECC.
             * Set media type for format.
             * ? - Send packet command.
             */
            /* All these functions return UNIMPLEMENTED. */
            var_put(hd_res, 0x01);
            int13_fail(regs, 0x01);

      } else {
            dprintf("int13_cdrom: AH=0x%02x\n", AH);
            /* Return UNSUPPORTED. */
            var_put(hd_res, 0x01);
            int13_fail(regs, 0x01);
      }
}

static void
int13_cdemu(struct regs *regs)
{
      unsigned char device;

      /* Recompute the device number. */
      device  = ebda_get(cdemu.controller_index) * 2;
      device += ebda_get(cdemu.device_spec);

      var_put(hd_res, 0x00);

      /*
       * Basic checks: emulation should be active, DL should equal the
       * emulated drive.
       */
      if (ebda_get(cdemu.active) == 0
       || ebda_get(cdemu.emulated_drive) != DL) {
            assert(0);
      }

      if (AH == 0x00          /* Disk controller reset. */
       || AH == 0x09          /* Initialize drive parameters. */
       || AH == 0x0c          /* Seek to specified cylinder. */
       || AH == 0x0d          /* Alternate disk reset. */
       || AH == 0x10          /* Check drive ready. */
       || AH == 0x11          /* Recalibrate. */
       || AH == 0x14          /* Controller internal diagnostic. */
       || AH == 0x16) { /* Detect disk change. */
            /* All these functions return SUCCESS. */
            var_put(hd_res, 0x00);
            int13_success(regs, 0x00);

      } else if (AH == 0x03   /* Write disk sectors. */
            || AH == 0x05) {/* Format disk track. */
            /* All these functions return DISK WRITE-PROTECTED. */
            var_put(hd_res, 0x03);
            int13_fail(regs, 0x03);
            
      } else if (AH == 0x01) {
            /* Read disk status. */
            unsigned char status;

            status = var_get(hd_res);

            var_put(hd_res, 0x00);
            if (status) {
                  int13_fail(regs, status);
            } else {
                  int13_success(regs, status);
            }

      } else if (AH == 0x02   /* Read disk sectors. */
            || AH == 0x04) {/* Verify disk sectors. */
            unsigned short vspt;
            unsigned short vcylinders;
            unsigned short vheads;
            unsigned short sector;
            unsigned short cylinder;
            unsigned short head;
            unsigned short nbsectors;
            unsigned short segment;
            unsigned short offset;
            unsigned long ilba;
            unsigned long vlba;
            unsigned long slba;
            unsigned long before;
            unsigned long elba;
            unsigned char status;

            vspt = ebda_get(cdemu.vdevice.spt);
            vcylinders = ebda_get(cdemu.vdevice.cylinders);
            vheads = ebda_get(cdemu.vdevice.heads);

            ilba = ebda_get(cdemu.ilba);

            sector = CL & 0x3f;
            cylinder = (((unsigned short) CL & 0xc0) << 2) | CH;
            head = DH;
            nbsectors = AL;

            /* No sector to read? */
            if (nbsectors == 0) {
                  var_put(hd_res, 0x00);
                  int13_success(regs, 0x00);
                  return;
            }

            /* Sanity checks. SCO OpenServer needs this! */
            if (vspt < sector
             || vcylinders <= cylinder
             || vheads <= head) {
                  var_put(hd_res, 0x01);
                  int13_fail(regs, 0x01);
                  return;
            }

            if (AH == 0x04) {
                  /* Verify disk sectors. */
                  var_put(hd_res, 0x00);
                  int13_success(regs, 0x00);
                  return;
            }

            segment = ES + (BX / 16);
            offset = BX % 16;

            /* Calculate the virtual lba inside the image. */
            vlba = ((((unsigned long) cylinder * (unsigned long) vheads)
                  + (unsigned long) head) * (unsigned long) vspt)
                  + (unsigned long) sector - 1;

            /* In advance so we don't loose the count. */
            AL = nbsectors;

            /* Start lba on CD. */
            slba = (unsigned long) vlba / 4;
            before = (unsigned long) vlba % 4;

            /* End lba on CD. */
            elba = (unsigned long) (vlba + nbsectors - 1) / 4;

            /* Read command. */
            status = read_sector_lba_atapi(device,
                        nbsectors, ilba * 4 + vlba,
                        segment, offset, 512);
            if (status != 0) {
                  AL = 0;
                  var_put(hd_res, 0x02);
                  int13_fail(regs, 0x02);
                  return;
            }

            var_put(hd_res, 0x00);
            int13_success(regs, 0x00);

      } else if (AH == 0x08) {
            /* Read disk drive parameters. */
            unsigned short vspt;
            unsigned short vcylinders;
            unsigned short vheads;

            vspt = ebda_get(cdemu.vdevice.spt);
            vcylinders = ebda_get(cdemu.vdevice.cylinders) - 1;
            vheads = ebda_get(cdemu.vdevice.heads) - 1;

            AL = 0x00;
            BL = 0x00;
            CH = vcylinders & 0xff;
            CL = ((vcylinders >> 2) & 0xc0) | (vspt & 0x3f);
            DH = vheads;
            DL = 0x02;  /* FIXME ElTorito Various. should send the real count of drives 1 or 2 */
                        /* FIXME ElTorito Harddisk. should send the HD count */
            if (ebda_get(cdemu.media) == 0x01) {
                  BL = 0x02;
            } else if (ebda_get(cdemu.media) == 0x02) {
                  BL = 0x04;
            } else if (ebda_get(cdemu.media) == 0x03) {
                  BL = 0x06;
            }

            /* Get diskette parameter table stored in INT vector 0x1e. */
            DI = get_word(0x0000, 0x1e * 4 + 0);
            ES = get_word(0x0000, 0x1e * 4 + 2);

            var_put(hd_res, 0x00);
            int13_success(regs, 0x00);

      } else if (AH == 0x15) {
            /* Identify drive. */
            /* What to do...? - FIXME VOSSI */
            var_put(hd_res, 0x00);
            int13_success(regs, 0x03);

      } else if (AH == 0x0a   /* Read disk sectors with ECC */
            || AH == 0x0b     /* Write disk sectors with ECC */
            || AH == 0x18     /* Set media type for format */
            || AH == 0x41     /* IBM/MS installation check */
            || AH == 0x42     /* IBM/MS extended read */
            || AH == 0x43     /* IBM/MS extended write */
            || AH == 0x44     /* IBM/MS verify sectors */
            || AH == 0x45     /* IBM/MS lock/unlock drive */
            || AH == 0x46     /* IBM/MS eject media */
            || AH == 0x47     /* IBM/MS extended seek */
            || AH == 0x48     /* IBM/MS get drive parameters */
            || AH == 0x49     /* IBM/MS extended media change */
            || AH == 0x4e     /* ? - set hardware configuration */
            || AH == 0x50     /* ? - send packet command */
            || 1) {           /* default */
            dprintf("int13_cdemu: AH=0x%02x\n", AH);
            /* All these functions return UNIMPLEMENTED. */
            var_put(hd_res, 0x01);
            int13_fail(regs, 0x01);
      }
}

C_ENTRY void
bios_13_xxxx(struct regs *regs)
{
      if (0x4a <= AH && AH <= 0x4d) {
            /*
             * El-torito functions.
             */
            int13_eltorito(regs);

      } else if (ebda_get(cdemu.active)) {
            if (DL == ebda_get(cdemu.emulated_drive)) {
                  /*
                   * Emulated drive.
                   */
                  int13_cdemu(regs);

            } else if ((DL & 0xe0) == ebda_get(cdemu.emulated_drive)) {
                  /*
                   * Drive in same class as emulated drive.
                   * -> change drive number
                   */
                  DL--;

                  /* int 13h,08h is special because it returns
                   * the number of drives in DL, so we mustn't
                   * change it before returning from this handler */
                  if(AH == 0x08) goto int13_legacy;

                  if (DL < 0x80) {
                        int13_floppy(regs);
                  } else if (DL < 0xe0) {
                        int13_harddisk(regs);
                  } else {
                        int13_cdrom(regs);
                  }

                  /* change back drive number as some software
                   * seems to rely on it */
                  DL++;
            } else {
                  /*
                   * Unaffected drive.
                   */
                  goto int13_legacy;
            }
      } else {
int13_legacy:
            /*
             * Standard int13 functions.
             */
            if (DL < 0x80) {
                  int13_floppy(regs);
            } else if (DL < 0xe0) {
                  int13_harddisk(regs);
            } else {
                  int13_cdrom(regs);
            }
      }
}

C_ENTRY void
bios_0e(struct regs *regs)
{
      var_put(f_recal, var_get(f_recal) | (1 << 7));

      eoi();

#if 0
      asm volatile (
            "pushw %ax\n"
            "movb $0x91, %ah\n" /* Signal end-of-operation. */
            "movb $0x01, %al\n" /* Device type is floppy. */
            "int $0x15\n"
            "popw %ax\n"
      );
#endif
}

#endif /* RUNTIME_RM */
/* ==================== REAL-MODE INIT ==================== */
#ifdef INIT_RM

CODE16;

#include "assert.h"
#include "stdio.h"
#include "string.h"
#include "in.h"
#include "io.h"
#include "cmos.h"
#include "var.h"
#include "el_torito.h"
#include "ptrace.h"
#include "disk.h"
#include "const.h"
#include "video.h"

/* cut && paste of umide.h */
/* structure returned by HDIO_GET_IDENTITY, as per ANSI ATA2 rev.2f spec */
struct hd_driveid {
      unsigned short config;  /* lots of obsolete bit flags */
      unsigned short cyls;    /* "physical" cyls */
      unsigned short reserved2;     /* reserved (word 2) */
      unsigned short heads;   /* "physical" heads */
      unsigned short track_bytes;   /* unformatted bytes per track */
      unsigned short sector_bytes;  /* unformatted bytes per sector */
      unsigned short sectors; /* "physical" sectors per track */
      unsigned short vendor0; /* vendor unique */
      unsigned short vendor1; /* vendor unique */
      unsigned short vendor2; /* vendor unique */
      unsigned char serial_no[20];  /* 0 = not_specified */
      unsigned short buf_type;
      unsigned short buf_size;    /* 512 byte increments; 0 = not_specified */
      unsigned short ecc_bytes;   /* for r/w long cmds; 0 = not_specified */
      unsigned char fw_rev[8];      /* 0 = not_specified */
      unsigned char model[40];      /* 0 = not_specified */
      unsigned char max_multsect;   /* 0=not_implemented */
      unsigned char vendor3;        /* vendor unique */
      unsigned short dword_io;      /* 0=not_implemented; 1=implemented */
      unsigned char vendor4;        /* vendor unique */
      unsigned char capability;    /* bits 0:DMA 1:LBA 2:IORDYsw 3:IORDYsup */
      unsigned short reserved50;    /* reserved (word 50) */
      unsigned char vendor5;        /* vendor unique */
      unsigned char tPIO;           /* 0=slow, 1=medium, 2=fast */
      unsigned char vendor6;        /* vendor unique */
      unsigned char tDMA;           /* 0=slow, 1=medium, 2=fast */
      unsigned short field_valid;   /* bits 0:cur_ok 1:eide_ok */
      unsigned short cur_cyls;      /* logical cylinders */
      unsigned short cur_heads;     /* logical heads */
      unsigned short cur_sectors;   /* logical sectors per track */
      unsigned short cur_capacity0; /* logical total sectors on drive */
      unsigned short cur_capacity1; /*  (2 words, misaligned int)     */
      unsigned char multsect;       /* current multiple sector count */
      unsigned char multsect_valid; /* when (bit0==1) multsect is ok */
      uint32_t lba_capacity;        /* total number of sectors */
      unsigned short dma_1word;     /* single-word dma info */
      unsigned short dma_mword;     /* multiple-word dma info */
      unsigned short eide_pio_modes;      /* bits 0:mode3 1:mode4 */
      unsigned short eide_dma_min;  /* min mword dma cycle time (ns) */
      unsigned short eide_dma_time;/* recommended mword dma cycle time (ns) */
      unsigned short eide_pio;      /* min cycle time (ns), no IORDY  */
      unsigned short eide_pio_iordy;      /* min cycle time (ns), with IORDY */
      unsigned short words69_70[2]; /* reserved words 69-70 */
      /* HDIO_GET_IDENTITY currently returns only words 0 through 70 */
      unsigned short words71_74[4]; /* reserved words 71-74 */
      unsigned short queue_depth;   /*  */
      unsigned short words76_79[4]; /* reserved words 76-79 */
      unsigned short major_rev_num; /*  */
      unsigned short minor_rev_num; /*  */
      /* bits 0:Smart 1:Security 2:Removable 3:PM */
      unsigned short command_set_1;
      unsigned short command_set_2; /* bits 14:Smart Enabled 13:0 zero */
      unsigned short cfsse;   /* command set-feature supported extensions */
      unsigned short cfs_enable_1;  /* command set-feature enabled */
      unsigned short cfs_enable_2;  /* command set-feature enabled */
      unsigned short csf_default;   /* command set-feature default */
      unsigned short dma_ultra;     /*  */
      unsigned short word89;  /* reserved (word 89) */
      unsigned short word90;  /* reserved (word 90) */
      unsigned short CurAPMvalues;  /* current APM values */
      unsigned short word92;  /* reserved (word 92) */
      unsigned short hw_config;     /* hardware config */
      unsigned short words94_125[32];     /* reserved words 94-125 */
      unsigned short last_lun;      /* reserved (word 126) */
      unsigned short word127; /* reserved (word 127) */
      unsigned short dlf;     /* device lock function
                         * 15:9        reserved
                         * 8   security level 1:max 0:high
                         * 7:6 reserved
                         * 5   enhanced erase
                         * 4   expire
                         * 3   frozen
                         * 2   locked
                         * 1   en/disabled
                         * 0   capability
                         */
      unsigned short csfo;    /* current set features options
                         * 15:4        reserved
                         * 3   auto reassign
                         * 2   reverting
                         * 1   read-look-ahead
                         * 0   write cache
                         */
      unsigned short words130_155[26]; /* reserved vendor words 130-155 */
      unsigned short word156;
      unsigned short words157_159[3];      /* reserved vendor words 157-159 */
      unsigned short words160_255[95]; /* reserved words 160-255 */
};


static unsigned char
get_identify_ide(
      unsigned short cmd, /* ATA COMMAND */
      unsigned char contr,
      unsigned char drive,
      unsigned short *buf
)
{
      uint16_t port;
      uint8_t status;
      unsigned int i;

      assert(cmd == WIN_IDENTIFY || cmd == WIN_PIDENTIFY);
      assert(/* 0 <= contr && */ contr < 6);
      assert(/* 0 <= drive && */ drive < 2);

      port = const_get(ide_port_table[contr]);

      /* Wait while busy. */
      for (i = 0; ; i++) {
            if (i == 0xffff) {
                  return 1;
            }
            status = inb(port + 0x206);
            if (! (status & IDE_BUSY_STAT)) {
                  break;
            }
      }

      /* Send command. */
      outb(drive << 4, port + 6);   /* unit */
      outb(cmd, port + 7);          /* command */

      /* Wait while controller busy. */
      do {
            status = inb(port + 0x206);
      } while (status & IDE_BUSY_STAT);

      if (! (status & IDE_DRQ_STAT)
       || (status & IDE_ERR_STAT)) {
            /* Strange response -> command not recognized. */
            return 1;
      }

      /* Read info. */
      for (i = 0; i < 512 / sizeof(unsigned short); i++) {
            buf[i] = inw(port);
      }

      return 0;
}

static uint8_t
recalibrate(unsigned char contr, unsigned char drive)
{
      uint16_t port;
      uint8_t status;

      assert(/* 0 <= contr && */ contr < 6);
      assert(/* 0 <= drive && */ drive < 2);

      port = const_get(ide_port_table[contr]);

      /* Send command. */
      outb(0, port + 1);            /* Features */
      outb(1, port + 2);            /* #Sectors */
      outb(0, port + 3);            /* Sector Number */
      outb(0, port + 4);            /* Cylinder Low */
      outb(0, port + 5);            /* Cylinder High */
      outb(drive << 4, port + 6);   /* Unit */
      outb(WIN_RESTORE, port + 7);  /* Command */

      /* Wait while controller busy. */
      do {
            status = inb(port + 0x206);
            if (status == 0xff) {
                  /* No drive present. */
                  return 1;
            }
      } while (status & IDE_BUSY_STAT);

      return 0;
}

const char *
get_identify_floppy(unsigned short drive)
{
      static const char * const floppy_type[] = {
            "None",
            "360K, 5.25 in.",
            "1.2M, 5.25 in.",
            "720K, 3.5 in.",
            "1.44M, 3.5 in.",
            "2.88M, 3.5 in.",
            "160K, 5.25 in.",
            "180K, 5.25 in.",
            "320K, 5.25 in.",
      };
      unsigned char drive_type;

      drive_type = cmos_get(fd_config);

      if (drive == 0) {
            drive_type >>= 4;
      } else {
            drive_type &= 0xf;
      }
      
      return floppy_type[drive_type];
}

void
floppy_init(void)
{
      uint8_t drive_type;
      uint16_t equipment;

      /* Reset floppy controller. */
      outb(0x08, 0x3f2);
      outb(0x0c, 0x3f2);

      /* Call sense interrupt status. */
      outb(0x08, 0x3f5);

      /* Get result bytes. */
      (void) inb(0x3f5);
      (void) inb(0x3f5);

      /* Call configure to enable implied seeks. */
      outb(0x13, 0x3f5); /* Configure command */
      outb(0x00, 0x3f5); /* Dummy */
      outb(0 << 7 /* ? */
         | 1 << 6 /* Enable implied seek */
         | 1 << 5 /* Enable FIFO */
         | 1 << 4 /* Disable polling */
         | 0 << 0, /* FIFO threshold = 1 */
            0x3f5);
      outb(0x00, 0x3f5); /* Precompensation beginning at track 0 */

      /* Get result bytes. */
      /* None. */

      /* int $0x1e vector is diskette_param_table. */
      put_word(0x0000, 0x1e * 4 + 0, PTR_OFF(&diskette_param_table));
      put_word(0x0000, 0x1e * 4 + 2, PTR_SEG(&diskette_param_table));

      /* adjust sys_conf settings for floppy drives */
      /* bit   0: drives are present */
      /* bit 7-6: number of drives - 1 */
      drive_type = cmos_get(fd_config);
      equipment = var_get(sys_conf) & ~0xc1;
      if ((drive_type >> 4) & 0xf) {
            equipment |= (1 << 0);
      }
      if ((drive_type >> 0) & 0xf) {
            equipment |= (1 << 6);
      }
      var_put(sys_conf, equipment);
}

static void
detecting(unsigned short contr, unsigned short unit)
{
      bprintf("  Detecting IDE ");
      if (contr == 0) {
            if (unit == 0) {
                  bprintf("Primary Master  ");
            } else {
                  bprintf("Primary Slave   ");
            }
      } else if (contr == 1) {
            if (unit == 0) {
                  bprintf("Secondary Master");
            } else {
                  bprintf("Secondary Slave ");
            }
      } else {
            bprintf("%d/%d             ", contr, unit);
      }
      bprintf("... ");
}


void
disk_init(void)
{
      unsigned char hd;
      unsigned char cd;
      unsigned short contr;
      unsigned short unit;
      unsigned short i;

      hd = 0x80;
      cd = 0xe0;
      ebda_put(harddrives, 0);
      ebda_put(cdcount, 0);
      var_put(hd_num, 0);
      cmos_put(hd_config, 0x00);
      cmos_put(hd_extconfig0, 0);
      cmos_put(hd_extconfig1, 0);
      cmos_put(hd_extconfig2, 0);
      cmos_put(hd_extconfig3, 0);

      for (contr = 0; contr < 6; contr++) {
            for (unit = 0; unit < 2; unit++) {
                  const unsigned int nr = contr * 2 + unit;
                  unsigned short buffer[512 / sizeof(unsigned short)];
                  struct hd_driveid *id = (struct hd_driveid *) buffer;
                  int type; /* none == -1; disk == 0; cdrom == 1*/

                  if (contr < 2) {
                        detecting(contr, unit);
                  }

                  /* First try disk. */
                  if (! get_identify_ide(WIN_IDENTIFY,
                              contr, unit, buffer)) {
                        /* Harddisk */
                        type = 0;
                        ebda_put(devices[nr].drive, hd);
                        ebda_put(devices[nr].pchs_cylinders, id->cyls);
                        ebda_put(devices[nr].pchs_heads, id->heads);
                        ebda_put(devices[nr].pchs_spt, id->sectors);
                        ebda_put(devices[nr].lchs_cylinders, id->cur_cyls);
                        ebda_put(devices[nr].lchs_heads, id->cur_heads);
                        ebda_put(devices[nr].lchs_spt, id->cur_sectors);

                        ebda_put(devices[nr].sectors, id->lba_capacity);
                        ebda_put(devices[nr].type, 0);

                        if (hd - 0x80 < 2) {
                              ebda_put(hdpt[hd - 0x80].cyls, id->cyls);
                              ebda_put(hdpt[hd - 0x80].heads, id->heads);
                              ebda_put(hdpt[hd - 0x80].reserved0, 0);
                              ebda_put(hdpt[hd - 0x80].precomp, -1);
                              ebda_put(hdpt[hd - 0x80].reserved1, 0);
                              ebda_put(hdpt[hd - 0x80].control, (8 < id->heads) << 3);
                              ebda_put(hdpt[hd - 0x80].reserved2, 0);
                              ebda_put(hdpt[hd - 0x80].reserved3, 0);
                              ebda_put(hdpt[hd - 0x80].reserved4, 0);
                              ebda_put(hdpt[hd - 0x80].landing, id->cyls - 1);
                              ebda_put(hdpt[hd - 0x80].secs, id->sectors);
                              ebda_put(hdpt[hd - 0x80].reserved5, 0);
                        }
                        if (hd == 0x80) {
                              cmos_put(hd_config,
                                    0xf0 | cmos_get(hd_config));
                              cmos_put(hd_extconfig0, 0x2f);
                        } else if (hd == 0x81) {
                              cmos_put(hd_config,
                                    0x0f | cmos_get(hd_config));
                              cmos_put(hd_extconfig1, 0x2f);
                        } else if (hd == 0x82) {
                              cmos_put(hd_extconfig2, 0x2f);
                        } else if (hd == 0x83) {
                              cmos_put(hd_extconfig2, 0x2f);
                        }
                        if (nr == 0) {
                              /* Primary Master */
                              cmos_put(hd_config_user0.cyls,
                                          id->cyls);
                              cmos_put(hd_config_user0.heads,
                                          id->heads);
                              cmos_put(hd_config_user0.precomp,
                                          -1);
                              cmos_put(hd_config_user0.landing,
                                          id->cyls - 1);
                              cmos_put(hd_config_user0.secs,
                                          id->sectors);
                        } else if (nr == 1) {
                              /* Primary Slave */
                              cmos_put(hd_config_user1.cyls,
                                          id->cyls);
                              cmos_put(hd_config_user1.heads,
                                          id->heads);
                              cmos_put(hd_config_user1.precomp,
                                          -1);
                              cmos_put(hd_config_user1.landing,
                                          id->cyls - 1);
                              cmos_put(hd_config_user1.secs,
                                          id->sectors);
                        } else if (nr == 2) {
                              /* Secondary Master */
                              cmos_put(hd_config_user2.cyls,
                                          id->cyls);
                              cmos_put(hd_config_user2.heads,
                                          id->heads);
                              cmos_put(hd_config_user2.precomp,
                                          -1);
                              cmos_put(hd_config_user2.landing,
                                          id->cyls - 1);
                              cmos_put(hd_config_user2.secs,
                                          id->sectors);
                        } else if (nr == 3) {
                              /* Secondary Slave */
                              cmos_put(hd_config_user3.cyls,
                                          id->cyls);
                              cmos_put(hd_config_user3.heads,
                                          id->heads);
                              cmos_put(hd_config_user3.precomp,
                                          -1);
                              cmos_put(hd_config_user3.landing,
                                          id->cyls - 1);
                              cmos_put(hd_config_user3.secs,
                                          id->sectors);
                        }

                        ebda_put(hdidmap[hd - 0x80], nr);

                        hd++;

                        ebda_put(harddrives, ebda_get(harddrives) + 1);
                        var_put(hd_num, var_get(hd_num) + 1);

                  } else if (! get_identify_ide(WIN_PIDENTIFY,
                              contr, unit, buffer)) {
                        /* CD-ROM */
                        type = 1;
                        ebda_put(devices[nr].drive, cd);
                        ebda_put(devices[nr].type, 1);

                        ebda_put(cdidmap[cd - 0xe0], nr);

                        cd++;

                        ebda_put(cdcount, ebda_get(cdcount) + 1);

                  } else {
                        /* nothing */
                        ebda_put(devices[nr].drive, 0xff);
                        ebda_put(devices[nr].type, 0xfe);
                        type = -1;
                  }

                  if (2 <= contr && type != -1) {
                        detecting(contr, unit);
                  }

                  if (contr < 2 && type == -1) {
                        bprintf("None\n");
                  } else if (type != -1) {
                        /* display device name */
                        for (i = 0; i < 40; i++) {
                              if (i % 2) {
                                    putchar(id->model[i-1]);
                              } else {
                                    putchar(id->model[i+1]);
                              }
                        }
                        putchar('\n');
                  }
            }
      }

      for ( ; hd < 0x80 + MAX_BIOS_HDS; hd++) {
            ebda_put(hdidmap[hd - 0x80], MAX_BIOS_HDS);
      }
      for ( ; cd < 0xe0 + MAX_BIOS_HDS; cd++) {
            ebda_put(cdidmap[cd - 0xe0], MAX_BIOS_HDS);
      }

      /* int $0x41 vector is harddisk0_param_table. */
      put_word(0x0000, 0x41 * 4 + 0, offsetof(struct ebda, hdpt[0]));
      put_word(0x0000, 0x41 * 4 + 2, EBDA_SEG);

      /* int $0x46 vector is harddisk1_param_table. */
      put_word(0x0000, 0x46 * 4 + 0, offsetof(struct ebda, hdpt[1]));
      put_word(0x0000, 0x46 * 4 + 2, EBDA_SEG);

      /*
       * WARNING: Minix3 needs HDs accessed last.
       * So just recalibrate HDs.
       */
      for (hd = 0x80; hd < 0x80 + MAX_BIOS_HDS; hd++) {
            unsigned int nr;

            nr = ebda_get(hdidmap[hd - 0x80]);

            if (nr != MAX_BIOS_HDS) {
                  recalibrate(nr / 2, nr % 2);
            }
      }
}

#endif /* INIT_RM */

Generated by  Doxygen 1.6.0   Back to index