gwj / at91sam9263 · Commits

Commit baca51faff03df59386c95d9478ede18b5be5ec8

Authored by bellard
1 parent bc51c5c9

updated floppy driver: formatting code, disk geometry auto detect (Jocelyn Mayer) 


git-svn-id: svn://svn.savannah.nongnu.org/qemu/trunk@671 c046a42c-6fe2-441c-8c8c-71466251a162
Inline Side-by-side
Showing 3 changed files with 944 additions and 545 deletions
hw/fdc.c
  View file @baca51f
... ... @@ -54,7 +54,8 @@ typedef enum fdisk_type_t {
54 54 FDRIVE_DISK_288 = 0x01, /* 2.88 MB disk */
55 55 FDRIVE_DISK_144 = 0x02, /* 1.44 MB disk */
56 56 FDRIVE_DISK_720 = 0x03, /* 720 kB disk */
57   - FDRIVE_DISK_NONE = 0x04, /* No disk */
  57 + FDRIVE_DISK_USER = 0x04, /* User defined geometry */
  58 + FDRIVE_DISK_NONE = 0x05, /* No disk */
58 59 } fdisk_type_t;
59 60  
60 61 typedef enum fdrive_type_t {
... ... @@ -64,13 +65,21 @@ typedef enum fdrive_type_t {
64 65 FDRIVE_DRV_NONE = 0x03, /* No drive connected */
65 66 } fdrive_type_t;
66 67  
  68 +typedef enum fdrive_flags_t {
  69 + FDRIVE_MOTOR_ON = 0x01, /* motor on/off */
  70 + FDRIVE_REVALIDATE = 0x02, /* Revalidated */
  71 +} fdrive_flags_t;
  72 +
  73 +typedef enum fdisk_flags_t {
  74 + FDISK_DBL_SIDES = 0x01,
  75 +} fdisk_flags_t;
  76 +
67 77 typedef struct fdrive_t {
68 78 BlockDriverState *bs;
69 79 /* Drive status */
70 80 fdrive_type_t drive;
71   - uint8_t motor; /* on/off */
  81 + fdrive_flags_t drflags;
72 82 uint8_t perpendicular; /* 2.88 MB access mode */
73   - uint8_t rv; /* Revalidated */
74 83 /* Position */
75 84 uint8_t head;
76 85 uint8_t track;
... ... @@ -80,8 +89,10 @@ typedef struct fdrive_t {
80 89 uint8_t rw; /* Read/write */
81 90 /* Media */
82 91 fdisk_type_t disk; /* Disk type */
  92 + fdisk_flags_t flags;
83 93 uint8_t last_sect; /* Nb sector per track */
84 94 uint8_t max_track; /* Nb of tracks */
  95 + uint16_t bps; /* Bytes per sector */
85 96 uint8_t ro; /* Is read-only */
86 97 } fdrive_t;
87 98  
... ... @@ -93,12 +104,11 @@ static void fd_init (fdrive_t *drv, BlockDriverState *bs)
93 104 drv->drive = FDRIVE_DRV_144;
94 105 else
95 106 drv->drive = FDRIVE_DRV_NONE;
96   - drv->motor = 0;
  107 + drv->drflags = 0;
97 108 drv->perpendicular = 0;
98   - drv->rv = 0;
99 109 /* Disk */
100 110 drv->disk = FDRIVE_DISK_NONE;
101   - drv->last_sect = 1;
  111 + drv->last_sect = 0;
102 112 drv->max_track = 0;
103 113 }
104 114  
... ... @@ -118,18 +128,25 @@ static int fd_seek (fdrive_t *drv, uint8_t head, uint8_t track, uint8_t sect,
118 128 int enable_seek)
119 129 {
120 130 uint32_t sector;
121   -
122   - if (track > drv->max_track) {
123   - FLOPPY_ERROR("try to read %d %02x %02x (max=%d %02x %02x)\n",
124   - head, track, sect, 1, drv->max_track, drv->last_sect);
  131 + int ret;
  132 +
  133 + if (track > drv->max_track ||
  134 + (head != 0 && (drv->flags & FDISK_DBL_SIDES) == 0)) {
  135 + FLOPPY_ERROR("try to read %d %02x %02x (max=%d %d %02x %02x)\n",
  136 + head, track, sect, 1,
  137 + (drv->flags & FDISK_DBL_SIDES) == 0 ? 0 : 1,
  138 + drv->max_track, drv->last_sect);
125 139 return 2;
126 140 }
127 141 if (sect > drv->last_sect) {
128   - FLOPPY_ERROR("try to read %d %02x %02x (max=%d %02x %02x)\n",
129   - head, track, sect, 1, drv->max_track, drv->last_sect);
  142 + FLOPPY_ERROR("try to read %d %02x %02x (max=%d %d %02x %02x)\n",
  143 + head, track, sect, 1,
  144 + (drv->flags & FDISK_DBL_SIDES) == 0 ? 0 : 1,
  145 + drv->max_track, drv->last_sect);
130 146 return 3;
131 147 }
132 148 sector = _fd_sector(head, track, sect, drv->last_sect);
  149 + ret = 0;
133 150 if (sector != fd_sector(drv)) {
134 151 #if 0
135 152 if (!enable_seek) {
... ... @@ -139,12 +156,13 @@ static int fd_seek (fdrive_t *drv, uint8_t head, uint8_t track, uint8_t sect,
139 156 }
140 157 #endif
141 158 drv->head = head;
  159 + if (drv->track != track)
  160 + ret = 1;
142 161 drv->track = track;
143 162 drv->sect = sect;
144   - return 1;
145 163 }
146 164  
147   - return 0;
  165 + return ret;
148 166 }
149 167  
150 168 /* Set drive back to track 0 */
... ... @@ -162,60 +180,275 @@ static void fd_recalibrate (fdrive_t *drv)
162 180 static void fd_revalidate (fdrive_t *drv)
163 181 {
164 182 int64_t nb_sectors;
  183 + int nb_heads, max_track, last_sect, ro;
165 184  
166 185 FLOPPY_DPRINTF("revalidate\n");
167   - drv->rv = 0;
  186 + drv->drflags &= ~FDRIVE_REVALIDATE;
  187 +
168 188 /* if no drive present, cannot do more */
169 189 if (!drv->bs)
170 190 return;
171   -
  191 +
172 192 if (bdrv_is_inserted(drv->bs)) {
  193 + ro = bdrv_is_read_only(drv->bs);
  194 + bdrv_get_geometry_hint(drv->bs, &max_track, &nb_heads, &last_sect);
  195 + if (nb_heads != 0 && max_track != 0 && last_sect != 0) {
  196 + drv->disk = FDRIVE_DISK_USER;
  197 + printf("User defined disk (%d %d %d)",
  198 + nb_heads - 1, max_track, last_sect);
  199 + if (nb_heads == 1) {
  200 + drv->flags &= ~FDISK_DBL_SIDES;
  201 + } else {
  202 + drv->flags |= FDISK_DBL_SIDES;
  203 + }
  204 + drv->max_track = max_track;
  205 + drv->last_sect = last_sect;
  206 + } else {
173 207 bdrv_get_geometry(drv->bs, &nb_sectors);
174   -#if 1
175   - if (nb_sectors > 2880)
176   -#endif
177   - {
178   - /* Pretend we have a 2.88 MB disk */
  208 + switch (nb_sectors) {
  209 + /* 2.88 MB 3"1/2 drive disks */
  210 + case 7680:
  211 + printf("3.84 Mb 3\"1/2 disk (1 80 48)");
  212 + drv->drive = FDRIVE_DRV_288;
  213 + drv->disk = FDRIVE_DISK_288;
  214 + drv->last_sect = 48;
  215 + drv->max_track = 80;
  216 + drv->flags |= FDISK_DBL_SIDES;
  217 + break;
  218 + case 7040:
  219 + printf("3.52 Mb 3\"1/2 disk (1 80 44)");
  220 + drv->drive = FDRIVE_DRV_288;
  221 + drv->disk = FDRIVE_DISK_288;
  222 + drv->last_sect = 44;
  223 + drv->max_track = 80;
  224 + drv->flags |= FDISK_DBL_SIDES;
  225 + break;
  226 + case 6400:
  227 + printf("3.2 Mb 3\"1/2 disk (1 80 40)");
  228 + drv->drive = FDRIVE_DRV_288;
  229 + drv->disk = FDRIVE_DISK_288;
  230 + drv->last_sect = 40;
  231 + drv->max_track = 80;
  232 + drv->flags |= FDISK_DBL_SIDES;
  233 + break;
  234 + case 6240:
  235 + printf("3.12 Mb 3\"1/2 disk (1 80 39)");
  236 + drv->drive = FDRIVE_DRV_288;
  237 + drv->disk = FDRIVE_DISK_288;
  238 + drv->last_sect = 39;
  239 + drv->max_track = 80;
  240 + drv->flags |= FDISK_DBL_SIDES;
  241 + break;
  242 + case 5760:
  243 + printf("2.88 Mb 3\"1/2 disk (1 80 36)");
  244 + drv->drive = FDRIVE_DRV_288;
179 245 drv->disk = FDRIVE_DISK_288;
180 246 drv->last_sect = 36;
181 247 drv->max_track = 80;
182   -#if 1
183   - } else if (nb_sectors > 1440) {
184   - /* Pretend we have a 1.44 MB disk */
  248 + drv->flags |= FDISK_DBL_SIDES;
  249 + break;
  250 +
  251 + /* 1.44 MB 3"1/2 drive disks */
  252 + case 3840:
  253 + printf("1.92 Mb 3\"1/2 disk (1 80 24)");
  254 + drv->drive = FDRIVE_DRV_144;
  255 + drv->disk = FDRIVE_DISK_144;
  256 + drv->last_sect = 24;
  257 + drv->max_track = 80;
  258 + drv->flags |= FDISK_DBL_SIDES;
  259 + break;
  260 + case 3680:
  261 + printf("1.84 Mb 3\"1/2 disk (1 80 23)");
  262 + drv->drive = FDRIVE_DRV_144;
  263 + drv->disk = FDRIVE_DISK_144;
  264 + drv->last_sect = 23;
  265 + drv->max_track = 80;
  266 + drv->flags |= FDISK_DBL_SIDES;
  267 + break;
  268 + case 3520:
  269 + printf("1.76 Mb 3\"1/2 disk (1 80 22)");
  270 + drv->drive = FDRIVE_DRV_144;
  271 + drv->disk = FDRIVE_DISK_144;
  272 + drv->last_sect = 22;
  273 + drv->max_track = 80;
  274 + drv->flags |= FDISK_DBL_SIDES;
  275 + break;
  276 + case 3486:
  277 + printf("1.74 Mb 3\"1/2 disk (1 83 21)");
  278 + drv->drive = FDRIVE_DRV_144;
  279 + drv->disk = FDRIVE_DISK_144;
  280 + drv->last_sect = 21;
  281 + drv->max_track = 83;
  282 + drv->flags |= FDISK_DBL_SIDES;
  283 + break;
  284 + case 3444:
  285 + printf("1.72 Mb 3\"1/2 disk (1 82 21)");
  286 + drv->drive = FDRIVE_DRV_144;
  287 + drv->disk = FDRIVE_DISK_144;
  288 + drv->last_sect = 21;
  289 + drv->max_track = 82;
  290 + drv->flags |= FDISK_DBL_SIDES;
  291 + break;
  292 + case 3360:
  293 + printf("1.68 Mb 3\"1/2 disk (1 80 21)");
  294 + drv->drive = FDRIVE_DRV_144;
  295 + drv->disk = FDRIVE_DISK_144;
  296 + drv->last_sect = 21;
  297 + drv->max_track = 80;
  298 + drv->flags |= FDISK_DBL_SIDES;
  299 + break;
  300 + case 3200:
  301 + printf("1.6 Mb 3\"1/2 disk (1 80 20)");
  302 + drv->drive = FDRIVE_DRV_144;
  303 + drv->disk = FDRIVE_DISK_144;
  304 + drv->last_sect = 20;
  305 + drv->max_track = 80;
  306 + drv->flags |= FDISK_DBL_SIDES;
  307 + break;
  308 + case 2880:
  309 + default:
  310 + printf("1.44 Mb 3\"1/2 disk (1 80 18)");
  311 + drv->drive = FDRIVE_DRV_144;
185 312 drv->disk = FDRIVE_DISK_144;
186 313 drv->last_sect = 18;
187 314 drv->max_track = 80;
188   - } else {
189   - /* Pretend we have a 720 kB disk */
  315 + drv->flags |= FDISK_DBL_SIDES;
  316 + break;
  317 +
  318 + /* 720 kB 3"1/2 drive disks */
  319 + case 2240:
  320 + printf("1.12 Mb 3\"1/2 disk (1 80 14)");
  321 + drv->drive = FDRIVE_DRV_144;
  322 + drv->disk = FDRIVE_DISK_720;
  323 + drv->last_sect = 14;
  324 + drv->max_track = 80;
  325 + drv->flags |= FDISK_DBL_SIDES;
  326 + break;
  327 + case 2080:
  328 + printf("1.04 Mb 3\"1/2 disk (1 80 13)");
  329 + drv->drive = FDRIVE_DRV_144;
  330 + drv->disk = FDRIVE_DISK_720;
  331 + drv->last_sect = 13;
  332 + drv->max_track = 80;
  333 + drv->flags |= FDISK_DBL_SIDES;
  334 + break;
  335 + case 1660:
  336 + printf("830 kb 3\"1/2 disk (1 83 10)");
  337 + drv->drive = FDRIVE_DRV_144;
  338 + drv->disk = FDRIVE_DISK_720;
  339 + drv->last_sect = 10;
  340 + drv->max_track = 83;
  341 + drv->flags |= FDISK_DBL_SIDES;
  342 + break;
  343 + case 1640:
  344 + printf("820 kb 3\"1/2 disk (1 82 10)");
  345 + drv->drive = FDRIVE_DRV_144;
  346 + drv->disk = FDRIVE_DISK_720;
  347 + drv->last_sect = 10;
  348 + drv->max_track = 82;
  349 + drv->flags |= FDISK_DBL_SIDES;
  350 + break;
  351 + case 1600:
  352 + printf("800 kb 3\"1/2 disk (1 80 10)");
  353 + drv->drive = FDRIVE_DRV_144;
  354 + drv->disk = FDRIVE_DISK_720;
  355 + drv->last_sect = 10;
  356 + drv->max_track = 80;
  357 + drv->flags |= FDISK_DBL_SIDES;
  358 + break;
  359 + case 1440:
  360 + printf("720 kb 3\"1/2 disk (1 80 9)");
  361 + drv->drive = FDRIVE_DRV_144;
190 362 drv->disk = FDRIVE_DISK_720;
191 363 drv->last_sect = 9;
192 364 drv->max_track = 80;
193   -#endif
  365 + drv->flags |= FDISK_DBL_SIDES;
  366 + break;
  367 +
  368 + /* 1.2 MB 5"1/4 drive disks */
  369 + case 2988:
  370 + printf("1.49 Mb 5\"1/4 disk (1 83 18)");
  371 + drv->drive = FDRIVE_DRV_120;
  372 + drv->disk = FDRIVE_DISK_144; /* ? */
  373 + drv->last_sect = 18;
  374 + drv->max_track = 83;
  375 + drv->flags |= FDISK_DBL_SIDES;
  376 + break;
  377 + case 2952:
  378 + printf("1.48 Mb 5\"1/4 disk (1 82 18)");
  379 + drv->drive = FDRIVE_DRV_120;
  380 + drv->disk = FDRIVE_DISK_144; /* ? */
  381 + drv->last_sect = 18;
  382 + drv->max_track = 82;
  383 + drv->flags |= FDISK_DBL_SIDES;
  384 + break;
  385 + case 2400:
  386 + printf("1.2 Mb 5\"1/4 disk (1 80 15)");
  387 + drv->drive = FDRIVE_DRV_120;
  388 + drv->disk = FDRIVE_DISK_144; /* ? */
  389 + drv->last_sect = 15;
  390 + drv->max_track = 80;
  391 + drv->flags |= FDISK_DBL_SIDES;
  392 + break;
  393 +
  394 + case 1760:
  395 + printf("880 kb 5\"1/4 disk (1 80 11)");
  396 + drv->drive = FDRIVE_DRV_120;
  397 + drv->disk = FDRIVE_DISK_144; /* ? */
  398 + drv->last_sect = 11;
  399 + drv->max_track = 80;
  400 + drv->flags |= FDISK_DBL_SIDES;
  401 + break;
  402 +
  403 + /* 360 kB 5"1/4 drive disks */
  404 + case 840:
  405 + /* 420 kB 5"1/4 disk */
  406 + printf("420 kb 5\"1/4 disk (1 42 10)");
  407 + drv->drive = FDRIVE_DRV_120;
  408 + drv->disk = FDRIVE_DISK_144; /* ? */
  409 + drv->last_sect = 10;
  410 + drv->max_track = 42;
  411 + drv->flags |= FDISK_DBL_SIDES;
  412 + case 820:
  413 + /* 410 kB 5"1/4 disk */
  414 + printf("410 kb 5\"1/4 disk (1 41 10)");
  415 + drv->drive = FDRIVE_DRV_120;
  416 + drv->disk = FDRIVE_DISK_144; /* ? */
  417 + drv->last_sect = 10;
  418 + drv->max_track = 41;
  419 + drv->flags |= FDISK_DBL_SIDES;
  420 + case 720:
  421 + /* 360 kB 5"1/4 disk */
  422 + printf("360 kb 5\"1/4 disk (1 40 9)");
  423 + drv->drive = FDRIVE_DRV_120;
  424 + drv->disk = FDRIVE_DISK_144; /* ? */
  425 + drv->last_sect = 9;
  426 + drv->max_track = 40;
  427 + drv->flags |= FDISK_DBL_SIDES;
  428 + break;
194 429 }
195   - drv->ro = bdrv_is_read_only(drv->bs);
  430 + printf(" %s\n", ro == 0 ? "rw" : "ro");
  431 + }
  432 + drv->ro = ro;
196 433 } else {
  434 + printf("No disk in drive\n");
197 435 drv->disk = FDRIVE_DISK_NONE;
198   - drv->last_sect = 1; /* Avoid eventual divide by 0 bugs */
199   - drv->ro = 0;
  436 + drv->last_sect = 0;
  437 + drv->max_track = 0;
  438 + drv->flags &= ~FDISK_DBL_SIDES;
200 439 }
201   - drv->rv = 1;
202   -}
203   -
204   -static void fd_change_cb (void *opaque)
205   -{
206   - fdrive_t *drv = opaque;
207   - fd_revalidate(drv);
  440 + drv->drflags |= FDRIVE_REVALIDATE;
208 441 }
209 442  
210 443 /* Motor control */
211 444 static void fd_start (fdrive_t *drv)
212 445 {
213   - drv->motor = 1;
  446 + drv->drflags |= FDRIVE_MOTOR_ON;
214 447 }
215 448  
216 449 static void fd_stop (fdrive_t *drv)
217 450 {
218   - drv->motor = 0;
  451 + drv->drflags &= ~FDRIVE_MOTOR_ON;
219 452 }
220 453  
221 454 /* Re-initialise a drives (motor off, repositioned) */
... ... @@ -228,21 +461,21 @@ static void fd_reset (fdrive_t *drv)
228 461 /********************************************************/
229 462 /* Intel 82078 floppy disk controler emulation */
230 463  
231   -static void fdctrl_reset (int do_irq);
232   -static void fdctrl_reset_fifo (void);
  464 +static void fdctrl_reset (fdctrl_t *fdctrl, int do_irq);
  465 +static void fdctrl_reset_fifo (fdctrl_t *fdctrl);
233 466 static int fdctrl_transfer_handler (void *opaque, target_ulong addr, int size);
234   -static void fdctrl_raise_irq (uint8_t status);
235   -
236   -static uint32_t fdctrl_read_statusB (void *opaque, uint32_t reg);
237   -static uint32_t fdctrl_read_dor (void *opaque, uint32_t reg);
238   -static void fdctrl_write_dor (void *opaque, uint32_t reg, uint32_t value);
239   -static uint32_t fdctrl_read_tape (void *opaque, uint32_t reg);
240   -static void fdctrl_write_tape (void *opaque, uint32_t reg, uint32_t value);
241   -static uint32_t fdctrl_read_main_status (void *opaque, uint32_t reg);
242   -static void fdctrl_write_rate (void *opaque, uint32_t reg, uint32_t value);
243   -static uint32_t fdctrl_read_data (void *opaque, uint32_t reg);
244   -static void fdctrl_write_data (void *opaque, uint32_t reg, uint32_t value);
245   -static uint32_t fdctrl_read_dir (void *opaque, uint32_t reg);
  467 +static void fdctrl_raise_irq (fdctrl_t *fdctrl, uint8_t status);
  468 +
  469 +static uint32_t fdctrl_read_statusB (fdctrl_t *fdctrl);
  470 +static uint32_t fdctrl_read_dor (fdctrl_t *fdctrl);
  471 +static void fdctrl_write_dor (fdctrl_t *fdctrl, uint32_t value);
  472 +static uint32_t fdctrl_read_tape (fdctrl_t *fdctrl);
  473 +static void fdctrl_write_tape (fdctrl_t *fdctrl, uint32_t value);
  474 +static uint32_t fdctrl_read_main_status (fdctrl_t *fdctrl);
  475 +static void fdctrl_write_rate (fdctrl_t *fdctrl, uint32_t value);
  476 +static uint32_t fdctrl_read_data (fdctrl_t *fdctrl);
  477 +static void fdctrl_write_data (fdctrl_t *fdctrl, uint32_t value);
  478 +static uint32_t fdctrl_read_dir (fdctrl_t *fdctrl);
246 479  
247 480 enum {
248 481 FD_CTRL_ACTIVE = 0x01,
... ... @@ -267,18 +500,24 @@ enum {
267 500 FD_STATE_STATE = 0x03,
268 501 FD_STATE_MULTI = 0x10,
269 502 FD_STATE_SEEK = 0x20,
  503 + FD_STATE_FORMAT = 0x40,
270 504 };
271 505  
272 506 #define FD_STATE(state) ((state) & FD_STATE_STATE)
  507 +#define FD_SET_STATE(state, new_state) \
  508 +do { (state) = ((state) & ~FD_STATE_STATE) | (new_state); } while (0)
273 509 #define FD_MULTI_TRACK(state) ((state) & FD_STATE_MULTI)
274 510 #define FD_DID_SEEK(state) ((state) & FD_STATE_SEEK)
  511 +#define FD_FORMAT_CMD(state) ((state) & FD_STATE_FORMAT)
275 512  
276   -typedef struct fdctrl_t {
  513 +struct fdctrl_t {
  514 + fdctrl_t *fdctrl;
277 515 /* Controler's identification */
278 516 uint8_t version;
279 517 /* HW */
280 518 int irq_lvl;
281 519 int dma_chann;
  520 + uint32_t io_base;
282 521 /* Controler state */
283 522 uint8_t state;
284 523 uint8_t dma_en;
... ... @@ -303,35 +542,91 @@ typedef struct fdctrl_t {
303 542 uint8_t pwrd;
304 543 /* Floppy drives */
305 544 fdrive_t drives[2];
306   -} fdctrl_t;
  545 +};
  546 +
  547 +static uint32_t fdctrl_read (void *opaque, uint32_t reg)
  548 +{
  549 + fdctrl_t *fdctrl = opaque;
  550 + uint32_t retval;
  551 +
  552 + if (reg == fdctrl->io_base + 0x01)
  553 + retval = fdctrl_read_statusB(fdctrl);
  554 + else if (reg == fdctrl->io_base + 0x02)
  555 + retval = fdctrl_read_dor(fdctrl);
  556 + else if (reg == fdctrl->io_base + 0x03)
  557 + retval = fdctrl_read_tape(fdctrl);
  558 + else if (reg == fdctrl->io_base + 0x04)
  559 + retval = fdctrl_read_main_status(fdctrl);
  560 + else if (reg == fdctrl->io_base + 0x05)
  561 + retval = fdctrl_read_data(fdctrl);
  562 + else if (reg == fdctrl->io_base + 0x07)
  563 + retval = fdctrl_read_dir(fdctrl);
  564 + else
  565 + retval = (uint32_t)(-1);
  566 +
  567 + return retval;
  568 +}
  569 +
  570 +static void fdctrl_write (void *opaque, uint32_t reg, uint32_t value)
  571 +{
  572 + fdctrl_t *fdctrl = opaque;
  573 +
  574 + if (reg == fdctrl->io_base + 0x02)
  575 + fdctrl_write_dor(fdctrl, value);
  576 + else if (reg == fdctrl->io_base + 0x03)
  577 + fdctrl_write_tape(fdctrl, value);
  578 + else if (reg == fdctrl->io_base + 0x04)
  579 + fdctrl_write_rate(fdctrl, value);
  580 + else if (reg == fdctrl->io_base + 0x05)
  581 + fdctrl_write_data(fdctrl, value);
  582 +}
  583 +
  584 +static void fd_change_cb (void *opaque)
  585 +{
  586 + fdrive_t *drv = opaque;
307 587  
308   -static fdctrl_t fdctrl;
  588 + FLOPPY_DPRINTF("disk change\n");
  589 + /* TODO: use command-line parameters to force geometry */
  590 + fd_revalidate(drv);
  591 +#if 0
  592 + fd_recalibrate(drv);
  593 + fdctrl_reset_fifo(drv->fdctrl);
  594 + fdctrl_raise_irq(drv->fdctrl, 0x20);
  595 +#endif
  596 +}
309 597  
310   -void fdctrl_init (int irq_lvl, int dma_chann, int mem_mapped, uint32_t base,
311   - BlockDriverState **fds)
  598 +fdctrl_t *fdctrl_init (int irq_lvl, int dma_chann, int mem_mapped,
  599 + uint32_t io_base,
  600 + BlockDriverState **fds)
312 601 {
  602 + fdctrl_t *fdctrl;
313 603 // int io_mem;
314 604 int i;
315 605  
316 606 FLOPPY_DPRINTF("init controler\n");
317   - memset(&fdctrl, 0, sizeof(fdctrl));
318   - fdctrl.version = 0x90; /* Intel 82078 controler */
319   - fdctrl.irq_lvl = irq_lvl;
320   - fdctrl.dma_chann = dma_chann;
321   - fdctrl.config = 0x40; /* Implicit seek, polling & FIFO enabled */
322   - if (fdctrl.dma_chann != -1) {
323   - fdctrl.dma_en = 1;
324   - DMA_register_channel(dma_chann, fdctrl_transfer_handler, &fdctrl);
  607 + fdctrl = qemu_mallocz(sizeof(fdctrl_t));
  608 + if (!fdctrl)
  609 + return NULL;
  610 + fdctrl->version = 0x90; /* Intel 82078 controler */
  611 + fdctrl->irq_lvl = irq_lvl;
  612 + fdctrl->dma_chann = dma_chann;
  613 + fdctrl->io_base = io_base;
  614 + fdctrl->config = 0x40; /* Implicit seek, polling & FIFO enabled */
  615 + if (fdctrl->dma_chann != -1) {
  616 + fdctrl->dma_en = 1;
  617 + DMA_register_channel(dma_chann, &fdctrl_transfer_handler, fdctrl);
325 618 } else {
326   - fdctrl.dma_en = 0;
  619 + fdctrl->dma_en = 0;
327 620 }
328   - for (i = 0; i < MAX_FD; i++) {
329   - fd_init(&fdctrl.drives[i], fds[i]);
330   - if (fds[i])
331   - bdrv_set_change_cb(fds[i], fd_change_cb, &fdctrl.drives[i]);
  621 + for (i = 0; i < 2; i++) {
  622 + fd_init(&fdctrl->drives[i], fds[i]);
  623 + if (fds[i]) {
  624 + bdrv_set_change_cb(fds[i],
  625 + &fd_change_cb, &fdctrl->drives[i]);
  626 + }
332 627 }
333   - fdctrl_reset(0);
334   - fdctrl.state = FD_CTRL_ACTIVE;
  628 + fdctrl_reset(fdctrl, 0);
  629 + fdctrl->state = FD_CTRL_ACTIVE;
335 630 if (mem_mapped) {
336 631 FLOPPY_ERROR("memory mapped floppy not supported by now !\n");
337 632 #if 0
... ... @@ -339,110 +634,113 @@ void fdctrl_init (int irq_lvl, int dma_chann, int mem_mapped, uint32_t base,
339 634 cpu_register_physical_memory(base, 0x08, io_mem);
340 635 #endif
341 636 } else {
342   - register_ioport_read(base + 0x01, 1, 1, fdctrl_read_statusB, NULL);
343   - register_ioport_read(base + 0x02, 1, 1, fdctrl_read_dor, NULL);
344   - register_ioport_write(base + 0x02, 1, 1, fdctrl_write_dor, NULL);
345   - register_ioport_read(base + 0x03, 1, 1, fdctrl_read_tape, NULL);
346   - register_ioport_write(base + 0x03, 1, 1, fdctrl_write_tape, NULL);
347   - register_ioport_read(base + 0x04, 1, 1, fdctrl_read_main_status, NULL);
348   - register_ioport_write(base + 0x04, 1, 1, fdctrl_write_rate, NULL);
349   - register_ioport_read(base + 0x05, 1, 1, fdctrl_read_data, NULL);
350   - register_ioport_write(base + 0x05, 1, 1, fdctrl_write_data, NULL);
351   - register_ioport_read(base + 0x07, 1, 1, fdctrl_read_dir, NULL);
  637 + register_ioport_read(io_base + 0x01, 5, 1, &fdctrl_read, fdctrl);
  638 + register_ioport_read(io_base + 0x07, 1, 1, &fdctrl_read, fdctrl);
  639 + register_ioport_write(io_base + 0x01, 5, 1, &fdctrl_write, fdctrl);
  640 + register_ioport_write(io_base + 0x07, 1, 1, &fdctrl_write, fdctrl);
352 641 }
353   - fdctrl.bootsel = 0;
354   -
355 642 for (i = 0; i < MAX_FD; i++) {
356   - fd_revalidate(&fdctrl.drives[i]);
  643 + fd_revalidate(&fdctrl->drives[i]);
357 644 }
  645 + return fdctrl;
358 646 }
359 647  
360   -int fdctrl_get_drive_type(int drive_num)
  648 +/* XXX: may change if moved to bdrv */
  649 +int fdctrl_get_drive_type(fdctrl_t *fdctrl, int drive_num)
361 650 {
362   - return fdctrl.drives[drive_num].drive;
  651 + return fdctrl->drives[drive_num].drive;
363 652 }
364 653  
365 654 /* Change IRQ state */
366   -static void fdctrl_reset_irq (void)
  655 +static void fdctrl_reset_irq (fdctrl_t *fdctrl)
367 656 {
368   - if (fdctrl.state & FD_CTRL_INTR) {
369   - pic_set_irq(fdctrl.irq_lvl, 0);
370   - fdctrl.state &= ~(FD_CTRL_INTR | FD_CTRL_SLEEP | FD_CTRL_BUSY);
  657 + if (fdctrl->state & FD_CTRL_INTR) {
  658 + pic_set_irq(fdctrl->irq_lvl, 0);
  659 + fdctrl->state &= ~(FD_CTRL_INTR | FD_CTRL_SLEEP | FD_CTRL_BUSY);
371 660 }
372 661 }
373 662  
374   -static void fdctrl_raise_irq (uint8_t status)
  663 +static void fdctrl_raise_irq (fdctrl_t *fdctrl, uint8_t status)
375 664 {
376   - if (~(fdctrl.state & FD_CTRL_INTR)) {
377   - pic_set_irq(fdctrl.irq_lvl, 1);
378   - fdctrl.state |= FD_CTRL_INTR;
  665 + if (~(fdctrl->state & FD_CTRL_INTR)) {
  666 + pic_set_irq(fdctrl->irq_lvl, 1);
  667 + fdctrl->state |= FD_CTRL_INTR;
379 668 }
380 669 FLOPPY_DPRINTF("Set interrupt status to 0x%02x\n", status);
381   - fdctrl.int_status = status;
  670 + fdctrl->int_status = status;
382 671 }
383 672  
384 673 /* Reset controler */
385   -static void fdctrl_reset (int do_irq)
  674 +static void fdctrl_reset (fdctrl_t *fdctrl, int do_irq)
386 675 {
387 676 int i;
388 677  
389 678 FLOPPY_DPRINTF("reset controler\n");
390   - fdctrl_reset_irq();
  679 + fdctrl_reset_irq(fdctrl);
391 680 /* Initialise controler */
392   - fdctrl.cur_drv = 0;
  681 + fdctrl->cur_drv = 0;
393 682 /* FIFO state */
394   - fdctrl.data_pos = 0;
395   - fdctrl.data_len = 0;
396   - fdctrl.data_state = FD_STATE_CMD;
397   - fdctrl.data_dir = FD_DIR_WRITE;
  683 + fdctrl->data_pos = 0;
  684 + fdctrl->data_len = 0;
  685 + fdctrl->data_state = FD_STATE_CMD;
  686 + fdctrl->data_dir = FD_DIR_WRITE;
398 687 for (i = 0; i < MAX_FD; i++)
399   - fd_reset(&fdctrl.drives[i]);
400   - fdctrl_reset_fifo();
  688 + fd_reset(&fdctrl->drives[i]);
  689 + fdctrl_reset_fifo(fdctrl);
401 690 if (do_irq)
402   - fdctrl_raise_irq(0x20);
  691 + fdctrl_raise_irq(fdctrl, 0x20);
  692 +}
  693 +
  694 +static inline fdrive_t *drv0 (fdctrl_t *fdctrl)
  695 +{
  696 + return &fdctrl->drives[fdctrl->bootsel];
  697 +}
  698 +
  699 +static inline fdrive_t *drv1 (fdctrl_t *fdctrl)
  700 +{
  701 + return &fdctrl->drives[1 - fdctrl->bootsel];
  702 +}
  703 +
  704 +static fdrive_t *get_cur_drv (fdctrl_t *fdctrl)
  705 +{
  706 + return fdctrl->cur_drv == 0 ? drv0(fdctrl) : drv1(fdctrl);
403 707 }
404 708  
405 709 /* Status B register : 0x01 (read-only) */
406   -static uint32_t fdctrl_read_statusB (void *opaque, uint32_t reg)
  710 +static uint32_t fdctrl_read_statusB (fdctrl_t *fdctrl)
407 711 {
408   - fdctrl_reset_irq();
  712 + fdctrl_reset_irq(fdctrl);
409 713 FLOPPY_DPRINTF("status register: 0x00\n");
410 714  
411 715 return 0;
412 716 }
413 717  
414 718 /* Digital output register : 0x02 */
415   -static uint32_t fdctrl_read_dor (void *opaque, uint32_t reg)
  719 +static uint32_t fdctrl_read_dor (fdctrl_t *fdctrl)
416 720 {
417   - fdrive_t *cur_drv, *drv0, *drv1;
418 721 uint32_t retval = 0;
419 722  
420   - drv0 = &fdctrl.drives[fdctrl.bootsel];
421   - drv1 = &fdctrl.drives[1 - fdctrl.bootsel];
422   - cur_drv = fdctrl.cur_drv == 0 ? drv0 : drv1;
423 723 /* Drive motors state indicators */
424   - retval |= drv1->motor << 5;
425   - retval |= drv0->motor << 4;
  724 + if (drv0(fdctrl)->drflags & FDRIVE_MOTOR_ON)
  725 + retval |= 1 << 5;
  726 + if (drv1(fdctrl)->drflags & FDRIVE_MOTOR_ON)
  727 + retval |= 1 << 4;
426 728 /* DMA enable */
427   - retval |= fdctrl.dma_en << 3;
  729 + retval |= fdctrl->dma_en << 3;
428 730 /* Reset indicator */
429   - retval |= (fdctrl.state & FD_CTRL_RESET) == 0 ? 0x04 : 0;
  731 + retval |= (fdctrl->state & FD_CTRL_RESET) == 0 ? 0x04 : 0;
430 732 /* Selected drive */
431   - retval |= fdctrl.cur_drv;
  733 + retval |= fdctrl->cur_drv;
432 734 FLOPPY_DPRINTF("digital output register: 0x%02x\n", retval);
433 735  
434 736 return retval;
435 737 }
436 738  
437   -static void fdctrl_write_dor (void *opaque, uint32_t reg, uint32_t value)
  739 +static void fdctrl_write_dor (fdctrl_t *fdctrl, uint32_t value)
438 740 {
439   - fdrive_t *drv0, *drv1;
440   -
441   - fdctrl_reset_irq();
442   - drv0 = &fdctrl.drives[fdctrl.bootsel];
443   - drv1 = &fdctrl.drives[1 - fdctrl.bootsel];
  741 + fdctrl_reset_irq(fdctrl);
444 742 /* Reset mode */
445   - if (fdctrl.state & FD_CTRL_RESET) {
  743 + if (fdctrl->state & FD_CTRL_RESET) {
446 744 if (!(value & 0x04)) {
447 745 FLOPPY_DPRINTF("Floppy controler in RESET state !\n");
448 746 return;
... ... @@ -451,81 +749,81 @@ static void fdctrl_write_dor (void *opaque, uint32_t reg, uint32_t value)
451 749 FLOPPY_DPRINTF("digital output register set to 0x%02x\n", value);
452 750 /* Drive motors state indicators */
453 751 if (value & 0x20)
454   - fd_start(drv1);
  752 + fd_start(drv1(fdctrl));
455 753 else
456   - fd_stop(drv1);
  754 + fd_stop(drv1(fdctrl));
457 755 if (value & 0x10)
458   - fd_start(drv0);
  756 + fd_start(drv0(fdctrl));
459 757 else
460   - fd_stop(drv0);
  758 + fd_stop(drv0(fdctrl));
461 759 /* DMA enable */
462 760 #if 0
463   - if (fdctrl.dma_chann != -1)
464   - fdctrl.dma_en = 1 - ((value >> 3) & 1);
  761 + if (fdctrl->dma_chann != -1)
  762 + fdctrl->dma_en = 1 - ((value >> 3) & 1);
465 763 #endif
466 764 /* Reset */
467 765 if (!(value & 0x04)) {
468   - if (!(fdctrl.state & FD_CTRL_RESET)) {
  766 + if (!(fdctrl->state & FD_CTRL_RESET)) {
469 767 FLOPPY_DPRINTF("controler enter RESET state\n");
470   - fdctrl.state |= FD_CTRL_RESET;
471   - fdctrl_reset(1);
  768 + fdctrl->state |= FD_CTRL_RESET;
  769 + fdctrl_reset(fdctrl, 1);
472 770 }
473 771 } else {
474   - if (fdctrl.state & FD_CTRL_RESET) {
  772 + if (fdctrl->state & FD_CTRL_RESET) {
475 773 FLOPPY_DPRINTF("controler out of RESET state\n");
476   - fdctrl.state &= ~(FD_CTRL_RESET | FD_CTRL_SLEEP);
  774 + fdctrl->state &= ~(FD_CTRL_RESET | FD_CTRL_SLEEP);
477 775 }
478 776 }
479 777 /* Selected drive */
480   - fdctrl.cur_drv = value & 1;
  778 + fdctrl->cur_drv = value & 1;
481 779 }
482 780  
483 781 /* Tape drive register : 0x03 */
484   -static uint32_t fdctrl_read_tape (void *opaque, uint32_t reg)
  782 +static uint32_t fdctrl_read_tape (fdctrl_t *fdctrl)
485 783 {
486 784 uint32_t retval = 0;
487 785  
488   - fdctrl_reset_irq();
  786 + fdctrl_reset_irq(fdctrl);
489 787 /* Disk boot selection indicator */
490   - retval |= fdctrl.bootsel << 2;
  788 + retval |= fdctrl->bootsel << 2;
491 789 /* Tape indicators: never allowed */
492 790 FLOPPY_DPRINTF("tape drive register: 0x%02x\n", retval);
493 791  
494 792 return retval;
495 793 }
496 794  
497   -static void fdctrl_write_tape (void *opaque, uint32_t reg, uint32_t value)
  795 +static void fdctrl_write_tape (fdctrl_t *fdctrl, uint32_t value)
498 796 {
499   - fdctrl_reset_irq();
  797 + fdctrl_reset_irq(fdctrl);
500 798 /* Reset mode */
501   - if (fdctrl.state & FD_CTRL_RESET) {
  799 + if (fdctrl->state & FD_CTRL_RESET) {
502 800 FLOPPY_DPRINTF("Floppy controler in RESET state !\n");
503 801 return;
504 802 }
505 803 FLOPPY_DPRINTF("tape drive register set to 0x%02x\n", value);
506 804 /* Disk boot selection indicator */
507   - fdctrl.bootsel = (value >> 2) & 1;
  805 + fdctrl->bootsel = (value >> 2) & 1;
508 806 /* Tape indicators: never allow */
509 807 }
510 808  
511 809 /* Main status register : 0x04 (read) */
512   -static uint32_t fdctrl_read_main_status (void *opaque, uint32_t reg)
  810 +static uint32_t fdctrl_read_main_status (fdctrl_t *fdctrl)
513 811 {
514 812 uint32_t retval = 0;
515 813  
516   - fdctrl_reset_irq();
517   - fdctrl.state &= ~(FD_CTRL_SLEEP | FD_CTRL_RESET);
518   - if (!(fdctrl.state & FD_CTRL_BUSY)) {
  814 + fdctrl_reset_irq(fdctrl);
  815 + fdctrl->state &= ~(FD_CTRL_SLEEP | FD_CTRL_RESET);
  816 + if (!(fdctrl->state & FD_CTRL_BUSY)) {
519 817 /* Data transfer allowed */
520 818 retval |= 0x80;
521 819 /* Data transfer direction indicator */
522   - if (fdctrl.data_dir == FD_DIR_READ)
  820 + if (fdctrl->data_dir == FD_DIR_READ)
523 821 retval |= 0x40;
524 822 }
525 823 /* Should handle 0x20 for SPECIFY command */
526 824 /* Command busy indicator */
527   - if (FD_STATE(fdctrl.data_state) == FD_STATE_DATA ||
528   - FD_STATE(fdctrl.data_state) == FD_STATE_STATUS)
  825 + if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA ||
  826 + FD_STATE(fdctrl->data_state) == FD_STATE_STATUS)
529 827 retval |= 0x10;
530 828 FLOPPY_DPRINTF("main status register: 0x%02x\n", retval);
531 829  
... ... @@ -533,150 +831,142 @@ static uint32_t fdctrl_read_main_status (void *opaque, uint32_t reg)
533 831 }
534 832  
535 833 /* Data select rate register : 0x04 (write) */
536   -static void fdctrl_write_rate (void *opaque, uint32_t reg, uint32_t value)
  834 +static void fdctrl_write_rate (fdctrl_t *fdctrl, uint32_t value)
537 835 {
538   - fdctrl_reset_irq();
  836 + fdctrl_reset_irq(fdctrl);
539 837 /* Reset mode */
540   - if (fdctrl.state & FD_CTRL_RESET) {
541   - if (reg != 0x2 || !(value & 0x04)) {
  838 + if (fdctrl->state & FD_CTRL_RESET) {
542 839 FLOPPY_DPRINTF("Floppy controler in RESET state !\n");
543 840 return;
544 841 }
545   - }
546 842 FLOPPY_DPRINTF("select rate register set to 0x%02x\n", value);
547 843 /* Reset: autoclear */
548 844 if (value & 0x80) {
549   - fdctrl.state |= FD_CTRL_RESET;
550   - fdctrl_reset(1);
551   - fdctrl.state &= ~FD_CTRL_RESET;
  845 + fdctrl->state |= FD_CTRL_RESET;
  846 + fdctrl_reset(fdctrl, 1);
  847 + fdctrl->state &= ~FD_CTRL_RESET;
552 848 }
553 849 if (value & 0x40) {
554   - fdctrl.state |= FD_CTRL_SLEEP;
555   - fdctrl_reset(1);
  850 + fdctrl->state |= FD_CTRL_SLEEP;
  851 + fdctrl_reset(fdctrl, 1);
556 852 }
557 853 // fdctrl.precomp = (value >> 2) & 0x07;
558 854 }
559 855  
560 856 /* Digital input register : 0x07 (read-only) */
561   -static uint32_t fdctrl_read_dir (void *opaque, uint32_t reg)
  857 +static uint32_t fdctrl_read_dir (fdctrl_t *fdctrl)
562 858 {
563   - fdrive_t *drv0, *drv1;
564 859 uint32_t retval = 0;
565 860  
566   - fdctrl_reset_irq();
567   - drv0 = &fdctrl.drives[fdctrl.bootsel];
568   - drv1 = &fdctrl.drives[1 - fdctrl.bootsel];
569   - if (drv0->rv || drv1->rv)
  861 + fdctrl_reset_irq(fdctrl);
  862 + if (drv0(fdctrl)->drflags & FDRIVE_REVALIDATE ||
  863 + drv1(fdctrl)->drflags & FDRIVE_REVALIDATE)
570 864 retval |= 0x80;
571 865 if (retval != 0)
572   - FLOPPY_ERROR("Floppy digital input register: 0x%02x\n", retval);
573   - drv0->rv = 0;
574   - drv1->rv = 0;
  866 + FLOPPY_DPRINTF("Floppy digital input register: 0x%02x\n", retval);
  867 + drv0(fdctrl)->drflags &= ~FDRIVE_REVALIDATE;
  868 + drv1(fdctrl)->drflags &= ~FDRIVE_REVALIDATE;
575 869  
576 870 return retval;
577 871 }
578 872  
579 873 /* FIFO state control */
580   -static void fdctrl_reset_fifo (void)
  874 +static void fdctrl_reset_fifo (fdctrl_t *fdctrl)
581 875 {
582   - fdctrl.data_dir = FD_DIR_WRITE;
583   - fdctrl.data_pos = 0;
584   - fdctrl.data_state = FD_STATE_CMD;
  876 + fdctrl->data_dir = FD_DIR_WRITE;
  877 + fdctrl->data_pos = 0;
  878 + FD_SET_STATE(fdctrl->data_state, FD_STATE_CMD);
585 879 }
586 880  
587 881 /* Set FIFO status for the host to read */
588   -static void fdctrl_set_fifo (int fifo_len, int do_irq)
  882 +static void fdctrl_set_fifo (fdctrl_t *fdctrl, int fifo_len, int do_irq)
589 883 {
590   - fdctrl.data_dir = FD_DIR_READ;
591   - fdctrl.data_len = fifo_len;
592   - fdctrl.data_pos = 0;
593   - fdctrl.data_state = FD_STATE_STATUS;
  884 + fdctrl->data_dir = FD_DIR_READ;
  885 + fdctrl->data_len = fifo_len;
  886 + fdctrl->data_pos = 0;
  887 + FD_SET_STATE(fdctrl->data_state, FD_STATE_STATUS);
594 888 if (do_irq)
595   - fdctrl_raise_irq(0x00);
  889 + fdctrl_raise_irq(fdctrl, 0x00);
596 890 }
597 891  
598 892 /* Set an error: unimplemented/unknown command */
599   -static void fdctrl_unimplemented (void)
  893 +static void fdctrl_unimplemented (fdctrl_t *fdctrl)
600 894 {
601 895 #if 0
602   - fdrive_t *cur_drv, *drv0, *drv1;
603   -
604   - drv0 = &fdctrl.drives[fdctrl.bootsel];
605   - drv1 = &fdctrl.drives[1 - fdctrl.bootsel];
606   - cur_drv = fdctrl.cur_drv == 0 ? drv0 : drv1;
607   - fdctrl.fifo[0] = 0x60 | (cur_drv->head << 1) | fdctrl.cur_drv;
608   - fdctrl.fifo[1] = 0x00;
609   - fdctrl.fifo[2] = 0x00;
610   - fdctrl_set_fifo(3, 1);
  896 + fdrive_t *cur_drv;
  897 +
  898 + cur_drv = get_cur_drv(fdctrl);
  899 + fdctrl->fifo[0] = 0x60 | (cur_drv->head << 1) | fdctrl->cur_drv;
  900 + fdctrl->fifo[1] = 0x00;
  901 + fdctrl->fifo[2] = 0x00;
  902 + fdctrl_set_fifo(fdctrl, 3, 1);
611 903 #else
612   - fdctrl_reset_fifo();
  904 + // fdctrl_reset_fifo(fdctrl);
  905 + fdctrl->fifo[0] = 0x80;
  906 + fdctrl_set_fifo(fdctrl, 1, 0);
613 907 #endif
614 908 }
615 909  
616 910 /* Callback for transfer end (stop or abort) */
617   -static void fdctrl_stop_transfer (uint8_t status0, uint8_t status1,
618   - uint8_t status2)
  911 +static void fdctrl_stop_transfer (fdctrl_t *fdctrl, uint8_t status0,
  912 + uint8_t status1, uint8_t status2)
619 913 {
620   - fdrive_t *cur_drv, *drv0, *drv1;
  914 + fdrive_t *cur_drv;
621 915  
622   - drv0 = &fdctrl.drives[fdctrl.bootsel];
623   - drv1 = &fdctrl.drives[1 - fdctrl.bootsel];
624   - cur_drv = fdctrl.cur_drv == 0 ? drv0 : drv1;
  916 + cur_drv = get_cur_drv(fdctrl);
625 917 FLOPPY_DPRINTF("transfer status: %02x %02x %02x (%02x)\n",
626 918 status0, status1, status2,
627   - status0 | (cur_drv->head << 1) | fdctrl.cur_drv);
628   - fdctrl.fifo[0] = status0 | (cur_drv->head << 1) | fdctrl.cur_drv;
629   - fdctrl.fifo[1] = status1;
630   - fdctrl.fifo[2] = status2;
631   - fdctrl.fifo[3] = cur_drv->track;
632   - fdctrl.fifo[4] = cur_drv->head;
633   - fdctrl.fifo[5] = cur_drv->sect;
634   - fdctrl.fifo[6] = FD_SECTOR_SC;
635   - fdctrl.data_dir = FD_DIR_READ;
636   - if (fdctrl.state & FD_CTRL_BUSY)
637   - DMA_release_DREQ(fdctrl.dma_chann);
638   - fdctrl_set_fifo(7, 1);
  919 + status0 | (cur_drv->head << 1) | fdctrl->cur_drv);
  920 + fdctrl->fifo[0] = status0 | (cur_drv->head << 1) | fdctrl->cur_drv;
  921 + fdctrl->fifo[1] = status1;
  922 + fdctrl->fifo[2] = status2;
  923 + fdctrl->fifo[3] = cur_drv->track;
  924 + fdctrl->fifo[4] = cur_drv->head;
  925 + fdctrl->fifo[5] = cur_drv->sect;
  926 + fdctrl->fifo[6] = FD_SECTOR_SC;
  927 + fdctrl->data_dir = FD_DIR_READ;
  928 + if (fdctrl->state & FD_CTRL_BUSY)
  929 + DMA_release_DREQ(fdctrl->dma_chann);
  930 + fdctrl_set_fifo(fdctrl, 7, 1);
639 931 }
640 932  
641 933 /* Prepare a data transfer (either DMA or FIFO) */
642   -static void fdctrl_start_transfer (int direction)
  934 +static void fdctrl_start_transfer (fdctrl_t *fdctrl, int direction)
643 935 {
644   - fdrive_t *cur_drv, *drv0, *drv1;
  936 + fdrive_t *cur_drv;
645 937 uint8_t kh, kt, ks;
646 938 int did_seek;
647 939  
648   - drv0 = &fdctrl.drives[fdctrl.bootsel];
649   - drv1 = &fdctrl.drives[1 - fdctrl.bootsel];
650   - fdctrl.cur_drv = fdctrl.fifo[1] & 1;
651   - cur_drv = fdctrl.cur_drv == 0 ? drv0 : drv1;
652   - kt = fdctrl.fifo[2];
653   - kh = fdctrl.fifo[3];
654   - ks = fdctrl.fifo[4];
  940 + fdctrl->cur_drv = fdctrl->fifo[1] & 1;
  941 + cur_drv = get_cur_drv(fdctrl);
  942 + kt = fdctrl->fifo[2];
  943 + kh = fdctrl->fifo[3];
  944 + ks = fdctrl->fifo[4];
655 945 FLOPPY_DPRINTF("Start tranfert at %d %d %02x %02x (%d)\n",
656   - fdctrl.cur_drv, kh, kt, ks,
  946 + fdctrl->cur_drv, kh, kt, ks,
657 947 _fd_sector(kh, kt, ks, cur_drv->last_sect));
658 948 did_seek = 0;
659   - switch (fd_seek(cur_drv, kh, kt, ks, fdctrl.config & 0x40)) {
  949 + switch (fd_seek(cur_drv, kh, kt, ks, fdctrl->config & 0x40)) {
660 950 case 2:
661 951 /* sect too big */
662   - fdctrl_stop_transfer(0x40, 0x00, 0x00);
663   - fdctrl.fifo[3] = kt;
664   - fdctrl.fifo[4] = kh;
665   - fdctrl.fifo[5] = ks;
  952 + fdctrl_stop_transfer(fdctrl, 0x40, 0x00, 0x00);
  953 + fdctrl->fifo[3] = kt;
  954 + fdctrl->fifo[4] = kh;
  955 + fdctrl->fifo[5] = ks;
666 956 return;
667 957 case 3:
668 958 /* track too big */
669   - fdctrl_stop_transfer(0x40, 0x80, 0x00);
670   - fdctrl.fifo[3] = kt;
671   - fdctrl.fifo[4] = kh;
672   - fdctrl.fifo[5] = ks;
  959 + fdctrl_stop_transfer(fdctrl, 0x40, 0x80, 0x00);
  960 + fdctrl->fifo[3] = kt;
  961 + fdctrl->fifo[4] = kh;
  962 + fdctrl->fifo[5] = ks;
673 963 return;
674 964 case 4:
675 965 /* No seek enabled */
676   - fdctrl_stop_transfer(0x40, 0x00, 0x00);
677   - fdctrl.fifo[3] = kt;
678   - fdctrl.fifo[4] = kh;
679   - fdctrl.fifo[5] = ks;
  966 + fdctrl_stop_transfer(fdctrl, 0x40, 0x00, 0x00);
  967 + fdctrl->fifo[3] = kt;
  968 + fdctrl->fifo[4] = kh;
  969 + fdctrl->fifo[5] = ks;
680 970 return;
681 971 case 1:
682 972 did_seek = 1;
... ... @@ -685,299 +975,379 @@ static void fdctrl_start_transfer (int direction)
685 975 break;
686 976 }
687 977 /* Set the FIFO state */
688   - fdctrl.data_dir = direction;
689   - fdctrl.data_pos = 0;
690   - fdctrl.data_state = FD_STATE_DATA; /* FIFO ready for data */
691   - if (fdctrl.fifo[0] & 0x80)
692   - fdctrl.data_state |= FD_STATE_MULTI;
  978 + fdctrl->data_dir = direction;
  979 + fdctrl->data_pos = 0;
  980 + FD_SET_STATE(fdctrl->data_state, FD_STATE_DATA); /* FIFO ready for data */
  981 + if (fdctrl->fifo[0] & 0x80)
  982 + fdctrl->data_state |= FD_STATE_MULTI;
  983 + else
  984 + fdctrl->data_state &= ~FD_STATE_MULTI;
693 985 if (did_seek)
694   - fdctrl.data_state |= FD_STATE_SEEK;
695   - if (fdctrl.dma_en) {
  986 + fdctrl->data_state |= FD_STATE_SEEK;
  987 + else
  988 + fdctrl->data_state &= ~FD_STATE_SEEK;
  989 + if (fdctrl->fifo[5] == 00) {
  990 + fdctrl->data_len = fdctrl->fifo[8];
  991 + } else {
  992 + int tmp;
  993 + fdctrl->data_len = 128 << fdctrl->fifo[5];
  994 + tmp = (cur_drv->last_sect - ks + 1);
  995 + if (fdctrl->fifo[0] & 0x80)
  996 + tmp += cur_drv->last_sect;
  997 + fdctrl->data_len *= tmp;
  998 + }
  999 + if (fdctrl->dma_en) {
696 1000 int dma_mode;
697 1001 /* DMA transfer are enabled. Check if DMA channel is well programmed */
698   - dma_mode = DMA_get_channel_mode(fdctrl.dma_chann);
  1002 + dma_mode = DMA_get_channel_mode(fdctrl->dma_chann);
699 1003 dma_mode = (dma_mode >> 2) & 3;
700   - FLOPPY_DPRINTF("dma_mode=%d direction=%d (%d)\n", dma_mode, direction,
701   - (128 << fdctrl.fifo[5]) *
702   - (cur_drv->last_sect - ks + 1));
  1004 + FLOPPY_DPRINTF("dma_mode=%d direction=%d (%d - %d)\n",
  1005 + dma_mode, direction,
  1006 + (128 << fdctrl->fifo[5]) *
  1007 + (cur_drv->last_sect - ks + 1), fdctrl->data_len);
703 1008 if (((direction == FD_DIR_SCANE || direction == FD_DIR_SCANL ||
704 1009 direction == FD_DIR_SCANH) && dma_mode == 0) ||
705 1010 (direction == FD_DIR_WRITE && dma_mode == 2) ||
706 1011 (direction == FD_DIR_READ && dma_mode == 1)) {
707 1012 /* No access is allowed until DMA transfer has completed */
708   - fdctrl.state |= FD_CTRL_BUSY;
  1013 + fdctrl->state |= FD_CTRL_BUSY;
709 1014 /* Now, we just have to wait for the DMA controler to
710 1015 * recall us...
711 1016 */
712   - DMA_hold_DREQ(fdctrl.dma_chann);
713   - DMA_schedule(fdctrl.dma_chann);
  1017 + DMA_hold_DREQ(fdctrl->dma_chann);
  1018 + DMA_schedule(fdctrl->dma_chann);
714 1019 return;
  1020 + } else {
  1021 + FLOPPY_ERROR("dma_mode=%d direction=%d\n", dma_mode, direction);
715 1022 }
716 1023 }
717 1024 FLOPPY_DPRINTF("start non-DMA transfer\n");
718 1025 /* IO based transfer: calculate len */
719   - if (fdctrl.fifo[5] == 00) {
720   - fdctrl.data_len = fdctrl.fifo[8];
721   - } else {
722   - fdctrl.data_len = 128 << fdctrl.fifo[5];
723   - fdctrl.data_len *= (cur_drv->last_sect - ks + 1);
724   - if (fdctrl.fifo[0] & 0x80)
725   - fdctrl.data_len *= 2;
726   - }
727   - fdctrl_raise_irq(0x00);
  1026 + fdctrl_raise_irq(fdctrl, 0x00);
728 1027  
729 1028 return;
730 1029 }
731 1030  
732 1031 /* Prepare a transfer of deleted data */
733   -static void fdctrl_start_transfer_del (int direction)
  1032 +static void fdctrl_start_transfer_del (fdctrl_t *fdctrl, int direction)
734 1033 {
735 1034 /* We don't handle deleted data,
736 1035 * so we don't return *ANYTHING*
737 1036 */
738   - fdctrl_stop_transfer(0x60, 0x00, 0x00);
  1037 + fdctrl_stop_transfer(fdctrl, 0x60, 0x00, 0x00);
739 1038 }
740 1039  
741 1040 /* handlers for DMA transfers */
742   -/* XXX: the partial transfer logic seems to be broken */
743 1041 static int fdctrl_transfer_handler (void *opaque, target_ulong addr, int size)
744 1042 {
745   - fdrive_t *cur_drv, *drv0, *drv1;
746   - int len;
  1043 + fdctrl_t *fdctrl;
  1044 + fdrive_t *cur_drv;
  1045 + int len, start_pos, rel_pos;
747 1046 uint8_t status0 = 0x00, status1 = 0x00, status2 = 0x00;
748   - uint8_t tmpbuf[FD_SECTOR_LEN];
749 1047  
750   - fdctrl_reset_irq();
751   - if (!(fdctrl.state & FD_CTRL_BUSY)) {
  1048 + fdctrl = opaque;
  1049 + fdctrl_reset_irq(fdctrl);
  1050 + if (!(fdctrl->state & FD_CTRL_BUSY)) {
752 1051 FLOPPY_DPRINTF("Not in DMA transfer mode !\n");
753 1052 return 0;
754 1053 }
755   - drv0 = &fdctrl.drives[fdctrl.bootsel];
756   - drv1 = &fdctrl.drives[1 - fdctrl.bootsel];
757   - cur_drv = fdctrl.cur_drv == 0 ? drv0 : drv1;
758   - if (fdctrl.data_dir == FD_DIR_SCANE || fdctrl.data_dir == FD_DIR_SCANL ||
759   - fdctrl.data_dir == FD_DIR_SCANH)
  1054 + cur_drv = get_cur_drv(fdctrl);
  1055 + if (fdctrl->data_dir == FD_DIR_SCANE || fdctrl->data_dir == FD_DIR_SCANL ||
  1056 + fdctrl->data_dir == FD_DIR_SCANH)
760 1057 status2 = 0x04;
761   - for (fdctrl.data_len = size; fdctrl.data_pos < fdctrl.data_len;
762   - fdctrl.data_pos += len) {
763   - len = size - fdctrl.data_pos;
764   - if (len > FD_SECTOR_LEN)
765   - len = FD_SECTOR_LEN;
766   - FLOPPY_DPRINTF("copy %d bytes (%d %d %d) %d pos %d %02x %02x "
767   - "(%d-0x%08x)\n", len, size, fdctrl.data_pos,
768   - fdctrl.data_len, fdctrl.cur_drv, cur_drv->head,
769   - cur_drv->track, cur_drv->sect, fd_sector(cur_drv),
770   - fd_sector(cur_drv) * 512);
771   - if (fdctrl.data_dir != FD_DIR_WRITE) {
772   - /* READ & SCAN commands */
  1058 + if (size > fdctrl->data_len)
  1059 + size = fdctrl->data_len;
773 1060 if (cur_drv->bs == NULL) {
774   - fdctrl_stop_transfer(0x40, 0x00, 0x00);
  1061 + if (fdctrl->data_dir == FD_DIR_WRITE)
  1062 + fdctrl_stop_transfer(fdctrl, 0x60, 0x00, 0x00);
  1063 + else
  1064 + fdctrl_stop_transfer(fdctrl, 0x40, 0x00, 0x00);
  1065 + len = 0;
775 1066 goto transfer_error;
776 1067 }
777   - if (bdrv_read(cur_drv->bs, fd_sector(cur_drv), tmpbuf, 1) < 0) {
  1068 + rel_pos = fdctrl->data_pos % FD_SECTOR_LEN;
  1069 + for (start_pos = fdctrl->data_pos; fdctrl->data_pos < size;) {
  1070 + len = size - fdctrl->data_pos;
  1071 + if (len + rel_pos > FD_SECTOR_LEN)
  1072 + len = FD_SECTOR_LEN - rel_pos;
  1073 + FLOPPY_DPRINTF("copy %d bytes (%d %d %d) %d pos %d %02x %02x "
  1074 + "(%d-0x%08x 0x%08x)\n", len, size, fdctrl->data_pos,
  1075 + fdctrl->data_len, fdctrl->cur_drv, cur_drv->head,
  1076 + cur_drv->track, cur_drv->sect, fd_sector(cur_drv),
  1077 + fd_sector(cur_drv) * 512, addr);
  1078 + if (fdctrl->data_dir != FD_DIR_WRITE ||
  1079 + len < FD_SECTOR_LEN || rel_pos != 0) {
  1080 + /* READ & SCAN commands and realign to a sector for WRITE */
  1081 + if (bdrv_read(cur_drv->bs, fd_sector(cur_drv),
  1082 + fdctrl->fifo, 1) < 0) {
778 1083 FLOPPY_DPRINTF("Floppy: error getting sector %d\n",
779 1084 fd_sector(cur_drv));
780 1085 /* Sure, image size is too small... */
781   - memset(tmpbuf, 0, FD_SECTOR_LEN);
  1086 + memset(fdctrl->fifo, 0, FD_SECTOR_LEN);
782 1087 }
783   - if (fdctrl.data_dir == FD_DIR_READ) {
784   - cpu_physical_memory_write(addr + fdctrl.data_pos,
785   - tmpbuf, len);
786   - if (len < FD_SECTOR_LEN) {
787   - memcpy(&fdctrl.fifo[0], tmpbuf + len, FD_SECTOR_LEN - len);
788   - memset(&fdctrl.fifo[FD_SECTOR_LEN - len], 0, len);
789 1088 }
790   - } else {
791   - int ret;
792   - /* XXX: what to do if not enough data ? */
793   - cpu_physical_memory_read(addr + fdctrl.data_pos,
794   - fdctrl.fifo, len);
795   - if (len < FD_SECTOR_LEN) {
796   - memset(&fdctrl.fifo[len], 0, FD_SECTOR_LEN - len);
  1089 + switch (fdctrl->data_dir) {
  1090 + case FD_DIR_READ:
  1091 + /* READ commands */
  1092 + cpu_physical_memory_write(addr + fdctrl->data_pos,
  1093 + fdctrl->fifo + rel_pos, len);
  1094 + break;
  1095 + case FD_DIR_WRITE:
  1096 + /* WRITE commands */
  1097 + cpu_physical_memory_read(addr + fdctrl->data_pos,
  1098 + fdctrl->fifo + rel_pos, len);
  1099 + if (bdrv_write(cur_drv->bs, fd_sector(cur_drv),
  1100 + fdctrl->fifo, 1) < 0) {
  1101 + FLOPPY_ERROR("writting sector %d\n", fd_sector(cur_drv));
  1102 + fdctrl_stop_transfer(fdctrl, 0x60, 0x00, 0x00);
  1103 + goto transfer_error;
797 1104 }
798   - ret = memcmp(tmpbuf, fdctrl.fifo, FD_SECTOR_LEN);
  1105 + break;
  1106 + default:
  1107 + /* SCAN commands */
  1108 + {
  1109 + uint8_t tmpbuf[FD_SECTOR_LEN];
  1110 + int ret;
  1111 + cpu_physical_memory_read(addr + fdctrl->data_pos,
  1112 + tmpbuf, len);
  1113 + ret = memcmp(tmpbuf, fdctrl->fifo + rel_pos, len);
799 1114 if (ret == 0) {
800 1115 status2 = 0x08;
801 1116 goto end_transfer;
802 1117 }
803   - if ((ret < 0 && fdctrl.data_dir == FD_DIR_SCANL) ||
804   - (ret > 0 && fdctrl.data_dir == FD_DIR_SCANH)) {
  1118 + if ((ret < 0 && fdctrl->data_dir == FD_DIR_SCANL) ||
  1119 + (ret > 0 && fdctrl->data_dir == FD_DIR_SCANH)) {
805 1120 status2 = 0x00;
806 1121 goto end_transfer;
807 1122 }
808 1123 }
809   - } else {
810   - /* WRITE commands */
811   - cpu_physical_memory_read(addr + fdctrl.data_pos, tmpbuf, len);
812   - if (len < FD_SECTOR_LEN) {
813   - memset(tmpbuf + len, 0, FD_SECTOR_LEN - len);
814   - }
815   - if (cur_drv->bs == NULL ||
816   - bdrv_write(cur_drv->bs, fd_sector(cur_drv), tmpbuf, 1) < 0) {
817   - FLOPPY_ERROR("writting sector %d\n", fd_sector(cur_drv));
818   - fdctrl_stop_transfer(0x60, 0x00, 0x00);
819   - goto transfer_error;
820   - }
  1124 + break;
821 1125 }
822   - if (len == FD_SECTOR_LEN) {
  1126 + fdctrl->data_pos += len;
  1127 + rel_pos = fdctrl->data_pos % FD_SECTOR_LEN;
  1128 + if (rel_pos == 0) {
823 1129 /* Seek to next sector */
824   - if (cur_drv->sect == cur_drv->last_sect) {
825   - if (cur_drv->head == 0) {
  1130 + cur_drv->sect++;
  1131 + FLOPPY_DPRINTF("seek to next sector (%d %02x %02x => %d) (%d)\n",
  1132 + cur_drv->head, cur_drv->track, cur_drv->sect,
  1133 + fd_sector(cur_drv),
  1134 + fdctrl->data_pos - size);
  1135 + if (cur_drv->sect > cur_drv->last_sect) {
  1136 + cur_drv->sect = 1;
  1137 + if (FD_MULTI_TRACK(fdctrl->data_state)) {
  1138 + if (cur_drv->head == 0 &&
  1139 + (cur_drv->flags & FDISK_DBL_SIDES) != 0) {
826 1140 cur_drv->head = 1;
827 1141 } else {
828   - cur_drv->track++;
829 1142 cur_drv->head = 0;
830   - }
831   - cur_drv->sect = 1;
832   - FLOPPY_DPRINTF("seek to next sector (%d %02x %02x => %d)\n",
833   - cur_drv->head, cur_drv->track, cur_drv->sect,
834   - fd_sector(cur_drv));
835   - if (cur_drv->head == 0) {
836   - FLOPPY_DPRINTF("end transfer\n");
837   - goto end_transfer;
838   - }
839   - if (!FD_MULTI_TRACK(fdctrl.data_state)) {
840   - /* Single track read */
841   - FLOPPY_DPRINTF("single track transfert: end transfer\n");
842   -// status1 |= 0x80;
843   - goto end_transfer;
  1143 + cur_drv->track++;
  1144 + if ((cur_drv->flags & FDISK_DBL_SIDES) == 0)
  1145 + break;
844 1146 }
845 1147 } else {
846   - cur_drv->sect++;
847   - FLOPPY_DPRINTF("seek to next sector (%d %02x %02x => %d)\n",
848   - cur_drv->head, cur_drv->track, cur_drv->sect,
849   - fd_sector(cur_drv));
  1148 + cur_drv->track++;
  1149 + break;
  1150 + }
  1151 + FLOPPY_DPRINTF("seek to next track (%d %02x %02x => %d)\n",
  1152 + cur_drv->head, cur_drv->track,
  1153 + cur_drv->sect, fd_sector(cur_drv));
850 1154 }
851 1155 }
852 1156 }
853 1157 end_transfer:
854   - if (fdctrl.data_dir == FD_DIR_SCANE ||
855   - fdctrl.data_dir == FD_DIR_SCANL ||
856   - fdctrl.data_dir == FD_DIR_SCANH)
  1158 + len = fdctrl->data_pos - start_pos;
  1159 + FLOPPY_DPRINTF("end transfer %d %d %d\n",
  1160 + fdctrl->data_pos, len, fdctrl->data_len);
  1161 + if (fdctrl->data_dir == FD_DIR_SCANE ||
  1162 + fdctrl->data_dir == FD_DIR_SCANL ||
  1163 + fdctrl->data_dir == FD_DIR_SCANH)
857 1164 status2 = 0x08;
858   - if (FD_DID_SEEK(fdctrl.data_state))
  1165 + if (FD_DID_SEEK(fdctrl->data_state))
859 1166 status0 |= 0x20;
860   - fdctrl_stop_transfer(status0, status1, status2);
  1167 + fdctrl->data_len -= len;
  1168 + // if (fdctrl->data_len == 0)
  1169 + fdctrl_stop_transfer(fdctrl, status0, status1, status2);
861 1170 transfer_error:
862 1171  
863   - return fdctrl.data_pos;
  1172 + return len;
864 1173 }
865 1174  
866 1175 /* Data register : 0x05 */
867   -static uint32_t fdctrl_read_data (void *opaque, uint32_t reg)
  1176 +static uint32_t fdctrl_read_data (fdctrl_t *fdctrl)
868 1177 {
869   - fdrive_t *cur_drv, *drv0, *drv1;
  1178 + fdrive_t *cur_drv;
870 1179 uint32_t retval = 0;
871 1180 int pos, len;
872 1181  
873   - fdctrl_reset_irq();
874   - drv0 = &fdctrl.drives[fdctrl.bootsel];
875   - drv1 = &fdctrl.drives[1 - fdctrl.bootsel];
876   - cur_drv = fdctrl.cur_drv == 0 ? drv0 : drv1;
877   - fdctrl.state &= ~FD_CTRL_SLEEP;
878   - if (FD_STATE(fdctrl.data_state) == FD_STATE_CMD) {
  1182 + fdctrl_reset_irq(fdctrl);
  1183 + cur_drv = get_cur_drv(fdctrl);
  1184 + fdctrl->state &= ~FD_CTRL_SLEEP;
  1185 + if (FD_STATE(fdctrl->data_state) == FD_STATE_CMD) {
879 1186 FLOPPY_ERROR("can't read data in CMD state\n");
880 1187 return 0;
881 1188 }
882   - pos = fdctrl.data_pos;
883   - if (FD_STATE(fdctrl.data_state) == FD_STATE_DATA) {
  1189 + pos = fdctrl->data_pos;
  1190 + if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA) {
884 1191 pos %= FD_SECTOR_LEN;
885 1192 if (pos == 0) {
886   - len = fdctrl.data_len - fdctrl.data_pos;
  1193 + len = fdctrl->data_len - fdctrl->data_pos;
887 1194 if (len > FD_SECTOR_LEN)
888 1195 len = FD_SECTOR_LEN;
889 1196 bdrv_read(cur_drv->bs, fd_sector(cur_drv),
890   - fdctrl.fifo, len);
  1197 + fdctrl->fifo, len);
891 1198 }
892 1199 }
893   - retval = fdctrl.fifo[pos];
894   - if (++fdctrl.data_pos == fdctrl.data_len) {
895   - fdctrl.data_pos = 0;
  1200 + retval = fdctrl->fifo[pos];
  1201 + if (++fdctrl->data_pos == fdctrl->data_len) {
  1202 + fdctrl->data_pos = 0;
896 1203 /* Switch from transfert mode to status mode
897 1204 * then from status mode to command mode
898 1205 */
899   - if (FD_STATE(fdctrl.data_state) == FD_STATE_DATA)
900   - fdctrl_stop_transfer(0x20, 0x00, 0x00);
  1206 + if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA)
  1207 + fdctrl_stop_transfer(fdctrl, 0x20, 0x00, 0x00);
901 1208 else
902   - fdctrl_reset_fifo();
  1209 + fdctrl_reset_fifo(fdctrl);
903 1210 }
904 1211 FLOPPY_DPRINTF("data register: 0x%02x\n", retval);
905 1212  
906 1213 return retval;
907 1214 }
908 1215  
909   -static void fdctrl_write_data (void *opaque, uint32_t reg, uint32_t value)
  1216 +static void fdctrl_format_sector (fdctrl_t *fdctrl)
910 1217 {
911   - fdrive_t *cur_drv, *drv0, *drv1;
  1218 + fdrive_t *cur_drv;
  1219 + uint8_t kh, kt, ks;
  1220 + int did_seek;
912 1221  
913   - fdctrl_reset_irq();
914   - drv0 = &fdctrl.drives[fdctrl.bootsel];
915   - drv1 = &fdctrl.drives[1 - fdctrl.bootsel];
916   - cur_drv = fdctrl.cur_drv == 0 ? drv0 : drv1;
  1222 + fdctrl->cur_drv = fdctrl->fifo[1] & 1;
  1223 + cur_drv = get_cur_drv(fdctrl);
  1224 + kt = fdctrl->fifo[6];
  1225 + kh = fdctrl->fifo[7];
  1226 + ks = fdctrl->fifo[8];
  1227 + FLOPPY_DPRINTF("format sector at %d %d %02x %02x (%d)\n",
  1228 + fdctrl->cur_drv, kh, kt, ks,
  1229 + _fd_sector(kh, kt, ks, cur_drv->last_sect));
  1230 + did_seek = 0;
  1231 + switch (fd_seek(cur_drv, kh, kt, ks, fdctrl->config & 0x40)) {
  1232 + case 2:
  1233 + /* sect too big */
  1234 + fdctrl_stop_transfer(fdctrl, 0x40, 0x00, 0x00);
  1235 + fdctrl->fifo[3] = kt;
  1236 + fdctrl->fifo[4] = kh;
  1237 + fdctrl->fifo[5] = ks;
  1238 + return;
  1239 + case 3:
  1240 + /* track too big */
  1241 + fdctrl_stop_transfer(fdctrl, 0x40, 0x80, 0x00);
  1242 + fdctrl->fifo[3] = kt;
  1243 + fdctrl->fifo[4] = kh;
  1244 + fdctrl->fifo[5] = ks;
  1245 + return;
  1246 + case 4:
  1247 + /* No seek enabled */
  1248 + fdctrl_stop_transfer(fdctrl, 0x40, 0x00, 0x00);
  1249 + fdctrl->fifo[3] = kt;
  1250 + fdctrl->fifo[4] = kh;
  1251 + fdctrl->fifo[5] = ks;
  1252 + return;
  1253 + case 1:
  1254 + did_seek = 1;
  1255 + fdctrl->data_state |= FD_STATE_SEEK;
  1256 + break;
  1257 + default:
  1258 + break;
  1259 + }
  1260 + memset(fdctrl->fifo, 0, FD_SECTOR_LEN);
  1261 + if (cur_drv->bs == NULL ||
  1262 + bdrv_write(cur_drv->bs, fd_sector(cur_drv), fdctrl->fifo, 1) < 0) {
  1263 + FLOPPY_ERROR("formating sector %d\n", fd_sector(cur_drv));
  1264 + fdctrl_stop_transfer(fdctrl, 0x60, 0x00, 0x00);
  1265 + } else {
  1266 + if (cur_drv->sect == cur_drv->last_sect) {
  1267 + fdctrl->data_state &= ~FD_STATE_FORMAT;
  1268 + /* Last sector done */
  1269 + if (FD_DID_SEEK(fdctrl->data_state))
  1270 + fdctrl_stop_transfer(fdctrl, 0x20, 0x00, 0x00);
  1271 + else
  1272 + fdctrl_stop_transfer(fdctrl, 0x00, 0x00, 0x00);
  1273 + } else {
  1274 + /* More to do */
  1275 + fdctrl->data_pos = 0;
  1276 + fdctrl->data_len = 4;
  1277 + }
  1278 + }
  1279 +}
  1280 +
  1281 +static void fdctrl_write_data (fdctrl_t *fdctrl, uint32_t value)
  1282 +{
  1283 + fdrive_t *cur_drv;
  1284 +
  1285 + fdctrl_reset_irq(fdctrl);
  1286 + cur_drv = get_cur_drv(fdctrl);
917 1287 /* Reset mode */
918   - if (fdctrl.state & FD_CTRL_RESET) {
  1288 + if (fdctrl->state & FD_CTRL_RESET) {
919 1289 FLOPPY_DPRINTF("Floppy controler in RESET state !\n");
920 1290 return;
921 1291 }
922   - fdctrl.state &= ~FD_CTRL_SLEEP;
923   - if ((fdctrl.data_state & FD_STATE_STATE) == FD_STATE_STATUS) {
  1292 + fdctrl->state &= ~FD_CTRL_SLEEP;
  1293 + if (FD_STATE(fdctrl->data_state) == FD_STATE_STATUS) {
924 1294 FLOPPY_ERROR("can't write data in status mode\n");
925 1295 return;
926 1296 }
927 1297 /* Is it write command time ? */
928   - if (FD_STATE(fdctrl.data_state) == FD_STATE_DATA) {
  1298 + if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA) {
929 1299 /* FIFO data write */
930   - fdctrl.fifo[fdctrl.data_pos++] = value;
931   - if (fdctrl.data_pos % FD_SECTOR_LEN == (FD_SECTOR_LEN - 1) ||
932   - fdctrl.data_pos == fdctrl.data_len) {
  1300 + fdctrl->fifo[fdctrl->data_pos++] = value;
  1301 + if (fdctrl->data_pos % FD_SECTOR_LEN == (FD_SECTOR_LEN - 1) ||
  1302 + fdctrl->data_pos == fdctrl->data_len) {
933 1303 bdrv_write(cur_drv->bs, fd_sector(cur_drv),
934   - fdctrl.fifo, FD_SECTOR_LEN);
  1304 + fdctrl->fifo, FD_SECTOR_LEN);
935 1305 }
936 1306 /* Switch from transfert mode to status mode
937 1307 * then from status mode to command mode
938 1308 */
939   - if (FD_STATE(fdctrl.data_state) == FD_STATE_DATA)
940   - fdctrl_stop_transfer(0x20, 0x00, 0x00);
  1309 + if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA)
  1310 + fdctrl_stop_transfer(fdctrl, 0x20, 0x00, 0x00);
941 1311 return;
942 1312 }
943   - if (fdctrl.data_pos == 0) {
  1313 + if (fdctrl->data_pos == 0) {
944 1314 /* Command */
945 1315 switch (value & 0x5F) {
946 1316 case 0x46:
947 1317 /* READ variants */
948 1318 FLOPPY_DPRINTF("READ command\n");
949 1319 /* 8 parameters cmd */
950   - fdctrl.data_len = 9;
  1320 + fdctrl->data_len = 9;
951 1321 goto enqueue;
952 1322 case 0x4C:
953 1323 /* READ_DELETED variants */
954 1324 FLOPPY_DPRINTF("READ_DELETED command\n");
955 1325 /* 8 parameters cmd */
956   - fdctrl.data_len = 9;
  1326 + fdctrl->data_len = 9;
957 1327 goto enqueue;
958 1328 case 0x50:
959 1329 /* SCAN_EQUAL variants */
960 1330 FLOPPY_DPRINTF("SCAN_EQUAL command\n");
961 1331 /* 8 parameters cmd */
962   - fdctrl.data_len = 9;
  1332 + fdctrl->data_len = 9;
963 1333 goto enqueue;
964 1334 case 0x56:
965 1335 /* VERIFY variants */
966 1336 FLOPPY_DPRINTF("VERIFY command\n");
967 1337 /* 8 parameters cmd */
968   - fdctrl.data_len = 9;
  1338 + fdctrl->data_len = 9;
969 1339 goto enqueue;
970 1340 case 0x59:
971 1341 /* SCAN_LOW_OR_EQUAL variants */
972 1342 FLOPPY_DPRINTF("SCAN_LOW_OR_EQUAL command\n");
973 1343 /* 8 parameters cmd */
974   - fdctrl.data_len = 9;
  1344 + fdctrl->data_len = 9;
975 1345 goto enqueue;
976 1346 case 0x5D:
977 1347 /* SCAN_HIGH_OR_EQUAL variants */
978 1348 FLOPPY_DPRINTF("SCAN_HIGH_OR_EQUAL command\n");
979 1349 /* 8 parameters cmd */
980   - fdctrl.data_len = 9;
  1350 + fdctrl->data_len = 9;
981 1351 goto enqueue;
982 1352 default:
983 1353 break;
... ... @@ -987,13 +1357,13 @@ static void fdctrl_write_data (void *opaque, uint32_t reg, uint32_t value)
987 1357 /* WRITE variants */
988 1358 FLOPPY_DPRINTF("WRITE command\n");
989 1359 /* 8 parameters cmd */
990   - fdctrl.data_len = 9;
  1360 + fdctrl->data_len = 9;
991 1361 goto enqueue;
992 1362 case 0x49:
993 1363 /* WRITE_DELETED variants */
994 1364 FLOPPY_DPRINTF("WRITE_DELETED command\n");
995 1365 /* 8 parameters cmd */
996   - fdctrl.data_len = 9;
  1366 + fdctrl->data_len = 9;
997 1367 goto enqueue;
998 1368 default:
999 1369 break;
... ... @@ -1003,422 +1373,447 @@ static void fdctrl_write_data (void *opaque, uint32_t reg, uint32_t value)
1003 1373 /* SPECIFY */
1004 1374 FLOPPY_DPRINTF("SPECIFY command\n");
1005 1375 /* 1 parameter cmd */
1006   - fdctrl.data_len = 3;
  1376 + fdctrl->data_len = 3;
1007 1377 goto enqueue;
1008 1378 case 0x04:
1009 1379 /* SENSE_DRIVE_STATUS */
1010 1380 FLOPPY_DPRINTF("SENSE_DRIVE_STATUS command\n");
1011 1381 /* 1 parameter cmd */
1012   - fdctrl.data_len = 2;
  1382 + fdctrl->data_len = 2;
1013 1383 goto enqueue;
1014 1384 case 0x07:
1015 1385 /* RECALIBRATE */
1016 1386 FLOPPY_DPRINTF("RECALIBRATE command\n");
1017 1387 /* 1 parameter cmd */
1018   - fdctrl.data_len = 2;
  1388 + fdctrl->data_len = 2;
1019 1389 goto enqueue;
1020 1390 case 0x08:
1021 1391 /* SENSE_INTERRUPT_STATUS */
1022 1392 FLOPPY_DPRINTF("SENSE_INTERRUPT_STATUS command (%02x)\n",
1023   - fdctrl.int_status);
  1393 + fdctrl->int_status);
1024 1394 /* No parameters cmd: returns status if no interrupt */
1025   - fdctrl.fifo[0] =
1026   - fdctrl.int_status | (cur_drv->head << 2) | fdctrl.cur_drv;
1027   - fdctrl.fifo[1] = cur_drv->track;
1028   - fdctrl_set_fifo(2, 0);
  1395 + fdctrl->fifo[0] =
  1396 + fdctrl->int_status | (cur_drv->head << 2) | fdctrl->cur_drv;
  1397 + fdctrl->fifo[1] = cur_drv->track;
  1398 + fdctrl_set_fifo(fdctrl, 2, 0);
  1399 + fdctrl_reset_irq(fdctrl);
  1400 + fdctrl->int_status = 0xC0;
1029 1401 return;
1030 1402 case 0x0E:
1031 1403 /* DUMPREG */
1032 1404 FLOPPY_DPRINTF("DUMPREG command\n");
1033 1405 /* Drives position */
1034   - fdctrl.fifo[0] = drv0->track;
1035   - fdctrl.fifo[1] = drv1->track;
1036   - fdctrl.fifo[2] = 0;
1037   - fdctrl.fifo[3] = 0;
  1406 + fdctrl->fifo[0] = drv0(fdctrl)->track;
  1407 + fdctrl->fifo[1] = drv1(fdctrl)->track;
  1408 + fdctrl->fifo[2] = 0;
  1409 + fdctrl->fifo[3] = 0;
1038 1410 /* timers */
1039   - fdctrl.fifo[4] = fdctrl.timer0;
1040   - fdctrl.fifo[5] = (fdctrl.timer1 << 1) | fdctrl.dma_en;
1041   - fdctrl.fifo[6] = cur_drv->last_sect;
1042   - fdctrl.fifo[7] = (fdctrl.lock << 7) |
  1411 + fdctrl->fifo[4] = fdctrl->timer0;
  1412 + fdctrl->fifo[5] = (fdctrl->timer1 << 1) | fdctrl->dma_en;
  1413 + fdctrl->fifo[6] = cur_drv->last_sect;
  1414 + fdctrl->fifo[7] = (fdctrl->lock << 7) |
1043 1415 (cur_drv->perpendicular << 2);
1044   - fdctrl.fifo[8] = fdctrl.config;
1045   - fdctrl.fifo[9] = fdctrl.precomp_trk;
1046   - fdctrl_set_fifo(10, 0);
  1416 + fdctrl->fifo[8] = fdctrl->config;
  1417 + fdctrl->fifo[9] = fdctrl->precomp_trk;
  1418 + fdctrl_set_fifo(fdctrl, 10, 0);
1047 1419 return;
1048 1420 case 0x0F:
1049 1421 /* SEEK */
1050 1422 FLOPPY_DPRINTF("SEEK command\n");
1051 1423 /* 2 parameters cmd */
1052   - fdctrl.data_len = 3;
  1424 + fdctrl->data_len = 3;
1053 1425 goto enqueue;
1054 1426 case 0x10:
1055 1427 /* VERSION */
1056 1428 FLOPPY_DPRINTF("VERSION command\n");
1057 1429 /* No parameters cmd */
1058 1430 /* Controler's version */
1059   - fdctrl.fifo[0] = fdctrl.version;
1060   - fdctrl_set_fifo(1, 1);
  1431 + fdctrl->fifo[0] = fdctrl->version;
  1432 + fdctrl_set_fifo(fdctrl, 1, 1);
1061 1433 return;
1062 1434 case 0x12:
1063 1435 /* PERPENDICULAR_MODE */
1064 1436 FLOPPY_DPRINTF("PERPENDICULAR_MODE command\n");
1065 1437 /* 1 parameter cmd */
1066   - fdctrl.data_len = 2;
  1438 + fdctrl->data_len = 2;
1067 1439 goto enqueue;
1068 1440 case 0x13:
1069 1441 /* CONFIGURE */
1070 1442 FLOPPY_DPRINTF("CONFIGURE command\n");
1071 1443 /* 3 parameters cmd */
1072   - fdctrl.data_len = 4;
  1444 + fdctrl->data_len = 4;
1073 1445 goto enqueue;
1074 1446 case 0x14:
1075 1447 /* UNLOCK */
1076 1448 FLOPPY_DPRINTF("UNLOCK command\n");
1077 1449 /* No parameters cmd */
1078   - fdctrl.lock = 0;
1079   - fdctrl.fifo[0] = 0;
1080   - fdctrl_set_fifo(1, 0);
  1450 + fdctrl->lock = 0;
  1451 + fdctrl->fifo[0] = 0;
  1452 + fdctrl_set_fifo(fdctrl, 1, 0);
1081 1453 return;
1082 1454 case 0x17:
1083 1455 /* POWERDOWN_MODE */
1084 1456 FLOPPY_DPRINTF("POWERDOWN_MODE command\n");
1085 1457 /* 2 parameters cmd */
1086   - fdctrl.data_len = 3;
  1458 + fdctrl->data_len = 3;
1087 1459 goto enqueue;
1088 1460 case 0x18:
1089 1461 /* PART_ID */
1090 1462 FLOPPY_DPRINTF("PART_ID command\n");
1091 1463 /* No parameters cmd */
1092   - fdctrl.fifo[0] = 0x41; /* Stepping 1 */
1093   - fdctrl_set_fifo(1, 0);
  1464 + fdctrl->fifo[0] = 0x41; /* Stepping 1 */
  1465 + fdctrl_set_fifo(fdctrl, 1, 0);
1094 1466 return;
1095 1467 case 0x2C:
1096 1468 /* SAVE */
1097 1469 FLOPPY_DPRINTF("SAVE command\n");
1098 1470 /* No parameters cmd */
1099   - fdctrl.fifo[0] = 0;
1100   - fdctrl.fifo[1] = 0;
  1471 + fdctrl->fifo[0] = 0;
  1472 + fdctrl->fifo[1] = 0;
1101 1473 /* Drives position */
1102   - fdctrl.fifo[2] = drv0->track;
1103   - fdctrl.fifo[3] = drv1->track;
1104   - fdctrl.fifo[4] = 0;
1105   - fdctrl.fifo[5] = 0;
  1474 + fdctrl->fifo[2] = drv0(fdctrl)->track;
  1475 + fdctrl->fifo[3] = drv1(fdctrl)->track;
  1476 + fdctrl->fifo[4] = 0;
  1477 + fdctrl->fifo[5] = 0;
1106 1478 /* timers */
1107   - fdctrl.fifo[6] = fdctrl.timer0;
1108   - fdctrl.fifo[7] = fdctrl.timer1;
1109   - fdctrl.fifo[8] = cur_drv->last_sect;
1110   - fdctrl.fifo[9] = (fdctrl.lock << 7) |
  1479 + fdctrl->fifo[6] = fdctrl->timer0;
  1480 + fdctrl->fifo[7] = fdctrl->timer1;
  1481 + fdctrl->fifo[8] = cur_drv->last_sect;
  1482 + fdctrl->fifo[9] = (fdctrl->lock << 7) |
1111 1483 (cur_drv->perpendicular << 2);
1112   - fdctrl.fifo[10] = fdctrl.config;
1113   - fdctrl.fifo[11] = fdctrl.precomp_trk;
1114   - fdctrl.fifo[12] = fdctrl.pwrd;
1115   - fdctrl.fifo[13] = 0;
1116   - fdctrl.fifo[14] = 0;
1117   - fdctrl_set_fifo(15, 1);
  1484 + fdctrl->fifo[10] = fdctrl->config;
  1485 + fdctrl->fifo[11] = fdctrl->precomp_trk;
  1486 + fdctrl->fifo[12] = fdctrl->pwrd;
  1487 + fdctrl->fifo[13] = 0;
  1488 + fdctrl->fifo[14] = 0;
  1489 + fdctrl_set_fifo(fdctrl, 15, 1);
1118 1490 return;
1119 1491 case 0x33:
1120 1492 /* OPTION */
1121 1493 FLOPPY_DPRINTF("OPTION command\n");
1122 1494 /* 1 parameter cmd */
1123   - fdctrl.data_len = 2;
  1495 + fdctrl->data_len = 2;
1124 1496 goto enqueue;
1125 1497 case 0x42:
1126 1498 /* READ_TRACK */
1127 1499 FLOPPY_DPRINTF("READ_TRACK command\n");
1128 1500 /* 8 parameters cmd */
1129   - fdctrl.data_len = 9;
  1501 + fdctrl->data_len = 9;
1130 1502 goto enqueue;
1131 1503 case 0x4A:
1132 1504 /* READ_ID */
1133 1505 FLOPPY_DPRINTF("READ_ID command\n");
1134 1506 /* 1 parameter cmd */
1135   - fdctrl.data_len = 2;
  1507 + fdctrl->data_len = 2;
1136 1508 goto enqueue;
1137 1509 case 0x4C:
1138 1510 /* RESTORE */
1139 1511 FLOPPY_DPRINTF("RESTORE command\n");
1140 1512 /* 17 parameters cmd */
1141   - fdctrl.data_len = 18;
  1513 + fdctrl->data_len = 18;
1142 1514 goto enqueue;
1143 1515 case 0x4D:
1144 1516 /* FORMAT_TRACK */
1145 1517 FLOPPY_DPRINTF("FORMAT_TRACK command\n");
1146 1518 /* 5 parameters cmd */
1147   - fdctrl.data_len = 9;
  1519 + fdctrl->data_len = 6;
1148 1520 goto enqueue;
1149 1521 case 0x8E:
1150 1522 /* DRIVE_SPECIFICATION_COMMAND */
1151 1523 FLOPPY_DPRINTF("DRIVE_SPECIFICATION_COMMAND command\n");
1152 1524 /* 5 parameters cmd */
1153   - fdctrl.data_len = 6;
  1525 + fdctrl->data_len = 6;
1154 1526 goto enqueue;
1155 1527 case 0x8F:
1156 1528 /* RELATIVE_SEEK_OUT */
1157 1529 FLOPPY_DPRINTF("RELATIVE_SEEK_OUT command\n");
1158 1530 /* 2 parameters cmd */
1159   - fdctrl.data_len = 3;
  1531 + fdctrl->data_len = 3;
1160 1532 goto enqueue;
1161 1533 case 0x94:
1162 1534 /* LOCK */
1163 1535 FLOPPY_DPRINTF("LOCK command\n");
1164 1536 /* No parameters cmd */
1165   - fdctrl.lock = 1;
1166   - fdctrl.fifo[0] = 0x10;
1167   - fdctrl_set_fifo(1, 1);
  1537 + fdctrl->lock = 1;
  1538 + fdctrl->fifo[0] = 0x10;
  1539 + fdctrl_set_fifo(fdctrl, 1, 1);
1168 1540 return;
1169 1541 case 0xCD:
1170 1542 /* FORMAT_AND_WRITE */
1171 1543 FLOPPY_DPRINTF("FORMAT_AND_WRITE command\n");
1172 1544 /* 10 parameters cmd */
1173   - fdctrl.data_len = 11;
  1545 + fdctrl->data_len = 11;
1174 1546 goto enqueue;
1175 1547 case 0xCF:
1176 1548 /* RELATIVE_SEEK_IN */
1177 1549 FLOPPY_DPRINTF("RELATIVE_SEEK_IN command\n");
1178 1550 /* 2 parameters cmd */
1179   - fdctrl.data_len = 3;
  1551 + fdctrl->data_len = 3;
1180 1552 goto enqueue;
1181 1553 default:
1182 1554 /* Unknown command */
1183 1555 FLOPPY_ERROR("unknown command: 0x%02x\n", value);
1184   - fdctrl_unimplemented();
  1556 + fdctrl_unimplemented(fdctrl);
1185 1557 return;
1186 1558 }
1187 1559 }
1188 1560 enqueue:
1189   - fdctrl.fifo[fdctrl.data_pos] = value;
1190   - if (++fdctrl.data_pos == fdctrl.data_len) {
  1561 + FLOPPY_DPRINTF("%s: %02x\n", __func__, value);
  1562 + fdctrl->fifo[fdctrl->data_pos] = value;
  1563 + if (++fdctrl->data_pos == fdctrl->data_len) {
1191 1564 /* We now have all parameters
1192 1565 * and will be able to treat the command
1193 1566 */
1194   - switch (fdctrl.fifo[0] & 0x1F) {
  1567 + if (fdctrl->data_state & FD_STATE_FORMAT) {
  1568 + fdctrl_format_sector(fdctrl);
  1569 + return;
  1570 + }
  1571 + switch (fdctrl->fifo[0] & 0x1F) {
1195 1572 case 0x06:
1196 1573 {
1197 1574 /* READ variants */
1198 1575 FLOPPY_DPRINTF("treat READ command\n");
1199   - fdctrl_start_transfer(FD_DIR_READ);
  1576 + fdctrl_start_transfer(fdctrl, FD_DIR_READ);
1200 1577 return;
1201 1578 }
1202 1579 case 0x0C:
1203 1580 /* READ_DELETED variants */
1204 1581 // FLOPPY_DPRINTF("treat READ_DELETED command\n");
1205 1582 FLOPPY_ERROR("treat READ_DELETED command\n");
1206   - fdctrl_start_transfer_del(1);
  1583 + fdctrl_start_transfer_del(fdctrl, FD_DIR_READ);
1207 1584 return;
1208 1585 case 0x16:
1209 1586 /* VERIFY variants */
1210 1587 // FLOPPY_DPRINTF("treat VERIFY command\n");
1211 1588 FLOPPY_ERROR("treat VERIFY command\n");
1212   - fdctrl_stop_transfer(0x20, 0x00, 0x00);
  1589 + fdctrl_stop_transfer(fdctrl, 0x20, 0x00, 0x00);
1213 1590 return;
1214 1591 case 0x10:
1215 1592 /* SCAN_EQUAL variants */
1216 1593 // FLOPPY_DPRINTF("treat SCAN_EQUAL command\n");
1217 1594 FLOPPY_ERROR("treat SCAN_EQUAL command\n");
1218   - fdctrl_start_transfer(FD_DIR_SCANE);
  1595 + fdctrl_start_transfer(fdctrl, FD_DIR_SCANE);
1219 1596 return;
1220 1597 case 0x19:
1221 1598 /* SCAN_LOW_OR_EQUAL variants */
1222 1599 // FLOPPY_DPRINTF("treat SCAN_LOW_OR_EQUAL command\n");
1223 1600 FLOPPY_ERROR("treat SCAN_LOW_OR_EQUAL command\n");
1224   - fdctrl_start_transfer(FD_DIR_SCANL);
  1601 + fdctrl_start_transfer(fdctrl, FD_DIR_SCANL);
1225 1602 return;
1226 1603 case 0x1D:
1227 1604 /* SCAN_HIGH_OR_EQUAL variants */
1228 1605 // FLOPPY_DPRINTF("treat SCAN_HIGH_OR_EQUAL command\n");
1229 1606 FLOPPY_ERROR("treat SCAN_HIGH_OR_EQUAL command\n");
1230   - fdctrl_start_transfer(FD_DIR_SCANH);
  1607 + fdctrl_start_transfer(fdctrl, FD_DIR_SCANH);
1231 1608 return;
1232 1609 default:
1233 1610 break;
1234 1611 }
1235   - switch (fdctrl.fifo[0] & 0x3F) {
  1612 + switch (fdctrl->fifo[0] & 0x3F) {
1236 1613 case 0x05:
1237 1614 /* WRITE variants */
1238   - FLOPPY_DPRINTF("treat WRITE command (%02x)\n", fdctrl.fifo[0]);
1239   - fdctrl_start_transfer(FD_DIR_WRITE);
  1615 + FLOPPY_DPRINTF("treat WRITE command (%02x)\n", fdctrl->fifo[0]);
  1616 + fdctrl_start_transfer(fdctrl, FD_DIR_WRITE);
1240 1617 return;
1241 1618 case 0x09:
1242 1619 /* WRITE_DELETED variants */
1243 1620 // FLOPPY_DPRINTF("treat WRITE_DELETED command\n");
1244 1621 FLOPPY_ERROR("treat WRITE_DELETED command\n");
1245   - fdctrl_start_transfer_del(FD_DIR_WRITE);
  1622 + fdctrl_start_transfer_del(fdctrl, FD_DIR_WRITE);
1246 1623 return;
1247 1624 default:
1248 1625 break;
1249 1626 }
1250   - switch (fdctrl.fifo[0]) {
  1627 + switch (fdctrl->fifo[0]) {
1251 1628 case 0x03:
1252 1629 /* SPECIFY */
1253 1630 FLOPPY_DPRINTF("treat SPECIFY command\n");
1254   - fdctrl.timer0 = (fdctrl.fifo[1] >> 4) & 0xF;
1255   - fdctrl.timer1 = fdctrl.fifo[1] >> 1;
  1631 + fdctrl->timer0 = (fdctrl->fifo[1] >> 4) & 0xF;
  1632 + fdctrl->timer1 = fdctrl->fifo[1] >> 1;
  1633 + fdctrl->dma_en = 1 - (fdctrl->fifo[2] & 1) ;
1256 1634 /* No result back */
1257   - fdctrl_reset_fifo();
  1635 + fdctrl_reset_fifo(fdctrl);
1258 1636 break;
1259 1637 case 0x04:
1260 1638 /* SENSE_DRIVE_STATUS */
1261 1639 FLOPPY_DPRINTF("treat SENSE_DRIVE_STATUS command\n");
1262   - fdctrl.cur_drv = fdctrl.fifo[1] & 1;
1263   - cur_drv = fdctrl.cur_drv == 0 ? drv0 : drv1;
1264   - cur_drv->head = (fdctrl.fifo[1] >> 2) & 1;
  1640 + fdctrl->cur_drv = fdctrl->fifo[1] & 1;
  1641 + cur_drv = get_cur_drv(fdctrl);
  1642 + cur_drv->head = (fdctrl->fifo[1] >> 2) & 1;
1265 1643 /* 1 Byte status back */
1266   - fdctrl.fifo[0] = (cur_drv->ro << 6) |
  1644 + fdctrl->fifo[0] = (cur_drv->ro << 6) |
1267 1645 (cur_drv->track == 0 ? 0x10 : 0x00) |
1268   - fdctrl.cur_drv;
1269   - fdctrl_set_fifo(1, 0);
  1646 + fdctrl->cur_drv;
  1647 + fdctrl_set_fifo(fdctrl, 1, 0);
1270 1648 break;
1271 1649 case 0x07:
1272 1650 /* RECALIBRATE */
1273 1651 FLOPPY_DPRINTF("treat RECALIBRATE command\n");
1274   - fdctrl.cur_drv = fdctrl.fifo[1] & 1;
1275   - cur_drv = fdctrl.cur_drv == 0 ? drv0 : drv1;
  1652 + fdctrl->cur_drv = fdctrl->fifo[1] & 1;
  1653 + cur_drv = get_cur_drv(fdctrl);
1276 1654 fd_recalibrate(cur_drv);
1277   - fdctrl_reset_fifo();
  1655 + fdctrl_reset_fifo(fdctrl);
1278 1656 /* Raise Interrupt */
1279   - fdctrl_raise_irq(0x20);
  1657 + fdctrl_raise_irq(fdctrl, 0x20);
1280 1658 break;
1281 1659 case 0x0F:
1282 1660 /* SEEK */
1283 1661 FLOPPY_DPRINTF("treat SEEK command\n");
1284   - fdctrl.cur_drv = fdctrl.fifo[1] & 1;
1285   - cur_drv = fdctrl.cur_drv == 0 ? drv0 : drv1;
1286   - if (fdctrl.fifo[2] <= cur_drv->track)
  1662 + fdctrl->cur_drv = fdctrl->fifo[1] & 1;
  1663 + cur_drv = get_cur_drv(fdctrl);
  1664 + fd_start(cur_drv);
  1665 + if (fdctrl->fifo[2] <= cur_drv->track)
1287 1666 cur_drv->dir = 1;
1288 1667 else
1289 1668 cur_drv->dir = 0;
1290   - cur_drv->head = (fdctrl.fifo[1] >> 2) & 1;
1291   - if (fdctrl.fifo[2] > cur_drv->max_track) {
1292   - fdctrl_raise_irq(0x60);
  1669 + fdctrl_reset_fifo(fdctrl);
  1670 + if (fdctrl->fifo[2] > cur_drv->max_track) {
  1671 + fdctrl_raise_irq(fdctrl, 0x60);
1293 1672 } else {
1294   - cur_drv->track = fdctrl.fifo[2];
1295   - fdctrl_reset_fifo();
  1673 + cur_drv->track = fdctrl->fifo[2];
1296 1674 /* Raise Interrupt */
1297   - fdctrl_raise_irq(0x20);
  1675 + fdctrl_raise_irq(fdctrl, 0x20);
1298 1676 }
1299 1677 break;
1300 1678 case 0x12:
1301 1679 /* PERPENDICULAR_MODE */
1302 1680 FLOPPY_DPRINTF("treat PERPENDICULAR_MODE command\n");
1303   - if (fdctrl.fifo[1] & 0x80)
1304   - cur_drv->perpendicular = fdctrl.fifo[1] & 0x7;
  1681 + if (fdctrl->fifo[1] & 0x80)
  1682 + cur_drv->perpendicular = fdctrl->fifo[1] & 0x7;
1305 1683 /* No result back */
1306   - fdctrl_reset_fifo();
  1684 + fdctrl_reset_fifo(fdctrl);
1307 1685 break;
1308 1686 case 0x13:
1309 1687 /* CONFIGURE */
1310 1688 FLOPPY_DPRINTF("treat CONFIGURE command\n");
1311   - fdctrl.config = fdctrl.fifo[2];
1312   - fdctrl.precomp_trk = fdctrl.fifo[3];
  1689 + fdctrl->config = fdctrl->fifo[2];
  1690 + fdctrl->precomp_trk = fdctrl->fifo[3];
1313 1691 /* No result back */
1314   - fdctrl_reset_fifo();
  1692 + fdctrl_reset_fifo(fdctrl);
1315 1693 break;
1316 1694 case 0x17:
1317 1695 /* POWERDOWN_MODE */
1318 1696 FLOPPY_DPRINTF("treat POWERDOWN_MODE command\n");
1319   - fdctrl.pwrd = fdctrl.fifo[1];
1320   - fdctrl.fifo[0] = fdctrl.fifo[1];
1321   - fdctrl_set_fifo(1, 1);
  1697 + fdctrl->pwrd = fdctrl->fifo[1];
  1698 + fdctrl->fifo[0] = fdctrl->fifo[1];
  1699 + fdctrl_set_fifo(fdctrl, 1, 1);
1322 1700 break;
1323 1701 case 0x33:
1324 1702 /* OPTION */
1325 1703 FLOPPY_DPRINTF("treat OPTION command\n");
1326 1704 /* No result back */
1327   - fdctrl_reset_fifo();
  1705 + fdctrl_reset_fifo(fdctrl);
1328 1706 break;
1329 1707 case 0x42:
1330 1708 /* READ_TRACK */
1331 1709 // FLOPPY_DPRINTF("treat READ_TRACK command\n");
1332 1710 FLOPPY_ERROR("treat READ_TRACK command\n");
1333   - fdctrl_unimplemented();
  1711 + fdctrl_start_transfer(fdctrl, FD_DIR_READ);
1334 1712 break;
1335 1713 case 0x4A:
1336 1714 /* READ_ID */
1337   -// FLOPPY_DPRINTF("treat READ_ID command\n");
1338   - FLOPPY_ERROR("treat READ_ID command\n");
1339   - fdctrl_stop_transfer(0x00, 0x00, 0x00);
  1715 + FLOPPY_DPRINTF("treat READ_ID command\n");
  1716 + fdctrl_stop_transfer(fdctrl, 0x00, 0x00, 0x00);
1340 1717 break;
1341 1718 case 0x4C:
1342 1719 /* RESTORE */
1343 1720 FLOPPY_DPRINTF("treat RESTORE command\n");
1344 1721 /* Drives position */
1345   - drv0->track = fdctrl.fifo[3];
1346   - drv1->track = fdctrl.fifo[4];
  1722 + drv0(fdctrl)->track = fdctrl->fifo[3];
  1723 + drv1(fdctrl)->track = fdctrl->fifo[4];
1347 1724 /* timers */
1348   - fdctrl.timer0 = fdctrl.fifo[7];
1349   - fdctrl.timer1 = fdctrl.fifo[8];
1350   - cur_drv->last_sect = fdctrl.fifo[9];
1351   - fdctrl.lock = fdctrl.fifo[10] >> 7;
1352   - cur_drv->perpendicular = (fdctrl.fifo[10] >> 2) & 0xF;
1353   - fdctrl.config = fdctrl.fifo[11];
1354   - fdctrl.precomp_trk = fdctrl.fifo[12];
1355   - fdctrl.pwrd = fdctrl.fifo[13];
1356   - fdctrl_reset_fifo();
  1725 + fdctrl->timer0 = fdctrl->fifo[7];
  1726 + fdctrl->timer1 = fdctrl->fifo[8];
  1727 + cur_drv->last_sect = fdctrl->fifo[9];
  1728 + fdctrl->lock = fdctrl->fifo[10] >> 7;
  1729 + cur_drv->perpendicular = (fdctrl->fifo[10] >> 2) & 0xF;
  1730 + fdctrl->config = fdctrl->fifo[11];
  1731 + fdctrl->precomp_trk = fdctrl->fifo[12];
  1732 + fdctrl->pwrd = fdctrl->fifo[13];
  1733 + fdctrl_reset_fifo(fdctrl);
1357 1734 break;
1358 1735 case 0x4D:
1359 1736 /* FORMAT_TRACK */
1360   -// FLOPPY_DPRINTF("treat FORMAT_TRACK command\n");
1361   - FLOPPY_ERROR("treat FORMAT_TRACK command\n");
1362   - fdctrl_unimplemented();
  1737 + FLOPPY_DPRINTF("treat FORMAT_TRACK command\n");
  1738 + fdctrl->cur_drv = fdctrl->fifo[1] & 1;
  1739 + cur_drv = get_cur_drv(fdctrl);
  1740 + fdctrl->data_state |= FD_STATE_FORMAT;
  1741 + if (fdctrl->fifo[0] & 0x80)
  1742 + fdctrl->data_state |= FD_STATE_MULTI;
  1743 + else
  1744 + fdctrl->data_state &= ~FD_STATE_MULTI;
  1745 + fdctrl->data_state &= ~FD_STATE_SEEK;
  1746 + cur_drv->bps =
  1747 + fdctrl->fifo[2] > 7 ? 16384 : 128 << fdctrl->fifo[2];
  1748 +#if 0
  1749 + cur_drv->last_sect =
  1750 + cur_drv->flags & FDISK_DBL_SIDES ? fdctrl->fifo[3] :
  1751 + fdctrl->fifo[3] / 2;
  1752 +#else
  1753 + cur_drv->last_sect = fdctrl->fifo[3];
  1754 +#endif
  1755 + /* Bochs BIOS is buggy and don't send format informations
  1756 + * for each sector. So, pretend all's done right now...
  1757 + */
  1758 + fdctrl->data_state &= ~FD_STATE_FORMAT;
  1759 + fdctrl_stop_transfer(fdctrl, 0x00, 0x00, 0x00);
1363 1760 break;
1364 1761 case 0x8E:
1365 1762 /* DRIVE_SPECIFICATION_COMMAND */
1366 1763 FLOPPY_DPRINTF("treat DRIVE_SPECIFICATION_COMMAND command\n");
1367   - if (fdctrl.fifo[fdctrl.data_pos - 1] & 0x80) {
  1764 + if (fdctrl->fifo[fdctrl->data_pos - 1] & 0x80) {
1368 1765 /* Command parameters done */
1369   - if (fdctrl.fifo[fdctrl.data_pos - 1] & 0x40) {
1370   - fdctrl.fifo[0] = fdctrl.fifo[1];
1371   - fdctrl.fifo[2] = 0;
1372   - fdctrl.fifo[3] = 0;
1373   - fdctrl_set_fifo(4, 1);
  1766 + if (fdctrl->fifo[fdctrl->data_pos - 1] & 0x40) {
  1767 + fdctrl->fifo[0] = fdctrl->fifo[1];
  1768 + fdctrl->fifo[2] = 0;
  1769 + fdctrl->fifo[3] = 0;
  1770 + fdctrl_set_fifo(fdctrl, 4, 1);
1374 1771 } else {
1375   - fdctrl_reset_fifo();
  1772 + fdctrl_reset_fifo(fdctrl);
1376 1773 }
1377   - } else if (fdctrl.data_len > 7) {
  1774 + } else if (fdctrl->data_len > 7) {
1378 1775 /* ERROR */
1379   - fdctrl.fifo[0] = 0x80 |
1380   - (cur_drv->head << 2) | fdctrl.cur_drv;
1381   - fdctrl_set_fifo(1, 1);
  1776 + fdctrl->fifo[0] = 0x80 |
  1777 + (cur_drv->head << 2) | fdctrl->cur_drv;
  1778 + fdctrl_set_fifo(fdctrl, 1, 1);
1382 1779 }
1383 1780 break;
1384 1781 case 0x8F:
1385 1782 /* RELATIVE_SEEK_OUT */
1386 1783 FLOPPY_DPRINTF("treat RELATIVE_SEEK_OUT command\n");
1387   - fdctrl.cur_drv = fdctrl.fifo[1] & 1;
1388   - cur_drv = fdctrl.cur_drv == 0 ? drv0 : drv1;
1389   - cur_drv->head = (fdctrl.fifo[1] >> 2) & 1;
1390   - if (fdctrl.fifo[2] + cur_drv->track > cur_drv->max_track) {
1391   - /* ERROR */
1392   - fdctrl_raise_irq(0x70);
1393   - } else {
1394   - cur_drv->track += fdctrl.fifo[2];
  1784 + fdctrl->cur_drv = fdctrl->fifo[1] & 1;
  1785 + cur_drv = get_cur_drv(fdctrl);
  1786 + fd_start(cur_drv);
1395 1787 cur_drv->dir = 0;
1396   - fdctrl_reset_fifo();
1397   - fdctrl_raise_irq(0x20);
  1788 + if (fdctrl->fifo[2] + cur_drv->track >= cur_drv->max_track) {
  1789 + cur_drv->track = cur_drv->max_track - 1;
  1790 + } else {
  1791 + cur_drv->track += fdctrl->fifo[2];
1398 1792 }
  1793 + fdctrl_reset_fifo(fdctrl);
  1794 + fdctrl_raise_irq(fdctrl, 0x20);
1399 1795 break;
1400 1796 case 0xCD:
1401 1797 /* FORMAT_AND_WRITE */
1402 1798 // FLOPPY_DPRINTF("treat FORMAT_AND_WRITE command\n");
1403 1799 FLOPPY_ERROR("treat FORMAT_AND_WRITE command\n");
1404   - fdctrl_unimplemented();
  1800 + fdctrl_unimplemented(fdctrl);
1405 1801 break;
1406 1802 case 0xCF:
1407 1803 /* RELATIVE_SEEK_IN */
1408 1804 FLOPPY_DPRINTF("treat RELATIVE_SEEK_IN command\n");
1409   - fdctrl.cur_drv = fdctrl.fifo[1] & 1;
1410   - cur_drv = fdctrl.cur_drv == 0 ? drv0 : drv1;
1411   - cur_drv->head = (fdctrl.fifo[1] >> 2) & 1;
1412   - if (fdctrl.fifo[2] > cur_drv->track) {
1413   - /* ERROR */
1414   - fdctrl_raise_irq(0x60);
1415   - } else {
1416   - fdctrl_reset_fifo();
1417   - cur_drv->track -= fdctrl.fifo[2];
  1805 + fdctrl->cur_drv = fdctrl->fifo[1] & 1;
  1806 + cur_drv = get_cur_drv(fdctrl);
  1807 + fd_start(cur_drv);
1418 1808 cur_drv->dir = 1;
1419   - /* Raise Interrupt */
1420   - fdctrl_raise_irq(0x20);
  1809 + if (fdctrl->fifo[2] > cur_drv->track) {
  1810 + cur_drv->track = 0;
  1811 + } else {
  1812 + cur_drv->track -= fdctrl->fifo[2];
1421 1813 }
  1814 + fdctrl_reset_fifo(fdctrl);
  1815 + /* Raise Interrupt */
  1816 + fdctrl_raise_irq(fdctrl, 0x20);
1422 1817 break;
1423 1818 }
1424 1819 }
... ...
... ... @@ -57,6 +57,7 @@
57 57  
58 58 int speaker_data_on;
59 59 int dummy_refresh_clock;
  60 +static fdctrl_t *floppy_controller;
60 61  
61 62 static void ioport80_write(void *opaque, uint32_t addr, uint32_t data)
62 63 {
... ... @@ -106,8 +107,8 @@ static void cmos_init(int ram_size, int boot_device)
106 107  
107 108 /* floppy type */
108 109  
109   - fd0 = fdctrl_get_drive_type(0);
110   - fd1 = fdctrl_get_drive_type(1);
  110 + fd0 = fdctrl_get_drive_type(floppy_controller, 0);
  111 + fd1 = fdctrl_get_drive_type(floppy_controller, 1);
111 112  
112 113 s->cmos_data[0x10] = 0;
113 114 switch (fd0) {
... ... @@ -370,7 +371,7 @@ void pc_init(int ram_size, int vga_ram_size, int boot_device,
370 371 DMA_init();
371 372 SB16_init();
372 373  
373   - fdctrl_init(6, 2, 0, 0x3f0, fd_table);
  374 + floppy_controller = fdctrl_init(6, 2, 0, 0x3f0, fd_table);
374 375  
375 376 cmos_init(ram_size, boot_device);
376 377 }
... ...
... ... @@ -186,9 +186,12 @@ void SB16_init (void);
186 186 #define MAX_FD 2
187 187 extern BlockDriverState *fd_table[MAX_FD];
188 188  
189   -void fdctrl_init (int irq_lvl, int dma_chann, int mem_mapped, uint32_t base,
190   - BlockDriverState **fds);
191   -int fdctrl_get_drive_type(int drive_num);
  189 +typedef struct fdctrl_t fdctrl_t;
  190 +
  191 +fdctrl_t *fdctrl_init (int irq_lvl, int dma_chann, int mem_mapped,
  192 + uint32_t io_base,
  193 + BlockDriverState **fds);
  194 +int fdctrl_get_drive_type(fdctrl_t *fdctrl, int drive_num);
192 195  
193 196 /* ne2000.c */
194 197  
... ...