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,7 +54,8 @@ typedef enum fdisk_type_t {
54 FDRIVE_DISK_288 = 0x01, /* 2.88 MB disk */ 54 FDRIVE_DISK_288 = 0x01, /* 2.88 MB disk */
55 FDRIVE_DISK_144 = 0x02, /* 1.44 MB disk */ 55 FDRIVE_DISK_144 = 0x02, /* 1.44 MB disk */
56 FDRIVE_DISK_720 = 0x03, /* 720 kB disk */ 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 } fdisk_type_t; 59 } fdisk_type_t;
59 60
60 typedef enum fdrive_type_t { 61 typedef enum fdrive_type_t {
@@ -64,13 +65,21 @@ typedef enum fdrive_type_t { @@ -64,13 +65,21 @@ typedef enum fdrive_type_t {
64 FDRIVE_DRV_NONE = 0x03, /* No drive connected */ 65 FDRIVE_DRV_NONE = 0x03, /* No drive connected */
65 } fdrive_type_t; 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 typedef struct fdrive_t { 77 typedef struct fdrive_t {
68 BlockDriverState *bs; 78 BlockDriverState *bs;
69 /* Drive status */ 79 /* Drive status */
70 fdrive_type_t drive; 80 fdrive_type_t drive;
71 - uint8_t motor; /* on/off */ 81 + fdrive_flags_t drflags;
72 uint8_t perpendicular; /* 2.88 MB access mode */ 82 uint8_t perpendicular; /* 2.88 MB access mode */
73 - uint8_t rv; /* Revalidated */  
74 /* Position */ 83 /* Position */
75 uint8_t head; 84 uint8_t head;
76 uint8_t track; 85 uint8_t track;
@@ -80,8 +89,10 @@ typedef struct fdrive_t { @@ -80,8 +89,10 @@ typedef struct fdrive_t {
80 uint8_t rw; /* Read/write */ 89 uint8_t rw; /* Read/write */
81 /* Media */ 90 /* Media */
82 fdisk_type_t disk; /* Disk type */ 91 fdisk_type_t disk; /* Disk type */
  92 + fdisk_flags_t flags;
83 uint8_t last_sect; /* Nb sector per track */ 93 uint8_t last_sect; /* Nb sector per track */
84 uint8_t max_track; /* Nb of tracks */ 94 uint8_t max_track; /* Nb of tracks */
  95 + uint16_t bps; /* Bytes per sector */
85 uint8_t ro; /* Is read-only */ 96 uint8_t ro; /* Is read-only */
86 } fdrive_t; 97 } fdrive_t;
87 98
@@ -93,12 +104,11 @@ static void fd_init (fdrive_t *drv, BlockDriverState *bs) @@ -93,12 +104,11 @@ static void fd_init (fdrive_t *drv, BlockDriverState *bs)
93 drv->drive = FDRIVE_DRV_144; 104 drv->drive = FDRIVE_DRV_144;
94 else 105 else
95 drv->drive = FDRIVE_DRV_NONE; 106 drv->drive = FDRIVE_DRV_NONE;
96 - drv->motor = 0; 107 + drv->drflags = 0;
97 drv->perpendicular = 0; 108 drv->perpendicular = 0;
98 - drv->rv = 0;  
99 /* Disk */ 109 /* Disk */
100 drv->disk = FDRIVE_DISK_NONE; 110 drv->disk = FDRIVE_DISK_NONE;
101 - drv->last_sect = 1; 111 + drv->last_sect = 0;
102 drv->max_track = 0; 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,18 +128,25 @@ static int fd_seek (fdrive_t *drv, uint8_t head, uint8_t track, uint8_t sect,
118 int enable_seek) 128 int enable_seek)
119 { 129 {
120 uint32_t sector; 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 return 2; 139 return 2;
126 } 140 }
127 if (sect > drv->last_sect) { 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 return 3; 146 return 3;
131 } 147 }
132 sector = _fd_sector(head, track, sect, drv->last_sect); 148 sector = _fd_sector(head, track, sect, drv->last_sect);
  149 + ret = 0;
133 if (sector != fd_sector(drv)) { 150 if (sector != fd_sector(drv)) {
134 #if 0 151 #if 0
135 if (!enable_seek) { 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,12 +156,13 @@ static int fd_seek (fdrive_t *drv, uint8_t head, uint8_t track, uint8_t sect,
139 } 156 }
140 #endif 157 #endif
141 drv->head = head; 158 drv->head = head;
  159 + if (drv->track != track)
  160 + ret = 1;
142 drv->track = track; 161 drv->track = track;
143 drv->sect = sect; 162 drv->sect = sect;
144 - return 1;  
145 } 163 }
146 164
147 - return 0; 165 + return ret;
148 } 166 }
149 167
150 /* Set drive back to track 0 */ 168 /* Set drive back to track 0 */
@@ -162,60 +180,275 @@ static void fd_recalibrate (fdrive_t *drv) @@ -162,60 +180,275 @@ static void fd_recalibrate (fdrive_t *drv)
162 static void fd_revalidate (fdrive_t *drv) 180 static void fd_revalidate (fdrive_t *drv)
163 { 181 {
164 int64_t nb_sectors; 182 int64_t nb_sectors;
  183 + int nb_heads, max_track, last_sect, ro;
165 184
166 FLOPPY_DPRINTF("revalidate\n"); 185 FLOPPY_DPRINTF("revalidate\n");
167 - drv->rv = 0; 186 + drv->drflags &= ~FDRIVE_REVALIDATE;
  187 +
168 /* if no drive present, cannot do more */ 188 /* if no drive present, cannot do more */
169 if (!drv->bs) 189 if (!drv->bs)
170 return; 190 return;
171 - 191 +
172 if (bdrv_is_inserted(drv->bs)) { 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 bdrv_get_geometry(drv->bs, &nb_sectors); 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 drv->disk = FDRIVE_DISK_288; 245 drv->disk = FDRIVE_DISK_288;
180 drv->last_sect = 36; 246 drv->last_sect = 36;
181 drv->max_track = 80; 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 drv->disk = FDRIVE_DISK_144; 312 drv->disk = FDRIVE_DISK_144;
186 drv->last_sect = 18; 313 drv->last_sect = 18;
187 drv->max_track = 80; 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 drv->disk = FDRIVE_DISK_720; 362 drv->disk = FDRIVE_DISK_720;
191 drv->last_sect = 9; 363 drv->last_sect = 9;
192 drv->max_track = 80; 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 } else { 433 } else {
  434 + printf("No disk in drive\n");
197 drv->disk = FDRIVE_DISK_NONE; 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 /* Motor control */ 443 /* Motor control */
211 static void fd_start (fdrive_t *drv) 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 static void fd_stop (fdrive_t *drv) 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 /* Re-initialise a drives (motor off, repositioned) */ 454 /* Re-initialise a drives (motor off, repositioned) */
@@ -228,21 +461,21 @@ static void fd_reset (fdrive_t *drv) @@ -228,21 +461,21 @@ static void fd_reset (fdrive_t *drv)
228 /********************************************************/ 461 /********************************************************/
229 /* Intel 82078 floppy disk controler emulation */ 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 static int fdctrl_transfer_handler (void *opaque, target_ulong addr, int size); 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 enum { 480 enum {
248 FD_CTRL_ACTIVE = 0x01, 481 FD_CTRL_ACTIVE = 0x01,
@@ -267,18 +500,24 @@ enum { @@ -267,18 +500,24 @@ enum {
267 FD_STATE_STATE = 0x03, 500 FD_STATE_STATE = 0x03,
268 FD_STATE_MULTI = 0x10, 501 FD_STATE_MULTI = 0x10,
269 FD_STATE_SEEK = 0x20, 502 FD_STATE_SEEK = 0x20,
  503 + FD_STATE_FORMAT = 0x40,
270 }; 504 };
271 505
272 #define FD_STATE(state) ((state) & FD_STATE_STATE) 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 #define FD_MULTI_TRACK(state) ((state) & FD_STATE_MULTI) 509 #define FD_MULTI_TRACK(state) ((state) & FD_STATE_MULTI)
274 #define FD_DID_SEEK(state) ((state) & FD_STATE_SEEK) 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 /* Controler's identification */ 515 /* Controler's identification */
278 uint8_t version; 516 uint8_t version;
279 /* HW */ 517 /* HW */
280 int irq_lvl; 518 int irq_lvl;
281 int dma_chann; 519 int dma_chann;
  520 + uint32_t io_base;
282 /* Controler state */ 521 /* Controler state */
283 uint8_t state; 522 uint8_t state;
284 uint8_t dma_en; 523 uint8_t dma_en;
@@ -303,35 +542,91 @@ typedef struct fdctrl_t { @@ -303,35 +542,91 @@ typedef struct fdctrl_t {
303 uint8_t pwrd; 542 uint8_t pwrd;
304 /* Floppy drives */ 543 /* Floppy drives */
305 fdrive_t drives[2]; 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 // int io_mem; 603 // int io_mem;
314 int i; 604 int i;
315 605
316 FLOPPY_DPRINTF("init controler\n"); 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 } else { 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 if (mem_mapped) { 630 if (mem_mapped) {
336 FLOPPY_ERROR("memory mapped floppy not supported by now !\n"); 631 FLOPPY_ERROR("memory mapped floppy not supported by now !\n");
337 #if 0 632 #if 0
@@ -339,110 +634,113 @@ void fdctrl_init (int irq_lvl, int dma_chann, int mem_mapped, uint32_t base, @@ -339,110 +634,113 @@ void fdctrl_init (int irq_lvl, int dma_chann, int mem_mapped, uint32_t base,
339 cpu_register_physical_memory(base, 0x08, io_mem); 634 cpu_register_physical_memory(base, 0x08, io_mem);
340 #endif 635 #endif
341 } else { 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 for (i = 0; i < MAX_FD; i++) { 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 /* Change IRQ state */ 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 FLOPPY_DPRINTF("Set interrupt status to 0x%02x\n", status); 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 /* Reset controler */ 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 int i; 676 int i;
388 677
389 FLOPPY_DPRINTF("reset controler\n"); 678 FLOPPY_DPRINTF("reset controler\n");
390 - fdctrl_reset_irq(); 679 + fdctrl_reset_irq(fdctrl);
391 /* Initialise controler */ 680 /* Initialise controler */
392 - fdctrl.cur_drv = 0; 681 + fdctrl->cur_drv = 0;
393 /* FIFO state */ 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 for (i = 0; i < MAX_FD; i++) 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 if (do_irq) 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 /* Status B register : 0x01 (read-only) */ 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 FLOPPY_DPRINTF("status register: 0x00\n"); 713 FLOPPY_DPRINTF("status register: 0x00\n");
410 714
411 return 0; 715 return 0;
412 } 716 }
413 717
414 /* Digital output register : 0x02 */ 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 uint32_t retval = 0; 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 /* Drive motors state indicators */ 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 /* DMA enable */ 728 /* DMA enable */
427 - retval |= fdctrl.dma_en << 3; 729 + retval |= fdctrl->dma_en << 3;
428 /* Reset indicator */ 730 /* Reset indicator */
429 - retval |= (fdctrl.state & FD_CTRL_RESET) == 0 ? 0x04 : 0; 731 + retval |= (fdctrl->state & FD_CTRL_RESET) == 0 ? 0x04 : 0;
430 /* Selected drive */ 732 /* Selected drive */
431 - retval |= fdctrl.cur_drv; 733 + retval |= fdctrl->cur_drv;
432 FLOPPY_DPRINTF("digital output register: 0x%02x\n", retval); 734 FLOPPY_DPRINTF("digital output register: 0x%02x\n", retval);
433 735
434 return retval; 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 /* Reset mode */ 742 /* Reset mode */
445 - if (fdctrl.state & FD_CTRL_RESET) { 743 + if (fdctrl->state & FD_CTRL_RESET) {
446 if (!(value & 0x04)) { 744 if (!(value & 0x04)) {
447 FLOPPY_DPRINTF("Floppy controler in RESET state !\n"); 745 FLOPPY_DPRINTF("Floppy controler in RESET state !\n");
448 return; 746 return;
@@ -451,81 +749,81 @@ static void fdctrl_write_dor (void *opaque, uint32_t reg, uint32_t value) @@ -451,81 +749,81 @@ static void fdctrl_write_dor (void *opaque, uint32_t reg, uint32_t value)
451 FLOPPY_DPRINTF("digital output register set to 0x%02x\n", value); 749 FLOPPY_DPRINTF("digital output register set to 0x%02x\n", value);
452 /* Drive motors state indicators */ 750 /* Drive motors state indicators */
453 if (value & 0x20) 751 if (value & 0x20)
454 - fd_start(drv1); 752 + fd_start(drv1(fdctrl));
455 else 753 else
456 - fd_stop(drv1); 754 + fd_stop(drv1(fdctrl));
457 if (value & 0x10) 755 if (value & 0x10)
458 - fd_start(drv0); 756 + fd_start(drv0(fdctrl));
459 else 757 else
460 - fd_stop(drv0); 758 + fd_stop(drv0(fdctrl));
461 /* DMA enable */ 759 /* DMA enable */
462 #if 0 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 #endif 763 #endif
466 /* Reset */ 764 /* Reset */
467 if (!(value & 0x04)) { 765 if (!(value & 0x04)) {
468 - if (!(fdctrl.state & FD_CTRL_RESET)) { 766 + if (!(fdctrl->state & FD_CTRL_RESET)) {
469 FLOPPY_DPRINTF("controler enter RESET state\n"); 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 } else { 771 } else {
474 - if (fdctrl.state & FD_CTRL_RESET) { 772 + if (fdctrl->state & FD_CTRL_RESET) {
475 FLOPPY_DPRINTF("controler out of RESET state\n"); 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 /* Selected drive */ 777 /* Selected drive */
480 - fdctrl.cur_drv = value & 1; 778 + fdctrl->cur_drv = value & 1;
481 } 779 }
482 780
483 /* Tape drive register : 0x03 */ 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 uint32_t retval = 0; 784 uint32_t retval = 0;
487 785
488 - fdctrl_reset_irq(); 786 + fdctrl_reset_irq(fdctrl);
489 /* Disk boot selection indicator */ 787 /* Disk boot selection indicator */
490 - retval |= fdctrl.bootsel << 2; 788 + retval |= fdctrl->bootsel << 2;
491 /* Tape indicators: never allowed */ 789 /* Tape indicators: never allowed */
492 FLOPPY_DPRINTF("tape drive register: 0x%02x\n", retval); 790 FLOPPY_DPRINTF("tape drive register: 0x%02x\n", retval);
493 791
494 return retval; 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 /* Reset mode */ 798 /* Reset mode */
501 - if (fdctrl.state & FD_CTRL_RESET) { 799 + if (fdctrl->state & FD_CTRL_RESET) {
502 FLOPPY_DPRINTF("Floppy controler in RESET state !\n"); 800 FLOPPY_DPRINTF("Floppy controler in RESET state !\n");
503 return; 801 return;
504 } 802 }
505 FLOPPY_DPRINTF("tape drive register set to 0x%02x\n", value); 803 FLOPPY_DPRINTF("tape drive register set to 0x%02x\n", value);
506 /* Disk boot selection indicator */ 804 /* Disk boot selection indicator */
507 - fdctrl.bootsel = (value >> 2) & 1; 805 + fdctrl->bootsel = (value >> 2) & 1;
508 /* Tape indicators: never allow */ 806 /* Tape indicators: never allow */
509 } 807 }
510 808
511 /* Main status register : 0x04 (read) */ 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 uint32_t retval = 0; 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 /* Data transfer allowed */ 817 /* Data transfer allowed */
520 retval |= 0x80; 818 retval |= 0x80;
521 /* Data transfer direction indicator */ 819 /* Data transfer direction indicator */
522 - if (fdctrl.data_dir == FD_DIR_READ) 820 + if (fdctrl->data_dir == FD_DIR_READ)
523 retval |= 0x40; 821 retval |= 0x40;
524 } 822 }
525 /* Should handle 0x20 for SPECIFY command */ 823 /* Should handle 0x20 for SPECIFY command */
526 /* Command busy indicator */ 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 retval |= 0x10; 827 retval |= 0x10;
530 FLOPPY_DPRINTF("main status register: 0x%02x\n", retval); 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,150 +831,142 @@ static uint32_t fdctrl_read_main_status (void *opaque, uint32_t reg)
533 } 831 }
534 832
535 /* Data select rate register : 0x04 (write) */ 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 /* Reset mode */ 837 /* Reset mode */
540 - if (fdctrl.state & FD_CTRL_RESET) {  
541 - if (reg != 0x2 || !(value & 0x04)) { 838 + if (fdctrl->state & FD_CTRL_RESET) {
542 FLOPPY_DPRINTF("Floppy controler in RESET state !\n"); 839 FLOPPY_DPRINTF("Floppy controler in RESET state !\n");
543 return; 840 return;
544 } 841 }
545 - }  
546 FLOPPY_DPRINTF("select rate register set to 0x%02x\n", value); 842 FLOPPY_DPRINTF("select rate register set to 0x%02x\n", value);
547 /* Reset: autoclear */ 843 /* Reset: autoclear */
548 if (value & 0x80) { 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 if (value & 0x40) { 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 // fdctrl.precomp = (value >> 2) & 0x07; 853 // fdctrl.precomp = (value >> 2) & 0x07;
558 } 854 }
559 855
560 /* Digital input register : 0x07 (read-only) */ 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 uint32_t retval = 0; 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 retval |= 0x80; 864 retval |= 0x80;
571 if (retval != 0) 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 return retval; 870 return retval;
577 } 871 }
578 872
579 /* FIFO state control */ 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 /* Set FIFO status for the host to read */ 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 if (do_irq) 888 if (do_irq)
595 - fdctrl_raise_irq(0x00); 889 + fdctrl_raise_irq(fdctrl, 0x00);
596 } 890 }
597 891
598 /* Set an error: unimplemented/unknown command */ 892 /* Set an error: unimplemented/unknown command */
599 -static void fdctrl_unimplemented (void) 893 +static void fdctrl_unimplemented (fdctrl_t *fdctrl)
600 { 894 {
601 #if 0 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 #else 903 #else
612 - fdctrl_reset_fifo(); 904 + // fdctrl_reset_fifo(fdctrl);
  905 + fdctrl->fifo[0] = 0x80;
  906 + fdctrl_set_fifo(fdctrl, 1, 0);
613 #endif 907 #endif
614 } 908 }
615 909
616 /* Callback for transfer end (stop or abort) */ 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 FLOPPY_DPRINTF("transfer status: %02x %02x %02x (%02x)\n", 917 FLOPPY_DPRINTF("transfer status: %02x %02x %02x (%02x)\n",
626 status0, status1, status2, 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 /* Prepare a data transfer (either DMA or FIFO) */ 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 uint8_t kh, kt, ks; 937 uint8_t kh, kt, ks;
646 int did_seek; 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 FLOPPY_DPRINTF("Start tranfert at %d %d %02x %02x (%d)\n", 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 _fd_sector(kh, kt, ks, cur_drv->last_sect)); 947 _fd_sector(kh, kt, ks, cur_drv->last_sect));
658 did_seek = 0; 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 case 2: 950 case 2:
661 /* sect too big */ 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 return; 956 return;
667 case 3: 957 case 3:
668 /* track too big */ 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 return; 963 return;
674 case 4: 964 case 4:
675 /* No seek enabled */ 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 return; 970 return;
681 case 1: 971 case 1:
682 did_seek = 1; 972 did_seek = 1;
@@ -685,299 +975,379 @@ static void fdctrl_start_transfer (int direction) @@ -685,299 +975,379 @@ static void fdctrl_start_transfer (int direction)
685 break; 975 break;
686 } 976 }
687 /* Set the FIFO state */ 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 if (did_seek) 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 int dma_mode; 1000 int dma_mode;
697 /* DMA transfer are enabled. Check if DMA channel is well programmed */ 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 dma_mode = (dma_mode >> 2) & 3; 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 if (((direction == FD_DIR_SCANE || direction == FD_DIR_SCANL || 1008 if (((direction == FD_DIR_SCANE || direction == FD_DIR_SCANL ||
704 direction == FD_DIR_SCANH) && dma_mode == 0) || 1009 direction == FD_DIR_SCANH) && dma_mode == 0) ||
705 (direction == FD_DIR_WRITE && dma_mode == 2) || 1010 (direction == FD_DIR_WRITE && dma_mode == 2) ||
706 (direction == FD_DIR_READ && dma_mode == 1)) { 1011 (direction == FD_DIR_READ && dma_mode == 1)) {
707 /* No access is allowed until DMA transfer has completed */ 1012 /* No access is allowed until DMA transfer has completed */
708 - fdctrl.state |= FD_CTRL_BUSY; 1013 + fdctrl->state |= FD_CTRL_BUSY;
709 /* Now, we just have to wait for the DMA controler to 1014 /* Now, we just have to wait for the DMA controler to
710 * recall us... 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 return; 1019 return;
  1020 + } else {
  1021 + FLOPPY_ERROR("dma_mode=%d direction=%d\n", dma_mode, direction);
715 } 1022 }
716 } 1023 }
717 FLOPPY_DPRINTF("start non-DMA transfer\n"); 1024 FLOPPY_DPRINTF("start non-DMA transfer\n");
718 /* IO based transfer: calculate len */ 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 return; 1028 return;
730 } 1029 }
731 1030
732 /* Prepare a transfer of deleted data */ 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 /* We don't handle deleted data, 1034 /* We don't handle deleted data,
736 * so we don't return *ANYTHING* 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 /* handlers for DMA transfers */ 1040 /* handlers for DMA transfers */
742 -/* XXX: the partial transfer logic seems to be broken */  
743 static int fdctrl_transfer_handler (void *opaque, target_ulong addr, int size) 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 uint8_t status0 = 0x00, status1 = 0x00, status2 = 0x00; 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 FLOPPY_DPRINTF("Not in DMA transfer mode !\n"); 1051 FLOPPY_DPRINTF("Not in DMA transfer mode !\n");
753 return 0; 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 status2 = 0x04; 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 if (cur_drv->bs == NULL) { 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 goto transfer_error; 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 FLOPPY_DPRINTF("Floppy: error getting sector %d\n", 1083 FLOPPY_DPRINTF("Floppy: error getting sector %d\n",
779 fd_sector(cur_drv)); 1084 fd_sector(cur_drv));
780 /* Sure, image size is too small... */ 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 if (ret == 0) { 1114 if (ret == 0) {
800 status2 = 0x08; 1115 status2 = 0x08;
801 goto end_transfer; 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 status2 = 0x00; 1120 status2 = 0x00;
806 goto end_transfer; 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 /* Seek to next sector */ 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 cur_drv->head = 1; 1140 cur_drv->head = 1;
827 } else { 1141 } else {
828 - cur_drv->track++;  
829 cur_drv->head = 0; 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 } else { 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 end_transfer: 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 status2 = 0x08; 1164 status2 = 0x08;
858 - if (FD_DID_SEEK(fdctrl.data_state)) 1165 + if (FD_DID_SEEK(fdctrl->data_state))
859 status0 |= 0x20; 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 transfer_error: 1170 transfer_error:
862 1171
863 - return fdctrl.data_pos; 1172 + return len;
864 } 1173 }
865 1174
866 /* Data register : 0x05 */ 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 uint32_t retval = 0; 1179 uint32_t retval = 0;
871 int pos, len; 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 FLOPPY_ERROR("can't read data in CMD state\n"); 1186 FLOPPY_ERROR("can't read data in CMD state\n");
880 return 0; 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 pos %= FD_SECTOR_LEN; 1191 pos %= FD_SECTOR_LEN;
885 if (pos == 0) { 1192 if (pos == 0) {
886 - len = fdctrl.data_len - fdctrl.data_pos; 1193 + len = fdctrl->data_len - fdctrl->data_pos;
887 if (len > FD_SECTOR_LEN) 1194 if (len > FD_SECTOR_LEN)
888 len = FD_SECTOR_LEN; 1195 len = FD_SECTOR_LEN;
889 bdrv_read(cur_drv->bs, fd_sector(cur_drv), 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 /* Switch from transfert mode to status mode 1203 /* Switch from transfert mode to status mode
897 * then from status mode to command mode 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 else 1208 else
902 - fdctrl_reset_fifo(); 1209 + fdctrl_reset_fifo(fdctrl);
903 } 1210 }
904 FLOPPY_DPRINTF("data register: 0x%02x\n", retval); 1211 FLOPPY_DPRINTF("data register: 0x%02x\n", retval);
905 1212
906 return retval; 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 /* Reset mode */ 1287 /* Reset mode */
918 - if (fdctrl.state & FD_CTRL_RESET) { 1288 + if (fdctrl->state & FD_CTRL_RESET) {
919 FLOPPY_DPRINTF("Floppy controler in RESET state !\n"); 1289 FLOPPY_DPRINTF("Floppy controler in RESET state !\n");
920 return; 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 FLOPPY_ERROR("can't write data in status mode\n"); 1294 FLOPPY_ERROR("can't write data in status mode\n");
925 return; 1295 return;
926 } 1296 }
927 /* Is it write command time ? */ 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 /* FIFO data write */ 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 bdrv_write(cur_drv->bs, fd_sector(cur_drv), 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 /* Switch from transfert mode to status mode 1306 /* Switch from transfert mode to status mode
937 * then from status mode to command mode 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 return; 1311 return;
942 } 1312 }
943 - if (fdctrl.data_pos == 0) { 1313 + if (fdctrl->data_pos == 0) {
944 /* Command */ 1314 /* Command */
945 switch (value & 0x5F) { 1315 switch (value & 0x5F) {
946 case 0x46: 1316 case 0x46:
947 /* READ variants */ 1317 /* READ variants */
948 FLOPPY_DPRINTF("READ command\n"); 1318 FLOPPY_DPRINTF("READ command\n");
949 /* 8 parameters cmd */ 1319 /* 8 parameters cmd */
950 - fdctrl.data_len = 9; 1320 + fdctrl->data_len = 9;
951 goto enqueue; 1321 goto enqueue;
952 case 0x4C: 1322 case 0x4C:
953 /* READ_DELETED variants */ 1323 /* READ_DELETED variants */
954 FLOPPY_DPRINTF("READ_DELETED command\n"); 1324 FLOPPY_DPRINTF("READ_DELETED command\n");
955 /* 8 parameters cmd */ 1325 /* 8 parameters cmd */
956 - fdctrl.data_len = 9; 1326 + fdctrl->data_len = 9;
957 goto enqueue; 1327 goto enqueue;
958 case 0x50: 1328 case 0x50:
959 /* SCAN_EQUAL variants */ 1329 /* SCAN_EQUAL variants */
960 FLOPPY_DPRINTF("SCAN_EQUAL command\n"); 1330 FLOPPY_DPRINTF("SCAN_EQUAL command\n");
961 /* 8 parameters cmd */ 1331 /* 8 parameters cmd */
962 - fdctrl.data_len = 9; 1332 + fdctrl->data_len = 9;
963 goto enqueue; 1333 goto enqueue;
964 case 0x56: 1334 case 0x56:
965 /* VERIFY variants */ 1335 /* VERIFY variants */
966 FLOPPY_DPRINTF("VERIFY command\n"); 1336 FLOPPY_DPRINTF("VERIFY command\n");
967 /* 8 parameters cmd */ 1337 /* 8 parameters cmd */
968 - fdctrl.data_len = 9; 1338 + fdctrl->data_len = 9;
969 goto enqueue; 1339 goto enqueue;
970 case 0x59: 1340 case 0x59:
971 /* SCAN_LOW_OR_EQUAL variants */ 1341 /* SCAN_LOW_OR_EQUAL variants */
972 FLOPPY_DPRINTF("SCAN_LOW_OR_EQUAL command\n"); 1342 FLOPPY_DPRINTF("SCAN_LOW_OR_EQUAL command\n");
973 /* 8 parameters cmd */ 1343 /* 8 parameters cmd */
974 - fdctrl.data_len = 9; 1344 + fdctrl->data_len = 9;
975 goto enqueue; 1345 goto enqueue;
976 case 0x5D: 1346 case 0x5D:
977 /* SCAN_HIGH_OR_EQUAL variants */ 1347 /* SCAN_HIGH_OR_EQUAL variants */
978 FLOPPY_DPRINTF("SCAN_HIGH_OR_EQUAL command\n"); 1348 FLOPPY_DPRINTF("SCAN_HIGH_OR_EQUAL command\n");
979 /* 8 parameters cmd */ 1349 /* 8 parameters cmd */
980 - fdctrl.data_len = 9; 1350 + fdctrl->data_len = 9;
981 goto enqueue; 1351 goto enqueue;
982 default: 1352 default:
983 break; 1353 break;
@@ -987,13 +1357,13 @@ static void fdctrl_write_data (void *opaque, uint32_t reg, uint32_t value) @@ -987,13 +1357,13 @@ static void fdctrl_write_data (void *opaque, uint32_t reg, uint32_t value)
987 /* WRITE variants */ 1357 /* WRITE variants */
988 FLOPPY_DPRINTF("WRITE command\n"); 1358 FLOPPY_DPRINTF("WRITE command\n");
989 /* 8 parameters cmd */ 1359 /* 8 parameters cmd */
990 - fdctrl.data_len = 9; 1360 + fdctrl->data_len = 9;
991 goto enqueue; 1361 goto enqueue;
992 case 0x49: 1362 case 0x49:
993 /* WRITE_DELETED variants */ 1363 /* WRITE_DELETED variants */
994 FLOPPY_DPRINTF("WRITE_DELETED command\n"); 1364 FLOPPY_DPRINTF("WRITE_DELETED command\n");
995 /* 8 parameters cmd */ 1365 /* 8 parameters cmd */
996 - fdctrl.data_len = 9; 1366 + fdctrl->data_len = 9;
997 goto enqueue; 1367 goto enqueue;
998 default: 1368 default:
999 break; 1369 break;
@@ -1003,422 +1373,447 @@ static void fdctrl_write_data (void *opaque, uint32_t reg, uint32_t value) @@ -1003,422 +1373,447 @@ static void fdctrl_write_data (void *opaque, uint32_t reg, uint32_t value)
1003 /* SPECIFY */ 1373 /* SPECIFY */
1004 FLOPPY_DPRINTF("SPECIFY command\n"); 1374 FLOPPY_DPRINTF("SPECIFY command\n");
1005 /* 1 parameter cmd */ 1375 /* 1 parameter cmd */
1006 - fdctrl.data_len = 3; 1376 + fdctrl->data_len = 3;
1007 goto enqueue; 1377 goto enqueue;
1008 case 0x04: 1378 case 0x04:
1009 /* SENSE_DRIVE_STATUS */ 1379 /* SENSE_DRIVE_STATUS */
1010 FLOPPY_DPRINTF("SENSE_DRIVE_STATUS command\n"); 1380 FLOPPY_DPRINTF("SENSE_DRIVE_STATUS command\n");
1011 /* 1 parameter cmd */ 1381 /* 1 parameter cmd */
1012 - fdctrl.data_len = 2; 1382 + fdctrl->data_len = 2;
1013 goto enqueue; 1383 goto enqueue;
1014 case 0x07: 1384 case 0x07:
1015 /* RECALIBRATE */ 1385 /* RECALIBRATE */
1016 FLOPPY_DPRINTF("RECALIBRATE command\n"); 1386 FLOPPY_DPRINTF("RECALIBRATE command\n");
1017 /* 1 parameter cmd */ 1387 /* 1 parameter cmd */
1018 - fdctrl.data_len = 2; 1388 + fdctrl->data_len = 2;
1019 goto enqueue; 1389 goto enqueue;
1020 case 0x08: 1390 case 0x08:
1021 /* SENSE_INTERRUPT_STATUS */ 1391 /* SENSE_INTERRUPT_STATUS */
1022 FLOPPY_DPRINTF("SENSE_INTERRUPT_STATUS command (%02x)\n", 1392 FLOPPY_DPRINTF("SENSE_INTERRUPT_STATUS command (%02x)\n",
1023 - fdctrl.int_status); 1393 + fdctrl->int_status);
1024 /* No parameters cmd: returns status if no interrupt */ 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 return; 1401 return;
1030 case 0x0E: 1402 case 0x0E:
1031 /* DUMPREG */ 1403 /* DUMPREG */
1032 FLOPPY_DPRINTF("DUMPREG command\n"); 1404 FLOPPY_DPRINTF("DUMPREG command\n");
1033 /* Drives position */ 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 /* timers */ 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 (cur_drv->perpendicular << 2); 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 return; 1419 return;
1048 case 0x0F: 1420 case 0x0F:
1049 /* SEEK */ 1421 /* SEEK */
1050 FLOPPY_DPRINTF("SEEK command\n"); 1422 FLOPPY_DPRINTF("SEEK command\n");
1051 /* 2 parameters cmd */ 1423 /* 2 parameters cmd */
1052 - fdctrl.data_len = 3; 1424 + fdctrl->data_len = 3;
1053 goto enqueue; 1425 goto enqueue;
1054 case 0x10: 1426 case 0x10:
1055 /* VERSION */ 1427 /* VERSION */
1056 FLOPPY_DPRINTF("VERSION command\n"); 1428 FLOPPY_DPRINTF("VERSION command\n");
1057 /* No parameters cmd */ 1429 /* No parameters cmd */
1058 /* Controler's version */ 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 return; 1433 return;
1062 case 0x12: 1434 case 0x12:
1063 /* PERPENDICULAR_MODE */ 1435 /* PERPENDICULAR_MODE */
1064 FLOPPY_DPRINTF("PERPENDICULAR_MODE command\n"); 1436 FLOPPY_DPRINTF("PERPENDICULAR_MODE command\n");
1065 /* 1 parameter cmd */ 1437 /* 1 parameter cmd */
1066 - fdctrl.data_len = 2; 1438 + fdctrl->data_len = 2;
1067 goto enqueue; 1439 goto enqueue;
1068 case 0x13: 1440 case 0x13:
1069 /* CONFIGURE */ 1441 /* CONFIGURE */
1070 FLOPPY_DPRINTF("CONFIGURE command\n"); 1442 FLOPPY_DPRINTF("CONFIGURE command\n");
1071 /* 3 parameters cmd */ 1443 /* 3 parameters cmd */
1072 - fdctrl.data_len = 4; 1444 + fdctrl->data_len = 4;
1073 goto enqueue; 1445 goto enqueue;
1074 case 0x14: 1446 case 0x14:
1075 /* UNLOCK */ 1447 /* UNLOCK */
1076 FLOPPY_DPRINTF("UNLOCK command\n"); 1448 FLOPPY_DPRINTF("UNLOCK command\n");
1077 /* No parameters cmd */ 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 return; 1453 return;
1082 case 0x17: 1454 case 0x17:
1083 /* POWERDOWN_MODE */ 1455 /* POWERDOWN_MODE */
1084 FLOPPY_DPRINTF("POWERDOWN_MODE command\n"); 1456 FLOPPY_DPRINTF("POWERDOWN_MODE command\n");
1085 /* 2 parameters cmd */ 1457 /* 2 parameters cmd */
1086 - fdctrl.data_len = 3; 1458 + fdctrl->data_len = 3;
1087 goto enqueue; 1459 goto enqueue;
1088 case 0x18: 1460 case 0x18:
1089 /* PART_ID */ 1461 /* PART_ID */
1090 FLOPPY_DPRINTF("PART_ID command\n"); 1462 FLOPPY_DPRINTF("PART_ID command\n");
1091 /* No parameters cmd */ 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 return; 1466 return;
1095 case 0x2C: 1467 case 0x2C:
1096 /* SAVE */ 1468 /* SAVE */
1097 FLOPPY_DPRINTF("SAVE command\n"); 1469 FLOPPY_DPRINTF("SAVE command\n");
1098 /* No parameters cmd */ 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 /* Drives position */ 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 /* timers */ 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 (cur_drv->perpendicular << 2); 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 return; 1490 return;
1119 case 0x33: 1491 case 0x33:
1120 /* OPTION */ 1492 /* OPTION */
1121 FLOPPY_DPRINTF("OPTION command\n"); 1493 FLOPPY_DPRINTF("OPTION command\n");
1122 /* 1 parameter cmd */ 1494 /* 1 parameter cmd */
1123 - fdctrl.data_len = 2; 1495 + fdctrl->data_len = 2;
1124 goto enqueue; 1496 goto enqueue;
1125 case 0x42: 1497 case 0x42:
1126 /* READ_TRACK */ 1498 /* READ_TRACK */
1127 FLOPPY_DPRINTF("READ_TRACK command\n"); 1499 FLOPPY_DPRINTF("READ_TRACK command\n");
1128 /* 8 parameters cmd */ 1500 /* 8 parameters cmd */
1129 - fdctrl.data_len = 9; 1501 + fdctrl->data_len = 9;
1130 goto enqueue; 1502 goto enqueue;
1131 case 0x4A: 1503 case 0x4A:
1132 /* READ_ID */ 1504 /* READ_ID */
1133 FLOPPY_DPRINTF("READ_ID command\n"); 1505 FLOPPY_DPRINTF("READ_ID command\n");
1134 /* 1 parameter cmd */ 1506 /* 1 parameter cmd */
1135 - fdctrl.data_len = 2; 1507 + fdctrl->data_len = 2;
1136 goto enqueue; 1508 goto enqueue;
1137 case 0x4C: 1509 case 0x4C:
1138 /* RESTORE */ 1510 /* RESTORE */
1139 FLOPPY_DPRINTF("RESTORE command\n"); 1511 FLOPPY_DPRINTF("RESTORE command\n");
1140 /* 17 parameters cmd */ 1512 /* 17 parameters cmd */
1141 - fdctrl.data_len = 18; 1513 + fdctrl->data_len = 18;
1142 goto enqueue; 1514 goto enqueue;
1143 case 0x4D: 1515 case 0x4D:
1144 /* FORMAT_TRACK */ 1516 /* FORMAT_TRACK */
1145 FLOPPY_DPRINTF("FORMAT_TRACK command\n"); 1517 FLOPPY_DPRINTF("FORMAT_TRACK command\n");
1146 /* 5 parameters cmd */ 1518 /* 5 parameters cmd */
1147 - fdctrl.data_len = 9; 1519 + fdctrl->data_len = 6;
1148 goto enqueue; 1520 goto enqueue;
1149 case 0x8E: 1521 case 0x8E:
1150 /* DRIVE_SPECIFICATION_COMMAND */ 1522 /* DRIVE_SPECIFICATION_COMMAND */
1151 FLOPPY_DPRINTF("DRIVE_SPECIFICATION_COMMAND command\n"); 1523 FLOPPY_DPRINTF("DRIVE_SPECIFICATION_COMMAND command\n");
1152 /* 5 parameters cmd */ 1524 /* 5 parameters cmd */
1153 - fdctrl.data_len = 6; 1525 + fdctrl->data_len = 6;
1154 goto enqueue; 1526 goto enqueue;
1155 case 0x8F: 1527 case 0x8F:
1156 /* RELATIVE_SEEK_OUT */ 1528 /* RELATIVE_SEEK_OUT */
1157 FLOPPY_DPRINTF("RELATIVE_SEEK_OUT command\n"); 1529 FLOPPY_DPRINTF("RELATIVE_SEEK_OUT command\n");
1158 /* 2 parameters cmd */ 1530 /* 2 parameters cmd */
1159 - fdctrl.data_len = 3; 1531 + fdctrl->data_len = 3;
1160 goto enqueue; 1532 goto enqueue;
1161 case 0x94: 1533 case 0x94:
1162 /* LOCK */ 1534 /* LOCK */
1163 FLOPPY_DPRINTF("LOCK command\n"); 1535 FLOPPY_DPRINTF("LOCK command\n");
1164 /* No parameters cmd */ 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 return; 1540 return;
1169 case 0xCD: 1541 case 0xCD:
1170 /* FORMAT_AND_WRITE */ 1542 /* FORMAT_AND_WRITE */
1171 FLOPPY_DPRINTF("FORMAT_AND_WRITE command\n"); 1543 FLOPPY_DPRINTF("FORMAT_AND_WRITE command\n");
1172 /* 10 parameters cmd */ 1544 /* 10 parameters cmd */
1173 - fdctrl.data_len = 11; 1545 + fdctrl->data_len = 11;
1174 goto enqueue; 1546 goto enqueue;
1175 case 0xCF: 1547 case 0xCF:
1176 /* RELATIVE_SEEK_IN */ 1548 /* RELATIVE_SEEK_IN */
1177 FLOPPY_DPRINTF("RELATIVE_SEEK_IN command\n"); 1549 FLOPPY_DPRINTF("RELATIVE_SEEK_IN command\n");
1178 /* 2 parameters cmd */ 1550 /* 2 parameters cmd */
1179 - fdctrl.data_len = 3; 1551 + fdctrl->data_len = 3;
1180 goto enqueue; 1552 goto enqueue;
1181 default: 1553 default:
1182 /* Unknown command */ 1554 /* Unknown command */
1183 FLOPPY_ERROR("unknown command: 0x%02x\n", value); 1555 FLOPPY_ERROR("unknown command: 0x%02x\n", value);
1184 - fdctrl_unimplemented(); 1556 + fdctrl_unimplemented(fdctrl);
1185 return; 1557 return;
1186 } 1558 }
1187 } 1559 }
1188 enqueue: 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 /* We now have all parameters 1564 /* We now have all parameters
1192 * and will be able to treat the command 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 case 0x06: 1572 case 0x06:
1196 { 1573 {
1197 /* READ variants */ 1574 /* READ variants */
1198 FLOPPY_DPRINTF("treat READ command\n"); 1575 FLOPPY_DPRINTF("treat READ command\n");
1199 - fdctrl_start_transfer(FD_DIR_READ); 1576 + fdctrl_start_transfer(fdctrl, FD_DIR_READ);
1200 return; 1577 return;
1201 } 1578 }
1202 case 0x0C: 1579 case 0x0C:
1203 /* READ_DELETED variants */ 1580 /* READ_DELETED variants */
1204 // FLOPPY_DPRINTF("treat READ_DELETED command\n"); 1581 // FLOPPY_DPRINTF("treat READ_DELETED command\n");
1205 FLOPPY_ERROR("treat READ_DELETED command\n"); 1582 FLOPPY_ERROR("treat READ_DELETED command\n");
1206 - fdctrl_start_transfer_del(1); 1583 + fdctrl_start_transfer_del(fdctrl, FD_DIR_READ);
1207 return; 1584 return;
1208 case 0x16: 1585 case 0x16:
1209 /* VERIFY variants */ 1586 /* VERIFY variants */
1210 // FLOPPY_DPRINTF("treat VERIFY command\n"); 1587 // FLOPPY_DPRINTF("treat VERIFY command\n");
1211 FLOPPY_ERROR("treat VERIFY command\n"); 1588 FLOPPY_ERROR("treat VERIFY command\n");
1212 - fdctrl_stop_transfer(0x20, 0x00, 0x00); 1589 + fdctrl_stop_transfer(fdctrl, 0x20, 0x00, 0x00);
1213 return; 1590 return;
1214 case 0x10: 1591 case 0x10:
1215 /* SCAN_EQUAL variants */ 1592 /* SCAN_EQUAL variants */
1216 // FLOPPY_DPRINTF("treat SCAN_EQUAL command\n"); 1593 // FLOPPY_DPRINTF("treat SCAN_EQUAL command\n");
1217 FLOPPY_ERROR("treat SCAN_EQUAL command\n"); 1594 FLOPPY_ERROR("treat SCAN_EQUAL command\n");
1218 - fdctrl_start_transfer(FD_DIR_SCANE); 1595 + fdctrl_start_transfer(fdctrl, FD_DIR_SCANE);
1219 return; 1596 return;
1220 case 0x19: 1597 case 0x19:
1221 /* SCAN_LOW_OR_EQUAL variants */ 1598 /* SCAN_LOW_OR_EQUAL variants */
1222 // FLOPPY_DPRINTF("treat SCAN_LOW_OR_EQUAL command\n"); 1599 // FLOPPY_DPRINTF("treat SCAN_LOW_OR_EQUAL command\n");
1223 FLOPPY_ERROR("treat SCAN_LOW_OR_EQUAL command\n"); 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 return; 1602 return;
1226 case 0x1D: 1603 case 0x1D:
1227 /* SCAN_HIGH_OR_EQUAL variants */ 1604 /* SCAN_HIGH_OR_EQUAL variants */
1228 // FLOPPY_DPRINTF("treat SCAN_HIGH_OR_EQUAL command\n"); 1605 // FLOPPY_DPRINTF("treat SCAN_HIGH_OR_EQUAL command\n");
1229 FLOPPY_ERROR("treat SCAN_HIGH_OR_EQUAL command\n"); 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 return; 1608 return;
1232 default: 1609 default:
1233 break; 1610 break;
1234 } 1611 }
1235 - switch (fdctrl.fifo[0] & 0x3F) { 1612 + switch (fdctrl->fifo[0] & 0x3F) {
1236 case 0x05: 1613 case 0x05:
1237 /* WRITE variants */ 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 return; 1617 return;
1241 case 0x09: 1618 case 0x09:
1242 /* WRITE_DELETED variants */ 1619 /* WRITE_DELETED variants */
1243 // FLOPPY_DPRINTF("treat WRITE_DELETED command\n"); 1620 // FLOPPY_DPRINTF("treat WRITE_DELETED command\n");
1244 FLOPPY_ERROR("treat WRITE_DELETED command\n"); 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 return; 1623 return;
1247 default: 1624 default:
1248 break; 1625 break;
1249 } 1626 }
1250 - switch (fdctrl.fifo[0]) { 1627 + switch (fdctrl->fifo[0]) {
1251 case 0x03: 1628 case 0x03:
1252 /* SPECIFY */ 1629 /* SPECIFY */
1253 FLOPPY_DPRINTF("treat SPECIFY command\n"); 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 /* No result back */ 1634 /* No result back */
1257 - fdctrl_reset_fifo(); 1635 + fdctrl_reset_fifo(fdctrl);
1258 break; 1636 break;
1259 case 0x04: 1637 case 0x04:
1260 /* SENSE_DRIVE_STATUS */ 1638 /* SENSE_DRIVE_STATUS */
1261 FLOPPY_DPRINTF("treat SENSE_DRIVE_STATUS command\n"); 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 /* 1 Byte status back */ 1643 /* 1 Byte status back */
1266 - fdctrl.fifo[0] = (cur_drv->ro << 6) | 1644 + fdctrl->fifo[0] = (cur_drv->ro << 6) |
1267 (cur_drv->track == 0 ? 0x10 : 0x00) | 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 break; 1648 break;
1271 case 0x07: 1649 case 0x07:
1272 /* RECALIBRATE */ 1650 /* RECALIBRATE */
1273 FLOPPY_DPRINTF("treat RECALIBRATE command\n"); 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 fd_recalibrate(cur_drv); 1654 fd_recalibrate(cur_drv);
1277 - fdctrl_reset_fifo(); 1655 + fdctrl_reset_fifo(fdctrl);
1278 /* Raise Interrupt */ 1656 /* Raise Interrupt */
1279 - fdctrl_raise_irq(0x20); 1657 + fdctrl_raise_irq(fdctrl, 0x20);
1280 break; 1658 break;
1281 case 0x0F: 1659 case 0x0F:
1282 /* SEEK */ 1660 /* SEEK */
1283 FLOPPY_DPRINTF("treat SEEK command\n"); 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 cur_drv->dir = 1; 1666 cur_drv->dir = 1;
1288 else 1667 else
1289 cur_drv->dir = 0; 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 } else { 1672 } else {
1294 - cur_drv->track = fdctrl.fifo[2];  
1295 - fdctrl_reset_fifo(); 1673 + cur_drv->track = fdctrl->fifo[2];
1296 /* Raise Interrupt */ 1674 /* Raise Interrupt */
1297 - fdctrl_raise_irq(0x20); 1675 + fdctrl_raise_irq(fdctrl, 0x20);
1298 } 1676 }
1299 break; 1677 break;
1300 case 0x12: 1678 case 0x12:
1301 /* PERPENDICULAR_MODE */ 1679 /* PERPENDICULAR_MODE */
1302 FLOPPY_DPRINTF("treat PERPENDICULAR_MODE command\n"); 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 /* No result back */ 1683 /* No result back */
1306 - fdctrl_reset_fifo(); 1684 + fdctrl_reset_fifo(fdctrl);
1307 break; 1685 break;
1308 case 0x13: 1686 case 0x13:
1309 /* CONFIGURE */ 1687 /* CONFIGURE */
1310 FLOPPY_DPRINTF("treat CONFIGURE command\n"); 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 /* No result back */ 1691 /* No result back */
1314 - fdctrl_reset_fifo(); 1692 + fdctrl_reset_fifo(fdctrl);
1315 break; 1693 break;
1316 case 0x17: 1694 case 0x17:
1317 /* POWERDOWN_MODE */ 1695 /* POWERDOWN_MODE */
1318 FLOPPY_DPRINTF("treat POWERDOWN_MODE command\n"); 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 break; 1700 break;
1323 case 0x33: 1701 case 0x33:
1324 /* OPTION */ 1702 /* OPTION */
1325 FLOPPY_DPRINTF("treat OPTION command\n"); 1703 FLOPPY_DPRINTF("treat OPTION command\n");
1326 /* No result back */ 1704 /* No result back */
1327 - fdctrl_reset_fifo(); 1705 + fdctrl_reset_fifo(fdctrl);
1328 break; 1706 break;
1329 case 0x42: 1707 case 0x42:
1330 /* READ_TRACK */ 1708 /* READ_TRACK */
1331 // FLOPPY_DPRINTF("treat READ_TRACK command\n"); 1709 // FLOPPY_DPRINTF("treat READ_TRACK command\n");
1332 FLOPPY_ERROR("treat READ_TRACK command\n"); 1710 FLOPPY_ERROR("treat READ_TRACK command\n");
1333 - fdctrl_unimplemented(); 1711 + fdctrl_start_transfer(fdctrl, FD_DIR_READ);
1334 break; 1712 break;
1335 case 0x4A: 1713 case 0x4A:
1336 /* READ_ID */ 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 break; 1717 break;
1341 case 0x4C: 1718 case 0x4C:
1342 /* RESTORE */ 1719 /* RESTORE */
1343 FLOPPY_DPRINTF("treat RESTORE command\n"); 1720 FLOPPY_DPRINTF("treat RESTORE command\n");
1344 /* Drives position */ 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 /* timers */ 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 break; 1734 break;
1358 case 0x4D: 1735 case 0x4D:
1359 /* FORMAT_TRACK */ 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 break; 1760 break;
1364 case 0x8E: 1761 case 0x8E:
1365 /* DRIVE_SPECIFICATION_COMMAND */ 1762 /* DRIVE_SPECIFICATION_COMMAND */
1366 FLOPPY_DPRINTF("treat DRIVE_SPECIFICATION_COMMAND command\n"); 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 /* Command parameters done */ 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 } else { 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 /* ERROR */ 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 break; 1780 break;
1384 case 0x8F: 1781 case 0x8F:
1385 /* RELATIVE_SEEK_OUT */ 1782 /* RELATIVE_SEEK_OUT */
1386 FLOPPY_DPRINTF("treat RELATIVE_SEEK_OUT command\n"); 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 cur_drv->dir = 0; 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 break; 1795 break;
1400 case 0xCD: 1796 case 0xCD:
1401 /* FORMAT_AND_WRITE */ 1797 /* FORMAT_AND_WRITE */
1402 // FLOPPY_DPRINTF("treat FORMAT_AND_WRITE command\n"); 1798 // FLOPPY_DPRINTF("treat FORMAT_AND_WRITE command\n");
1403 FLOPPY_ERROR("treat FORMAT_AND_WRITE command\n"); 1799 FLOPPY_ERROR("treat FORMAT_AND_WRITE command\n");
1404 - fdctrl_unimplemented(); 1800 + fdctrl_unimplemented(fdctrl);
1405 break; 1801 break;
1406 case 0xCF: 1802 case 0xCF:
1407 /* RELATIVE_SEEK_IN */ 1803 /* RELATIVE_SEEK_IN */
1408 FLOPPY_DPRINTF("treat RELATIVE_SEEK_IN command\n"); 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 cur_drv->dir = 1; 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 break; 1817 break;
1423 } 1818 }
1424 } 1819 }
@@ -57,6 +57,7 @@ @@ -57,6 +57,7 @@
57 57
58 int speaker_data_on; 58 int speaker_data_on;
59 int dummy_refresh_clock; 59 int dummy_refresh_clock;
  60 +static fdctrl_t *floppy_controller;
60 61
61 static void ioport80_write(void *opaque, uint32_t addr, uint32_t data) 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,8 +107,8 @@ static void cmos_init(int ram_size, int boot_device)
106 107
107 /* floppy type */ 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 s->cmos_data[0x10] = 0; 113 s->cmos_data[0x10] = 0;
113 switch (fd0) { 114 switch (fd0) {
@@ -370,7 +371,7 @@ void pc_init(int ram_size, int vga_ram_size, int boot_device, @@ -370,7 +371,7 @@ void pc_init(int ram_size, int vga_ram_size, int boot_device,
370 DMA_init(); 371 DMA_init();
371 SB16_init(); 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 cmos_init(ram_size, boot_device); 376 cmos_init(ram_size, boot_device);
376 } 377 }
@@ -186,9 +186,12 @@ void SB16_init (void); @@ -186,9 +186,12 @@ void SB16_init (void);
186 #define MAX_FD 2 186 #define MAX_FD 2
187 extern BlockDriverState *fd_table[MAX_FD]; 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 /* ne2000.c */ 196 /* ne2000.c */
194 197