Commit baca51faff03df59386c95d9478ede18b5be5ec8
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
Showing
3 changed files
with
944 additions
and
545 deletions
hw/fdc.c
@@ -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 | } |
hw/pc.c
@@ -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 | } |
vl.h
@@ -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 |