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

ide_gen_disk.c

/*
 * $Id: ide_gen_disk.c,v 1.111 2009-02-25 08:16:50 vrsieh Exp $ 
 *
 * Copyright (C) 2007-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.
 */

/* Debugging Options */
#define DEBUG_CONTROL_FLOW    0
#define DEBUG_INTERRUPT       0
#define DEBUG_OPCODES         0

/* Configuration Options */
#define MAX_IDE_MULTIPLE_SECTOR           16
#define DISK_RECALIBRATE_SUPORT           1
#define DISK_READ_BUFFER_SUPPORT    1
#define DISK_WRITE_BUFFER_SUPPORT   1
#define DISK_SMART_SUPPORT          1
#define DISK_POWER_MANAGEMENT_SUPPORT     1


#include "config.h"
#include "compiler.h"

#include <assert.h>
#include "fixme.h"
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include "glue-main.h"
#include "glue-shm.h"
#include "glue-storage.h"
#include "gui.h"
#include "sig_ide.h"

#include "ide_gen_disk.h"

#define COMP "ide_gen_disk"

struct cpssp {
      /*
       * Config
       */
      unsigned int unit;
      unsigned int serial_number;
      char media_name[128];
      char media_image[128];
      int media_cow;
      int media_create;
      int media_sync;
      int media_sparse;

      int phys_cyls;
      int phys_heads;
      int phys_secs;

      /*
       * Ports
       */
      struct sig_boolean *sig_5V;
      struct sig_boolean *sig_12V;
      struct sig_ide_bus *cable;
      int fi_fd;

      /*
       * State
       */
      struct storage media;

      unsigned long cyls;
      unsigned long heads;
      unsigned long secs;

      /* Note: We can read/write up to 256 blocks at most. */
      /*volatile*/ unsigned char buf[256 * 512];

      volatile unsigned int head;
      volatile unsigned int tail;
      volatile unsigned int count;

      unsigned int reset;
      unsigned int newcmd;
      unsigned int blocked;

      unsigned int irq;

      volatile unsigned char error; /* error reg   (read only)  */
      volatile unsigned char feature;     /* feature reg (write only) */
      volatile unsigned char nsector;     /* sector count reg */
      volatile unsigned char sector;      /* sector reg */
      volatile unsigned char lcyl;  /* low cylinder reg */
      volatile unsigned char hcyl;  /* high cylinder reg */
      volatile unsigned char select;      /* select reg */
      volatile unsigned char status;      /* status reg */
      volatile unsigned char command;     /* command reg */
      unsigned char control;        /* device control reg */

      unsigned char multsect;       /* current multiple sector */

      unsigned char pio_mode;
      unsigned char mdma_mode;
      unsigned char udma_mode;

#if DISK_SMART_SUPPORT
      int smart_enabled;
#endif

      /*
       * Faults
       */
      int faulty_disk;
      unsigned long long faulty_blk[10];

      /*
       * Processes
       */
      struct process process;
};


#define UNIT   ((cpssp->select >> 4) & 1)

static void
ide_gen_disk_irq_update(struct cpssp *cpssp)
{
      unsigned int val;

      if (cpssp->irq
       && ((cpssp->control >> 1) & 1) == 0
       && ((cpssp->select >> 4) & 1) == cpssp->unit) {
            val = 1;
      } else {
            val = 0;
      }
      if (DEBUG_INTERRUPT
       && loglevel) {
            fprintf(stderr, "%s: irq=%d\n", __FUNCTION__, val);
      }
      sig_ide_bus_irq(cpssp->cable, cpssp, val);
}

static void
ide_gen_disk_gen_irq(struct cpssp *cpssp)
{
      cpssp->irq = 1;
      ide_gen_disk_irq_update(cpssp);
}

static void
ide_gen_disk_ugen_irq(struct cpssp *cpssp)
{
      cpssp->irq = 0;
      ide_gen_disk_irq_update(cpssp);
}

static void
ide_gen_disk_reset(struct cpssp *cpssp)
{
      cpssp->cyls = cpssp->phys_cyls;
      cpssp->heads = cpssp->phys_heads;
      cpssp->secs = cpssp->phys_secs;

      cpssp->head      = 0;
      cpssp->tail      = 0;
      cpssp->count     = 0;

      ide_gen_disk_ugen_irq(cpssp);

      /*
       * Diagnostic code
       * (dev 0 passed, dev 1 passed or not present)
       * (see Execute Device Diagnostic, page 106)
       */
      cpssp->error     = 1;

      cpssp->feature   = 0;

      /*
       * Reset Signature
       */
      cpssp->nsector   = 1;
      cpssp->sector    = 1;
      cpssp->lcyl      = 0;
      cpssp->hcyl      = 0;
      cpssp->select    = 0;

      cpssp->status    = READY_STAT | SEEK_STAT;
      cpssp->command   = 0;
      cpssp->control   = 0;

      cpssp->multsect  = 0;

      cpssp->pio_mode  = 0;
      cpssp->mdma_mode = 0;
      cpssp->udma_mode = 0;
}

static void
ide_gen_disk_send(struct cpssp *cpssp, unsigned int size, int dma, int block)
{
      cpssp->head = size;
      cpssp->tail = 0;
      cpssp->count = size;

      cpssp->status |= READY_STAT | DRQ_STAT;
      cpssp->status &= ~BUSY_STAT;
      if (dma) {
            sig_ide_bus_dmarq_set(cpssp->cable, 1);
      }
      while (block
          && ! cpssp->reset
          && ! cpssp->newcmd
          && 0 < cpssp->count) {
            cpssp->blocked = 1;
            sched_sleep();
            cpssp->blocked = 0;
      }
}

static void
ide_gen_disk_recv(struct cpssp *cpssp, unsigned int size, int dma)
{
      cpssp->head = 0;
      cpssp->tail = 0;
      cpssp->count = size;

      cpssp->status |= READY_STAT | DRQ_STAT;
      cpssp->status &= ~BUSY_STAT;
      if (dma) {
            sig_ide_bus_dmarq_set(cpssp->cable, 1);
      }
      while (! cpssp->reset
          && ! cpssp->newcmd
          && 0 < cpssp->count) {
            cpssp->blocked = 1;
            sched_sleep();
            cpssp->blocked = 0;
      }
}

static int
umide_gen_disk_check_chs(
      unsigned long cyls,
      unsigned long heads,
      unsigned long secs,
      unsigned long cyl,
      unsigned long head,
      unsigned long sec
) {
      if (cyls <= cyl
       || heads <= head
       || secs <= sec - 1) {
            fprintf(stderr, "Bad C/H/S: %ld/%ld/%ld\n",
                        cyl, head, sec);
            fprintf(stderr, "Max C/H/S: %ld/%ld/%ld\n",
                        cyls, heads, secs + 1);
            return -1;
      }

      return 0;
}

static int
umide_gen_disk_read_lba(
      struct cpssp *cpssp,
      unsigned char *buffer,
      unsigned long blkno,
      unsigned int blocks
)
{
      unsigned int i;

      for (i = 0; ; i++) {
            if (i == sizeof(cpssp->faulty_blk) / sizeof(cpssp->faulty_blk[0])) {
                  /* Not a bad sector. */
                  break;
            }
            if (blkno == cpssp->faulty_blk[i]) {
                  return -1;
            }
      }

      return storage_read_write(IO_READ, &cpssp->media,
                  buffer, blkno * 512ULL, blocks * 512);
}


static int
umide_gen_disk_read_chs(
      struct cpssp *cpssp,
      unsigned char *buffer,
      unsigned short cylp,
      unsigned char headp,
      unsigned char secp,
      unsigned int blocks
)
{
      unsigned long blkno;

      if (umide_gen_disk_check_chs(cpssp->cyls, cpssp->heads, cpssp->secs,
                        cylp, headp, secp) < 0) {
            /* FIXME tg */
            fprintf(stderr, "%s: Should raise error and abort FIXME tg\n", __FUNCTION__);
            assert(0);
            return -1;
      }

      blkno = (cylp * cpssp->heads + headp) * cpssp->secs + secp - 1;

      return umide_gen_disk_read_lba(cpssp, buffer, blkno, blocks);
}

static int
umide_gen_disk_write_lba(
      struct cpssp *cpssp,
      unsigned char *buffer,
      unsigned long blkno,
      unsigned int blocks
)
{
      unsigned int i;

      for (i = 0; ; i++) {
            if (i == sizeof(cpssp->faulty_blk) / sizeof(cpssp->faulty_blk[0])) {
                  /* Not a bad sector. */
                  break;
            }
            if (blkno == cpssp->faulty_blk[i]) {
                  /* It's a bad sector. */
                  return -1;
            }
      }

      return storage_read_write(IO_WRITE, &cpssp->media,
                  buffer, blkno * 512ULL, blocks * 512);
}

static int
umide_gen_disk_write_chs(
      struct cpssp *cpssp,
      unsigned char *buffer,
      unsigned short cylp,
      unsigned char headp,
      unsigned char secp,
      unsigned int blocks
)
{
      unsigned long blkno;

      if (umide_gen_disk_check_chs(cpssp->cyls, cpssp->heads, cpssp->secs,
                        cylp, headp, secp) < 0) {
            /* FIXME tg */
            fprintf(stderr, "%s: Should raise error and abort FIXME tg\n", __FUNCTION__);
            assert(0);
            return -1;
      }

      blkno = (cylp * cpssp->heads + headp) * cpssp->secs + secp - 1;

      return umide_gen_disk_write_lba(cpssp, buffer, blkno, blocks);
}

static void
umide_not_implemented(struct cpssp *cpssp)
{
      cpssp->error = 1 << 2; /* Abort */
      cpssp->status |= ERR_STAT;
}

#if DISK_RECALIBRATE_SUPORT
/* 0x10 */
static void
ide_gen_disk_recalibrate(struct cpssp *cpssp)
{
      ide_gen_disk_gen_irq(cpssp);
}
#endif /* DISK_RECALIBRATE_SUPORT */

/* 0x20 */
static void
ide_gen_disk_read(struct cpssp *cpssp)
{
      int lba;
      uint16_t cyl;
      uint8_t head;
      uint8_t sector;
      unsigned int blocks;

      lba = (cpssp->select >> 6) & 1;
      cyl = (uint16_t) cpssp->lcyl + (uint16_t) cpssp->hcyl * 256;
      head = (uint8_t) cpssp->select & 0x0f;
      sector = (uint8_t) cpssp->sector;

      do {
            /*
             * How many blocks should we read at once?
             */
            if (cpssp->nsector == 0) {
                  blocks = 256;
            } else {
                  blocks = cpssp->nsector;
            }
            switch (cpssp->command) {
            case WIN_READ:
            case WIN_READ + 1:
                  blocks = 1;
                  break;
            case WIN_READDMA:
            case WIN_READDMA + 1:
                  break;
            case WIN_MULTREAD:
                  if (cpssp->multsect == 0) {
                        blocks = 1;
                  } else if (cpssp->multsect < blocks) {
                        blocks = cpssp->multsect;
                  }
                  break;
            default:
                  assert(0); /* Mustn't happen. */
            }

            /*
             * Read blocks.
             */
            if (lba) {
                  /* LBA mode */
                  uint32_t pos;

                  pos = (head << 24) | (cyl << 8) | (sector << 0);

                  umide_gen_disk_read_lba(cpssp,
                              cpssp->buf, pos, blocks);

                  pos += blocks;
                  head = (pos >> 24) & 0x0f;
                  cyl = (pos >> 8) & 0xffff;
                  sector = (pos >> 0) & 0xff;

            } else {
                  /* C/H/S mode */
                  unsigned int i;

                  umide_gen_disk_read_chs(cpssp,
                              cpssp->buf, cyl, head, sector,
                              blocks);

                  for (i = 0; i < blocks; i++) {
                        sector++;
                        if (cpssp->secs < sector) {
                              sector = 1;
                              head += 1;
                              if (cpssp->heads <= head) {
                                    head = 0;
                                    cyl += 1;
                              }
                        }
                  }
            }

#if 0
            if (ret != 512) {
                  /* UNC = Uncorrectable data error - Schmidt page 59/60 */
                  cpssp->error = 1 << 6;
                  cpssp->status |= ERR_STAT;
            }
#endif

            /*
             * Update C/H/S info.
             */
            cpssp->nsector -= blocks;
            cpssp->hcyl = (cyl >> 8) & 0xff;
            cpssp->lcyl = (cyl >> 0) & 0xff;
            cpssp->select = (cpssp->select & 0xf0) | head;
            cpssp->sector = sector;

            /*
             * Send blocks to host.
             */
            if (cpssp->command != WIN_READDMA
             && cpssp->command != WIN_READDMA + 1) {
                  ide_gen_disk_gen_irq(cpssp);
            }
            ide_gen_disk_send(cpssp, blocks * 512,
                        cpssp->command == WIN_READDMA
                        || cpssp->command == WIN_READDMA + 1,
                        cpssp->nsector != 0
                        || cpssp->command == WIN_READDMA
                        || cpssp->command == WIN_READDMA + 1);
      } while (! cpssp->reset
            && ! cpssp->newcmd
            && cpssp->nsector);

      if (cpssp->command == WIN_READDMA
       || cpssp->command == WIN_READDMA + 1) {
            ide_gen_disk_gen_irq(cpssp);
      }
}

static void
ide_gen_disk_write(struct cpssp *cpssp)
{
      int lba;
      uint16_t cyl;
      uint8_t head;
      uint8_t sector;
      unsigned int blocks;

      lba = (cpssp->select >> 6) & 1;
      cyl = (uint16_t) cpssp->lcyl + (uint16_t) cpssp->hcyl * 256;
      head = (uint8_t) cpssp->select & 0x0f;
      sector = (uint8_t) cpssp->sector;

      do {
            /*
             * How many blocks should we write at once?
             */
            if (cpssp->nsector == 0) {
                  blocks = 256;
            } else {
                  blocks = cpssp->nsector;
            }
            switch (cpssp->command) {
            case WIN_WRITE:
            case WIN_WRITE + 1:
                  blocks = 1;
                  break;
            case WIN_WRITEDMA:
            case WIN_WRITEDMA + 1:
                  break;
            case WIN_MULTWRITE:
                  if (cpssp->multsect == 0) {
                        blocks = 1;
                  } else if (cpssp->multsect < blocks) {
                        blocks = cpssp->multsect;
                  }
                  break;
            default:
                  assert(0); /* Mustn't happen. */
            }

            /*
             * Receive blocks from host.
             */
            ide_gen_disk_recv(cpssp, blocks * 512,
                        cpssp->command == WIN_WRITEDMA
                        || cpssp->command == WIN_WRITEDMA + 1);
            if (cpssp->reset
             || cpssp->newcmd) {
                  break;
            }

            /*
             * Write blocks.
             */
            if (lba) {
                  /* LBA */
                  uint32_t pos;

                  pos = (head << 24) | (cyl << 8) | (sector << 0);

                  umide_gen_disk_write_lba(cpssp,
                              cpssp->buf, pos, blocks);

                  pos += blocks;
                  head = (pos >> 24) & 0x0f;
                  cyl = (pos >> 8) & 0xffff;
                  sector = (pos >> 0) & 0xff;

            } else {
                  /* C/H/S */
                  unsigned int i;

                  umide_gen_disk_write_chs(cpssp,
                              cpssp->buf, cyl, head, sector,
                              blocks);

                  for (i = 0; i < blocks; i++) {
                        sector += 1;
                        if (cpssp->secs < sector) {
                              sector = 1;
                              head += 1;
                              if (cpssp->heads <= head) {
                                    head = 0;
                                    cyl += 1;
                              }
                        }
                  }
            }

#if 0
            if (ret != 512) {
                  /* UNC = Uncorrectable data error - Schmidt page 59/60 */
                  cpssp->error = 1 << 6;
                  cpssp->status |= ERR_STAT;
            }
#endif

            /*
             * Update C/H/S info.
             */
            cpssp->nsector -= blocks;
            cpssp->hcyl = (cyl >> 8) & 0xff;
            cpssp->lcyl = (cyl >> 0) & 0xff;
            cpssp->select = (cpssp->select & 0xf0) | head;
            cpssp->sector = sector;

            if (cpssp->command != WIN_WRITEDMA
             && cpssp->command != WIN_WRITEDMA + 1) {
                  ide_gen_disk_gen_irq(cpssp);
            }
      } while (cpssp->nsector);

      if (cpssp->command == WIN_WRITEDMA
       || cpssp->command == WIN_WRITEDMA + 1) {
            ide_gen_disk_gen_irq(cpssp);
      }
}

/* 0x40 */
static void
ide_gen_disk_read_verify_sectors_with_retries(struct cpssp *cpssp)
{
      /* FIXME */
      ide_gen_disk_gen_irq(cpssp);
}

/* 0x41 */
static void
ide_gen_disk_read_verify_sectors_without_retries(struct cpssp *cpssp)
{
      /* FIXME */
      ide_gen_disk_gen_irq(cpssp);
}

/* 0x70 */
static void
ide_gen_disk_seek(struct cpssp *cpssp)
{
      /* FIXME */
      ide_gen_disk_gen_irq(cpssp);
}

/* 0x90 */
static void
ide_gen_disk_execute_device_diagnostic(struct cpssp *cpssp)
{
      cpssp->lcyl = 0x00;
      cpssp->hcyl = 0x00;
      cpssp->nsector = 0x01;
      cpssp->select = 0x00;
      cpssp->sector = 0x01;
      cpssp->error = 0x01; /* Passed */

      ide_gen_disk_gen_irq(cpssp);
}

/* 0x91 */
static void
ide_gen_disk_initialize_device_parameters(struct cpssp *cpssp)
{
      unsigned int nsectors;
      unsigned long blocks;

      blocks = cpssp->phys_cyls * cpssp->phys_heads * cpssp->phys_secs;

      nsectors = (cpssp->nsector == 0) ? 256 : cpssp->nsector;

      cpssp->secs = nsectors;
      blocks /= nsectors;
      cpssp->heads = (cpssp->select & 0x0f) + 1;
      blocks /= (cpssp->select & 0x0f) + 1;
      cpssp->cyls = blocks;

      ide_gen_disk_gen_irq(cpssp);
}

#if DISK_POWER_MANAGEMENT_SUPPORT
/* 0x94 */
/* 0xe0 */
static void
ide_gen_disk_standby_immediate(struct cpssp *cpssp)
{
      /* FIXME */
      ide_gen_disk_gen_irq(cpssp);
}

/* 0x95 */
/* 0xe1 */
static void
ide_gen_disk_idle_immediate(struct cpssp *cpssp)
{
      /* FIXME */
      ide_gen_disk_gen_irq(cpssp);
}

/* 0x96 */
/* 0xe2 */
static void
ide_gen_disk_standby(struct cpssp *cpssp)
{
      /* FIXME */
      ide_gen_disk_gen_irq(cpssp);
}

/* 0x97 */
/* 0xe3 */
static void
ide_gen_disk_idle(struct cpssp *cpssp)
{
      /* FIXME */
      ide_gen_disk_gen_irq(cpssp);
}

/* 0x98 */
/* 0xe5 */
static void
ide_gen_disk_check_power_mode(struct cpssp *cpssp)
{
      cpssp->nsector = 0xff; /* Active Mode */

      ide_gen_disk_gen_irq(cpssp);
}

/* 0x99 */
/* 0xe6 */
static void
ide_gen_disk_sleep(struct cpssp *cpssp)
{
      /* FIXME */
      ide_gen_disk_gen_irq(cpssp);
}
#endif /* DISK_POWER_MANAGEMENT_SUPPORT */

#if DISK_SMART_SUPPORT
/* 0xb0 */
static void
ide_gen_disk_smart(struct cpssp *cpssp)
{
      uint16_t *p16;
      uint8_t *p8;
      uint8_t sum;
      unsigned int attr;
      unsigned int i;

      if (cpssp->lcyl != 0x4f
       || cpssp->hcyl != 0xc2) {
            /* Unknown SMART command. */
      unknown:;
      disabled:;
            cpssp->error = 1 << 2; /* Abort */
            cpssp->status |= ERR_STAT;

            ide_gen_disk_gen_irq(cpssp);

      } else switch (cpssp->feature) {
      case 0xd0:
            /*
             * Read attribute values.
             * Optional and not recommended.
             */
            if (! cpssp->smart_enabled) goto disabled;

            p16 = (uint16_t *) cpssp->buf;
            *p16++ = 0x0004; /* Revision Number */
            for (attr = 0; attr < 30; attr++) {
                  *p16++ = 0;
                  *p16++ = 0;
                  *p16++ = 0;
                  *p16++ = 0;
                  *p16++ = 0;
                  *p16++ = 0;
            }
            *p16++ = 0x0000; /* Reserved */
            *p16++ = 0x0000; /* Reserved */
            *p16++ = 0x0000; /* Reserved */
            *p16++ = (1 << 1) /* AUTOSAVE supported */
                  | (1 << 0); /* Pre-Power saving supported */
            for (i = 0; i < 16 / 2; i++) {
                  *p16++ = 0x0000; /* Reserved */
            }
            for (i = 0; i < 126 / 2; i++) {
                  *p16++ = 0x0000; /* Vendor specific */
            }
            assert(p16 == (uint16_t *) &cpssp->buf[512]);

            sum = 0;
            p8 = (uint8_t *) cpssp->buf;
            for (i = 0; i < 511; i++) {
                  sum += *p8++;
            }
            *p8++ = sum;
            assert(p8 == (uint8_t *) &cpssp->buf[512]);

            ide_gen_disk_gen_irq(cpssp);
            ide_gen_disk_send(cpssp, 512, 0, 0);
            break;

      case 0xd1:
            /*
             * Read attribute thresholds.
             * Optional and not recommended.
             */
            if (! cpssp->smart_enabled) goto disabled;

            p16 = (uint16_t *) cpssp->buf;
            *p16++ = 0x0004; /* Revision Number */
            for (attr = 0; attr < 30; attr++) {
                  *p16++ = 0;
                  *p16++ = 0;
                  *p16++ = 0;
                  *p16++ = 0;
                  *p16++ = 0;
                  *p16++ = 0;
            }
            for (i = 0; i < 18 / 2; i++) {
                  *p16++ = 0x0000; /* Reserved */
            }
            for (i = 0; i < 132 / 2; i++) {
                  *p16++ = 0x0000; /* Vendor specific */
            }
            assert(p16 == (uint16_t *) &cpssp->buf[512]);

            sum = 0;
            p8 = (uint8_t *) cpssp->buf;
            for (i = 0; i < 511; i++) {
                  sum += *p8++;
            }
            *p8++ = sum;
            assert(p8 == (uint8_t *) &cpssp->buf[512]);

            ide_gen_disk_gen_irq(cpssp);
            ide_gen_disk_send(cpssp, 512, 0, 0);
            break;

      case 0xd2:
            /* Enable/disable attribute autosave. */
            if (! cpssp->smart_enabled) goto disabled;

            switch (cpssp->nsector) {
            case 0x00:
                  /* Disable attribute autosave. */
                  /* FIXME */
                  break;
            case 0xf1:
                  /* Enable attribute autosave. */
                  /* FIXME */
                  break;
            default:
                  goto unknown;
            }

            ide_gen_disk_gen_irq(cpssp);
            break;

      case 0xd3:
            /*
             * Save attribute values.
             * Optional and not recommended.
             */
            if (! cpssp->smart_enabled) goto disabled;

            /* FIXME */
            break;

      case 0xd8:
            /* SMART enable operations */
            cpssp->smart_enabled = 1;

            ide_gen_disk_gen_irq(cpssp);
            break;

      case 0xd9:
            /* SMART disable operations */
            if (! cpssp->smart_enabled) goto disabled;

            cpssp->smart_enabled = 1;
            ide_gen_disk_gen_irq(cpssp);
            break;

      case 0xda:
            /* Return status. */
            if (! cpssp->smart_enabled) goto disabled;

            /* FIXME */
            ide_gen_disk_gen_irq(cpssp);
            break;

      default:
            goto unknown;
      }
}
#endif /* DISK_SMART_SUPPORT */

/* 0xc4 */
static void
ide_gen_disk_read_multiple(struct cpssp *cpssp)
{
      ide_gen_disk_read(cpssp);
}

/* 0xc5 */
static void
ide_gen_disk_write_multiple(struct cpssp *cpssp)
{
      ide_gen_disk_write(cpssp);
}

/* 0xc6 */
static void
ide_gen_disk_set_multiple_mode(struct cpssp *cpssp)
{
      if (cpssp->nsector <= MAX_IDE_MULTIPLE_SECTOR) {
            cpssp->multsect = cpssp->nsector;

      } else {
            /* not supported blocksize => disable */
            cpssp->multsect = 0;
            cpssp->error = 1 << 6;
            cpssp->status |= ERR_STAT;
      }

      ide_gen_disk_gen_irq(cpssp);
}

/* 0xc8 */
static void
ide_gen_disk_read_dma(struct cpssp *cpssp)
{
      ide_gen_disk_read(cpssp);
}

/* 0xca */
static void
ide_gen_disk_write_dma(struct cpssp *cpssp)
{
      ide_gen_disk_write(cpssp);
}

#if DISK_READ_BUFFER_SUPPORT
/* 0xe4 */
static void
ide_gen_disk_read_buffer(struct cpssp *cpssp)
{
      /* Just send bytes from buffer. */
      ide_gen_disk_gen_irq(cpssp);
      ide_gen_disk_send(cpssp, 512, 0, 0);
}
#endif /* DISK_READ_BUFFER_SUPPORT */

#if DISK_WRITE_BUFFER_SUPPORT
/* 0xe8 */
static void
ide_gen_disk_write_buffer(struct cpssp *cpssp)
{
      /* Just receive bytes to buffer. */
      ide_gen_disk_recv(cpssp, 512, 0);
      ide_gen_disk_gen_irq(cpssp);
}
#endif /* DISK_WRITE_BUFFER_SUPPORT */

/* 0xec */
static void
ide_gen_disk_identify_device(struct cpssp *cpssp)
{
      uint16_t *p = (uint16_t *) cpssp->buf;

      /*
       * See
       * ATA-3
       * page 49 ff.
       */
      memset(p, 0, 512);
      /* 0: General configuration: */
      p[0] |= 0 << 15; /* ATA Device */
      p[0] |= 0 << 14; /* Obsolete */
      p[0] |= 0 << 13; /* Obsolete */
      p[0] |= 0 << 12; /* Obsolete */
      p[0] |= 0 << 11; /* Obsolete */
      p[0] |= 0 << 10; /* Obsolete */
      p[0] |= 0 << 9; /* Obsolete */
      p[0] |= 0 << 8; /* Obsolete */
      p[0] |= 0 << 7; /* Not removable */
      p[0] |= 1 << 6; /* Not removable controller and/or device */
      p[0] |= 0 << 5; /* Obsolete */
      p[0] |= 0 << 4; /* Obsolete */
      p[0] |= 0 << 3; /* Obsolete */
      p[0] |= 0 << 2; /* Obsolete */
      p[0] |= 0 << 1; /* Obsolete */
      p[0] |= 0 << 0; /* Reserved */
      /* 1: Number of logical cylinders */
      p[1] = cpssp->phys_cyls;
      /* 2: Reserved */
      /* 3: Number of logical heads */
      p[3] = cpssp->phys_heads;
      /* 4-5: Obsolete */
      /* 6: Number of logical sectors per logical track */
      p[6] = cpssp->phys_secs;
      /* 7-9: Vendor specific */
      /* 10-19: Serial number */
      {
            char buf[21];

            sprintf(buf, "%020u", cpssp->serial_number);
            p[10] = (buf[0] << 8) | (buf[1] << 0);
            p[11] = (buf[2] << 8) | (buf[3] << 0);
            p[12] = (buf[4] << 8) | (buf[5] << 0);
            p[13] = (buf[6] << 8) | (buf[7] << 0);
            p[14] = (buf[8] << 8) | (buf[9] << 0);
            p[15] = (buf[10] << 8) | (buf[11] << 0);
            p[16] = (buf[12] << 8) | (buf[13] << 0);
            p[17] = (buf[14] << 8) | (buf[15] << 0);
            p[18] = (buf[16] << 8) | (buf[17] << 0);
            p[19] = (buf[18] << 8) | (buf[19] << 0);
      }
      /* 20-21: Obsolete */
      /* 22: Number of vendor specific bytes available on READ/WRITE LONG */
      p[22] = 4;
      /* 23-26: Firmware revision ("1.0")*/
      p[23] = ('1' << 8) | ('.' << 0);
      p[24] = ('0' << 8) | (' ' << 0);
      p[25] = (' ' << 8) | (' ' << 0);
      p[26] = (' ' << 8) | (' ' << 0);
      /* 27-46: Model number ("FAUmachine IDE") */
      p[27] = ('F' << 8) | ('A' << 0);
      p[28] = ('U' << 8) | ('m' << 0);
      p[29] = ('a' << 8) | ('c' << 0);
      p[30] = ('h' << 8) | ('i' << 0);
      p[31] = ('n' << 8) | ('e' << 0);
      p[32] = (' ' << 8) | ('I' << 0);
      p[33] = ('D' << 8) | ('E' << 0);
      p[34] = (' ' << 8) | (' ' << 0);
      p[35] = (' ' << 8) | (' ' << 0);
      p[36] = (' ' << 8) | (' ' << 0);
      p[37] = (' ' << 8) | (' ' << 0);
      p[38] = (' ' << 8) | (' ' << 0);
      p[39] = (' ' << 8) | (' ' << 0);
      p[40] = (' ' << 8) | (' ' << 0);
      p[41] = (' ' << 8) | (' ' << 0);
      p[42] = (' ' << 8) | (' ' << 0);
      p[43] = (' ' << 8) | (' ' << 0);
      p[44] = (' ' << 8) | (' ' << 0);
      p[45] = (' ' << 8) | (' ' << 0);
      p[46] = (' ' << 8) | (' ' << 0);
      /* 47: */
      p[47] |= 0x00 << 8; /* Vendor specific */
      p[47] |= MAX_IDE_MULTIPLE_SECTOR << 0; /* Maximum number of sectors */
      /* 48: Reserved */
      /* 49: Capabilities */
      p[49] |= 0 << 15; /* Reserved */
      p[49] |= 0 << 14; /* Reserved */
      p[49] |= DISK_POWER_MANAGEMENT_SUPPORT << 13; /* Standby timer */
      p[49] |= 0 << 12; /* Reserved */
      p[49] |= 1 << 11; /* IORDY supported */
      p[49] |= 1 << 10; /* IORDY can be disabled */
      p[49] |= 0 << 9; /* Obsolete */
      p[49] |= 0 << 8; /* Obsolete */
      p[49] |= 0 << 7; /* Vendor specific */
      /* ... */
      p[49] |= 0 << 0; /* Vendor specific */
      /* 50: Reserved */
      /* 51: */
      p[51] |= (0x02 << 8); /* PIO data transfer cycle timing mode */
      p[51] |= 0x00 << 0; /* Vendor specific */
      /* 52: Obsolete/Vendor specific */
      /* 53: */
      p[53] |= 0 << 15; /* Reserved */
      /* ... */
      p[53] |= 0 << 2; /* Reserved */
      p[53] |= 1 << 1; /* The fields reported in word 64-70 are valid */
      p[53] |= 1 << 0; /* The fields reported in word 54-58 are valid */
      /* 54: Number of current logical cylinders */
      p[54] = cpssp->cyls;
      /* 55: Number of current logical heads */
      p[55] = cpssp->heads;
      /* 56: Number of current logical sectors per track */
      p[56] = cpssp->secs;
      /* 57-58: Current capacity in sectors */
      p[57] = (cpssp->secs * cpssp->heads * cpssp->cyls) & 0xFFFF;
      p[58] = (cpssp->secs * cpssp->heads * cpssp->cyls) >> 16;
      /* 59: */
      p[59] |= 0 << 15; /* Reserved */
      /* ... */
      p[59] |= 0 << 9; /* Reserved */
      p[59] |= (cpssp->multsect != 0) << 8; /* Multiple sector setting is valid. */
      p[59] |= cpssp->multsect << 0;
      /* 60-61: Total number of user addressable sectors (LBA mode only) */
      p[60] = (cpssp->phys_secs * cpssp->phys_heads * cpssp->phys_cyls) & 0xFFFF;
      p[61] = (cpssp->phys_secs * cpssp->phys_heads * cpssp->phys_cyls) >> 16;
      /* 62: Obsolete */
      /* 63: */
      p[63] |= 1 << (cpssp->mdma_mode + 8); /* Selected Multiword DMA mode */
      p[63] |= 0 << 7; /* Reserved */
      /* ... */
      p[63] |= 0 << 3; /* Reserved */
      p[63] |= 1 << 2; /* Multiword DMA mode 2 and below are supported. */
      p[63] |= 1 << 1; /* Multiword DMA mode 1 and below are supported. */
      p[63] |= 1 << 0; /* Multiword DMA mode 0 is supported. */
      /* 64: PIO modes supported. */
      p[64] |= 0 << 15; /* Reserved */
      /* ... */
      p[64] |= 0 << 8; /* Reserved */
      p[64] |= 0 << 7; /* Reserved */
      /* ... */
      p[64] |= 0 << 2; /* Reserved */
      p[64] |= 1 << 1; /* Advanced PIO transfer mode 4 supported */
      p[64] |= 1 << 0; /* Advanced PIO transfer mode 3 supported */
      /* 65: Minimum Multiword DMA transfer cycle time per word (ns) */
      p[65] = 0x0078;
      /* 66: Manufacturer's recommended Multiword DMA transfer cycle time */
      p[66] = 0x0078;
      /* 67: Minimum PIO transfer cycle time without flow control */
      p[67] = 0x0078;
      /* 68: Minimum PIO transfer cycle time IORDY flow control */
      p[68] = 0x0078;
      /* 69-79: Reserved */
      /* 80: Major version number */
      p[80] |= 0 << 15; /* Reserved */
      p[80] |= 0 << 14; /* Reserved for ATA/ATAPI-14 */
      /* ... */
      p[80] |= 0 << 8; /* Reserved for ATA/ATAPI-8 */
      p[80] |= 0 << 7; /* Reserved for ATA/ATAPI-7 */
      p[80] |= 0 << 6; /* Reserved for ATA/ATAPI-6 */
      p[80] |= 0 << 5; /* Reserved for ATA/ATAPI-5 */
      p[80] |= 0 << 4; /* Reserved for ATA/ATAPI-4 */
      p[80] |= 1 << 3; /* We support ATA/ATAPI-3 */
      p[80] |= 1 << 2; /* We support ATA/ATAPI-2 */
      p[80] |= 1 << 1; /* We support ATA/ATAPI-1 */
      p[80] |= 0 << 0; /* Reserved */
      /* 81: Minor version number */
      p[81] = 0x000c; /* ATA-3 X3T13 2008D revision 7 */
      /* 82: Command set supported */
      p[82] |= 0 << 15; /* Reserved */
      /* ... */
      p[82] |= 0 << 4; /* Reserved */
      p[82] |= DISK_POWER_MANAGEMENT_SUPPORT << 3; /* Power Management */
      p[82] |= 0 << 2; /* Removable Media */
      p[82] |= 0 << 1; /* Security Mode */
      p[82] |= DISK_SMART_SUPPORT << 0; /* SMART feature */
      /* 83: Command sets supported */
      p[83] |= 0 << 15; /* Shall be cleared to zero */
      p[83] |= 1 << 14; /* Shall be set to one */
      p[83] |= 0 << 13; /* Reserved */
      /* ... */
      p[83] |= 0 << 0; /* Reserved */
      /* 84-127: Reserved */
      /* 128: Security status */
      p[128] |= 0 << 15; /* Reserved */
      /* ... */
      p[128] |= 0 << 9; /* Reserved */
      p[128] |= 0 << 8; /* Security level 0=High, 1=Maximum */
      p[128] |= 0 << 7; /* Reserved */
      /* ... */
      p[128] |= 0 << 5; /* Reserved */
      p[128] |= 0 << 4; /* 1=Security count expired */
      p[128] |= 0 << 3; /* 1=Security frozen */
      p[128] |= 0 << 2; /* 1=Security locked*/
      p[128] |= 0 << 1; /* 1=Security enabled */
      p[128] |= 0 << 0; /* 1=Security supported */
      /* 129-159: Vendor specific */
      /* 160-255: Reserved */

      ide_gen_disk_gen_irq(cpssp);
      ide_gen_disk_send(cpssp, 512, 0, 0);
}

/* 0xef */
static void
ide_gen_disk_set_features(struct cpssp *cpssp)
{
      switch (cpssp->feature) {
      case 0x02: /* Enable write cache */
      case 0x82: /* Disable write cache */
            /* FIXME --tg 21:04 05-01-25 */
            break;

      case 0x03: /* Set transfer mode */
            switch (cpssp->nsector & 0xF8) {
            case 0x08: /* PIO Mode */
                  cpssp->pio_mode = cpssp->nsector & 0x07;
                  break;

      /* Multiword- *or* Ultra- DMA is selected */

            case 0x20: /* Multiword DMA Mode */
                  cpssp->mdma_mode = cpssp->nsector & 0x07;
                  cpssp->udma_mode = 0;
                  break;
            case 0x40: /* UDMA Mode */
                  cpssp->udma_mode = cpssp->nsector & 0x07;
                  cpssp->mdma_mode = 0;
                  break;
            }
            break;

      case 0x55: /* Disable read look-ahead. */
            /* Nothing to do (yet)... */
            break;

      case 0xaa: /* Enable read look-ahead. */
            /* Nothing to do (yet)... */
            break;

      default:
            fprintf(stderr, "%s: Warning: tried to set feature: 0x%02x: not implemented\n",
                        __FUNCTION__, cpssp->feature);
            umide_not_implemented(cpssp);
      }

      ide_gen_disk_gen_irq(cpssp);
}

static void
ide_report_command(struct cpssp *cpssp)
{
      const char *p;

      switch (cpssp->command) {
#if DISK_RECALIBRATE_SUPORT
      case 0x10:                  p = "Restore"; break;
#endif
      case WIN_READ:              p = "Read (with retries)"; break;
      case WIN_READ + 1:          p = "Read (without retries)"; break;
      case WIN_WRITE:             p = "Write (with retries)"; break;
      case WIN_WRITE + 1:         p = "Write (without retries)"; break;
      case 0x40:                  p = "Read Verify Sector(s) (with retries)";
                                  break;
      case 0x40 + 1:              p = "Read Verify Sector(s) (without retries)";
                                  break;
      case 0x70:                  p = "Seek"; break;
      case 0x90:                  p = "Execute Device Diagnostic"; break;
      case 0x91:                  p = "Initialize Device Parameters"; break;
#if DISK_POWER_MANAGEMENT_SUPPORT
      case 0x94:
      case 0xe0:                  p = "Standby Immediate"; break;
      case 0x95:
      case 0xe1:                  p = "Idle Immediate"; break;
      case 0x96:
      case 0xe2:                  p = "Standby"; break;
      case 0x97:
      case 0xe3:                  p = "Idle"; break;
      case 0x98:
      case 0xe5:                  p = "Check Power Mode"; break;
      case 0x99:
      case 0xe6:                  p = "Sleep"; break;
#endif
#if DISK_SMART_SUPPORT
      case 0xb0:                  p = "Smart"; break;
#endif
      case WIN_MULTREAD:          p = "Multiple Read"; break;
      case WIN_MULTWRITE:         p = "Multiple Write"; break;
      case 0xc6:                  p = "Set Multiple Mode"; break;
      case WIN_READDMA:           p = "Read DMA (with retries)"; break;
      case WIN_READDMA + 1:       p = "Read DMA (without retries)"; break;
      case WIN_WRITEDMA:          p = "Write DMA (with retries)"; break;
      case WIN_WRITEDMA + 1:      p = "Write DMA (without retries)"; break;
#if DISK_READ_BUFFER_SUPPORT
      case 0xe4:                  p = "Read Buffer"; break;
#endif
#if DISK_WRITE_BUFFER_SUPPORT
      case 0xe8:                  p = "Write Buffer"; break;
#endif
      case 0xec:                  p = "Identify"; break;
      case 0xef:                  p = "Set Features"; break;
      default:                    p = "Unkown"; break;
      }

      fprintf(stderr, "%30s(0x%02x) (%u/%u/%u)", p, cpssp->command,
                  cpssp->cyls, cpssp->heads, cpssp->secs);

      if (cpssp->command == WIN_READ
       || cpssp->command == WIN_READ + 1
       || cpssp->command == WIN_WRITE
       || cpssp->command == WIN_WRITE + 1
       || cpssp->command == 0x70 /* Seek */
       || cpssp->command == WIN_MULTREAD
       || cpssp->command == WIN_MULTWRITE
       || cpssp->command == WIN_READDMA
       || cpssp->command == WIN_READDMA + 1
       || cpssp->command == WIN_WRITEDMA
       || cpssp->command == WIN_WRITEDMA + 1) {
            unsigned int blocks;
            unsigned int cyl;
            unsigned int head;
            unsigned int sector;

            blocks = (cpssp->nsector == 0) ? 256 : cpssp->nsector;
            cyl = (unsigned int) cpssp->lcyl + (unsigned int) cpssp->hcyl * 256;
            head = (unsigned char) cpssp->select & 0x0f;
            sector = (unsigned char) cpssp->sector;

            if ((cpssp->select >> 6) & 1) {
                  /* LBA */
                  unsigned long pos;

                  pos = (head << 24) | (cyl << 8) | (sector << 0);
                  fprintf(stderr, " lba = %lu", pos);

            } else {
                  /* C/H/S */
                  fprintf(stderr, " chs = %u/%u/%u", cyl, head, sector);
            }

            fprintf(stderr, " sectors = %u", blocks);
      }

      fprintf(stderr, "\n");
}

static void __attribute__((__noreturn__))
ide_gen_disk_process(void *_cpssp)
{
      struct cpssp *cpssp = _cpssp;

      cpssp->status = SEEK_STAT;

      for (;;) {
            /*
             * Wait for command.
             */
            while (! cpssp->reset
                && ! cpssp->newcmd) {
                  cpssp->status &= ~BUSY_STAT;
                  cpssp->status |= READY_STAT;
                  sched_sleep();
            }
            if (cpssp->reset) {
                  /*
                   * Execute Reset
                   */
                  cpssp->reset = 0;
                  ide_gen_disk_reset(cpssp);

            } else { assert(cpssp->newcmd);
                  /*
                   * Execute Command
                   */
                  cpssp->newcmd = 0;

                  if (DEBUG_OPCODES
                   && loglevel) {
                        ide_report_command(cpssp);
                  }

                  switch (cpssp->command) {
#if DISK_RECALIBRATE_SUPORT
                  case 0x10:
                        ide_gen_disk_recalibrate(cpssp);
                        break;
#endif /* DISK_RECALIBRATE_SUPORT */
                  case WIN_READ:          /* 0x20 */
                  case WIN_READ + 1:      /* 0x21 */
                        ide_gen_disk_read(cpssp);
                        break;
                  case WIN_WRITE:         /* 0x30 */
                  case WIN_WRITE + 1:     /* 0x31 */
                        ide_gen_disk_write(cpssp);
                        break;
                  case 0x40:
                        ide_gen_disk_read_verify_sectors_with_retries(cpssp);
                        break;
                  case 0x41:
                        ide_gen_disk_read_verify_sectors_without_retries(cpssp);
                        break;
                  case 0x70:
                        ide_gen_disk_seek(cpssp);
                        break;
                  case 0x90:
                        ide_gen_disk_execute_device_diagnostic(cpssp);
                        break;
                  case 0x91:
                        ide_gen_disk_initialize_device_parameters(cpssp);
                        break;
#if DISK_POWER_MANAGEMENT_SUPPORT
                  case 0x94:
                  case 0xe0:
                        ide_gen_disk_standby_immediate(cpssp);
                        break;
                  case 0x95:
                  case 0xe1:
                        ide_gen_disk_idle_immediate(cpssp);
                        break;
                  case 0x96:
                  case 0xe2:
                        ide_gen_disk_standby(cpssp);
                        break;
                  case 0x97:
                  case 0xe3:
                        ide_gen_disk_idle(cpssp);
                        break;
                  case 0x98:
                  case 0xe5:
                        ide_gen_disk_check_power_mode(cpssp);
                        break;
                  case 0x99:
                  case 0xe6:
                        ide_gen_disk_sleep(cpssp);
                        break;
#endif /* DISK_POWER_MANAGEMENT_SUPPORT */
#if DISK_SMART_SUPPORT
                  case 0xb0:
                        ide_gen_disk_smart(cpssp);
                        break;
#endif /* DISK_SMART_SUPPORT */
                  case WIN_MULTREAD:      /* 0xc4 */
                        ide_gen_disk_read_multiple(cpssp);
                        break;
                  case WIN_MULTWRITE:     /* 0xc5 */
                        ide_gen_disk_write_multiple(cpssp);
                        break;
                  case 0xc6:
                        ide_gen_disk_set_multiple_mode(cpssp);
                        break;
                  case WIN_READDMA: /* 0xc8 */
                  case WIN_READDMA + 1:   /* 0xc9 */
                        ide_gen_disk_read_dma(cpssp);
                        break;
                  case WIN_WRITEDMA:      /* 0xca */
                  case WIN_WRITEDMA + 1:  /* 0xcb */
                        ide_gen_disk_write_dma(cpssp);
                        break;
#if DISK_READ_BUFFER_SUPPORT
                  case 0xe4:
                        ide_gen_disk_read_buffer(cpssp);
                        break;
#endif /* DISK_READ_BUFFER_SUPPORT */
#if DISK_WRITE_BUFFER_SUPPORT
                  case 0xe8:
                        ide_gen_disk_write_buffer(cpssp);
                        break;
#endif /* DISK_WRITE_BUFFER_SUPPORT */
                  case 0xec:
                        ide_gen_disk_identify_device(cpssp);
                        break;
                  case 0xef:
                        ide_gen_disk_set_features(cpssp);
                        break;
                  default:
                        cpssp->error = 1 << 2; /* Abort */
                        cpssp->status |= ERR_STAT;
                        ide_gen_disk_gen_irq(cpssp);
                        break;
                  }
            }
      }
}

static int
ide_gen_disk_inw(void *_cpssp, unsigned short port, uint16_t *valp)
{
      struct cpssp *cpssp = (struct cpssp *) _cpssp;

      if (DEBUG_CONTROL_FLOW
       && loglevel) {
            fprintf(stderr, "%s called(port=0x%04x)\n", __FUNCTION__, port);
      }

      if (cpssp->faulty_disk) {
            return 1;
      }

      if (UNIT != cpssp->unit) {
            return 1;
      }

      switch (port) {
      case 0:           /* data port */
            if (! (cpssp->status & BUSY_STAT)
             && cpssp->status & DRQ_STAT) {
                  assert(sizeof(*valp) <= cpssp->count);
                  assert(cpssp->tail + sizeof(*valp) <= cpssp->head);

                  memcpy(valp, &cpssp->buf[cpssp->tail], sizeof(*valp));
                  cpssp->tail += sizeof(*valp);
                  cpssp->count -= sizeof(*valp);

                  if (cpssp->count == 0) {
                        sig_ide_bus_dmarq_set(cpssp->cable, 0);
                        cpssp->status &= ~DRQ_STAT;
                        if (cpssp->blocked) {
                              cpssp->status |= BUSY_STAT;
                              cpssp->status &= ~READY_STAT;
                              sched_wakeup(&cpssp->process);
                        }
                  }
            } else {
                  fprintf(stderr, "WARNING: %s: inw: status=0x%02x, count=%u.\n",
                              COMP, cpssp->status, cpssp->count);
                  *valp = 0;
            }
            break;
      case 1:           /* error register */
            *valp = cpssp->error;
            break;
      case 2:           /* sector count register */
            *valp = cpssp->nsector;
            break;
      case 3:           /* sector number register */
            *valp = cpssp->sector;
            break;
      case 4:           /* cylinder low register */
            *valp = cpssp->lcyl;
            break;
      case 5:           /* cylinder high register */
            *valp = cpssp->hcyl;
            break;
      case 6:           /* drive/head register */
            *valp = cpssp->select | 0xa0;
            ide_gen_disk_irq_update(cpssp);
            break;

      case 7:           /* status register */
            *valp = cpssp->status;
            ide_gen_disk_ugen_irq(cpssp);
            break;
      case 8:           /* altstatus register */
            *valp = cpssp->status;
#if 0
            cpssp->status &= ~ERR_STAT;
#endif
            break;
      default:
            assert(0);
            *valp = 0;  /* Just to make gcc happy... */
            /*NOTREACHED*/
      }

      if (DEBUG_CONTROL_FLOW
       && loglevel) {
            fprintf(stderr, "%s return(*valp=0x%04x)\n",
                        __FUNCTION__, *valp);
      }
      return 0;
}

static void
ide_gen_disk_outw(void *_cpssp, unsigned short port, uint16_t value)
{
      struct cpssp *cpssp = (struct cpssp *) _cpssp;

      if (DEBUG_CONTROL_FLOW
       && loglevel) {
            fprintf(stderr, "%s called(value=0x%04x, port=0x%04x)\n",
                        __FUNCTION__, value, port);
      }

      if (cpssp->faulty_disk) {
            return;
      }

      switch (port) {
      case 1:           /* feature register (write only) */
            cpssp->feature = value;
            return;
      case 2:           /* sector count register */
            cpssp->nsector = value;
            return;
      case 3:           /* sector number register */
            cpssp->sector = value;
            return;
      case 4:           /* cylinder low register */
            cpssp->lcyl = value;
            return;
      case 5:           /* cylinder high register */
            cpssp->hcyl = value;
            return;
      case 6:           /* drive/head register */
            cpssp->select = value & ~0xa0;
            return;
      case 8:           /* device control register */
            cpssp->control = value;
            if (value & 0x04) {
                  /* if SFRS bit set -> reset */
                  cpssp->status |= BUSY_STAT;
                  cpssp->reset = 1;
                  sched_wakeup(&cpssp->process);
            }
            return;
      }
      
      if (UNIT != cpssp->unit) {
            return;
      }

      switch (port) {
      case 0:           /* data port */
            if (! (cpssp->status & BUSY_STAT)
             && cpssp->status & DRQ_STAT) {
                  assert(sizeof(value) <= cpssp->count);
                  assert(cpssp->head + sizeof(value) <= sizeof(cpssp->buf));

                  memcpy(&cpssp->buf[cpssp->head], &value, sizeof(value));
                  cpssp->head += sizeof(value);
                  cpssp->count -= sizeof(value);

                  if (cpssp->count == 0) {
                        sig_ide_bus_dmarq_set(cpssp->cable, 0);
                        cpssp->status &= ~DRQ_STAT;
                        if (cpssp->blocked) {
                              cpssp->status |= BUSY_STAT;
                              cpssp->status &= ~READY_STAT;
                              sched_wakeup(&cpssp->process);
                        }
                  }
            } else {
                  fprintf(stderr, "WARNING: %s: outw: status=0x%02x, count=%u.\n",
                              COMP, cpssp->status, cpssp->count);
            }
            break;
      case 7:           /* command register */
            if (! (cpssp->status & BUSY_STAT)
             /* && cpssp->status & READY_STAT */) {
                  cpssp->command = value;
                  cpssp->status |= BUSY_STAT;
                  cpssp->status &= ~(READY_STAT | DRQ_STAT | ERR_STAT);
                  cpssp->newcmd = 1;
                  ide_gen_disk_ugen_irq(cpssp);
                  sched_wakeup(&cpssp->process);
            } else {
                  fprintf(stderr, "WARNING: %s: outw: status=0x%02x.\n",
                              COMP, cpssp->status);
            }
            break;
      default:
            assert(0);
            /* NOTREACHED --tg 21:26 2002-05-20 */
      }

      if (DEBUG_CONTROL_FLOW
       && loglevel) {
            fprintf(stderr, "%s return()\n", __FUNCTION__);
      }
}

static void
ide_gen_disk_power_set(void *_css, unsigned int val)
{
      struct cpssp *css = (struct cpssp *) _css;

      if (val) {
            /* Power On Event */
            ide_gen_disk_reset(css);

      } else {
            /* Power Off Event */
            /* Nothing to do (yet). */
      }
}

static void
ide_gen_disk_disk_fault_set(
      void *_cpssp,
      unsigned long long loc0,
      unsigned long long loc1,
      unsigned int val
)
{
      struct cpssp *cpssp = (struct cpssp *) _cpssp;

      if (val) {
            /* Add new fault. */
            assert(! cpssp->faulty_disk);
            cpssp->faulty_disk = 1;

      } else {
            /* Remove old fault. */
            assert(cpssp->faulty_disk);
            cpssp->faulty_disk = 0;
      }
}

static void
ide_gen_disk_block_fault_set(
      void *_cpssp,
      unsigned long long loc0,
      unsigned long long loc1,
      unsigned int val
)
{
      struct cpssp *cpssp = (struct cpssp *) _cpssp;
      unsigned int i;

      if (val) {
            /* Add new fault. */
            for (i = 0; ; i++) {
                  if (i == sizeof(cpssp->faulty_blk) / sizeof(cpssp->faulty_blk[0])) {
                        /* Too many faults. */
                        fixme(); /* FIXME */
                        break;
                  }
                  if (cpssp->faulty_blk[i] == -1) {
                        cpssp->faulty_blk[i] = loc0;
                        break;
                  }
            }
      } else {
            /* Remove old fault. */
            for (i = 0; ; i++) {
                  if (i == sizeof(cpssp->faulty_blk) / sizeof(cpssp->faulty_blk[0])) {
                        /* Fault not found. */
                        fixme(); /* FIXME */
                        break;
                  }
                  if (cpssp->faulty_blk[i] == loc0) {
                        cpssp->faulty_blk[i] = -1;
                        break;
                  }
            }
      }
}

void
ide_gen_disk_init(
      unsigned int nr,
      struct sig_power_device *port_power,
      struct sig_ide_bus *port_ide,
      struct sig_fault *fault_disk_fault,
      struct sig_fault *fault_block_fault
)
{
      static const struct sig_boolean_funcs power_funcs = {
            .set = ide_gen_disk_power_set,
      };
      static const struct sig_ide_bus_device_funcs funcs = {
            .inw = ide_gen_disk_inw,
            .outw = ide_gen_disk_outw,
      };
      static const struct sig_fault_funcs disk_fault_funcs = {
            .set = ide_gen_disk_disk_fault_set,
      };
      static const struct sig_fault_funcs block_fault_funcs = {
            .set = ide_gen_disk_block_fault_set,
      };
      struct cpssp *cpssp;

      cpssp = shm_map(COMP, nr, sizeof(*cpssp), 0);

      cpssp->sig_5V = port_power->power_5V;
      sig_boolean_connect_in(port_power->power_5V, cpssp, &power_funcs);
      cpssp->sig_12V = port_power->power_12V;

      cpssp->cable = port_ide;
      sig_ide_bus_connect_device(port_ide, cpssp, &funcs);

      sig_fault_connect(fault_disk_fault, cpssp, &disk_fault_funcs);
      sig_fault_connect(fault_block_fault, cpssp, &block_fault_funcs);

      storage_init(&cpssp->media);
      storage_open(&cpssp->media, 1);

      sched_process_init(&cpssp->process, ide_gen_disk_process, cpssp);
}

void
ide_gen_disk_create(
      unsigned int nr,
      const char *name,
      const char *cow,
      const char *create,
      const char *sparse,
      const char *_sync,
      const char *image,
      const char *cylinders,
      const char *heads,
      const char *sectors,
      const char *unit,
      const char *size
)
{
      struct cpssp *cpssp;
      char path[1024];  /* 4 would be enough (.cow/.map) */
      unsigned int i;

      shm_create(COMP, nr, sizeof(*cpssp));
      cpssp = shm_map(COMP, nr, sizeof(*cpssp), 0);

      if (! cow) cow = "no";
      if (! create) create = "no";
      if (! sparse) sparse = "no";
      if (! _sync) _sync = "no";
      if (! image) image = "";
      if (! unit) unit = "0";

      if (cylinders && heads && sectors && ! size) {
            /* Set geometry via C/H/S setting. */
            cpssp->phys_secs = strtoul(sectors, NULL, 0);
            cpssp->phys_heads = strtoul(heads, NULL, 0);
            cpssp->phys_cyls = strtoul(cylinders, NULL, 0);

      } else if (! cylinders && ! heads && ! sectors && size) {
            /* Set geometry via capacity setting. */
            unsigned long blocks;

            blocks = strtoul(size, NULL, 0) * 1024 * 1024 / 512;
            cpssp->phys_secs = 63;
            blocks /= 63;
            cpssp->phys_heads = 16;
            blocks /= 16;
            cpssp->phys_cyls = (0x4000 <= blocks) ? 0x4000 : blocks;

      } else {
            fprintf(stderr, "%s: Bad C/H/S or size setting.\n", COMP);
            exit(1);
      }

      cpssp->media_cow = (*cow == 'Y' || *cow == 'y');
      cpssp->media_create = (*create == 'Y' || *create == 'y');
      cpssp->media_sparse = (*sparse == 'Y' || *sparse == 'y');
      cpssp->media_sync = (*_sync == 'Y' || *_sync == 'y');

      strcpy(cpssp->media_image, image);

      cpssp->unit = strtoul(unit, NULL, 0);
      cpssp->serial_number = nr;

      assert(strlen(COMP "-XX") + 1 < sizeof(cpssp->media_name));
      sprintf(cpssp->media_name, COMP "-%d", nr);

      assert(strlen(cpssp->media_name) + strlen(".media") < sizeof(path));
      sprintf(path, "%s.media", cpssp->media_name);
      storage_create(&cpssp->media,
                  path,
                  1,    /* writable */
                  cpssp->media_image,
                  ((unsigned long) cpssp->phys_cyls
                        * (unsigned long) cpssp->phys_heads
                        * (unsigned long) cpssp->phys_secs
                        * 512UL + 1024*1024-1) / (1024 * 1024),
                  512,  /* blocksize */
                  cpssp->media_create,
                  cpssp->media_cow,
                  cpssp->media_sync,
                  cpssp->media_sparse);

#if DISK_SMART_SUPPORT
      cpssp->smart_enabled = 1;
#endif

      cpssp->faulty_disk = 0;
      for (i = 0; i < sizeof(cpssp->faulty_blk) / sizeof(cpssp->faulty_blk[0]); i++) {
            cpssp->faulty_blk[i] = -1;
      }

      shm_unmap(cpssp, sizeof(*cpssp));
}

void
ide_gen_disk_destroy(unsigned int nr)
{
      struct cpssp *cpssp;

      cpssp = shm_map(COMP, nr, sizeof(*cpssp), 0);

      storage_destroy(&cpssp->media);

      shm_unmap(cpssp, sizeof(*cpssp));
      shm_destroy(COMP, nr);
}

Generated by  Doxygen 1.6.0   Back to index