git.maemo.org Git - qemu/blob - hw/esp.c

   1 /*
   2  * QEMU ESP/NCR53C9x emulation
   3  *
   4  * Copyright (c) 2005-2006 Fabrice Bellard
   5  *
   6  * Permission is hereby granted, free of charge, to any person obtaining a copy
   7  * of this software and associated documentation files (the "Software"), to deal
   8  * in the Software without restriction, including without limitation the rights
   9  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
  10  * copies of the Software, and to permit persons to whom the Software is
  11  * furnished to do so, subject to the following conditions:
  12  *
  13  * The above copyright notice and this permission notice shall be included in
  14  * all copies or substantial portions of the Software.
  15  *
  16  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  17  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  18  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
  19  * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
  20  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
  21  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
  22  * THE SOFTWARE.
  23  */
  24
  25 #include "hw.h"
  26 #include "scsi-disk.h"
  27 #include "scsi.h"
  28
  29 /* debug ESP card */
  30 //#define DEBUG_ESP
  31
  32 /*
  33  * On Sparc32, this is the ESP (NCR53C90) part of chip STP2000 (Master I/O),
  34  * also produced as NCR89C100. See
  35  * http://www.ibiblio.org/pub/historic-linux/early-ports/Sparc/NCR/NCR89C100.txt
  36  * and
  37  * http://www.ibiblio.org/pub/historic-linux/early-ports/Sparc/NCR/NCR53C9X.txt
  38  */
  39
  40 #ifdef DEBUG_ESP
  41 #define DPRINTF(fmt, args...) \
  42 do { printf("ESP: " fmt , ##args); } while (0)
  43 #else
  44 #define DPRINTF(fmt, args...) do {} while (0)
  45 #endif
  46
  47 #define ESP_REGS 16
  48 #define TI_BUFSZ 32
  49
  50 typedef struct ESPState ESPState;
  51
  52 struct ESPState {
  53     uint32_t it_shift;
  54     qemu_irq irq;
  55     uint8_t rregs[ESP_REGS];
  56     uint8_t wregs[ESP_REGS];
  57     int32_t ti_size;
  58     uint32_t ti_rptr, ti_wptr;
  59     uint8_t ti_buf[TI_BUFSZ];
  60     uint32_t sense;
  61     uint32_t dma;
  62     SCSIDevice *scsi_dev[ESP_MAX_DEVS];
  63     SCSIDevice *current_dev;
  64     uint8_t cmdbuf[TI_BUFSZ];
  65     uint32_t cmdlen;
  66     uint32_t do_cmd;
  67
  68     /* The amount of data left in the current DMA transfer.  */
  69     uint32_t dma_left;
  70     /* The size of the current DMA transfer.  Zero if no transfer is in
  71        progress.  */
  72     uint32_t dma_counter;
  73     uint8_t *async_buf;
  74     uint32_t async_len;
  75
  76     espdma_memory_read_write dma_memory_read;
  77     espdma_memory_read_write dma_memory_write;
  78     void *dma_opaque;
  79 };
  80
  81 #define ESP_TCLO   0x0
  82 #define ESP_TCMID  0x1
  83 #define ESP_FIFO   0x2
  84 #define ESP_CMD    0x3
  85 #define ESP_RSTAT  0x4
  86 #define ESP_WBUSID 0x4
  87 #define ESP_RINTR  0x5
  88 #define ESP_WSEL   0x5
  89 #define ESP_RSEQ   0x6
  90 #define ESP_WSYNTP 0x6
  91 #define ESP_RFLAGS 0x7
  92 #define ESP_WSYNO  0x7
  93 #define ESP_CFG1   0x8
  94 #define ESP_RRES1  0x9
  95 #define ESP_WCCF   0x9
  96 #define ESP_RRES2  0xa
  97 #define ESP_WTEST  0xa
  98 #define ESP_CFG2   0xb
  99 #define ESP_CFG3   0xc
 100 #define ESP_RES3   0xd
 101 #define ESP_TCHI   0xe
 102 #define ESP_RES4   0xf
 103
 104 #define CMD_DMA 0x80
 105 #define CMD_CMD 0x7f
 106
 107 #define CMD_NOP      0x00
 108 #define CMD_FLUSH    0x01
 109 #define CMD_RESET    0x02
 110 #define CMD_BUSRESET 0x03
 111 #define CMD_TI       0x10
 112 #define CMD_ICCS     0x11
 113 #define CMD_MSGACC   0x12
 114 #define CMD_SATN     0x1a
 115 #define CMD_SELATN   0x42
 116 #define CMD_SELATNS  0x43
 117 #define CMD_ENSEL    0x44
 118
 119 #define STAT_DO 0x00
 120 #define STAT_DI 0x01
 121 #define STAT_CD 0x02
 122 #define STAT_ST 0x03
 123 #define STAT_MI 0x06
 124 #define STAT_MO 0x07
 125 #define STAT_PIO_MASK 0x06
 126
 127 #define STAT_TC 0x10
 128 #define STAT_PE 0x20
 129 #define STAT_GE 0x40
 130 #define STAT_INT 0x80
 131
 132 #define INTR_FC 0x08
 133 #define INTR_BS 0x10
 134 #define INTR_DC 0x20
 135 #define INTR_RST 0x80
 136
 137 #define SEQ_0 0x0
 138 #define SEQ_CD 0x4
 139
 140 #define CFG1_RESREPT 0x40
 141
 142 #define CFG2_MASK 0x15
 143
 144 #define TCHI_FAS100A 0x4
 145
 146 static void esp_raise_irq(ESPState *s)
 147 {
 148     if (!(s->rregs[ESP_RSTAT] & STAT_INT)) {
 149         s->rregs[ESP_RSTAT] |= STAT_INT;
 150         qemu_irq_raise(s->irq);
 151     }
 152 }
 153
 154 static void esp_lower_irq(ESPState *s)
 155 {
 156     if (s->rregs[ESP_RSTAT] & STAT_INT) {
 157         s->rregs[ESP_RSTAT] &= ~STAT_INT;
 158         qemu_irq_lower(s->irq);
 159     }
 160 }
 161
 162 static uint32_t get_cmd(ESPState *s, uint8_t *buf)
 163 {
 164     uint32_t dmalen;
 165     int target;
 166
 167     dmalen = s->rregs[ESP_TCLO] | (s->rregs[ESP_TCMID] << 8);
 168     target = s->wregs[ESP_WBUSID] & 7;
 169     DPRINTF("get_cmd: len %d target %d\n", dmalen, target);
 170     if (s->dma) {
 171         s->dma_memory_read(s->dma_opaque, buf, dmalen);
 172     } else {
 173         buf[0] = 0;
 174         memcpy(&buf[1], s->ti_buf, dmalen);
 175         dmalen++;
 176     }
 177
 178     s->ti_size = 0;
 179     s->ti_rptr = 0;
 180     s->ti_wptr = 0;
 181
 182     if (s->current_dev) {
 183         /* Started a new command before the old one finished.  Cancel it.  */
 184         s->current_dev->cancel_io(s->current_dev, 0);
 185         s->async_len = 0;
 186     }
 187
 188     if (target >= ESP_MAX_DEVS || !s->scsi_dev[target]) {
 189         // No such drive
 190         s->rregs[ESP_RSTAT] = 0;
 191         s->rregs[ESP_RINTR] = INTR_DC;
 192         s->rregs[ESP_RSEQ] = SEQ_0;
 193         esp_raise_irq(s);
 194         return 0;
 195     }
 196     s->current_dev = s->scsi_dev[target];
 197     return dmalen;
 198 }
 199
 200 static void do_cmd(ESPState *s, uint8_t *buf)
 201 {
 202     int32_t datalen;
 203     int lun;
 204
 205     DPRINTF("do_cmd: busid 0x%x\n", buf[0]);
 206     lun = buf[0] & 7;
 207     datalen = s->current_dev->send_command(s->current_dev, 0, &buf[1], lun);
 208     s->ti_size = datalen;
 209     if (datalen != 0) {
 210         s->rregs[ESP_RSTAT] = STAT_TC;
 211         s->dma_left = 0;
 212         s->dma_counter = 0;
 213         if (datalen > 0) {
 214             s->rregs[ESP_RSTAT] |= STAT_DI;
 215             s->current_dev->read_data(s->current_dev, 0);
 216         } else {
 217             s->rregs[ESP_RSTAT] |= STAT_DO;
 218             s->current_dev->write_data(s->current_dev, 0);
 219         }
 220     }
 221     s->rregs[ESP_RINTR] = INTR_BS | INTR_FC;
 222     s->rregs[ESP_RSEQ] = SEQ_CD;
 223     esp_raise_irq(s);
 224 }
 225
 226 static void handle_satn(ESPState *s)
 227 {
 228     uint8_t buf[32];
 229     int len;
 230
 231     len = get_cmd(s, buf);
 232     if (len)
 233         do_cmd(s, buf);
 234 }
 235
 236 static void handle_satn_stop(ESPState *s)
 237 {
 238     s->cmdlen = get_cmd(s, s->cmdbuf);
 239     if (s->cmdlen) {
 240         DPRINTF("Set ATN & Stop: cmdlen %d\n", s->cmdlen);
 241         s->do_cmd = 1;
 242         s->rregs[ESP_RSTAT] = STAT_TC | STAT_CD;
 243         s->rregs[ESP_RINTR] = INTR_BS | INTR_FC;
 244         s->rregs[ESP_RSEQ] = SEQ_CD;
 245         esp_raise_irq(s);
 246     }
 247 }
 248
 249 static void write_response(ESPState *s)
 250 {
 251     DPRINTF("Transfer status (sense=%d)\n", s->sense);
 252     s->ti_buf[0] = s->sense;
 253     s->ti_buf[1] = 0;
 254     if (s->dma) {
 255         s->dma_memory_write(s->dma_opaque, s->ti_buf, 2);
 256         s->rregs[ESP_RSTAT] = STAT_TC | STAT_ST;
 257         s->rregs[ESP_RINTR] = INTR_BS | INTR_FC;
 258         s->rregs[ESP_RSEQ] = SEQ_CD;
 259     } else {
 260         s->ti_size = 2;
 261         s->ti_rptr = 0;
 262         s->ti_wptr = 0;
 263         s->rregs[ESP_RFLAGS] = 2;
 264     }
 265     esp_raise_irq(s);
 266 }
 267
 268 static void esp_dma_done(ESPState *s)
 269 {
 270     s->rregs[ESP_RSTAT] |= STAT_TC;
 271     s->rregs[ESP_RINTR] = INTR_BS;
 272     s->rregs[ESP_RSEQ] = 0;
 273     s->rregs[ESP_RFLAGS] = 0;
 274     s->rregs[ESP_TCLO] = 0;
 275     s->rregs[ESP_TCMID] = 0;
 276     esp_raise_irq(s);
 277 }
 278
 279 static void esp_do_dma(ESPState *s)
 280 {
 281     uint32_t len;
 282     int to_device;
 283
 284     to_device = (s->ti_size < 0);
 285     len = s->dma_left;
 286     if (s->do_cmd) {
 287         DPRINTF("command len %d + %d\n", s->cmdlen, len);
 288         s->dma_memory_read(s->dma_opaque, &s->cmdbuf[s->cmdlen], len);
 289         s->ti_size = 0;
 290         s->cmdlen = 0;
 291         s->do_cmd = 0;
 292         do_cmd(s, s->cmdbuf);
 293         return;
 294     }
 295     if (s->async_len == 0) {
 296         /* Defer until data is available.  */
 297         return;
 298     }
 299     if (len > s->async_len) {
 300         len = s->async_len;
 301     }
 302     if (to_device) {
 303         s->dma_memory_read(s->dma_opaque, s->async_buf, len);
 304     } else {
 305         s->dma_memory_write(s->dma_opaque, s->async_buf, len);
 306     }
 307     s->dma_left -= len;
 308     s->async_buf += len;
 309     s->async_len -= len;
 310     if (to_device)
 311         s->ti_size += len;
 312     else
 313         s->ti_size -= len;
 314     if (s->async_len == 0) {
 315         if (to_device) {
 316             // ti_size is negative
 317             s->current_dev->write_data(s->current_dev, 0);
 318         } else {
 319             s->current_dev->read_data(s->current_dev, 0);
 320             /* If there is still data to be read from the device then
 321                complete the DMA operation immeriately.  Otherwise defer
 322                until the scsi layer has completed.  */
 323             if (s->dma_left == 0 && s->ti_size > 0) {
 324                 esp_dma_done(s);
 325             }
 326         }
 327     } else {
 328         /* Partially filled a scsi buffer. Complete immediately.  */
 329         esp_dma_done(s);
 330     }
 331 }
 332
 333 static void esp_command_complete(void *opaque, int reason, uint32_t tag,
 334                                  uint32_t arg)
 335 {
 336     ESPState *s = (ESPState *)opaque;
 337
 338     if (reason == SCSI_REASON_DONE) {
 339         DPRINTF("SCSI Command complete\n");
 340         if (s->ti_size != 0)
 341             DPRINTF("SCSI command completed unexpectedly\n");
 342         s->ti_size = 0;
 343         s->dma_left = 0;
 344         s->async_len = 0;
 345         if (arg)
 346             DPRINTF("Command failed\n");
 347         s->sense = arg;
 348         s->rregs[ESP_RSTAT] = STAT_ST;
 349         esp_dma_done(s);
 350         s->current_dev = NULL;
 351     } else {
 352         DPRINTF("transfer %d/%d\n", s->dma_left, s->ti_size);
 353         s->async_len = arg;
 354         s->async_buf = s->current_dev->get_buf(s->current_dev, 0);
 355         if (s->dma_left) {
 356             esp_do_dma(s);
 357         } else if (s->dma_counter != 0 && s->ti_size <= 0) {
 358             /* If this was the last part of a DMA transfer then the
 359                completion interrupt is deferred to here.  */
 360             esp_dma_done(s);
 361         }
 362     }
 363 }
 364
 365 static void handle_ti(ESPState *s)
 366 {
 367     uint32_t dmalen, minlen;
 368
 369     dmalen = s->rregs[ESP_TCLO] | (s->rregs[ESP_TCMID] << 8);
 370     if (dmalen==0) {
 371       dmalen=0x10000;
 372     }
 373     s->dma_counter = dmalen;
 374
 375     if (s->do_cmd)
 376         minlen = (dmalen < 32) ? dmalen : 32;
 377     else if (s->ti_size < 0)
 378         minlen = (dmalen < -s->ti_size) ? dmalen : -s->ti_size;
 379     else
 380         minlen = (dmalen < s->ti_size) ? dmalen : s->ti_size;
 381     DPRINTF("Transfer Information len %d\n", minlen);
 382     if (s->dma) {
 383         s->dma_left = minlen;
 384         s->rregs[ESP_RSTAT] &= ~STAT_TC;
 385         esp_do_dma(s);
 386     } else if (s->do_cmd) {
 387         DPRINTF("command len %d\n", s->cmdlen);
 388         s->ti_size = 0;
 389         s->cmdlen = 0;
 390         s->do_cmd = 0;
 391         do_cmd(s, s->cmdbuf);
 392         return;
 393     }
 394 }
 395
 396 static void esp_reset(void *opaque)
 397 {
 398     ESPState *s = opaque;
 399
 400     esp_lower_irq(s);
 401
 402     memset(s->rregs, 0, ESP_REGS);
 403     memset(s->wregs, 0, ESP_REGS);
 404     s->rregs[ESP_TCHI] = TCHI_FAS100A; // Indicate fas100a
 405     s->ti_size = 0;
 406     s->ti_rptr = 0;
 407     s->ti_wptr = 0;
 408     s->dma = 0;
 409     s->do_cmd = 0;
 410 }
 411
 412 static void parent_esp_reset(void *opaque, int irq, int level)
 413 {
 414     if (level)
 415         esp_reset(opaque);
 416 }
 417
 418 static uint32_t esp_mem_readb(void *opaque, target_phys_addr_t addr)
 419 {
 420     ESPState *s = opaque;
 421     uint32_t saddr;
 422
 423     saddr = (addr >> s->it_shift) & (ESP_REGS - 1);
 424     DPRINTF("read reg[%d]: 0x%2.2x\n", saddr, s->rregs[saddr]);
 425     switch (saddr) {
 426     case ESP_FIFO:
 427         if (s->ti_size > 0) {
 428             s->ti_size--;
 429             if ((s->rregs[ESP_RSTAT] & STAT_PIO_MASK) == 0) {
 430                 /* Data in/out.  */
 431                 fprintf(stderr, "esp: PIO data read not implemented\n");
 432                 s->rregs[ESP_FIFO] = 0;
 433             } else {
 434                 s->rregs[ESP_FIFO] = s->ti_buf[s->ti_rptr++];
 435             }
 436             esp_raise_irq(s);
 437         }
 438         if (s->ti_size == 0) {
 439             s->ti_rptr = 0;
 440             s->ti_wptr = 0;
 441         }
 442         break;
 443     case ESP_RINTR:
 444         // Clear interrupt/error status bits
 445         s->rregs[ESP_RSTAT] &= ~(STAT_GE | STAT_PE);
 446         esp_lower_irq(s);
 447         break;
 448     default:
 449         break;
 450     }
 451     return s->rregs[saddr];
 452 }
 453
 454 static void esp_mem_writeb(void *opaque, target_phys_addr_t addr, uint32_t val)
 455 {
 456     ESPState *s = opaque;
 457     uint32_t saddr;
 458
 459     saddr = (addr >> s->it_shift) & (ESP_REGS - 1);
 460     DPRINTF("write reg[%d]: 0x%2.2x -> 0x%2.2x\n", saddr, s->wregs[saddr],
 461             val);
 462     switch (saddr) {
 463     case ESP_TCLO:
 464     case ESP_TCMID:
 465         s->rregs[ESP_RSTAT] &= ~STAT_TC;
 466         break;
 467     case ESP_FIFO:
 468         if (s->do_cmd) {
 469             s->cmdbuf[s->cmdlen++] = val & 0xff;
 470         } else if ((s->rregs[ESP_RSTAT] & STAT_PIO_MASK) == 0) {
 471             uint8_t buf;
 472             buf = val & 0xff;
 473             s->ti_size--;
 474             fprintf(stderr, "esp: PIO data write not implemented\n");
 475         } else {
 476             s->ti_size++;
 477             s->ti_buf[s->ti_wptr++] = val & 0xff;
 478         }
 479         break;
 480     case ESP_CMD:
 481         s->rregs[saddr] = val;
 482         if (val & CMD_DMA) {
 483             s->dma = 1;
 484             /* Reload DMA counter.  */
 485             s->rregs[ESP_TCLO] = s->wregs[ESP_TCLO];
 486             s->rregs[ESP_TCMID] = s->wregs[ESP_TCMID];
 487         } else {
 488             s->dma = 0;
 489         }
 490         switch(val & CMD_CMD) {
 491         case CMD_NOP:
 492             DPRINTF("NOP (%2.2x)\n", val);
 493             break;
 494         case CMD_FLUSH:
 495             DPRINTF("Flush FIFO (%2.2x)\n", val);
 496             //s->ti_size = 0;
 497             s->rregs[ESP_RINTR] = INTR_FC;
 498             s->rregs[ESP_RSEQ] = 0;
 499             s->rregs[ESP_RFLAGS] = 0;
 500             break;
 501         case CMD_RESET:
 502             DPRINTF("Chip reset (%2.2x)\n", val);
 503             esp_reset(s);
 504             break;
 505         case CMD_BUSRESET:
 506             DPRINTF("Bus reset (%2.2x)\n", val);
 507             s->rregs[ESP_RINTR] = INTR_RST;
 508             if (!(s->wregs[ESP_CFG1] & CFG1_RESREPT)) {
 509                 esp_raise_irq(s);
 510             }
 511             break;
 512         case CMD_TI:
 513             handle_ti(s);
 514             break;
 515         case CMD_ICCS:
 516             DPRINTF("Initiator Command Complete Sequence (%2.2x)\n", val);
 517             write_response(s);
 518             break;
 519         case CMD_MSGACC:
 520             DPRINTF("Message Accepted (%2.2x)\n", val);
 521             write_response(s);
 522             s->rregs[ESP_RINTR] = INTR_DC;
 523             s->rregs[ESP_RSEQ] = 0;
 524             break;
 525         case CMD_SATN:
 526             DPRINTF("Set ATN (%2.2x)\n", val);
 527             break;
 528         case CMD_SELATN:
 529             DPRINTF("Set ATN (%2.2x)\n", val);
 530             handle_satn(s);
 531             break;
 532         case CMD_SELATNS:
 533             DPRINTF("Set ATN & stop (%2.2x)\n", val);
 534             handle_satn_stop(s);
 535             break;
 536         case CMD_ENSEL:
 537             DPRINTF("Enable selection (%2.2x)\n", val);
 538             break;
 539         default:
 540             DPRINTF("Unhandled ESP command (%2.2x)\n", val);
 541             break;
 542         }
 543         break;
 544     case ESP_WBUSID ... ESP_WSYNO:
 545         break;
 546     case ESP_CFG1:
 547         s->rregs[saddr] = val;
 548         break;
 549     case ESP_WCCF ... ESP_WTEST:
 550         break;
 551     case ESP_CFG2:
 552         s->rregs[saddr] = val & CFG2_MASK;
 553         break;
 554     case ESP_CFG3 ... ESP_RES4:
 555         s->rregs[saddr] = val;
 556         break;
 557     default:
 558         break;
 559     }
 560     s->wregs[saddr] = val;
 561 }
 562
 563 static CPUReadMemoryFunc *esp_mem_read[3] = {
 564     esp_mem_readb,
 565     NULL,
 566     NULL,
 567 };
 568
 569 static CPUWriteMemoryFunc *esp_mem_write[3] = {
 570     esp_mem_writeb,
 571     NULL,
 572     NULL,
 573 };
 574
 575 static void esp_save(QEMUFile *f, void *opaque)
 576 {
 577     ESPState *s = opaque;
 578
 579     qemu_put_buffer(f, s->rregs, ESP_REGS);
 580     qemu_put_buffer(f, s->wregs, ESP_REGS);
 581     qemu_put_be32s(f, (uint32_t *)&s->ti_size);
 582     qemu_put_be32s(f, &s->ti_rptr);
 583     qemu_put_be32s(f, &s->ti_wptr);
 584     qemu_put_buffer(f, s->ti_buf, TI_BUFSZ);
 585     qemu_put_be32s(f, &s->sense);
 586     qemu_put_be32s(f, &s->dma);
 587     qemu_put_buffer(f, s->cmdbuf, TI_BUFSZ);
 588     qemu_put_be32s(f, &s->cmdlen);
 589     qemu_put_be32s(f, &s->do_cmd);
 590     qemu_put_be32s(f, &s->dma_left);
 591     // There should be no transfers in progress, so dma_counter is not saved
 592 }
 593
 594 static int esp_load(QEMUFile *f, void *opaque, int version_id)
 595 {
 596     ESPState *s = opaque;
 597
 598     if (version_id != 3)
 599         return -EINVAL; // Cannot emulate 2
 600
 601     qemu_get_buffer(f, s->rregs, ESP_REGS);
 602     qemu_get_buffer(f, s->wregs, ESP_REGS);
 603     qemu_get_be32s(f, (uint32_t *)&s->ti_size);
 604     qemu_get_be32s(f, &s->ti_rptr);
 605     qemu_get_be32s(f, &s->ti_wptr);
 606     qemu_get_buffer(f, s->ti_buf, TI_BUFSZ);
 607     qemu_get_be32s(f, &s->sense);
 608     qemu_get_be32s(f, &s->dma);
 609     qemu_get_buffer(f, s->cmdbuf, TI_BUFSZ);
 610     qemu_get_be32s(f, &s->cmdlen);
 611     qemu_get_be32s(f, &s->do_cmd);
 612     qemu_get_be32s(f, &s->dma_left);
 613
 614     return 0;
 615 }
 616
 617 void esp_scsi_attach(void *opaque, BlockDriverState *bd, int id)
 618 {
 619     ESPState *s = (ESPState *)opaque;
 620
 621     if (id < 0) {
 622         for (id = 0; id < ESP_MAX_DEVS; id++) {
 623             if (s->scsi_dev[id] == NULL)
 624                 break;
 625         }
 626     }
 627     if (id >= ESP_MAX_DEVS) {
 628         DPRINTF("Bad Device ID %d\n", id);
 629         return;
 630     }
 631     if (s->scsi_dev[id]) {
 632         DPRINTF("Destroying device %d\n", id);
 633         s->scsi_dev[id]->destroy(s->scsi_dev[id]);
 634     }
 635     DPRINTF("Attaching block device %d\n", id);
 636     /* Command queueing is not implemented.  */
 637     s->scsi_dev[id] = scsi_generic_init(bd, 0, esp_command_complete, s);
 638     if (s->scsi_dev[id] == NULL)
 639         s->scsi_dev[id] = scsi_disk_init(bd, 0, esp_command_complete, s);
 640 }
 641
 642 void *esp_init(target_phys_addr_t espaddr, int it_shift,
 643                espdma_memory_read_write dma_memory_read,
 644                espdma_memory_read_write dma_memory_write,
 645                void *dma_opaque, qemu_irq irq, qemu_irq *reset)
 646 {
 647     ESPState *s;
 648     int esp_io_memory;
 649
 650     s = qemu_mallocz(sizeof(ESPState));
 651     if (!s)
 652         return NULL;
 653
 654     s->irq = irq;
 655     s->it_shift = it_shift;
 656     s->dma_memory_read = dma_memory_read;
 657     s->dma_memory_write = dma_memory_write;
 658     s->dma_opaque = dma_opaque;
 659
 660     esp_io_memory = cpu_register_io_memory(0, esp_mem_read, esp_mem_write, s);
 661     cpu_register_physical_memory(espaddr, ESP_REGS << it_shift, esp_io_memory);
 662
 663     esp_reset(s);
 664
 665     register_savevm("esp", espaddr, 3, esp_save, esp_load, s);
 666     qemu_register_reset(esp_reset, s);
 667
 668     *reset = *qemu_allocate_irqs(parent_esp_reset, s, 1);
 669
 670     return s;
 671 }