Diff for /qemu/hw/fdc.c between versions 1.1.1.3 and 1.1.1.4

version 1.1.1.3, 2018/04/24 16:48:50 version 1.1.1.4, 2018/04/24 16:53:37
Line 2 Line 2
  * QEMU Floppy disk emulator (Intel 82078)   * QEMU Floppy disk emulator (Intel 82078)
  *   *
  * Copyright (c) 2003, 2007 Jocelyn Mayer   * Copyright (c) 2003, 2007 Jocelyn Mayer
    * Copyright (c) 2008 Hervé Poussineau
  *   *
  * Permission is hereby granted, free of charge, to any person obtaining a copy   * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal   * of this software and associated documentation files (the "Software"), to deal
Line 48  do { printf("FLOPPY ERROR: %s: " fmt, __ Line 49  do { printf("FLOPPY ERROR: %s: " fmt, __
 /********************************************************/  /********************************************************/
 /* Floppy drive emulation                               */  /* Floppy drive emulation                               */
   
   #define GET_CUR_DRV(fdctrl) ((fdctrl)->cur_drv)
   #define SET_CUR_DRV(fdctrl, drive) ((fdctrl)->cur_drv = (drive))
   
 /* Will always be a fixed parameter for us */  /* Will always be a fixed parameter for us */
 #define FD_SECTOR_LEN 512  #define FD_SECTOR_LEN          512
 #define FD_SECTOR_SC  2   /* Sector size code */  #define FD_SECTOR_SC           2   /* Sector size code */
   #define FD_RESET_SENSEI_COUNT  4   /* Number of sense interrupts on RESET */
   
 /* Floppy disk drive emulation */  /* Floppy disk drive emulation */
 typedef enum fdisk_type_t {  typedef enum fdisk_type_t {
Line 68  typedef enum fdrive_type_t { Line 73  typedef enum fdrive_type_t {
     FDRIVE_DRV_NONE = 0x03,   /* No drive connected     */      FDRIVE_DRV_NONE = 0x03,   /* No drive connected     */
 } fdrive_type_t;  } fdrive_type_t;
   
 typedef enum fdrive_flags_t {  
     FDRIVE_MOTOR_ON   = 0x01, /* motor on/off           */  
 } fdrive_flags_t;  
   
 typedef enum fdisk_flags_t {  typedef enum fdisk_flags_t {
     FDISK_DBL_SIDES  = 0x01,      FDISK_DBL_SIDES  = 0x01,
 } fdisk_flags_t;  } fdisk_flags_t;
Line 80  typedef struct fdrive_t { Line 81  typedef struct fdrive_t {
     BlockDriverState *bs;      BlockDriverState *bs;
     /* Drive status */      /* Drive status */
     fdrive_type_t drive;      fdrive_type_t drive;
     fdrive_flags_t drflags;  
     uint8_t perpendicular;    /* 2.88 MB access mode    */      uint8_t perpendicular;    /* 2.88 MB access mode    */
     /* Position */      /* Position */
     uint8_t head;      uint8_t head;
     uint8_t track;      uint8_t track;
     uint8_t sect;      uint8_t sect;
     /* Last operation status */  
     uint8_t dir;              /* Direction              */  
     uint8_t rw;               /* Read/write             */  
     /* Media */      /* Media */
     fdisk_flags_t flags;      fdisk_flags_t flags;
     uint8_t last_sect;        /* Nb sector per track    */      uint8_t last_sect;        /* Nb sector per track    */
Line 102  static void fd_init (fdrive_t *drv, Bloc Line 99  static void fd_init (fdrive_t *drv, Bloc
     /* Drive */      /* Drive */
     drv->bs = bs;      drv->bs = bs;
     drv->drive = FDRIVE_DRV_NONE;      drv->drive = FDRIVE_DRV_NONE;
     drv->drflags = 0;  
     drv->perpendicular = 0;      drv->perpendicular = 0;
     /* Disk */      /* Disk */
     drv->last_sect = 0;      drv->last_sect = 0;
Line 121  static int fd_sector (fdrive_t *drv) Line 117  static int fd_sector (fdrive_t *drv)
     return _fd_sector(drv->head, drv->track, drv->sect, drv->last_sect);      return _fd_sector(drv->head, drv->track, drv->sect, drv->last_sect);
 }  }
   
   /* Seek to a new position:
    * returns 0 if already on right track
    * returns 1 if track changed
    * returns 2 if track is invalid
    * returns 3 if sector is invalid
    * returns 4 if seek is disabled
    */
 static int fd_seek (fdrive_t *drv, uint8_t head, uint8_t track, uint8_t sect,  static int fd_seek (fdrive_t *drv, uint8_t head, uint8_t track, uint8_t sect,
                     int enable_seek)                      int enable_seek)
 {  {
Line 169  static void fd_recalibrate (fdrive_t *dr Line 172  static void fd_recalibrate (fdrive_t *dr
     drv->head = 0;      drv->head = 0;
     drv->track = 0;      drv->track = 0;
     drv->sect = 1;      drv->sect = 1;
     drv->dir = 1;  
     drv->rw = 0;  
 }  }
   
 /* Recognize floppy formats */  /* Recognize floppy formats */
Line 295  static void fd_revalidate (fdrive_t *drv Line 296  static void fd_revalidate (fdrive_t *drv
     }      }
 }  }
   
 /* Motor control */  
 static void fd_start (fdrive_t *drv)  
 {  
     drv->drflags |= FDRIVE_MOTOR_ON;  
 }  
   
 static void fd_stop (fdrive_t *drv)  
 {  
     drv->drflags &= ~FDRIVE_MOTOR_ON;  
 }  
   
 /* Re-initialise a drives (motor off, repositioned) */  
 static void fd_reset (fdrive_t *drv)  
 {  
     fd_stop(drv);  
     fd_recalibrate(drv);  
 }  
   
 /********************************************************/  /********************************************************/
 /* Intel 82078 floppy disk controller emulation          */  /* Intel 82078 floppy disk controller emulation          */
   
Line 320  static void fdctrl_reset (fdctrl_t *fdct Line 303  static void fdctrl_reset (fdctrl_t *fdct
 static void fdctrl_reset_fifo (fdctrl_t *fdctrl);  static void fdctrl_reset_fifo (fdctrl_t *fdctrl);
 static int fdctrl_transfer_handler (void *opaque, int nchan,  static int fdctrl_transfer_handler (void *opaque, int nchan,
                                     int dma_pos, int dma_len);                                      int dma_pos, int dma_len);
 static void fdctrl_raise_irq (fdctrl_t *fdctrl, uint8_t status);  static void fdctrl_raise_irq (fdctrl_t *fdctrl, uint8_t status0);
 static void fdctrl_result_timer(void *opaque);  
   
   static uint32_t fdctrl_read_statusA (fdctrl_t *fdctrl);
 static uint32_t fdctrl_read_statusB (fdctrl_t *fdctrl);  static uint32_t fdctrl_read_statusB (fdctrl_t *fdctrl);
 static uint32_t fdctrl_read_dor (fdctrl_t *fdctrl);  static uint32_t fdctrl_read_dor (fdctrl_t *fdctrl);
 static void fdctrl_write_dor (fdctrl_t *fdctrl, uint32_t value);  static void fdctrl_write_dor (fdctrl_t *fdctrl, uint32_t value);
Line 335  static void fdctrl_write_data (fdctrl_t  Line 318  static void fdctrl_write_data (fdctrl_t 
 static uint32_t fdctrl_read_dir (fdctrl_t *fdctrl);  static uint32_t fdctrl_read_dir (fdctrl_t *fdctrl);
   
 enum {  enum {
     FD_CTRL_ACTIVE = 0x01, /* XXX: suppress that */  
     FD_CTRL_RESET  = 0x02,  
     FD_CTRL_SLEEP  = 0x04, /* XXX: suppress that */  
     FD_CTRL_BUSY   = 0x08, /* dma transfer in progress */  
     FD_CTRL_INTR   = 0x10,  
 };  
   
 enum {  
     FD_DIR_WRITE   = 0,      FD_DIR_WRITE   = 0,
     FD_DIR_READ    = 1,      FD_DIR_READ    = 1,
     FD_DIR_SCANE   = 2,      FD_DIR_SCANE   = 2,
Line 351  enum { Line 326  enum {
 };  };
   
 enum {  enum {
     FD_STATE_CMD    = 0x00,      FD_STATE_MULTI  = 0x01,     /* multi track flag */
     FD_STATE_STATUS = 0x01,      FD_STATE_FORMAT = 0x02,     /* format flag */
     FD_STATE_DATA   = 0x02,      FD_STATE_SEEK   = 0x04,     /* seek flag */
     FD_STATE_STATE  = 0x03,  };
     FD_STATE_MULTI  = 0x10,  
     FD_STATE_SEEK   = 0x20,  enum {
     FD_STATE_FORMAT = 0x40,      FD_REG_SRA = 0x00,
       FD_REG_SRB = 0x01,
       FD_REG_DOR = 0x02,
       FD_REG_TDR = 0x03,
       FD_REG_MSR = 0x04,
       FD_REG_DSR = 0x04,
       FD_REG_FIFO = 0x05,
       FD_REG_DIR = 0x07,
   };
   
   enum {
       FD_CMD_READ_TRACK = 0x02,
       FD_CMD_SPECIFY = 0x03,
       FD_CMD_SENSE_DRIVE_STATUS = 0x04,
       FD_CMD_WRITE = 0x05,
       FD_CMD_READ = 0x06,
       FD_CMD_RECALIBRATE = 0x07,
       FD_CMD_SENSE_INTERRUPT_STATUS = 0x08,
       FD_CMD_WRITE_DELETED = 0x09,
       FD_CMD_READ_ID = 0x0a,
       FD_CMD_READ_DELETED = 0x0c,
       FD_CMD_FORMAT_TRACK = 0x0d,
       FD_CMD_DUMPREG = 0x0e,
       FD_CMD_SEEK = 0x0f,
       FD_CMD_VERSION = 0x10,
       FD_CMD_SCAN_EQUAL = 0x11,
       FD_CMD_PERPENDICULAR_MODE = 0x12,
       FD_CMD_CONFIGURE = 0x13,
       FD_CMD_LOCK = 0x14,
       FD_CMD_VERIFY = 0x16,
       FD_CMD_POWERDOWN_MODE = 0x17,
       FD_CMD_PART_ID = 0x18,
       FD_CMD_SCAN_LOW_OR_EQUAL = 0x19,
       FD_CMD_SCAN_HIGH_OR_EQUAL = 0x1d,
       FD_CMD_SAVE = 0x2c,
       FD_CMD_OPTION = 0x33,
       FD_CMD_RESTORE = 0x4c,
       FD_CMD_DRIVE_SPECIFICATION_COMMAND = 0x8e,
       FD_CMD_RELATIVE_SEEK_OUT = 0x8f,
       FD_CMD_FORMAT_AND_WRITE = 0xcd,
       FD_CMD_RELATIVE_SEEK_IN = 0xcf,
   };
   
   enum {
       FD_CONFIG_PRETRK = 0xff, /* Pre-compensation set to track 0 */
       FD_CONFIG_FIFOTHR = 0x0f, /* FIFO threshold set to 1 byte */
       FD_CONFIG_POLL  = 0x10, /* Poll enabled */
       FD_CONFIG_EFIFO = 0x20, /* FIFO disabled */
       FD_CONFIG_EIS   = 0x40, /* No implied seeks */
   };
   
   enum {
       FD_SR0_EQPMT    = 0x10,
       FD_SR0_SEEK     = 0x20,
       FD_SR0_ABNTERM  = 0x40,
       FD_SR0_INVCMD   = 0x80,
       FD_SR0_RDYCHG   = 0xc0,
   };
   
   enum {
       FD_SR1_EC       = 0x80, /* End of cylinder */
   };
   
   enum {
       FD_SR2_SNS      = 0x04, /* Scan not satisfied */
       FD_SR2_SEH      = 0x08, /* Scan equal hit */
   };
   
   enum {
       FD_SRA_DIR      = 0x01,
       FD_SRA_nWP      = 0x02,
       FD_SRA_nINDX    = 0x04,
       FD_SRA_HDSEL    = 0x08,
       FD_SRA_nTRK0    = 0x10,
       FD_SRA_STEP     = 0x20,
       FD_SRA_nDRV2    = 0x40,
       FD_SRA_INTPEND  = 0x80,
   };
   
   enum {
       FD_SRB_MTR0     = 0x01,
       FD_SRB_MTR1     = 0x02,
       FD_SRB_WGATE    = 0x04,
       FD_SRB_RDATA    = 0x08,
       FD_SRB_WDATA    = 0x10,
       FD_SRB_DR0      = 0x20,
   };
   
   enum {
   #if MAX_FD == 4
       FD_DOR_SELMASK  = 0x03,
   #else
       FD_DOR_SELMASK  = 0x01,
   #endif
       FD_DOR_nRESET   = 0x04,
       FD_DOR_DMAEN    = 0x08,
       FD_DOR_MOTEN0   = 0x10,
       FD_DOR_MOTEN1   = 0x20,
       FD_DOR_MOTEN2   = 0x40,
       FD_DOR_MOTEN3   = 0x80,
   };
   
   enum {
   #if MAX_FD == 4
       FD_TDR_BOOTSEL  = 0x0c,
   #else
       FD_TDR_BOOTSEL  = 0x04,
   #endif
   };
   
   enum {
       FD_DSR_DRATEMASK= 0x03,
       FD_DSR_PWRDOWN  = 0x40,
       FD_DSR_SWRESET  = 0x80,
   };
   
   enum {
       FD_MSR_DRV0BUSY = 0x01,
       FD_MSR_DRV1BUSY = 0x02,
       FD_MSR_DRV2BUSY = 0x04,
       FD_MSR_DRV3BUSY = 0x08,
       FD_MSR_CMDBUSY  = 0x10,
       FD_MSR_NONDMA   = 0x20,
       FD_MSR_DIO      = 0x40,
       FD_MSR_RQM      = 0x80,
   };
   
   enum {
       FD_DIR_DSKCHG   = 0x80,
 };  };
   
 #define FD_STATE(state) ((state) & FD_STATE_STATE)  
 #define FD_SET_STATE(state, new_state) \  
 do { (state) = ((state) & ~FD_STATE_STATE) | (new_state); } while (0)  
 #define FD_MULTI_TRACK(state) ((state) & FD_STATE_MULTI)  #define FD_MULTI_TRACK(state) ((state) & FD_STATE_MULTI)
 #define FD_DID_SEEK(state) ((state) & FD_STATE_SEEK)  #define FD_DID_SEEK(state) ((state) & FD_STATE_SEEK)
 #define FD_FORMAT_CMD(state) ((state) & FD_STATE_FORMAT)  #define FD_FORMAT_CMD(state) ((state) & FD_STATE_FORMAT)
   
 struct fdctrl_t {  struct fdctrl_t {
     fdctrl_t *fdctrl;  
     /* Controller's identification */      /* Controller's identification */
     uint8_t version;      uint8_t version;
     /* HW */      /* HW */
Line 377  struct fdctrl_t { Line 476  struct fdctrl_t {
     target_phys_addr_t io_base;      target_phys_addr_t io_base;
     /* Controller state */      /* Controller state */
     QEMUTimer *result_timer;      QEMUTimer *result_timer;
     uint8_t state;      uint8_t sra;
     uint8_t dma_en;      uint8_t srb;
       uint8_t dor;
       uint8_t tdr;
       uint8_t dsr;
       uint8_t msr;
     uint8_t cur_drv;      uint8_t cur_drv;
     uint8_t bootsel;      uint8_t status0;
       uint8_t status1;
       uint8_t status2;
     /* Command FIFO */      /* Command FIFO */
     uint8_t *fifo;      uint8_t *fifo;
     uint32_t data_pos;      uint32_t data_pos;
     uint32_t data_len;      uint32_t data_len;
     uint8_t data_state;      uint8_t data_state;
     uint8_t data_dir;      uint8_t data_dir;
     uint8_t int_status;  
     uint8_t eot; /* last wanted sector */      uint8_t eot; /* last wanted sector */
     /* States kept only to be returned back */      /* States kept only to be returned back */
     /* Timers state */      /* Timers state */
Line 402  struct fdctrl_t { Line 506  struct fdctrl_t {
     /* Sun4m quirks? */      /* Sun4m quirks? */
     int sun4m;      int sun4m;
     /* Floppy drives */      /* Floppy drives */
     fdrive_t drives[2];      fdrive_t drives[MAX_FD];
       int reset_sensei;
 };  };
   
 static uint32_t fdctrl_read (void *opaque, uint32_t reg)  static uint32_t fdctrl_read (void *opaque, uint32_t reg)
Line 410  static uint32_t fdctrl_read (void *opaqu Line 515  static uint32_t fdctrl_read (void *opaqu
     fdctrl_t *fdctrl = opaque;      fdctrl_t *fdctrl = opaque;
     uint32_t retval;      uint32_t retval;
   
     switch (reg & 0x07) {      switch (reg) {
     case 0x00:      case FD_REG_SRA:
         if (fdctrl->sun4m) {          retval = fdctrl_read_statusA(fdctrl);
             // Identify to Linux as S82078B  
             retval = fdctrl_read_statusB(fdctrl);  
         } else {  
             retval = (uint32_t)(-1);  
         }  
         break;          break;
     case 0x01:      case FD_REG_SRB:
         retval = fdctrl_read_statusB(fdctrl);          retval = fdctrl_read_statusB(fdctrl);
         break;          break;
     case 0x02:      case FD_REG_DOR:
         retval = fdctrl_read_dor(fdctrl);          retval = fdctrl_read_dor(fdctrl);
         break;          break;
     case 0x03:      case FD_REG_TDR:
         retval = fdctrl_read_tape(fdctrl);          retval = fdctrl_read_tape(fdctrl);
         break;          break;
     case 0x04:      case FD_REG_MSR:
         retval = fdctrl_read_main_status(fdctrl);          retval = fdctrl_read_main_status(fdctrl);
         break;          break;
     case 0x05:      case FD_REG_FIFO:
         retval = fdctrl_read_data(fdctrl);          retval = fdctrl_read_data(fdctrl);
         break;          break;
     case 0x07:      case FD_REG_DIR:
         retval = fdctrl_read_dir(fdctrl);          retval = fdctrl_read_dir(fdctrl);
         break;          break;
     default:      default:
Line 452  static void fdctrl_write (void *opaque,  Line 552  static void fdctrl_write (void *opaque, 
   
     FLOPPY_DPRINTF("write reg%d: 0x%02x\n", reg & 7, value);      FLOPPY_DPRINTF("write reg%d: 0x%02x\n", reg & 7, value);
   
     switch (reg & 0x07) {      switch (reg) {
     case 0x02:      case FD_REG_DOR:
         fdctrl_write_dor(fdctrl, value);          fdctrl_write_dor(fdctrl, value);
         break;          break;
     case 0x03:      case FD_REG_TDR:
         fdctrl_write_tape(fdctrl, value);          fdctrl_write_tape(fdctrl, value);
         break;          break;
     case 0x04:      case FD_REG_DSR:
         fdctrl_write_rate(fdctrl, value);          fdctrl_write_rate(fdctrl, value);
         break;          break;
     case 0x05:      case FD_REG_FIFO:
         fdctrl_write_data(fdctrl, value);          fdctrl_write_data(fdctrl, value);
         break;          break;
     default:      default:
Line 470  static void fdctrl_write (void *opaque,  Line 570  static void fdctrl_write (void *opaque, 
     }      }
 }  }
   
   static uint32_t fdctrl_read_port (void *opaque, uint32_t reg)
   {
       return fdctrl_read(opaque, reg & 7);
   }
   
   static void fdctrl_write_port (void *opaque, uint32_t reg, uint32_t value)
   {
       fdctrl_write(opaque, reg & 7, value);
   }
   
 static uint32_t fdctrl_read_mem (void *opaque, target_phys_addr_t reg)  static uint32_t fdctrl_read_mem (void *opaque, target_phys_addr_t reg)
 {  {
     return fdctrl_read(opaque, (uint32_t)reg);      return fdctrl_read(opaque, (uint32_t)reg);
Line 507  static CPUWriteMemoryFunc *fdctrl_mem_wr Line 617  static CPUWriteMemoryFunc *fdctrl_mem_wr
   
 static void fd_save (QEMUFile *f, fdrive_t *fd)  static void fd_save (QEMUFile *f, fdrive_t *fd)
 {  {
     uint8_t tmp;  
   
     tmp = fd->drflags;  
     qemu_put_8s(f, &tmp);  
     qemu_put_8s(f, &fd->head);      qemu_put_8s(f, &fd->head);
     qemu_put_8s(f, &fd->track);      qemu_put_8s(f, &fd->track);
     qemu_put_8s(f, &fd->sect);      qemu_put_8s(f, &fd->sect);
     qemu_put_8s(f, &fd->dir);  
     qemu_put_8s(f, &fd->rw);  
 }  }
   
 static void fdc_save (QEMUFile *f, void *opaque)  static void fdc_save (QEMUFile *f, void *opaque)
 {  {
     fdctrl_t *s = opaque;      fdctrl_t *s = opaque;
       uint8_t tmp;
       int i;
       uint8_t dor = s->dor | GET_CUR_DRV(s);
   
     qemu_put_8s(f, &s->state);      /* Controller state */
     qemu_put_8s(f, &s->dma_en);      qemu_put_8s(f, &s->sra);
     qemu_put_8s(f, &s->cur_drv);      qemu_put_8s(f, &s->srb);
     qemu_put_8s(f, &s->bootsel);      qemu_put_8s(f, &dor);
       qemu_put_8s(f, &s->tdr);
       qemu_put_8s(f, &s->dsr);
       qemu_put_8s(f, &s->msr);
       qemu_put_8s(f, &s->status0);
       qemu_put_8s(f, &s->status1);
       qemu_put_8s(f, &s->status2);
       /* Command FIFO */
     qemu_put_buffer(f, s->fifo, FD_SECTOR_LEN);      qemu_put_buffer(f, s->fifo, FD_SECTOR_LEN);
     qemu_put_be32s(f, &s->data_pos);      qemu_put_be32s(f, &s->data_pos);
     qemu_put_be32s(f, &s->data_len);      qemu_put_be32s(f, &s->data_len);
     qemu_put_8s(f, &s->data_state);      qemu_put_8s(f, &s->data_state);
     qemu_put_8s(f, &s->data_dir);      qemu_put_8s(f, &s->data_dir);
     qemu_put_8s(f, &s->int_status);  
     qemu_put_8s(f, &s->eot);      qemu_put_8s(f, &s->eot);
       /* States kept only to be returned back */
     qemu_put_8s(f, &s->timer0);      qemu_put_8s(f, &s->timer0);
     qemu_put_8s(f, &s->timer1);      qemu_put_8s(f, &s->timer1);
     qemu_put_8s(f, &s->precomp_trk);      qemu_put_8s(f, &s->precomp_trk);
     qemu_put_8s(f, &s->config);      qemu_put_8s(f, &s->config);
     qemu_put_8s(f, &s->lock);      qemu_put_8s(f, &s->lock);
     qemu_put_8s(f, &s->pwrd);      qemu_put_8s(f, &s->pwrd);
     fd_save(f, &s->drives[0]);  
     fd_save(f, &s->drives[1]);      tmp = MAX_FD;
       qemu_put_8s(f, &tmp);
       for (i = 0; i < MAX_FD; i++)
           fd_save(f, &s->drives[i]);
 }  }
   
 static int fd_load (QEMUFile *f, fdrive_t *fd)  static int fd_load (QEMUFile *f, fdrive_t *fd)
 {  {
     uint8_t tmp;  
   
     qemu_get_8s(f, &tmp);  
     fd->drflags = tmp;  
     qemu_get_8s(f, &fd->head);      qemu_get_8s(f, &fd->head);
     qemu_get_8s(f, &fd->track);      qemu_get_8s(f, &fd->track);
     qemu_get_8s(f, &fd->sect);      qemu_get_8s(f, &fd->sect);
     qemu_get_8s(f, &fd->dir);  
     qemu_get_8s(f, &fd->rw);  
   
     return 0;      return 0;
 }  }
Line 561  static int fd_load (QEMUFile *f, fdrive_ Line 672  static int fd_load (QEMUFile *f, fdrive_
 static int fdc_load (QEMUFile *f, void *opaque, int version_id)  static int fdc_load (QEMUFile *f, void *opaque, int version_id)
 {  {
     fdctrl_t *s = opaque;      fdctrl_t *s = opaque;
     int ret;      int i, ret = 0;
       uint8_t n;
   
     if (version_id != 1)      if (version_id != 2)
         return -EINVAL;          return -EINVAL;
   
     qemu_get_8s(f, &s->state);      /* Controller state */
     qemu_get_8s(f, &s->dma_en);      qemu_get_8s(f, &s->sra);
     qemu_get_8s(f, &s->cur_drv);      qemu_get_8s(f, &s->srb);
     qemu_get_8s(f, &s->bootsel);      qemu_get_8s(f, &s->dor);
       SET_CUR_DRV(s, s->dor & FD_DOR_SELMASK);
       s->dor &= ~FD_DOR_SELMASK;
       qemu_get_8s(f, &s->tdr);
       qemu_get_8s(f, &s->dsr);
       qemu_get_8s(f, &s->msr);
       qemu_get_8s(f, &s->status0);
       qemu_get_8s(f, &s->status1);
       qemu_get_8s(f, &s->status2);
       /* Command FIFO */
     qemu_get_buffer(f, s->fifo, FD_SECTOR_LEN);      qemu_get_buffer(f, s->fifo, FD_SECTOR_LEN);
     qemu_get_be32s(f, &s->data_pos);      qemu_get_be32s(f, &s->data_pos);
     qemu_get_be32s(f, &s->data_len);      qemu_get_be32s(f, &s->data_len);
     qemu_get_8s(f, &s->data_state);      qemu_get_8s(f, &s->data_state);
     qemu_get_8s(f, &s->data_dir);      qemu_get_8s(f, &s->data_dir);
     qemu_get_8s(f, &s->int_status);  
     qemu_get_8s(f, &s->eot);      qemu_get_8s(f, &s->eot);
       /* States kept only to be returned back */
     qemu_get_8s(f, &s->timer0);      qemu_get_8s(f, &s->timer0);
     qemu_get_8s(f, &s->timer1);      qemu_get_8s(f, &s->timer1);
     qemu_get_8s(f, &s->precomp_trk);      qemu_get_8s(f, &s->precomp_trk);
     qemu_get_8s(f, &s->config);      qemu_get_8s(f, &s->config);
     qemu_get_8s(f, &s->lock);      qemu_get_8s(f, &s->lock);
     qemu_get_8s(f, &s->pwrd);      qemu_get_8s(f, &s->pwrd);
       qemu_get_8s(f, &n);
   
     ret = fd_load(f, &s->drives[0]);      if (n > MAX_FD)
     if (ret == 0)          return -EINVAL;
         ret = fd_load(f, &s->drives[1]);  
       for (i = 0; i < n; i++) {
           ret = fd_load(f, &s->drives[i]);
           if (ret != 0)
               break;
       }
   
     return ret;      return ret;
 }  }
Line 598  static void fdctrl_external_reset(void * Line 725  static void fdctrl_external_reset(void *
     fdctrl_reset(s, 0);      fdctrl_reset(s, 0);
 }  }
   
 static fdctrl_t *fdctrl_init_common (qemu_irq irq, int dma_chann,  static void fdctrl_handle_tc(void *opaque, int irq, int level)
                                      target_phys_addr_t io_base,  
                                      BlockDriverState **fds)  
 {  
     fdctrl_t *fdctrl;  
     int i;  
   
     FLOPPY_DPRINTF("init controller\n");  
     fdctrl = qemu_mallocz(sizeof(fdctrl_t));  
     if (!fdctrl)  
         return NULL;  
     fdctrl->fifo = qemu_memalign(512, FD_SECTOR_LEN);  
     if (fdctrl->fifo == NULL) {  
         qemu_free(fdctrl);  
         return NULL;  
     }  
     fdctrl->result_timer = qemu_new_timer(vm_clock,  
                                           fdctrl_result_timer, fdctrl);  
   
     fdctrl->version = 0x90; /* Intel 82078 controller */  
     fdctrl->irq = irq;  
     fdctrl->dma_chann = dma_chann;  
     fdctrl->io_base = io_base;  
     fdctrl->config = 0x60; /* Implicit seek, polling & FIFO enabled */  
     if (fdctrl->dma_chann != -1) {  
         fdctrl->dma_en = 1;  
         DMA_register_channel(dma_chann, &fdctrl_transfer_handler, fdctrl);  
     } else {  
         fdctrl->dma_en = 0;  
     }  
     for (i = 0; i < 2; i++) {  
         fd_init(&fdctrl->drives[i], fds[i]);  
     }  
     fdctrl_reset(fdctrl, 0);  
     fdctrl->state = FD_CTRL_ACTIVE;  
     register_savevm("fdc", io_base, 1, fdc_save, fdc_load, fdctrl);  
     qemu_register_reset(fdctrl_external_reset, fdctrl);  
     for (i = 0; i < 2; i++) {  
         fd_revalidate(&fdctrl->drives[i]);  
     }  
   
     return fdctrl;  
 }  
   
 fdctrl_t *fdctrl_init (qemu_irq irq, int dma_chann, int mem_mapped,  
                        target_phys_addr_t io_base,  
                        BlockDriverState **fds)  
 {  {
     fdctrl_t *fdctrl;      //fdctrl_t *s = opaque;
     int io_mem;  
   
     fdctrl = fdctrl_init_common(irq, dma_chann, io_base, fds);      if (level) {
           // XXX
     fdctrl->sun4m = 0;          FLOPPY_DPRINTF("TC pulsed\n");
     if (mem_mapped) {  
         io_mem = cpu_register_io_memory(0, fdctrl_mem_read, fdctrl_mem_write,  
                                         fdctrl);  
         cpu_register_physical_memory(io_base, 0x08, io_mem);  
     } else {  
         register_ioport_read((uint32_t)io_base + 0x01, 5, 1, &fdctrl_read,  
                              fdctrl);  
         register_ioport_read((uint32_t)io_base + 0x07, 1, 1, &fdctrl_read,  
                              fdctrl);  
         register_ioport_write((uint32_t)io_base + 0x01, 5, 1, &fdctrl_write,  
                               fdctrl);  
         register_ioport_write((uint32_t)io_base + 0x07, 1, 1, &fdctrl_write,  
                               fdctrl);  
     }      }
   
     return fdctrl;  
 }  
   
 fdctrl_t *sun4m_fdctrl_init (qemu_irq irq, target_phys_addr_t io_base,  
                              BlockDriverState **fds)  
 {  
     fdctrl_t *fdctrl;  
     int io_mem;  
   
     fdctrl = fdctrl_init_common(irq, 0, io_base, fds);  
     fdctrl->sun4m = 1;  
     io_mem = cpu_register_io_memory(0, fdctrl_mem_read_strict,  
                                     fdctrl_mem_write_strict,  
                                     fdctrl);  
     cpu_register_physical_memory(io_base, 0x08, io_mem);  
   
     return fdctrl;  
 }  }
   
 /* XXX: may change if moved to bdrv */  /* XXX: may change if moved to bdrv */
Line 695  int fdctrl_get_drive_type(fdctrl_t *fdct Line 744  int fdctrl_get_drive_type(fdctrl_t *fdct
 /* Change IRQ state */  /* Change IRQ state */
 static void fdctrl_reset_irq (fdctrl_t *fdctrl)  static void fdctrl_reset_irq (fdctrl_t *fdctrl)
 {  {
       if (!(fdctrl->sra & FD_SRA_INTPEND))
           return;
     FLOPPY_DPRINTF("Reset interrupt\n");      FLOPPY_DPRINTF("Reset interrupt\n");
     qemu_set_irq(fdctrl->irq, 0);      qemu_set_irq(fdctrl->irq, 0);
     fdctrl->state &= ~FD_CTRL_INTR;      fdctrl->sra &= ~FD_SRA_INTPEND;
 }  }
   
 static void fdctrl_raise_irq (fdctrl_t *fdctrl, uint8_t status)  static void fdctrl_raise_irq (fdctrl_t *fdctrl, uint8_t status0)
 {  {
     // Sparc mutation      /* Sparc mutation */
     if (fdctrl->sun4m && !fdctrl->dma_en) {      if (fdctrl->sun4m && (fdctrl->msr & FD_MSR_CMDBUSY)) {
         fdctrl->state &= ~FD_CTRL_BUSY;          /* XXX: not sure */
         fdctrl->int_status = status;          fdctrl->msr &= ~FD_MSR_CMDBUSY;
           fdctrl->msr |= FD_MSR_RQM | FD_MSR_DIO;
           fdctrl->status0 = status0;
         return;          return;
     }      }
     if (~(fdctrl->state & FD_CTRL_INTR)) {      if (!(fdctrl->sra & FD_SRA_INTPEND)) {
         qemu_set_irq(fdctrl->irq, 1);          qemu_set_irq(fdctrl->irq, 1);
         fdctrl->state |= FD_CTRL_INTR;          fdctrl->sra |= FD_SRA_INTPEND;
     }      }
     FLOPPY_DPRINTF("Set interrupt status to 0x%02x\n", status);      fdctrl->reset_sensei = 0;
     fdctrl->int_status = status;      fdctrl->status0 = status0;
       FLOPPY_DPRINTF("Set interrupt status to 0x%02x\n", fdctrl->status0);
 }  }
   
 /* Reset controller */  /* Reset controller */
Line 724  static void fdctrl_reset (fdctrl_t *fdct Line 778  static void fdctrl_reset (fdctrl_t *fdct
     FLOPPY_DPRINTF("reset controller\n");      FLOPPY_DPRINTF("reset controller\n");
     fdctrl_reset_irq(fdctrl);      fdctrl_reset_irq(fdctrl);
     /* Initialise controller */      /* Initialise controller */
       fdctrl->sra = 0;
       fdctrl->srb = 0xc0;
       if (!fdctrl->drives[1].bs)
           fdctrl->sra |= FD_SRA_nDRV2;
     fdctrl->cur_drv = 0;      fdctrl->cur_drv = 0;
       fdctrl->dor = FD_DOR_nRESET;
       fdctrl->dor |= (fdctrl->dma_chann != -1) ? FD_DOR_DMAEN : 0;
       fdctrl->msr = FD_MSR_RQM;
     /* FIFO state */      /* FIFO state */
     fdctrl->data_pos = 0;      fdctrl->data_pos = 0;
     fdctrl->data_len = 0;      fdctrl->data_len = 0;
     fdctrl->data_state = FD_STATE_CMD;      fdctrl->data_state = 0;
     fdctrl->data_dir = FD_DIR_WRITE;      fdctrl->data_dir = FD_DIR_WRITE;
     for (i = 0; i < MAX_FD; i++)      for (i = 0; i < MAX_FD; i++)
         fd_reset(&fdctrl->drives[i]);          fd_recalibrate(&fdctrl->drives[i]);
     fdctrl_reset_fifo(fdctrl);      fdctrl_reset_fifo(fdctrl);
     if (do_irq)      if (do_irq) {
         fdctrl_raise_irq(fdctrl, 0xc0);          fdctrl_raise_irq(fdctrl, FD_SR0_RDYCHG);
           fdctrl->reset_sensei = FD_RESET_SENSEI_COUNT;
       }
 }  }
   
 static inline fdrive_t *drv0 (fdctrl_t *fdctrl)  static inline fdrive_t *drv0 (fdctrl_t *fdctrl)
 {  {
     return &fdctrl->drives[fdctrl->bootsel];      return &fdctrl->drives[(fdctrl->tdr & FD_TDR_BOOTSEL) >> 2];
 }  }
   
 static inline fdrive_t *drv1 (fdctrl_t *fdctrl)  static inline fdrive_t *drv1 (fdctrl_t *fdctrl)
 {  {
     return &fdctrl->drives[1 - fdctrl->bootsel];      if ((fdctrl->tdr & FD_TDR_BOOTSEL) < (1 << 2))
           return &fdctrl->drives[1];
       else
           return &fdctrl->drives[0];
   }
   
   #if MAX_FD == 4
   static inline fdrive_t *drv2 (fdctrl_t *fdctrl)
   {
       if ((fdctrl->tdr & FD_TDR_BOOTSEL) < (2 << 2))
           return &fdctrl->drives[2];
       else
           return &fdctrl->drives[1];
   }
   
   static inline fdrive_t *drv3 (fdctrl_t *fdctrl)
   {
       if ((fdctrl->tdr & FD_TDR_BOOTSEL) < (3 << 2))
           return &fdctrl->drives[3];
       else
           return &fdctrl->drives[2];
 }  }
   #endif
   
 static fdrive_t *get_cur_drv (fdctrl_t *fdctrl)  static fdrive_t *get_cur_drv (fdctrl_t *fdctrl)
 {  {
     return fdctrl->cur_drv == 0 ? drv0(fdctrl) : drv1(fdctrl);      switch (fdctrl->cur_drv) {
           case 0: return drv0(fdctrl);
           case 1: return drv1(fdctrl);
   #if MAX_FD == 4
           case 2: return drv2(fdctrl);
           case 3: return drv3(fdctrl);
   #endif
           default: return NULL;
       }
   }
   
   /* Status A register : 0x00 (read-only) */
   static uint32_t fdctrl_read_statusA (fdctrl_t *fdctrl)
   {
       uint32_t retval = fdctrl->sra;
   
       FLOPPY_DPRINTF("status register A: 0x%02x\n", retval);
   
       return retval;
 }  }
   
 /* Status B register : 0x01 (read-only) */  /* Status B register : 0x01 (read-only) */
 static uint32_t fdctrl_read_statusB (fdctrl_t *fdctrl)  static uint32_t fdctrl_read_statusB (fdctrl_t *fdctrl)
 {  {
     FLOPPY_DPRINTF("status register: 0x00\n");      uint32_t retval = fdctrl->srb;
     return 0;  
       FLOPPY_DPRINTF("status register B: 0x%02x\n", retval);
   
       return retval;
 }  }
   
 /* Digital output register : 0x02 */  /* Digital output register : 0x02 */
 static uint32_t fdctrl_read_dor (fdctrl_t *fdctrl)  static uint32_t fdctrl_read_dor (fdctrl_t *fdctrl)
 {  {
     uint32_t retval = 0;      uint32_t retval = fdctrl->dor;
   
     /* Drive motors state indicators */  
     if (drv0(fdctrl)->drflags & FDRIVE_MOTOR_ON)  
         retval |= 1 << 5;  
     if (drv1(fdctrl)->drflags & FDRIVE_MOTOR_ON)  
         retval |= 1 << 4;  
     /* DMA enable */  
     retval |= fdctrl->dma_en << 3;  
     /* Reset indicator */  
     retval |= (fdctrl->state & FD_CTRL_RESET) == 0 ? 0x04 : 0;  
     /* Selected drive */      /* Selected drive */
     retval |= fdctrl->cur_drv;      retval |= fdctrl->cur_drv;
     FLOPPY_DPRINTF("digital output register: 0x%02x\n", retval);      FLOPPY_DPRINTF("digital output register: 0x%02x\n", retval);
Line 782  static uint32_t fdctrl_read_dor (fdctrl_ Line 878  static uint32_t fdctrl_read_dor (fdctrl_
   
 static void fdctrl_write_dor (fdctrl_t *fdctrl, uint32_t value)  static void fdctrl_write_dor (fdctrl_t *fdctrl, uint32_t value)
 {  {
     /* Reset mode */  
     if (fdctrl->state & FD_CTRL_RESET) {  
         if (!(value & 0x04)) {  
             FLOPPY_DPRINTF("Floppy controller in RESET state !\n");  
             return;  
         }  
     }  
     FLOPPY_DPRINTF("digital output register set to 0x%02x\n", value);      FLOPPY_DPRINTF("digital output register set to 0x%02x\n", value);
     /* Drive motors state indicators */  
     if (value & 0x20)      /* Motors */
         fd_start(drv1(fdctrl));      if (value & FD_DOR_MOTEN0)
           fdctrl->srb |= FD_SRB_MTR0;
     else      else
         fd_stop(drv1(fdctrl));          fdctrl->srb &= ~FD_SRB_MTR0;
     if (value & 0x10)      if (value & FD_DOR_MOTEN1)
         fd_start(drv0(fdctrl));          fdctrl->srb |= FD_SRB_MTR1;
     else      else
         fd_stop(drv0(fdctrl));          fdctrl->srb &= ~FD_SRB_MTR1;
     /* DMA enable */  
 #if 0      /* Drive */
     if (fdctrl->dma_chann != -1)      if (value & 1)
         fdctrl->dma_en = 1 - ((value >> 3) & 1);          fdctrl->srb |= FD_SRB_DR0;
 #endif      else
           fdctrl->srb &= ~FD_SRB_DR0;
   
     /* Reset */      /* Reset */
     if (!(value & 0x04)) {      if (!(value & FD_DOR_nRESET)) {
         if (!(fdctrl->state & FD_CTRL_RESET)) {          if (fdctrl->dor & FD_DOR_nRESET) {
             FLOPPY_DPRINTF("controller enter RESET state\n");              FLOPPY_DPRINTF("controller enter RESET state\n");
             fdctrl->state |= FD_CTRL_RESET;  
         }          }
     } else {      } else {
         if (fdctrl->state & FD_CTRL_RESET) {          if (!(fdctrl->dor & FD_DOR_nRESET)) {
             FLOPPY_DPRINTF("controller out of RESET state\n");              FLOPPY_DPRINTF("controller out of RESET state\n");
             fdctrl_reset(fdctrl, 1);              fdctrl_reset(fdctrl, 1);
             fdctrl->state &= ~(FD_CTRL_RESET | FD_CTRL_SLEEP);              fdctrl->dsr &= ~FD_DSR_PWRDOWN;
         }          }
     }      }
     /* Selected drive */      /* Selected drive */
     fdctrl->cur_drv = value & 1;      fdctrl->cur_drv = value & FD_DOR_SELMASK;
   
       fdctrl->dor = value;
 }  }
   
 /* Tape drive register : 0x03 */  /* Tape drive register : 0x03 */
 static uint32_t fdctrl_read_tape (fdctrl_t *fdctrl)  static uint32_t fdctrl_read_tape (fdctrl_t *fdctrl)
 {  {
     uint32_t retval = 0;      uint32_t retval = fdctrl->tdr;
   
     /* Disk boot selection indicator */  
     retval |= fdctrl->bootsel << 2;  
     /* Tape indicators: never allowed */  
     FLOPPY_DPRINTF("tape drive register: 0x%02x\n", retval);      FLOPPY_DPRINTF("tape drive register: 0x%02x\n", retval);
   
     return retval;      return retval;
Line 837  static uint32_t fdctrl_read_tape (fdctrl Line 927  static uint32_t fdctrl_read_tape (fdctrl
 static void fdctrl_write_tape (fdctrl_t *fdctrl, uint32_t value)  static void fdctrl_write_tape (fdctrl_t *fdctrl, uint32_t value)
 {  {
     /* Reset mode */      /* Reset mode */
     if (fdctrl->state & FD_CTRL_RESET) {      if (!(fdctrl->dor & FD_DOR_nRESET)) {
         FLOPPY_DPRINTF("Floppy controller in RESET state !\n");          FLOPPY_DPRINTF("Floppy controller in RESET state !\n");
         return;          return;
     }      }
     FLOPPY_DPRINTF("tape drive register set to 0x%02x\n", value);      FLOPPY_DPRINTF("tape drive register set to 0x%02x\n", value);
     /* Disk boot selection indicator */      /* Disk boot selection indicator */
     fdctrl->bootsel = (value >> 2) & 1;      fdctrl->tdr = value & FD_TDR_BOOTSEL;
     /* Tape indicators: never allow */      /* Tape indicators: never allow */
 }  }
   
 /* Main status register : 0x04 (read) */  /* Main status register : 0x04 (read) */
 static uint32_t fdctrl_read_main_status (fdctrl_t *fdctrl)  static uint32_t fdctrl_read_main_status (fdctrl_t *fdctrl)
 {  {
     uint32_t retval = 0;      uint32_t retval = fdctrl->msr;
   
       fdctrl->dsr &= ~FD_DSR_PWRDOWN;
       fdctrl->dor |= FD_DOR_nRESET;
   
     fdctrl->state &= ~(FD_CTRL_SLEEP | FD_CTRL_RESET);  
     if (!(fdctrl->state & FD_CTRL_BUSY)) {  
         /* Data transfer allowed */  
         retval |= 0x80;  
         /* Data transfer direction indicator */  
         if (fdctrl->data_dir == FD_DIR_READ)  
             retval |= 0x40;  
     }  
     /* Should handle 0x20 for SPECIFY command */  
     /* Command busy indicator */  
     if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA ||  
         FD_STATE(fdctrl->data_state) == FD_STATE_STATUS)  
         retval |= 0x10;  
     FLOPPY_DPRINTF("main status register: 0x%02x\n", retval);      FLOPPY_DPRINTF("main status register: 0x%02x\n", retval);
   
     return retval;      return retval;
Line 874  static uint32_t fdctrl_read_main_status  Line 954  static uint32_t fdctrl_read_main_status 
 static void fdctrl_write_rate (fdctrl_t *fdctrl, uint32_t value)  static void fdctrl_write_rate (fdctrl_t *fdctrl, uint32_t value)
 {  {
     /* Reset mode */      /* Reset mode */
     if (fdctrl->state & FD_CTRL_RESET) {      if (!(fdctrl->dor & FD_DOR_nRESET)) {
         FLOPPY_DPRINTF("Floppy controller in RESET state !\n");          FLOPPY_DPRINTF("Floppy controller in RESET state !\n");
         return;          return;
     }      }
     FLOPPY_DPRINTF("select rate register set to 0x%02x\n", value);      FLOPPY_DPRINTF("select rate register set to 0x%02x\n", value);
     /* Reset: autoclear */      /* Reset: autoclear */
     if (value & 0x80) {      if (value & FD_DSR_SWRESET) {
         fdctrl->state |= FD_CTRL_RESET;          fdctrl->dor &= ~FD_DOR_nRESET;
         fdctrl_reset(fdctrl, 1);          fdctrl_reset(fdctrl, 1);
         fdctrl->state &= ~FD_CTRL_RESET;          fdctrl->dor |= FD_DOR_nRESET;
     }      }
     if (value & 0x40) {      if (value & FD_DSR_PWRDOWN) {
         fdctrl->state |= FD_CTRL_SLEEP;  
         fdctrl_reset(fdctrl, 1);          fdctrl_reset(fdctrl, 1);
     }      }
 //        fdctrl.precomp = (value >> 2) & 0x07;      fdctrl->dsr = value;
 }  }
   
 static int fdctrl_media_changed(fdrive_t *drv)  static int fdctrl_media_changed(fdrive_t *drv)
Line 910  static uint32_t fdctrl_read_dir (fdctrl_ Line 989  static uint32_t fdctrl_read_dir (fdctrl_
 {  {
     uint32_t retval = 0;      uint32_t retval = 0;
   
     if (fdctrl_media_changed(drv0(fdctrl)) ||      if (fdctrl_media_changed(drv0(fdctrl))
         fdctrl_media_changed(drv1(fdctrl)))       || fdctrl_media_changed(drv1(fdctrl))
         retval |= 0x80;  #if MAX_FD == 4
        || fdctrl_media_changed(drv2(fdctrl))
        || fdctrl_media_changed(drv3(fdctrl))
   #endif
           )
           retval |= FD_DIR_DSKCHG;
     if (retval != 0)      if (retval != 0)
         FLOPPY_DPRINTF("Floppy digital input register: 0x%02x\n", retval);          FLOPPY_DPRINTF("Floppy digital input register: 0x%02x\n", retval);
   
Line 924  static void fdctrl_reset_fifo (fdctrl_t  Line 1008  static void fdctrl_reset_fifo (fdctrl_t 
 {  {
     fdctrl->data_dir = FD_DIR_WRITE;      fdctrl->data_dir = FD_DIR_WRITE;
     fdctrl->data_pos = 0;      fdctrl->data_pos = 0;
     FD_SET_STATE(fdctrl->data_state, FD_STATE_CMD);      fdctrl->msr &= ~(FD_MSR_CMDBUSY | FD_MSR_DIO);
 }  }
   
 /* Set FIFO status for the host to read */  /* Set FIFO status for the host to read */
Line 933  static void fdctrl_set_fifo (fdctrl_t *f Line 1017  static void fdctrl_set_fifo (fdctrl_t *f
     fdctrl->data_dir = FD_DIR_READ;      fdctrl->data_dir = FD_DIR_READ;
     fdctrl->data_len = fifo_len;      fdctrl->data_len = fifo_len;
     fdctrl->data_pos = 0;      fdctrl->data_pos = 0;
     FD_SET_STATE(fdctrl->data_state, FD_STATE_STATUS);      fdctrl->msr |= FD_MSR_CMDBUSY | FD_MSR_RQM | FD_MSR_DIO;
     if (do_irq)      if (do_irq)
         fdctrl_raise_irq(fdctrl, 0x00);          fdctrl_raise_irq(fdctrl, 0x00);
 }  }
   
 /* Set an error: unimplemented/unknown command */  /* Set an error: unimplemented/unknown command */
 static void fdctrl_unimplemented (fdctrl_t *fdctrl)  static void fdctrl_unimplemented (fdctrl_t *fdctrl, int direction)
 {  {
 #if 0      FLOPPY_ERROR("unimplemented command 0x%02x\n", fdctrl->fifo[0]);
     fdrive_t *cur_drv;      fdctrl->fifo[0] = FD_SR0_INVCMD;
   
     cur_drv = get_cur_drv(fdctrl);  
     fdctrl->fifo[0] = 0x60 | (cur_drv->head << 2) | fdctrl->cur_drv;  
     fdctrl->fifo[1] = 0x00;  
     fdctrl->fifo[2] = 0x00;  
     fdctrl_set_fifo(fdctrl, 3, 1);  
 #else  
     //    fdctrl_reset_fifo(fdctrl);  
     fdctrl->fifo[0] = 0x80;  
     fdctrl_set_fifo(fdctrl, 1, 0);      fdctrl_set_fifo(fdctrl, 1, 0);
 #endif  }
   
   /* Seek to next sector */
   static int fdctrl_seek_to_next_sect (fdctrl_t *fdctrl, fdrive_t *cur_drv)
   {
       FLOPPY_DPRINTF("seek to next sector (%d %02x %02x => %d)\n",
                      cur_drv->head, cur_drv->track, cur_drv->sect,
                      fd_sector(cur_drv));
       /* XXX: cur_drv->sect >= cur_drv->last_sect should be an
          error in fact */
       if (cur_drv->sect >= cur_drv->last_sect ||
           cur_drv->sect == fdctrl->eot) {
           cur_drv->sect = 1;
           if (FD_MULTI_TRACK(fdctrl->data_state)) {
               if (cur_drv->head == 0 &&
                   (cur_drv->flags & FDISK_DBL_SIDES) != 0) {
                   cur_drv->head = 1;
               } else {
                   cur_drv->head = 0;
                   cur_drv->track++;
                   if ((cur_drv->flags & FDISK_DBL_SIDES) == 0)
                       return 0;
               }
           } else {
               cur_drv->track++;
               return 0;
           }
           FLOPPY_DPRINTF("seek to next track (%d %02x %02x => %d)\n",
                          cur_drv->head, cur_drv->track,
                          cur_drv->sect, fd_sector(cur_drv));
       } else {
           cur_drv->sect++;
       }
       return 1;
 }  }
   
 /* Callback for transfer end (stop or abort) */  /* Callback for transfer end (stop or abort) */
Line 965  static void fdctrl_stop_transfer (fdctrl Line 1073  static void fdctrl_stop_transfer (fdctrl
     cur_drv = get_cur_drv(fdctrl);      cur_drv = get_cur_drv(fdctrl);
     FLOPPY_DPRINTF("transfer status: %02x %02x %02x (%02x)\n",      FLOPPY_DPRINTF("transfer status: %02x %02x %02x (%02x)\n",
                    status0, status1, status2,                     status0, status1, status2,
                    status0 | (cur_drv->head << 2) | fdctrl->cur_drv);                     status0 | (cur_drv->head << 2) | GET_CUR_DRV(fdctrl));
     fdctrl->fifo[0] = status0 | (cur_drv->head << 2) | fdctrl->cur_drv;      fdctrl->fifo[0] = status0 | (cur_drv->head << 2) | GET_CUR_DRV(fdctrl);
     fdctrl->fifo[1] = status1;      fdctrl->fifo[1] = status1;
     fdctrl->fifo[2] = status2;      fdctrl->fifo[2] = status2;
     fdctrl->fifo[3] = cur_drv->track;      fdctrl->fifo[3] = cur_drv->track;
Line 974  static void fdctrl_stop_transfer (fdctrl Line 1082  static void fdctrl_stop_transfer (fdctrl
     fdctrl->fifo[5] = cur_drv->sect;      fdctrl->fifo[5] = cur_drv->sect;
     fdctrl->fifo[6] = FD_SECTOR_SC;      fdctrl->fifo[6] = FD_SECTOR_SC;
     fdctrl->data_dir = FD_DIR_READ;      fdctrl->data_dir = FD_DIR_READ;
     if (fdctrl->state & FD_CTRL_BUSY) {      if (!(fdctrl->msr & FD_MSR_NONDMA)) {
         DMA_release_DREQ(fdctrl->dma_chann);          DMA_release_DREQ(fdctrl->dma_chann);
         fdctrl->state &= ~FD_CTRL_BUSY;  
     }      }
       fdctrl->msr |= FD_MSR_RQM | FD_MSR_DIO;
       fdctrl->msr &= ~FD_MSR_NONDMA;
     fdctrl_set_fifo(fdctrl, 7, 1);      fdctrl_set_fifo(fdctrl, 7, 1);
 }  }
   
Line 986  static void fdctrl_start_transfer (fdctr Line 1095  static void fdctrl_start_transfer (fdctr
 {  {
     fdrive_t *cur_drv;      fdrive_t *cur_drv;
     uint8_t kh, kt, ks;      uint8_t kh, kt, ks;
     int did_seek;      int did_seek = 0;
   
     fdctrl->cur_drv = fdctrl->fifo[1] & 1;      SET_CUR_DRV(fdctrl, fdctrl->fifo[1] & FD_DOR_SELMASK);
     cur_drv = get_cur_drv(fdctrl);      cur_drv = get_cur_drv(fdctrl);
     kt = fdctrl->fifo[2];      kt = fdctrl->fifo[2];
     kh = fdctrl->fifo[3];      kh = fdctrl->fifo[3];
     ks = fdctrl->fifo[4];      ks = fdctrl->fifo[4];
     FLOPPY_DPRINTF("Start transfer at %d %d %02x %02x (%d)\n",      FLOPPY_DPRINTF("Start transfer at %d %d %02x %02x (%d)\n",
                    fdctrl->cur_drv, kh, kt, ks,                     GET_CUR_DRV(fdctrl), kh, kt, ks,
                    _fd_sector(kh, kt, ks, cur_drv->last_sect));                     _fd_sector(kh, kt, ks, cur_drv->last_sect));
     did_seek = 0;      switch (fd_seek(cur_drv, kh, kt, ks, fdctrl->config & FD_CONFIG_EIS)) {
     switch (fd_seek(cur_drv, kh, kt, ks, fdctrl->config & 0x40)) {  
     case 2:      case 2:
         /* sect too big */          /* sect too big */
         fdctrl_stop_transfer(fdctrl, 0x40, 0x00, 0x00);          fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM, 0x00, 0x00);
         fdctrl->fifo[3] = kt;          fdctrl->fifo[3] = kt;
         fdctrl->fifo[4] = kh;          fdctrl->fifo[4] = kh;
         fdctrl->fifo[5] = ks;          fdctrl->fifo[5] = ks;
         return;          return;
     case 3:      case 3:
         /* track too big */          /* track too big */
         fdctrl_stop_transfer(fdctrl, 0x40, 0x80, 0x00);          fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM, FD_SR1_EC, 0x00);
         fdctrl->fifo[3] = kt;          fdctrl->fifo[3] = kt;
         fdctrl->fifo[4] = kh;          fdctrl->fifo[4] = kh;
         fdctrl->fifo[5] = ks;          fdctrl->fifo[5] = ks;
         return;          return;
     case 4:      case 4:
         /* No seek enabled */          /* No seek enabled */
         fdctrl_stop_transfer(fdctrl, 0x40, 0x00, 0x00);          fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM, 0x00, 0x00);
         fdctrl->fifo[3] = kt;          fdctrl->fifo[3] = kt;
         fdctrl->fifo[4] = kh;          fdctrl->fifo[4] = kh;
         fdctrl->fifo[5] = ks;          fdctrl->fifo[5] = ks;
Line 1025  static void fdctrl_start_transfer (fdctr Line 1133  static void fdctrl_start_transfer (fdctr
     default:      default:
         break;          break;
     }      }
   
     /* Set the FIFO state */      /* Set the FIFO state */
     fdctrl->data_dir = direction;      fdctrl->data_dir = direction;
     fdctrl->data_pos = 0;      fdctrl->data_pos = 0;
     FD_SET_STATE(fdctrl->data_state, FD_STATE_DATA); /* FIFO ready for data */      fdctrl->msr |= FD_MSR_CMDBUSY;
     if (fdctrl->fifo[0] & 0x80)      if (fdctrl->fifo[0] & 0x80)
         fdctrl->data_state |= FD_STATE_MULTI;          fdctrl->data_state |= FD_STATE_MULTI;
     else      else
Line 1042  static void fdctrl_start_transfer (fdctr Line 1151  static void fdctrl_start_transfer (fdctr
     } else {      } else {
         int tmp;          int tmp;
         fdctrl->data_len = 128 << (fdctrl->fifo[5] > 7 ? 7 : fdctrl->fifo[5]);          fdctrl->data_len = 128 << (fdctrl->fifo[5] > 7 ? 7 : fdctrl->fifo[5]);
         tmp = (cur_drv->last_sect - ks + 1);          tmp = (fdctrl->fifo[6] - ks + 1);
         if (fdctrl->fifo[0] & 0x80)          if (fdctrl->fifo[0] & 0x80)
             tmp += cur_drv->last_sect;              tmp += fdctrl->fifo[6];
         fdctrl->data_len *= tmp;          fdctrl->data_len *= tmp;
     }      }
     fdctrl->eot = fdctrl->fifo[6];      fdctrl->eot = fdctrl->fifo[6];
     if (fdctrl->dma_en) {      if (fdctrl->dor & FD_DOR_DMAEN) {
         int dma_mode;          int dma_mode;
         /* DMA transfer are enabled. Check if DMA channel is well programmed */          /* DMA transfer are enabled. Check if DMA channel is well programmed */
         dma_mode = DMA_get_channel_mode(fdctrl->dma_chann);          dma_mode = DMA_get_channel_mode(fdctrl->dma_chann);
Line 1062  static void fdctrl_start_transfer (fdctr Line 1171  static void fdctrl_start_transfer (fdctr
             (direction == FD_DIR_WRITE && dma_mode == 2) ||              (direction == FD_DIR_WRITE && dma_mode == 2) ||
             (direction == FD_DIR_READ && dma_mode == 1)) {              (direction == FD_DIR_READ && dma_mode == 1)) {
             /* No access is allowed until DMA transfer has completed */              /* No access is allowed until DMA transfer has completed */
             fdctrl->state |= FD_CTRL_BUSY;              fdctrl->msr &= ~FD_MSR_RQM;
             /* Now, we just have to wait for the DMA controller to              /* Now, we just have to wait for the DMA controller to
              * recall us...               * recall us...
              */               */
Line 1074  static void fdctrl_start_transfer (fdctr Line 1183  static void fdctrl_start_transfer (fdctr
         }          }
     }      }
     FLOPPY_DPRINTF("start non-DMA transfer\n");      FLOPPY_DPRINTF("start non-DMA transfer\n");
       fdctrl->msr |= FD_MSR_NONDMA;
       if (direction != FD_DIR_WRITE)
           fdctrl->msr |= FD_MSR_DIO;
     /* IO based transfer: calculate len */      /* IO based transfer: calculate len */
     fdctrl_raise_irq(fdctrl, 0x00);      fdctrl_raise_irq(fdctrl, 0x00);
   
Line 1083  static void fdctrl_start_transfer (fdctr Line 1195  static void fdctrl_start_transfer (fdctr
 /* Prepare a transfer of deleted data */  /* Prepare a transfer of deleted data */
 static void fdctrl_start_transfer_del (fdctrl_t *fdctrl, int direction)  static void fdctrl_start_transfer_del (fdctrl_t *fdctrl, int direction)
 {  {
       FLOPPY_ERROR("fdctrl_start_transfer_del() unimplemented\n");
   
     /* We don't handle deleted data,      /* We don't handle deleted data,
      * so we don't return *ANYTHING*       * so we don't return *ANYTHING*
      */       */
     fdctrl_stop_transfer(fdctrl, 0x60, 0x00, 0x00);      fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM | FD_SR0_SEEK, 0x00, 0x00);
 }  }
   
 /* handlers for DMA transfers */  /* handlers for DMA transfers */
Line 1099  static int fdctrl_transfer_handler (void Line 1213  static int fdctrl_transfer_handler (void
     uint8_t status0 = 0x00, status1 = 0x00, status2 = 0x00;      uint8_t status0 = 0x00, status1 = 0x00, status2 = 0x00;
   
     fdctrl = opaque;      fdctrl = opaque;
     if (!(fdctrl->state & FD_CTRL_BUSY)) {      if (fdctrl->msr & FD_MSR_RQM) {
         FLOPPY_DPRINTF("Not in DMA transfer mode !\n");          FLOPPY_DPRINTF("Not in DMA transfer mode !\n");
         return 0;          return 0;
     }      }
     cur_drv = get_cur_drv(fdctrl);      cur_drv = get_cur_drv(fdctrl);
     if (fdctrl->data_dir == FD_DIR_SCANE || fdctrl->data_dir == FD_DIR_SCANL ||      if (fdctrl->data_dir == FD_DIR_SCANE || fdctrl->data_dir == FD_DIR_SCANL ||
         fdctrl->data_dir == FD_DIR_SCANH)          fdctrl->data_dir == FD_DIR_SCANH)
         status2 = 0x04;          status2 = FD_SR2_SNS;
     if (dma_len > fdctrl->data_len)      if (dma_len > fdctrl->data_len)
         dma_len = fdctrl->data_len;          dma_len = fdctrl->data_len;
     if (cur_drv->bs == NULL) {      if (cur_drv->bs == NULL) {
         if (fdctrl->data_dir == FD_DIR_WRITE)          if (fdctrl->data_dir == FD_DIR_WRITE)
             fdctrl_stop_transfer(fdctrl, 0x60, 0x00, 0x00);              fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM | FD_SR0_SEEK, 0x00, 0x00);
         else          else
             fdctrl_stop_transfer(fdctrl, 0x40, 0x00, 0x00);              fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM, 0x00, 0x00);
         len = 0;          len = 0;
         goto transfer_error;          goto transfer_error;
     }      }
Line 1124  static int fdctrl_transfer_handler (void Line 1238  static int fdctrl_transfer_handler (void
             len = FD_SECTOR_LEN - rel_pos;              len = FD_SECTOR_LEN - rel_pos;
         FLOPPY_DPRINTF("copy %d bytes (%d %d %d) %d pos %d %02x "          FLOPPY_DPRINTF("copy %d bytes (%d %d %d) %d pos %d %02x "
                        "(%d-0x%08x 0x%08x)\n", len, dma_len, fdctrl->data_pos,                         "(%d-0x%08x 0x%08x)\n", len, dma_len, fdctrl->data_pos,
                        fdctrl->data_len, fdctrl->cur_drv, cur_drv->head,                         fdctrl->data_len, GET_CUR_DRV(fdctrl), cur_drv->head,
                        cur_drv->track, cur_drv->sect, fd_sector(cur_drv),                         cur_drv->track, cur_drv->sect, fd_sector(cur_drv),
                        fd_sector(cur_drv) * 512);                         fd_sector(cur_drv) * FD_SECTOR_LEN);
         if (fdctrl->data_dir != FD_DIR_WRITE ||          if (fdctrl->data_dir != FD_DIR_WRITE ||
             len < FD_SECTOR_LEN || rel_pos != 0) {              len < FD_SECTOR_LEN || rel_pos != 0) {
             /* READ & SCAN commands and realign to a sector for WRITE */              /* READ & SCAN commands and realign to a sector for WRITE */
Line 1150  static int fdctrl_transfer_handler (void Line 1264  static int fdctrl_transfer_handler (void
                              fdctrl->data_pos, len);                               fdctrl->data_pos, len);
             if (bdrv_write(cur_drv->bs, fd_sector(cur_drv),              if (bdrv_write(cur_drv->bs, fd_sector(cur_drv),
                            fdctrl->fifo, 1) < 0) {                             fdctrl->fifo, 1) < 0) {
                 FLOPPY_ERROR("writting sector %d\n", fd_sector(cur_drv));                  FLOPPY_ERROR("writing sector %d\n", fd_sector(cur_drv));
                 fdctrl_stop_transfer(fdctrl, 0x60, 0x00, 0x00);                  fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM | FD_SR0_SEEK, 0x00, 0x00);
                 goto transfer_error;                  goto transfer_error;
             }              }
             break;              break;
Line 1163  static int fdctrl_transfer_handler (void Line 1277  static int fdctrl_transfer_handler (void
                 DMA_read_memory (nchan, tmpbuf, fdctrl->data_pos, len);                  DMA_read_memory (nchan, tmpbuf, fdctrl->data_pos, len);
                 ret = memcmp(tmpbuf, fdctrl->fifo + rel_pos, len);                  ret = memcmp(tmpbuf, fdctrl->fifo + rel_pos, len);
                 if (ret == 0) {                  if (ret == 0) {
                     status2 = 0x08;                      status2 = FD_SR2_SEH;
                     goto end_transfer;                      goto end_transfer;
                 }                  }
                 if ((ret < 0 && fdctrl->data_dir == FD_DIR_SCANL) ||                  if ((ret < 0 && fdctrl->data_dir == FD_DIR_SCANL) ||
Line 1178  static int fdctrl_transfer_handler (void Line 1292  static int fdctrl_transfer_handler (void
         rel_pos = fdctrl->data_pos % FD_SECTOR_LEN;          rel_pos = fdctrl->data_pos % FD_SECTOR_LEN;
         if (rel_pos == 0) {          if (rel_pos == 0) {
             /* Seek to next sector */              /* Seek to next sector */
             FLOPPY_DPRINTF("seek to next sector (%d %02x %02x => %d) (%d)\n",              if (!fdctrl_seek_to_next_sect(fdctrl, cur_drv))
                            cur_drv->head, cur_drv->track, cur_drv->sect,                  break;
                            fd_sector(cur_drv),  
                            fdctrl->data_pos - len);  
             /* XXX: cur_drv->sect >= cur_drv->last_sect should be an  
                error in fact */  
             if (cur_drv->sect >= cur_drv->last_sect ||  
                 cur_drv->sect == fdctrl->eot) {  
                 cur_drv->sect = 1;  
                 if (FD_MULTI_TRACK(fdctrl->data_state)) {  
                     if (cur_drv->head == 0 &&  
                         (cur_drv->flags & FDISK_DBL_SIDES) != 0) {  
                         cur_drv->head = 1;  
                     } else {  
                         cur_drv->head = 0;  
                         cur_drv->track++;  
                         if ((cur_drv->flags & FDISK_DBL_SIDES) == 0)  
                             break;  
                     }  
                 } else {  
                     cur_drv->track++;  
                     break;  
                 }  
                 FLOPPY_DPRINTF("seek to next track (%d %02x %02x => %d)\n",  
                                cur_drv->head, cur_drv->track,  
                                cur_drv->sect, fd_sector(cur_drv));  
             } else {  
                 cur_drv->sect++;  
             }  
         }          }
     }      }
  end_transfer:   end_transfer:
Line 1216  static int fdctrl_transfer_handler (void Line 1303  static int fdctrl_transfer_handler (void
     if (fdctrl->data_dir == FD_DIR_SCANE ||      if (fdctrl->data_dir == FD_DIR_SCANE ||
         fdctrl->data_dir == FD_DIR_SCANL ||          fdctrl->data_dir == FD_DIR_SCANL ||
         fdctrl->data_dir == FD_DIR_SCANH)          fdctrl->data_dir == FD_DIR_SCANH)
         status2 = 0x08;          status2 = FD_SR2_SEH;
     if (FD_DID_SEEK(fdctrl->data_state))      if (FD_DID_SEEK(fdctrl->data_state))
         status0 |= 0x20;          status0 |= FD_SR0_SEEK;
     fdctrl->data_len -= len;      fdctrl->data_len -= len;
     //    if (fdctrl->data_len == 0)  
     fdctrl_stop_transfer(fdctrl, status0, status1, status2);      fdctrl_stop_transfer(fdctrl, status0, status1, status2);
  transfer_error:   transfer_error:
   
Line 1232  static uint32_t fdctrl_read_data (fdctrl Line 1318  static uint32_t fdctrl_read_data (fdctrl
 {  {
     fdrive_t *cur_drv;      fdrive_t *cur_drv;
     uint32_t retval = 0;      uint32_t retval = 0;
     int pos, len;      int pos;
   
     cur_drv = get_cur_drv(fdctrl);      cur_drv = get_cur_drv(fdctrl);
     fdctrl->state &= ~FD_CTRL_SLEEP;      fdctrl->dsr &= ~FD_DSR_PWRDOWN;
     if (FD_STATE(fdctrl->data_state) == FD_STATE_CMD) {      if (!(fdctrl->msr & FD_MSR_RQM) || !(fdctrl->msr & FD_MSR_DIO)) {
         FLOPPY_ERROR("can't read data in CMD state\n");          FLOPPY_ERROR("controller not ready for reading\n");
         return 0;          return 0;
     }      }
     pos = fdctrl->data_pos;      pos = fdctrl->data_pos;
     if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA) {      if (fdctrl->msr & FD_MSR_NONDMA) {
         pos %= FD_SECTOR_LEN;          pos %= FD_SECTOR_LEN;
         if (pos == 0) {          if (pos == 0) {
             len = fdctrl->data_len - fdctrl->data_pos;              if (fdctrl->data_pos != 0)
             if (len > FD_SECTOR_LEN)                  if (!fdctrl_seek_to_next_sect(fdctrl, cur_drv)) {
                 len = FD_SECTOR_LEN;                      FLOPPY_DPRINTF("error seeking to next sector %d\n",
             bdrv_read(cur_drv->bs, fd_sector(cur_drv), fdctrl->fifo, 1);                                     fd_sector(cur_drv));
                       return 0;
                   }
               if (bdrv_read(cur_drv->bs, fd_sector(cur_drv), fdctrl->fifo, 1) < 0) {
                   FLOPPY_DPRINTF("error getting sector %d\n",
                                  fd_sector(cur_drv));
                   /* Sure, image size is too small... */
                   memset(fdctrl->fifo, 0, FD_SECTOR_LEN);
               }
         }          }
     }      }
     retval = fdctrl->fifo[pos];      retval = fdctrl->fifo[pos];
Line 1256  static uint32_t fdctrl_read_data (fdctrl Line 1350  static uint32_t fdctrl_read_data (fdctrl
         /* Switch from transfer mode to status mode          /* Switch from transfer mode to status mode
          * then from status mode to command mode           * then from status mode to command mode
          */           */
         if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA) {          if (fdctrl->msr & FD_MSR_NONDMA) {
             fdctrl_stop_transfer(fdctrl, 0x20, 0x00, 0x00);              fdctrl_stop_transfer(fdctrl, FD_SR0_SEEK, 0x00, 0x00);
         } else {          } else {
             fdctrl_reset_fifo(fdctrl);              fdctrl_reset_fifo(fdctrl);
             fdctrl_reset_irq(fdctrl);              fdctrl_reset_irq(fdctrl);
Line 1272  static void fdctrl_format_sector (fdctrl Line 1366  static void fdctrl_format_sector (fdctrl
 {  {
     fdrive_t *cur_drv;      fdrive_t *cur_drv;
     uint8_t kh, kt, ks;      uint8_t kh, kt, ks;
     int did_seek;  
   
     fdctrl->cur_drv = fdctrl->fifo[1] & 1;      SET_CUR_DRV(fdctrl, fdctrl->fifo[1] & FD_DOR_SELMASK);
     cur_drv = get_cur_drv(fdctrl);      cur_drv = get_cur_drv(fdctrl);
     kt = fdctrl->fifo[6];      kt = fdctrl->fifo[6];
     kh = fdctrl->fifo[7];      kh = fdctrl->fifo[7];
     ks = fdctrl->fifo[8];      ks = fdctrl->fifo[8];
     FLOPPY_DPRINTF("format sector at %d %d %02x %02x (%d)\n",      FLOPPY_DPRINTF("format sector at %d %d %02x %02x (%d)\n",
                    fdctrl->cur_drv, kh, kt, ks,                     GET_CUR_DRV(fdctrl), kh, kt, ks,
                    _fd_sector(kh, kt, ks, cur_drv->last_sect));                     _fd_sector(kh, kt, ks, cur_drv->last_sect));
     did_seek = 0;      switch (fd_seek(cur_drv, kh, kt, ks, fdctrl->config & FD_CONFIG_EIS)) {
     switch (fd_seek(cur_drv, kh, kt, ks, fdctrl->config & 0x40)) {  
     case 2:      case 2:
         /* sect too big */          /* sect too big */
         fdctrl_stop_transfer(fdctrl, 0x40, 0x00, 0x00);          fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM, 0x00, 0x00);
         fdctrl->fifo[3] = kt;          fdctrl->fifo[3] = kt;
         fdctrl->fifo[4] = kh;          fdctrl->fifo[4] = kh;
         fdctrl->fifo[5] = ks;          fdctrl->fifo[5] = ks;
         return;          return;
     case 3:      case 3:
         /* track too big */          /* track too big */
         fdctrl_stop_transfer(fdctrl, 0x40, 0x80, 0x00);          fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM, FD_SR1_EC, 0x00);
         fdctrl->fifo[3] = kt;          fdctrl->fifo[3] = kt;
         fdctrl->fifo[4] = kh;          fdctrl->fifo[4] = kh;
         fdctrl->fifo[5] = ks;          fdctrl->fifo[5] = ks;
         return;          return;
     case 4:      case 4:
         /* No seek enabled */          /* No seek enabled */
         fdctrl_stop_transfer(fdctrl, 0x40, 0x00, 0x00);          fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM, 0x00, 0x00);
         fdctrl->fifo[3] = kt;          fdctrl->fifo[3] = kt;
         fdctrl->fifo[4] = kh;          fdctrl->fifo[4] = kh;
         fdctrl->fifo[5] = ks;          fdctrl->fifo[5] = ks;
         return;          return;
     case 1:      case 1:
         did_seek = 1;  
         fdctrl->data_state |= FD_STATE_SEEK;          fdctrl->data_state |= FD_STATE_SEEK;
         break;          break;
     default:      default:
Line 1316  static void fdctrl_format_sector (fdctrl Line 1407  static void fdctrl_format_sector (fdctrl
     if (cur_drv->bs == NULL ||      if (cur_drv->bs == NULL ||
         bdrv_write(cur_drv->bs, fd_sector(cur_drv), fdctrl->fifo, 1) < 0) {          bdrv_write(cur_drv->bs, fd_sector(cur_drv), fdctrl->fifo, 1) < 0) {
         FLOPPY_ERROR("formatting sector %d\n", fd_sector(cur_drv));          FLOPPY_ERROR("formatting sector %d\n", fd_sector(cur_drv));
         fdctrl_stop_transfer(fdctrl, 0x60, 0x00, 0x00);          fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM | FD_SR0_SEEK, 0x00, 0x00);
     } else {      } else {
         if (cur_drv->sect == cur_drv->last_sect) {          if (cur_drv->sect == cur_drv->last_sect) {
             fdctrl->data_state &= ~FD_STATE_FORMAT;              fdctrl->data_state &= ~FD_STATE_FORMAT;
             /* Last sector done */              /* Last sector done */
             if (FD_DID_SEEK(fdctrl->data_state))              if (FD_DID_SEEK(fdctrl->data_state))
                 fdctrl_stop_transfer(fdctrl, 0x20, 0x00, 0x00);                  fdctrl_stop_transfer(fdctrl, FD_SR0_SEEK, 0x00, 0x00);
             else              else
                 fdctrl_stop_transfer(fdctrl, 0x00, 0x00, 0x00);                  fdctrl_stop_transfer(fdctrl, 0x00, 0x00, 0x00);
         } else {          } else {
Line 1333  static void fdctrl_format_sector (fdctrl Line 1424  static void fdctrl_format_sector (fdctrl
     }      }
 }  }
   
 static void fdctrl_write_data (fdctrl_t *fdctrl, uint32_t value)  static void fdctrl_handle_lock (fdctrl_t *fdctrl, int direction)
 {  {
     fdrive_t *cur_drv;      fdctrl->lock = (fdctrl->fifo[0] & 0x80) ? 1 : 0;
       fdctrl->fifo[0] = fdctrl->lock << 4;
       fdctrl_set_fifo(fdctrl, 1, fdctrl->lock);
   }
   
     cur_drv = get_cur_drv(fdctrl);  static void fdctrl_handle_dumpreg (fdctrl_t *fdctrl, int direction)
     /* Reset mode */  {
     if (fdctrl->state & FD_CTRL_RESET) {      fdrive_t *cur_drv = get_cur_drv(fdctrl);
         FLOPPY_DPRINTF("Floppy controller in RESET state !\n");  
         return;      /* Drives position */
     }      fdctrl->fifo[0] = drv0(fdctrl)->track;
     fdctrl->state &= ~FD_CTRL_SLEEP;      fdctrl->fifo[1] = drv1(fdctrl)->track;
     if (FD_STATE(fdctrl->data_state) == FD_STATE_STATUS) {  #if MAX_FD == 4
         FLOPPY_ERROR("can't write data in status mode\n");      fdctrl->fifo[2] = drv2(fdctrl)->track;
         return;      fdctrl->fifo[3] = drv3(fdctrl)->track;
     }  
     /* Is it write command time ? */  
     if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA) {  
         /* FIFO data write */  
         fdctrl->fifo[fdctrl->data_pos++] = value;  
         if (fdctrl->data_pos % FD_SECTOR_LEN == (FD_SECTOR_LEN - 1) ||  
             fdctrl->data_pos == fdctrl->data_len) {  
             bdrv_write(cur_drv->bs, fd_sector(cur_drv), fdctrl->fifo, 1);  
         }  
         /* Switch from transfer mode to status mode  
          * then from status mode to command mode  
          */  
         if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA)  
             fdctrl_stop_transfer(fdctrl, 0x20, 0x00, 0x00);  
         return;  
     }  
     if (fdctrl->data_pos == 0) {  
         /* Command */  
         switch (value & 0x5F) {  
         case 0x46:  
             /* READ variants */  
             FLOPPY_DPRINTF("READ command\n");  
             /* 8 parameters cmd */  
             fdctrl->data_len = 9;  
             goto enqueue;  
         case 0x4C:  
             /* READ_DELETED variants */  
             FLOPPY_DPRINTF("READ_DELETED command\n");  
             /* 8 parameters cmd */  
             fdctrl->data_len = 9;  
             goto enqueue;  
         case 0x50:  
             /* SCAN_EQUAL variants */  
             FLOPPY_DPRINTF("SCAN_EQUAL command\n");  
             /* 8 parameters cmd */  
             fdctrl->data_len = 9;  
             goto enqueue;  
         case 0x56:  
             /* VERIFY variants */  
             FLOPPY_DPRINTF("VERIFY command\n");  
             /* 8 parameters cmd */  
             fdctrl->data_len = 9;  
             goto enqueue;  
         case 0x59:  
             /* SCAN_LOW_OR_EQUAL variants */  
             FLOPPY_DPRINTF("SCAN_LOW_OR_EQUAL command\n");  
             /* 8 parameters cmd */  
             fdctrl->data_len = 9;  
             goto enqueue;  
         case 0x5D:  
             /* SCAN_HIGH_OR_EQUAL variants */  
             FLOPPY_DPRINTF("SCAN_HIGH_OR_EQUAL command\n");  
             /* 8 parameters cmd */  
             fdctrl->data_len = 9;  
             goto enqueue;  
         default:  
             break;  
         }  
         switch (value & 0x7F) {  
         case 0x45:  
             /* WRITE variants */  
             FLOPPY_DPRINTF("WRITE command\n");  
             /* 8 parameters cmd */  
             fdctrl->data_len = 9;  
             goto enqueue;  
         case 0x49:  
             /* WRITE_DELETED variants */  
             FLOPPY_DPRINTF("WRITE_DELETED command\n");  
             /* 8 parameters cmd */  
             fdctrl->data_len = 9;  
             goto enqueue;  
         default:  
             break;  
         }  
         switch (value) {  
         case 0x03:  
             /* SPECIFY */  
             FLOPPY_DPRINTF("SPECIFY command\n");  
             /* 1 parameter cmd */  
             fdctrl->data_len = 3;  
             goto enqueue;  
         case 0x04:  
             /* SENSE_DRIVE_STATUS */  
             FLOPPY_DPRINTF("SENSE_DRIVE_STATUS command\n");  
             /* 1 parameter cmd */  
             fdctrl->data_len = 2;  
             goto enqueue;  
         case 0x07:  
             /* RECALIBRATE */  
             FLOPPY_DPRINTF("RECALIBRATE command\n");  
             /* 1 parameter cmd */  
             fdctrl->data_len = 2;  
             goto enqueue;  
         case 0x08:  
             /* SENSE_INTERRUPT_STATUS */  
             FLOPPY_DPRINTF("SENSE_INTERRUPT_STATUS command (%02x)\n",  
                            fdctrl->int_status);  
             /* No parameters cmd: returns status if no interrupt */  
 #if 0  
             fdctrl->fifo[0] =  
                 fdctrl->int_status | (cur_drv->head << 2) | fdctrl->cur_drv;  
 #else  #else
             /* XXX: int_status handling is broken for read/write      fdctrl->fifo[2] = 0;
                commands, so we do this hack. It should be suppressed      fdctrl->fifo[3] = 0;
                ASAP */  
             fdctrl->fifo[0] =  
                 0x20 | (cur_drv->head << 2) | fdctrl->cur_drv;  
 #endif  #endif
             fdctrl->fifo[1] = cur_drv->track;      /* timers */
             fdctrl_set_fifo(fdctrl, 2, 0);      fdctrl->fifo[4] = fdctrl->timer0;
             fdctrl_reset_irq(fdctrl);      fdctrl->fifo[5] = (fdctrl->timer1 << 1) | (fdctrl->dor & FD_DOR_DMAEN ? 1 : 0);
             fdctrl->int_status = 0xC0;      fdctrl->fifo[6] = cur_drv->last_sect;
             return;      fdctrl->fifo[7] = (fdctrl->lock << 7) |
         case 0x0E:          (cur_drv->perpendicular << 2);
             /* DUMPREG */      fdctrl->fifo[8] = fdctrl->config;
             FLOPPY_DPRINTF("DUMPREG command\n");      fdctrl->fifo[9] = fdctrl->precomp_trk;
             /* Drives position */      fdctrl_set_fifo(fdctrl, 10, 0);
             fdctrl->fifo[0] = drv0(fdctrl)->track;  }
             fdctrl->fifo[1] = drv1(fdctrl)->track;  
             fdctrl->fifo[2] = 0;  static void fdctrl_handle_version (fdctrl_t *fdctrl, int direction)
             fdctrl->fifo[3] = 0;  {
             /* timers */      /* Controller's version */
             fdctrl->fifo[4] = fdctrl->timer0;      fdctrl->fifo[0] = fdctrl->version;
             fdctrl->fifo[5] = (fdctrl->timer1 << 1) | fdctrl->dma_en;      fdctrl_set_fifo(fdctrl, 1, 1);
             fdctrl->fifo[6] = cur_drv->last_sect;  }
             fdctrl->fifo[7] = (fdctrl->lock << 7) |  
                 (cur_drv->perpendicular << 2);  static void fdctrl_handle_partid (fdctrl_t *fdctrl, int direction)
             fdctrl->fifo[8] = fdctrl->config;  {
             fdctrl->fifo[9] = fdctrl->precomp_trk;      fdctrl->fifo[0] = 0x41; /* Stepping 1 */
             fdctrl_set_fifo(fdctrl, 10, 0);      fdctrl_set_fifo(fdctrl, 1, 0);
             return;  }
         case 0x0F:  
             /* SEEK */  static void fdctrl_handle_restore (fdctrl_t *fdctrl, int direction)
             FLOPPY_DPRINTF("SEEK command\n");  {
             /* 2 parameters cmd */      fdrive_t *cur_drv = get_cur_drv(fdctrl);
             fdctrl->data_len = 3;  
             goto enqueue;      /* Drives position */
         case 0x10:      drv0(fdctrl)->track = fdctrl->fifo[3];
             /* VERSION */      drv1(fdctrl)->track = fdctrl->fifo[4];
             FLOPPY_DPRINTF("VERSION command\n");  #if MAX_FD == 4
             /* No parameters cmd */      drv2(fdctrl)->track = fdctrl->fifo[5];
             /* Controller's version */      drv3(fdctrl)->track = fdctrl->fifo[6];
             fdctrl->fifo[0] = fdctrl->version;  #endif
             fdctrl_set_fifo(fdctrl, 1, 1);      /* timers */
             return;      fdctrl->timer0 = fdctrl->fifo[7];
         case 0x12:      fdctrl->timer1 = fdctrl->fifo[8];
             /* PERPENDICULAR_MODE */      cur_drv->last_sect = fdctrl->fifo[9];
             FLOPPY_DPRINTF("PERPENDICULAR_MODE command\n");      fdctrl->lock = fdctrl->fifo[10] >> 7;
             /* 1 parameter cmd */      cur_drv->perpendicular = (fdctrl->fifo[10] >> 2) & 0xF;
             fdctrl->data_len = 2;      fdctrl->config = fdctrl->fifo[11];
             goto enqueue;      fdctrl->precomp_trk = fdctrl->fifo[12];
         case 0x13:      fdctrl->pwrd = fdctrl->fifo[13];
             /* CONFIGURE */      fdctrl_reset_fifo(fdctrl);
             FLOPPY_DPRINTF("CONFIGURE command\n");  }
             /* 3 parameters cmd */  
             fdctrl->data_len = 4;  static void fdctrl_handle_save (fdctrl_t *fdctrl, int direction)
             goto enqueue;  {
         case 0x14:      fdrive_t *cur_drv = get_cur_drv(fdctrl);
             /* UNLOCK */  
             FLOPPY_DPRINTF("UNLOCK command\n");      fdctrl->fifo[0] = 0;
             /* No parameters cmd */      fdctrl->fifo[1] = 0;
             fdctrl->lock = 0;      /* Drives position */
             fdctrl->fifo[0] = 0;      fdctrl->fifo[2] = drv0(fdctrl)->track;
             fdctrl_set_fifo(fdctrl, 1, 0);      fdctrl->fifo[3] = drv1(fdctrl)->track;
             return;  #if MAX_FD == 4
         case 0x17:      fdctrl->fifo[4] = drv2(fdctrl)->track;
             /* POWERDOWN_MODE */      fdctrl->fifo[5] = drv3(fdctrl)->track;
             FLOPPY_DPRINTF("POWERDOWN_MODE command\n");  #else
             /* 2 parameters cmd */      fdctrl->fifo[4] = 0;
             fdctrl->data_len = 3;      fdctrl->fifo[5] = 0;
             goto enqueue;  #endif
         case 0x18:      /* timers */
             /* PART_ID */      fdctrl->fifo[6] = fdctrl->timer0;
             FLOPPY_DPRINTF("PART_ID command\n");      fdctrl->fifo[7] = fdctrl->timer1;
             /* No parameters cmd */      fdctrl->fifo[8] = cur_drv->last_sect;
             fdctrl->fifo[0] = 0x41; /* Stepping 1 */      fdctrl->fifo[9] = (fdctrl->lock << 7) |
             fdctrl_set_fifo(fdctrl, 1, 0);          (cur_drv->perpendicular << 2);
             return;      fdctrl->fifo[10] = fdctrl->config;
         case 0x2C:      fdctrl->fifo[11] = fdctrl->precomp_trk;
             /* SAVE */      fdctrl->fifo[12] = fdctrl->pwrd;
             FLOPPY_DPRINTF("SAVE command\n");      fdctrl->fifo[13] = 0;
             /* No parameters cmd */      fdctrl->fifo[14] = 0;
             fdctrl->fifo[0] = 0;      fdctrl_set_fifo(fdctrl, 15, 1);
             fdctrl->fifo[1] = 0;  }
             /* Drives position */  
             fdctrl->fifo[2] = drv0(fdctrl)->track;  static void fdctrl_handle_readid (fdctrl_t *fdctrl, int direction)
             fdctrl->fifo[3] = drv1(fdctrl)->track;  {
             fdctrl->fifo[4] = 0;      fdrive_t *cur_drv = get_cur_drv(fdctrl);
             fdctrl->fifo[5] = 0;  
             /* timers */      /* XXX: should set main status register to busy */
             fdctrl->fifo[6] = fdctrl->timer0;      cur_drv->head = (fdctrl->fifo[1] >> 2) & 1;
             fdctrl->fifo[7] = fdctrl->timer1;      qemu_mod_timer(fdctrl->result_timer,
             fdctrl->fifo[8] = cur_drv->last_sect;                     qemu_get_clock(vm_clock) + (ticks_per_sec / 50));
             fdctrl->fifo[9] = (fdctrl->lock << 7) |  }
                 (cur_drv->perpendicular << 2);  
             fdctrl->fifo[10] = fdctrl->config;  static void fdctrl_handle_format_track (fdctrl_t *fdctrl, int direction)
             fdctrl->fifo[11] = fdctrl->precomp_trk;  {
             fdctrl->fifo[12] = fdctrl->pwrd;      fdrive_t *cur_drv;
             fdctrl->fifo[13] = 0;  
             fdctrl->fifo[14] = 0;      SET_CUR_DRV(fdctrl, fdctrl->fifo[1] & FD_DOR_SELMASK);
             fdctrl_set_fifo(fdctrl, 15, 1);      cur_drv = get_cur_drv(fdctrl);
             return;      fdctrl->data_state |= FD_STATE_FORMAT;
         case 0x33:      if (fdctrl->fifo[0] & 0x80)
             /* OPTION */          fdctrl->data_state |= FD_STATE_MULTI;
             FLOPPY_DPRINTF("OPTION command\n");      else
             /* 1 parameter cmd */          fdctrl->data_state &= ~FD_STATE_MULTI;
             fdctrl->data_len = 2;      fdctrl->data_state &= ~FD_STATE_SEEK;
             goto enqueue;      cur_drv->bps =
         case 0x42:          fdctrl->fifo[2] > 7 ? 16384 : 128 << fdctrl->fifo[2];
             /* READ_TRACK */  #if 0
             FLOPPY_DPRINTF("READ_TRACK command\n");      cur_drv->last_sect =
             /* 8 parameters cmd */          cur_drv->flags & FDISK_DBL_SIDES ? fdctrl->fifo[3] :
             fdctrl->data_len = 9;          fdctrl->fifo[3] / 2;
             goto enqueue;  #else
         case 0x4A:      cur_drv->last_sect = fdctrl->fifo[3];
             /* READ_ID */  #endif
             FLOPPY_DPRINTF("READ_ID command\n");      /* TODO: implement format using DMA expected by the Bochs BIOS
             /* 1 parameter cmd */       * and Linux fdformat (read 3 bytes per sector via DMA and fill
             fdctrl->data_len = 2;       * the sector with the specified fill byte
             goto enqueue;       */
         case 0x4C:      fdctrl->data_state &= ~FD_STATE_FORMAT;
             /* RESTORE */      fdctrl_stop_transfer(fdctrl, 0x00, 0x00, 0x00);
             FLOPPY_DPRINTF("RESTORE command\n");  }
             /* 17 parameters cmd */  
             fdctrl->data_len = 18;  static void fdctrl_handle_specify (fdctrl_t *fdctrl, int direction)
             goto enqueue;  {
         case 0x4D:      fdctrl->timer0 = (fdctrl->fifo[1] >> 4) & 0xF;
             /* FORMAT_TRACK */      fdctrl->timer1 = fdctrl->fifo[2] >> 1;
             FLOPPY_DPRINTF("FORMAT_TRACK command\n");      if (fdctrl->fifo[2] & 1)
             /* 5 parameters cmd */          fdctrl->dor &= ~FD_DOR_DMAEN;
             fdctrl->data_len = 6;      else
             goto enqueue;          fdctrl->dor |= FD_DOR_DMAEN;
         case 0x8E:      /* No result back */
             /* DRIVE_SPECIFICATION_COMMAND */      fdctrl_reset_fifo(fdctrl);
             FLOPPY_DPRINTF("DRIVE_SPECIFICATION_COMMAND command\n");  }
             /* 5 parameters cmd */  
             fdctrl->data_len = 6;  static void fdctrl_handle_sense_drive_status (fdctrl_t *fdctrl, int direction)
             goto enqueue;  {
         case 0x8F:      fdrive_t *cur_drv;
             /* RELATIVE_SEEK_OUT */  
             FLOPPY_DPRINTF("RELATIVE_SEEK_OUT command\n");      SET_CUR_DRV(fdctrl, fdctrl->fifo[1] & FD_DOR_SELMASK);
             /* 2 parameters cmd */      cur_drv = get_cur_drv(fdctrl);
             fdctrl->data_len = 3;      cur_drv->head = (fdctrl->fifo[1] >> 2) & 1;
             goto enqueue;      /* 1 Byte status back */
         case 0x94:      fdctrl->fifo[0] = (cur_drv->ro << 6) |
             /* LOCK */          (cur_drv->track == 0 ? 0x10 : 0x00) |
             FLOPPY_DPRINTF("LOCK command\n");          (cur_drv->head << 2) |
             /* No parameters cmd */          GET_CUR_DRV(fdctrl) |
             fdctrl->lock = 1;          0x28;
             fdctrl->fifo[0] = 0x10;      fdctrl_set_fifo(fdctrl, 1, 0);
             fdctrl_set_fifo(fdctrl, 1, 1);  }
             return;  
         case 0xCD:  static void fdctrl_handle_recalibrate (fdctrl_t *fdctrl, int direction)
             /* FORMAT_AND_WRITE */  {
             FLOPPY_DPRINTF("FORMAT_AND_WRITE command\n");      fdrive_t *cur_drv;
             /* 10 parameters cmd */  
             fdctrl->data_len = 11;      SET_CUR_DRV(fdctrl, fdctrl->fifo[1] & FD_DOR_SELMASK);
             goto enqueue;      cur_drv = get_cur_drv(fdctrl);
         case 0xCF:      fd_recalibrate(cur_drv);
             /* RELATIVE_SEEK_IN */      fdctrl_reset_fifo(fdctrl);
             FLOPPY_DPRINTF("RELATIVE_SEEK_IN command\n");      /* Raise Interrupt */
             /* 2 parameters cmd */      fdctrl_raise_irq(fdctrl, FD_SR0_SEEK);
             fdctrl->data_len = 3;  }
             goto enqueue;  
         default:  static void fdctrl_handle_sense_interrupt_status (fdctrl_t *fdctrl, int direction)
             /* Unknown command */  {
             FLOPPY_ERROR("unknown command: 0x%02x\n", value);      fdrive_t *cur_drv = get_cur_drv(fdctrl);
             fdctrl_unimplemented(fdctrl);  
             return;      if(fdctrl->reset_sensei > 0) {
           fdctrl->fifo[0] =
               FD_SR0_RDYCHG + FD_RESET_SENSEI_COUNT - fdctrl->reset_sensei;
           fdctrl->reset_sensei--;
       } else {
           /* XXX: status0 handling is broken for read/write
              commands, so we do this hack. It should be suppressed
              ASAP */
           fdctrl->fifo[0] =
               FD_SR0_SEEK | (cur_drv->head << 2) | GET_CUR_DRV(fdctrl);
       }
   
       fdctrl->fifo[1] = cur_drv->track;
       fdctrl_set_fifo(fdctrl, 2, 0);
       fdctrl_reset_irq(fdctrl);
       fdctrl->status0 = FD_SR0_RDYCHG;
   }
   
   static void fdctrl_handle_seek (fdctrl_t *fdctrl, int direction)
   {
       fdrive_t *cur_drv;
   
       SET_CUR_DRV(fdctrl, fdctrl->fifo[1] & FD_DOR_SELMASK);
       cur_drv = get_cur_drv(fdctrl);
       fdctrl_reset_fifo(fdctrl);
       if (fdctrl->fifo[2] > cur_drv->max_track) {
           fdctrl_raise_irq(fdctrl, FD_SR0_ABNTERM | FD_SR0_SEEK);
       } else {
           cur_drv->track = fdctrl->fifo[2];
           /* Raise Interrupt */
           fdctrl_raise_irq(fdctrl, FD_SR0_SEEK);
       }
   }
   
   static void fdctrl_handle_perpendicular_mode (fdctrl_t *fdctrl, int direction)
   {
       fdrive_t *cur_drv = get_cur_drv(fdctrl);
   
       if (fdctrl->fifo[1] & 0x80)
           cur_drv->perpendicular = fdctrl->fifo[1] & 0x7;
       /* No result back */
       fdctrl_reset_fifo(fdctrl);
   }
   
   static void fdctrl_handle_configure (fdctrl_t *fdctrl, int direction)
   {
       fdctrl->config = fdctrl->fifo[2];
       fdctrl->precomp_trk =  fdctrl->fifo[3];
       /* No result back */
       fdctrl_reset_fifo(fdctrl);
   }
   
   static void fdctrl_handle_powerdown_mode (fdctrl_t *fdctrl, int direction)
   {
       fdctrl->pwrd = fdctrl->fifo[1];
       fdctrl->fifo[0] = fdctrl->fifo[1];
       fdctrl_set_fifo(fdctrl, 1, 1);
   }
   
   static void fdctrl_handle_option (fdctrl_t *fdctrl, int direction)
   {
       /* No result back */
       fdctrl_reset_fifo(fdctrl);
   }
   
   static void fdctrl_handle_drive_specification_command (fdctrl_t *fdctrl, int direction)
   {
       fdrive_t *cur_drv = get_cur_drv(fdctrl);
   
       if (fdctrl->fifo[fdctrl->data_pos - 1] & 0x80) {
           /* Command parameters done */
           if (fdctrl->fifo[fdctrl->data_pos - 1] & 0x40) {
               fdctrl->fifo[0] = fdctrl->fifo[1];
               fdctrl->fifo[2] = 0;
               fdctrl->fifo[3] = 0;
               fdctrl_set_fifo(fdctrl, 4, 1);
           } else {
               fdctrl_reset_fifo(fdctrl);
         }          }
       } else if (fdctrl->data_len > 7) {
           /* ERROR */
           fdctrl->fifo[0] = 0x80 |
               (cur_drv->head << 2) | GET_CUR_DRV(fdctrl);
           fdctrl_set_fifo(fdctrl, 1, 1);
       }
   }
   
   static void fdctrl_handle_relative_seek_out (fdctrl_t *fdctrl, int direction)
   {
       fdrive_t *cur_drv;
   
       SET_CUR_DRV(fdctrl, fdctrl->fifo[1] & FD_DOR_SELMASK);
       cur_drv = get_cur_drv(fdctrl);
       if (fdctrl->fifo[2] + cur_drv->track >= cur_drv->max_track) {
           cur_drv->track = cur_drv->max_track - 1;
       } else {
           cur_drv->track += fdctrl->fifo[2];
     }      }
  enqueue:      fdctrl_reset_fifo(fdctrl);
       /* Raise Interrupt */
       fdctrl_raise_irq(fdctrl, FD_SR0_SEEK);
   }
   
   static void fdctrl_handle_relative_seek_in (fdctrl_t *fdctrl, int direction)
   {
       fdrive_t *cur_drv;
   
       SET_CUR_DRV(fdctrl, fdctrl->fifo[1] & FD_DOR_SELMASK);
       cur_drv = get_cur_drv(fdctrl);
       if (fdctrl->fifo[2] > cur_drv->track) {
           cur_drv->track = 0;
       } else {
           cur_drv->track -= fdctrl->fifo[2];
       }
       fdctrl_reset_fifo(fdctrl);
       /* Raise Interrupt */
       fdctrl_raise_irq(fdctrl, FD_SR0_SEEK);
   }
   
   static const struct {
       uint8_t value;
       uint8_t mask;
       const char* name;
       int parameters;
       void (*handler)(fdctrl_t *fdctrl, int direction);
       int direction;
   } handlers[] = {
       { FD_CMD_READ, 0x1f, "READ", 8, fdctrl_start_transfer, FD_DIR_READ },
       { FD_CMD_WRITE, 0x3f, "WRITE", 8, fdctrl_start_transfer, FD_DIR_WRITE },
       { FD_CMD_SEEK, 0xff, "SEEK", 2, fdctrl_handle_seek },
       { FD_CMD_SENSE_INTERRUPT_STATUS, 0xff, "SENSE INTERRUPT STATUS", 0, fdctrl_handle_sense_interrupt_status },
       { FD_CMD_RECALIBRATE, 0xff, "RECALIBRATE", 1, fdctrl_handle_recalibrate },
       { FD_CMD_FORMAT_TRACK, 0xbf, "FORMAT TRACK", 5, fdctrl_handle_format_track },
       { FD_CMD_READ_TRACK, 0xbf, "READ TRACK", 8, fdctrl_start_transfer, FD_DIR_READ },
       { FD_CMD_RESTORE, 0xff, "RESTORE", 17, fdctrl_handle_restore }, /* part of READ DELETED DATA */
       { FD_CMD_SAVE, 0xff, "SAVE", 0, fdctrl_handle_save }, /* part of READ DELETED DATA */
       { FD_CMD_READ_DELETED, 0x1f, "READ DELETED DATA", 8, fdctrl_start_transfer_del, FD_DIR_READ },
       { FD_CMD_SCAN_EQUAL, 0x1f, "SCAN EQUAL", 8, fdctrl_start_transfer, FD_DIR_SCANE },
       { FD_CMD_VERIFY, 0x1f, "VERIFY", 8, fdctrl_unimplemented },
       { FD_CMD_SCAN_LOW_OR_EQUAL, 0x1f, "SCAN LOW OR EQUAL", 8, fdctrl_start_transfer, FD_DIR_SCANL },
       { FD_CMD_SCAN_HIGH_OR_EQUAL, 0x1f, "SCAN HIGH OR EQUAL", 8, fdctrl_start_transfer, FD_DIR_SCANH },
       { FD_CMD_WRITE_DELETED, 0x3f, "WRITE DELETED DATA", 8, fdctrl_start_transfer_del, FD_DIR_WRITE },
       { FD_CMD_READ_ID, 0xbf, "READ ID", 1, fdctrl_handle_readid },
       { FD_CMD_SPECIFY, 0xff, "SPECIFY", 2, fdctrl_handle_specify },
       { FD_CMD_SENSE_DRIVE_STATUS, 0xff, "SENSE DRIVE STATUS", 1, fdctrl_handle_sense_drive_status },
       { FD_CMD_PERPENDICULAR_MODE, 0xff, "PERPENDICULAR MODE", 1, fdctrl_handle_perpendicular_mode },
       { FD_CMD_CONFIGURE, 0xff, "CONFIGURE", 3, fdctrl_handle_configure },
       { FD_CMD_POWERDOWN_MODE, 0xff, "POWERDOWN MODE", 2, fdctrl_handle_powerdown_mode },
       { FD_CMD_OPTION, 0xff, "OPTION", 1, fdctrl_handle_option },
       { FD_CMD_DRIVE_SPECIFICATION_COMMAND, 0xff, "DRIVE SPECIFICATION COMMAND", 5, fdctrl_handle_drive_specification_command },
       { FD_CMD_RELATIVE_SEEK_OUT, 0xff, "RELATIVE SEEK OUT", 2, fdctrl_handle_relative_seek_out },
       { FD_CMD_FORMAT_AND_WRITE, 0xff, "FORMAT AND WRITE", 10, fdctrl_unimplemented },
       { FD_CMD_RELATIVE_SEEK_IN, 0xff, "RELATIVE SEEK IN", 2, fdctrl_handle_relative_seek_in },
       { FD_CMD_LOCK, 0x7f, "LOCK", 0, fdctrl_handle_lock },
       { FD_CMD_DUMPREG, 0xff, "DUMPREG", 0, fdctrl_handle_dumpreg },
       { FD_CMD_VERSION, 0xff, "VERSION", 0, fdctrl_handle_version },
       { FD_CMD_PART_ID, 0xff, "PART ID", 0, fdctrl_handle_partid },
       { FD_CMD_WRITE, 0x1f, "WRITE (BeOS)", 8, fdctrl_start_transfer, FD_DIR_WRITE }, /* not in specification ; BeOS 4.5 bug */
       { 0, 0, "unknown", 0, fdctrl_unimplemented }, /* default handler */
   };
   /* Associate command to an index in the 'handlers' array */
   static uint8_t command_to_handler[256];
   
   static void fdctrl_write_data (fdctrl_t *fdctrl, uint32_t value)
   {
       fdrive_t *cur_drv;
       int pos;
   
       /* Reset mode */
       if (!(fdctrl->dor & FD_DOR_nRESET)) {
           FLOPPY_DPRINTF("Floppy controller in RESET state !\n");
           return;
       }
       if (!(fdctrl->msr & FD_MSR_RQM) || (fdctrl->msr & FD_MSR_DIO)) {
           FLOPPY_ERROR("controller not ready for writing\n");
           return;
       }
       fdctrl->dsr &= ~FD_DSR_PWRDOWN;
       /* Is it write command time ? */
       if (fdctrl->msr & FD_MSR_NONDMA) {
           /* FIFO data write */
           pos = fdctrl->data_pos++;
           pos %= FD_SECTOR_LEN;
           fdctrl->fifo[pos] = value;
           if (pos == FD_SECTOR_LEN - 1 ||
               fdctrl->data_pos == fdctrl->data_len) {
               cur_drv = get_cur_drv(fdctrl);
               if (bdrv_write(cur_drv->bs, fd_sector(cur_drv), fdctrl->fifo, 1) < 0) {
                   FLOPPY_ERROR("writing sector %d\n", fd_sector(cur_drv));
                   return;
               }
               if (!fdctrl_seek_to_next_sect(fdctrl, cur_drv)) {
                   FLOPPY_DPRINTF("error seeking to next sector %d\n",
                                  fd_sector(cur_drv));
                   return;
               }
           }
           /* Switch from transfer mode to status mode
            * then from status mode to command mode
            */
           if (fdctrl->data_pos == fdctrl->data_len)
               fdctrl_stop_transfer(fdctrl, FD_SR0_SEEK, 0x00, 0x00);
           return;
       }
       if (fdctrl->data_pos == 0) {
           /* Command */
           pos = command_to_handler[value & 0xff];
           FLOPPY_DPRINTF("%s command\n", handlers[pos].name);
           fdctrl->data_len = handlers[pos].parameters + 1;
       }
   
     FLOPPY_DPRINTF("%s: %02x\n", __func__, value);      FLOPPY_DPRINTF("%s: %02x\n", __func__, value);
     fdctrl->fifo[fdctrl->data_pos] = value;      fdctrl->fifo[fdctrl->data_pos++] = value;
     if (++fdctrl->data_pos == fdctrl->data_len) {      if (fdctrl->data_pos == fdctrl->data_len) {
         /* We now have all parameters          /* We now have all parameters
          * and will be able to treat the command           * and will be able to treat the command
          */           */
Line 1629  static void fdctrl_write_data (fdctrl_t  Line 1826  static void fdctrl_write_data (fdctrl_t 
             fdctrl_format_sector(fdctrl);              fdctrl_format_sector(fdctrl);
             return;              return;
         }          }
         switch (fdctrl->fifo[0] & 0x1F) {  
         case 0x06:          pos = command_to_handler[fdctrl->fifo[0] & 0xff];
             {          FLOPPY_DPRINTF("treat %s command\n", handlers[pos].name);
                 /* READ variants */          (*handlers[pos].handler)(fdctrl, handlers[pos].direction);
                 FLOPPY_DPRINTF("treat READ command\n");  
                 fdctrl_start_transfer(fdctrl, FD_DIR_READ);  
                 return;  
             }  
         case 0x0C:  
             /* READ_DELETED variants */  
 //            FLOPPY_DPRINTF("treat READ_DELETED command\n");  
             FLOPPY_ERROR("treat READ_DELETED command\n");  
             fdctrl_start_transfer_del(fdctrl, FD_DIR_READ);  
             return;  
         case 0x16:  
             /* VERIFY variants */  
 //            FLOPPY_DPRINTF("treat VERIFY command\n");  
             FLOPPY_ERROR("treat VERIFY command\n");  
             fdctrl_stop_transfer(fdctrl, 0x20, 0x00, 0x00);  
             return;  
         case 0x10:  
             /* SCAN_EQUAL variants */  
 //            FLOPPY_DPRINTF("treat SCAN_EQUAL command\n");  
             FLOPPY_ERROR("treat SCAN_EQUAL command\n");  
             fdctrl_start_transfer(fdctrl, FD_DIR_SCANE);  
             return;  
         case 0x19:  
             /* SCAN_LOW_OR_EQUAL variants */  
 //            FLOPPY_DPRINTF("treat SCAN_LOW_OR_EQUAL command\n");  
             FLOPPY_ERROR("treat SCAN_LOW_OR_EQUAL command\n");  
             fdctrl_start_transfer(fdctrl, FD_DIR_SCANL);  
             return;  
         case 0x1D:  
             /* SCAN_HIGH_OR_EQUAL variants */  
 //            FLOPPY_DPRINTF("treat SCAN_HIGH_OR_EQUAL command\n");  
             FLOPPY_ERROR("treat SCAN_HIGH_OR_EQUAL command\n");  
             fdctrl_start_transfer(fdctrl, FD_DIR_SCANH);  
             return;  
         default:  
             break;  
         }  
         switch (fdctrl->fifo[0] & 0x3F) {  
         case 0x05:  
             /* WRITE variants */  
             FLOPPY_DPRINTF("treat WRITE command (%02x)\n", fdctrl->fifo[0]);  
             fdctrl_start_transfer(fdctrl, FD_DIR_WRITE);  
             return;  
         case 0x09:  
             /* WRITE_DELETED variants */  
 //            FLOPPY_DPRINTF("treat WRITE_DELETED command\n");  
             FLOPPY_ERROR("treat WRITE_DELETED command\n");  
             fdctrl_start_transfer_del(fdctrl, FD_DIR_WRITE);  
             return;  
         default:  
             break;  
         }  
         switch (fdctrl->fifo[0]) {  
         case 0x03:  
             /* SPECIFY */  
             FLOPPY_DPRINTF("treat SPECIFY command\n");  
             fdctrl->timer0 = (fdctrl->fifo[1] >> 4) & 0xF;  
             fdctrl->timer1 = fdctrl->fifo[2] >> 1;  
             fdctrl->dma_en = 1 - (fdctrl->fifo[2] & 1) ;  
             /* No result back */  
             fdctrl_reset_fifo(fdctrl);  
             break;  
         case 0x04:  
             /* SENSE_DRIVE_STATUS */  
             FLOPPY_DPRINTF("treat SENSE_DRIVE_STATUS command\n");  
             fdctrl->cur_drv = fdctrl->fifo[1] & 1;  
             cur_drv = get_cur_drv(fdctrl);  
             cur_drv->head = (fdctrl->fifo[1] >> 2) & 1;  
             /* 1 Byte status back */  
             fdctrl->fifo[0] = (cur_drv->ro << 6) |  
                 (cur_drv->track == 0 ? 0x10 : 0x00) |  
                 (cur_drv->head << 2) |  
                 fdctrl->cur_drv |  
                 0x28;  
             fdctrl_set_fifo(fdctrl, 1, 0);  
             break;  
         case 0x07:  
             /* RECALIBRATE */  
             FLOPPY_DPRINTF("treat RECALIBRATE command\n");  
             fdctrl->cur_drv = fdctrl->fifo[1] & 1;  
             cur_drv = get_cur_drv(fdctrl);  
             fd_recalibrate(cur_drv);  
             fdctrl_reset_fifo(fdctrl);  
             /* Raise Interrupt */  
             fdctrl_raise_irq(fdctrl, 0x20);  
             break;  
         case 0x0F:  
             /* SEEK */  
             FLOPPY_DPRINTF("treat SEEK command\n");  
             fdctrl->cur_drv = fdctrl->fifo[1] & 1;  
             cur_drv = get_cur_drv(fdctrl);  
             fd_start(cur_drv);  
             if (fdctrl->fifo[2] <= cur_drv->track)  
                 cur_drv->dir = 1;  
             else  
                 cur_drv->dir = 0;  
             fdctrl_reset_fifo(fdctrl);  
             if (fdctrl->fifo[2] > cur_drv->max_track) {  
                 fdctrl_raise_irq(fdctrl, 0x60);  
             } else {  
                 cur_drv->track = fdctrl->fifo[2];  
                 /* Raise Interrupt */  
                 fdctrl_raise_irq(fdctrl, 0x20);  
             }  
             break;  
         case 0x12:  
             /* PERPENDICULAR_MODE */  
             FLOPPY_DPRINTF("treat PERPENDICULAR_MODE command\n");  
             if (fdctrl->fifo[1] & 0x80)  
                 cur_drv->perpendicular = fdctrl->fifo[1] & 0x7;  
             /* No result back */  
             fdctrl_reset_fifo(fdctrl);  
             break;  
         case 0x13:  
             /* CONFIGURE */  
             FLOPPY_DPRINTF("treat CONFIGURE command\n");  
             fdctrl->config = fdctrl->fifo[2];  
             fdctrl->precomp_trk =  fdctrl->fifo[3];  
             /* No result back */  
             fdctrl_reset_fifo(fdctrl);  
             break;  
         case 0x17:  
             /* POWERDOWN_MODE */  
             FLOPPY_DPRINTF("treat POWERDOWN_MODE command\n");  
             fdctrl->pwrd = fdctrl->fifo[1];  
             fdctrl->fifo[0] = fdctrl->fifo[1];  
             fdctrl_set_fifo(fdctrl, 1, 1);  
             break;  
         case 0x33:  
             /* OPTION */  
             FLOPPY_DPRINTF("treat OPTION command\n");  
             /* No result back */  
             fdctrl_reset_fifo(fdctrl);  
             break;  
         case 0x42:  
             /* READ_TRACK */  
 //            FLOPPY_DPRINTF("treat READ_TRACK command\n");  
             FLOPPY_ERROR("treat READ_TRACK command\n");  
             fdctrl_start_transfer(fdctrl, FD_DIR_READ);  
             break;  
         case 0x4A:  
             /* READ_ID */  
             FLOPPY_DPRINTF("treat READ_ID command\n");  
             /* XXX: should set main status register to busy */  
             cur_drv->head = (fdctrl->fifo[1] >> 2) & 1;  
             qemu_mod_timer(fdctrl->result_timer,  
                            qemu_get_clock(vm_clock) + (ticks_per_sec / 50));  
             break;  
         case 0x4C:  
             /* RESTORE */  
             FLOPPY_DPRINTF("treat RESTORE command\n");  
             /* Drives position */  
             drv0(fdctrl)->track = fdctrl->fifo[3];  
             drv1(fdctrl)->track = fdctrl->fifo[4];  
             /* timers */  
             fdctrl->timer0 = fdctrl->fifo[7];  
             fdctrl->timer1 = fdctrl->fifo[8];  
             cur_drv->last_sect = fdctrl->fifo[9];  
             fdctrl->lock = fdctrl->fifo[10] >> 7;  
             cur_drv->perpendicular = (fdctrl->fifo[10] >> 2) & 0xF;  
             fdctrl->config = fdctrl->fifo[11];  
             fdctrl->precomp_trk = fdctrl->fifo[12];  
             fdctrl->pwrd = fdctrl->fifo[13];  
             fdctrl_reset_fifo(fdctrl);  
             break;  
         case 0x4D:  
             /* FORMAT_TRACK */  
             FLOPPY_DPRINTF("treat FORMAT_TRACK command\n");  
             fdctrl->cur_drv = fdctrl->fifo[1] & 1;  
             cur_drv = get_cur_drv(fdctrl);  
             fdctrl->data_state |= FD_STATE_FORMAT;  
             if (fdctrl->fifo[0] & 0x80)  
                 fdctrl->data_state |= FD_STATE_MULTI;  
             else  
                 fdctrl->data_state &= ~FD_STATE_MULTI;  
             fdctrl->data_state &= ~FD_STATE_SEEK;  
             cur_drv->bps =  
                 fdctrl->fifo[2] > 7 ? 16384 : 128 << fdctrl->fifo[2];  
 #if 0  
             cur_drv->last_sect =  
                 cur_drv->flags & FDISK_DBL_SIDES ? fdctrl->fifo[3] :  
                 fdctrl->fifo[3] / 2;  
 #else  
             cur_drv->last_sect = fdctrl->fifo[3];  
 #endif  
             /* TODO: implement format using DMA expected by the Bochs BIOS  
              * and Linux fdformat (read 3 bytes per sector via DMA and fill  
              * the sector with the specified fill byte  
              */  
             fdctrl->data_state &= ~FD_STATE_FORMAT;  
             fdctrl_stop_transfer(fdctrl, 0x00, 0x00, 0x00);  
             break;  
         case 0x8E:  
             /* DRIVE_SPECIFICATION_COMMAND */  
             FLOPPY_DPRINTF("treat DRIVE_SPECIFICATION_COMMAND command\n");  
             if (fdctrl->fifo[fdctrl->data_pos - 1] & 0x80) {  
                 /* Command parameters done */  
                 if (fdctrl->fifo[fdctrl->data_pos - 1] & 0x40) {  
                     fdctrl->fifo[0] = fdctrl->fifo[1];  
                     fdctrl->fifo[2] = 0;  
                     fdctrl->fifo[3] = 0;  
                     fdctrl_set_fifo(fdctrl, 4, 1);  
                 } else {  
                     fdctrl_reset_fifo(fdctrl);  
                 }  
             } else if (fdctrl->data_len > 7) {  
                 /* ERROR */  
                 fdctrl->fifo[0] = 0x80 |  
                     (cur_drv->head << 2) | fdctrl->cur_drv;  
                 fdctrl_set_fifo(fdctrl, 1, 1);  
             }  
             break;  
         case 0x8F:  
             /* RELATIVE_SEEK_OUT */  
             FLOPPY_DPRINTF("treat RELATIVE_SEEK_OUT command\n");  
             fdctrl->cur_drv = fdctrl->fifo[1] & 1;  
             cur_drv = get_cur_drv(fdctrl);  
             fd_start(cur_drv);  
             cur_drv->dir = 0;  
             if (fdctrl->fifo[2] + cur_drv->track >= cur_drv->max_track) {  
                 cur_drv->track = cur_drv->max_track - 1;  
             } else {  
                 cur_drv->track += fdctrl->fifo[2];  
             }  
             fdctrl_reset_fifo(fdctrl);  
             fdctrl_raise_irq(fdctrl, 0x20);  
             break;  
         case 0xCD:  
             /* FORMAT_AND_WRITE */  
 //                FLOPPY_DPRINTF("treat FORMAT_AND_WRITE command\n");  
             FLOPPY_ERROR("treat FORMAT_AND_WRITE command\n");  
             fdctrl_unimplemented(fdctrl);  
             break;  
         case 0xCF:  
             /* RELATIVE_SEEK_IN */  
             FLOPPY_DPRINTF("treat RELATIVE_SEEK_IN command\n");  
             fdctrl->cur_drv = fdctrl->fifo[1] & 1;  
             cur_drv = get_cur_drv(fdctrl);  
             fd_start(cur_drv);  
             cur_drv->dir = 1;  
             if (fdctrl->fifo[2] > cur_drv->track) {  
                 cur_drv->track = 0;  
             } else {  
                 cur_drv->track -= fdctrl->fifo[2];  
             }  
             fdctrl_reset_fifo(fdctrl);  
             /* Raise Interrupt */  
             fdctrl_raise_irq(fdctrl, 0x20);  
             break;  
         }  
     }      }
 }  }
   
Line 1900  static void fdctrl_result_timer(void *op Line 1847  static void fdctrl_result_timer(void *op
     }      }
     fdctrl_stop_transfer(fdctrl, 0x00, 0x00, 0x00);      fdctrl_stop_transfer(fdctrl, 0x00, 0x00, 0x00);
 }  }
   
   /* Init functions */
   static fdctrl_t *fdctrl_init_common (qemu_irq irq, int dma_chann,
                                        target_phys_addr_t io_base,
                                        BlockDriverState **fds)
   {
       fdctrl_t *fdctrl;
       int i, j;
   
       /* Fill 'command_to_handler' lookup table */
       for (i = ARRAY_SIZE(handlers) - 1; i >= 0; i--) {
           for (j = 0; j < sizeof(command_to_handler); j++) {
               if ((j & handlers[i].mask) == handlers[i].value)
                   command_to_handler[j] = i;
           }
       }
   
       FLOPPY_DPRINTF("init controller\n");
       fdctrl = qemu_mallocz(sizeof(fdctrl_t));
       fdctrl->fifo = qemu_memalign(512, FD_SECTOR_LEN);
       fdctrl->result_timer = qemu_new_timer(vm_clock,
                                             fdctrl_result_timer, fdctrl);
   
       fdctrl->version = 0x90; /* Intel 82078 controller */
       fdctrl->irq = irq;
       fdctrl->dma_chann = dma_chann;
       fdctrl->io_base = io_base;
       fdctrl->config = FD_CONFIG_EIS | FD_CONFIG_EFIFO; /* Implicit seek, polling & FIFO enabled */
       if (fdctrl->dma_chann != -1) {
           DMA_register_channel(dma_chann, &fdctrl_transfer_handler, fdctrl);
       }
       for (i = 0; i < MAX_FD; i++) {
           fd_init(&fdctrl->drives[i], fds[i]);
       }
       fdctrl_external_reset(fdctrl);
       register_savevm("fdc", io_base, 2, fdc_save, fdc_load, fdctrl);
       qemu_register_reset(fdctrl_external_reset, fdctrl);
       for (i = 0; i < MAX_FD; i++) {
           fd_revalidate(&fdctrl->drives[i]);
       }
   
       return fdctrl;
   }
   
   fdctrl_t *fdctrl_init (qemu_irq irq, int dma_chann, int mem_mapped,
                          target_phys_addr_t io_base,
                          BlockDriverState **fds)
   {
       fdctrl_t *fdctrl;
       int io_mem;
   
       fdctrl = fdctrl_init_common(irq, dma_chann, io_base, fds);
   
       fdctrl->sun4m = 0;
       if (mem_mapped) {
           io_mem = cpu_register_io_memory(0, fdctrl_mem_read, fdctrl_mem_write,
                                           fdctrl);
           cpu_register_physical_memory(io_base, 0x08, io_mem);
       } else {
           register_ioport_read((uint32_t)io_base + 0x01, 5, 1,
                                &fdctrl_read_port, fdctrl);
           register_ioport_read((uint32_t)io_base + 0x07, 1, 1,
                                &fdctrl_read_port, fdctrl);
           register_ioport_write((uint32_t)io_base + 0x01, 5, 1,
                                 &fdctrl_write_port, fdctrl);
           register_ioport_write((uint32_t)io_base + 0x07, 1, 1,
                                 &fdctrl_write_port, fdctrl);
       }
   
       return fdctrl;
   }
   
   fdctrl_t *sun4m_fdctrl_init (qemu_irq irq, target_phys_addr_t io_base,
                                BlockDriverState **fds, qemu_irq *fdc_tc)
   {
       fdctrl_t *fdctrl;
       int io_mem;
   
       fdctrl = fdctrl_init_common(irq, -1, io_base, fds);
       fdctrl->sun4m = 1;
       io_mem = cpu_register_io_memory(0, fdctrl_mem_read_strict,
                                       fdctrl_mem_write_strict,
                                       fdctrl);
       cpu_register_physical_memory(io_base, 0x08, io_mem);
       *fdc_tc = *qemu_allocate_irqs(fdctrl_handle_tc, fdctrl, 1);
   
       return fdctrl;
   }

Removed from v.1.1.1.3  
changed lines
  Added in v.1.1.1.4


unix.superglobalmegacorp.com