8ac69b52c4889926d68fa9f9f466be29f5a722ec
[monolithium.git] / kernel / src / drivers / floppy.c
1 /*
2  * floppy.c
3  *
4  * Copyright (C) 2016 Aleksandar Andrejevic <theflash@sdf.lonestar.org>
5  *
6  * This program is free software: you can redistribute it and/or modify
7  * it under the terms of the GNU Affero General Public License as
8  * published by the Free Software Foundation, either version 3 of the
9  * License, or (at your option) any later version.
10  *
11  * This program is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
14  * GNU Affero General Public License for more details.
15  *
16  * You should have received a copy of the GNU Affero General Public License
17  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
18  */
19
20 #include "floppy.h"
21 #include <memory.h>
22 #include <isa_dma.h>
23 #include <irq.h>
24 #include <thread.h>
25 #include <heap.h>
26
27 static dword_t floppy_motor_status[FLOPPY_DRIVES];
28 static lock_t floppy_lock = 0, floppy_motor_lock = 0;
29 static semaphore_t irq_mutex;
30 static thread_t *floppy_thread = NULL;
31
32 static dword_t floppy_init(void);
33 static dword_t floppy_cleanup(void);
34 static dword_t floppy_read(device_t *device, void *buffer, qword_t offset, size_t length, size_t *bytes_read);
35 static dword_t floppy_write(device_t *device, const void *buffer, qword_t offset, size_t length, size_t *bytes_written);
36 static dword_t floppy_ioctl(
37     device_t *device,
38     dword_t control_code,
39     const void *in_buffer,
40     size_t in_length,
41     void *out_buffer,
42     size_t out_length
43 );
44
45 static block_dev_driver_t floppy_driver_block =
46 {
47     .init_proc = floppy_init,
48     .cleanup_proc = floppy_cleanup,
49     .read_proc = floppy_read,
50     .write_proc = floppy_write,
51     .ioctl_proc = floppy_ioctl
52 };
53
54 static void floppy_irq_handler(registers_t *regs, byte_t irq_num)
55 {
56     UNUSED_PARAMETER(regs);
57     UNUSED_PARAMETER(irq_num);
58
59     release_mutex(&irq_mutex);
60 }
61
62 static dword_t floppy_wait_irq(dword_t timeout)
63 {
64     return wait_mutex(&irq_mutex, timeout);
65 }
66
67 static bool_t floppy_fifo_write(byte_t byte)
68 {
69     dword_t timeout = FLOPPY_IO_TIMEOUT;
70
71     while (timeout--)
72     {
73         byte_t msr = inportb(FLOPPY_MSR);
74         if (msr & 0x80) break;
75         sleep(1);
76     }
77
78     if (timeout != 0) outportb(FLOPPY_FIFO, byte);
79     else return FALSE;
80
81     return TRUE;
82 }
83
84 static bool_t floppy_fifo_read(byte_t *byte)
85 {
86     dword_t timeout = FLOPPY_IO_TIMEOUT;
87
88     while (timeout--)
89     {
90         byte_t msr = inportb(FLOPPY_MSR);
91         if (msr & 0x80) break;
92         sleep(1);
93     }
94
95     if (timeout != 0) *byte = inportb(FLOPPY_FIFO);
96     else return FALSE;
97
98     return TRUE;
99 }
100
101 static word_t floppy_sense_interrupt(void)
102 {
103     byte_t st0, cylinder;
104
105     if (!floppy_fifo_write(FLOPPY_CMD_SENSE_INT)) return 0;
106     if (!floppy_fifo_read(&st0)) return 0;
107     if (!floppy_fifo_read(&cylinder)) return 0;
108
109     return (st0 << 8) | cylinder;
110 }
111
112 static void floppy_select(byte_t drive)
113 {
114     byte_t dor = inportb(FLOPPY_DOR);
115     dor &= 0xFC;
116     dor |= drive & 1;
117     outportb(FLOPPY_DOR, dor);
118 }
119
120 static void floppy_reset()
121 {
122     dword_t i;
123
124     outportb(FLOPPY_DOR, 0x00);
125     outportb(FLOPPY_DOR, 0x0C);
126     floppy_wait_irq(NO_TIMEOUT);
127
128     for (i = 0; i < 4; i++) floppy_sense_interrupt();
129
130     floppy_fifo_write(FLOPPY_CMD_CONFIGURE);
131     floppy_fifo_write(0);
132     floppy_fifo_write(0x57);
133     floppy_fifo_write(0);
134
135     floppy_fifo_write(FLOPPY_CMD_SPECIFY);
136     floppy_fifo_write(FLOPPY_SRT << 4 | FLOPPY_HUT);
137     floppy_fifo_write(FLOPPY_HLT << 1);
138
139     outportb(FLOPPY_CCR, 0x00);
140     floppy_select(0);
141 }
142
143 static dword_t floppy_motor_thread(void *param)
144 {
145     dword_t i;
146
147     while (TRUE)
148     {
149         acquire_lock(&floppy_motor_lock);
150
151         for (i = 0; i < FLOPPY_DRIVES; i++)
152         {
153             if (floppy_motor_status[i] == FLOPPY_MOTOR_IDLE)
154             {
155                 floppy_motor_status[i] = FLOPPY_MOTOR_STOPPING;
156             }
157             else if (floppy_motor_status[i] == FLOPPY_MOTOR_STOPPING)
158             {
159                 byte_t dor = inportb(FLOPPY_DOR);
160                 dor &= ~(1 << (4 + i));
161                 outportb(FLOPPY_DOR, dor);
162
163                 floppy_motor_status[i] = FLOPPY_MOTOR_STOPPED;
164             }
165         }
166
167         release_lock(&floppy_motor_lock);
168         sleep(3000);
169     }
170
171     return 0;
172 }
173
174 static void floppy_motor_on(dword_t drive)
175 {
176     acquire_lock(&floppy_motor_lock);
177
178     if (floppy_motor_status[drive & 1] == FLOPPY_MOTOR_STOPPED)
179     {
180         byte_t dor = inportb(FLOPPY_DOR);
181         dor |= 1 << (4 + (drive & 1));
182         outportb(FLOPPY_DOR, dor);
183
184         sleep(300);
185     }
186
187     floppy_motor_status[drive & 1] = FLOPPY_MOTOR_RUNNING;
188     release_lock(&floppy_motor_lock);
189 }
190
191 static void floppy_motor_off(dword_t drive)
192 {
193     acquire_lock(&floppy_motor_lock);
194     floppy_motor_status[drive & 1] = FLOPPY_MOTOR_IDLE;
195     release_lock(&floppy_motor_lock);
196 }
197
198 static bool_t floppy_recalibrate(byte_t drive)
199 {
200     bool_t success = FALSE;
201     int retries = 5;
202
203     drive &= 1;
204
205     floppy_select(drive);
206     floppy_motor_on(drive);
207
208     while (!success && ((retries--) > 0))
209     {
210         floppy_fifo_write(FLOPPY_CMD_RECALIBRATE);
211         floppy_fifo_write(drive);
212         floppy_wait_irq(FLOPPY_SEEK_TIMEOUT);
213
214         byte_t st0 = floppy_sense_interrupt() >> 8;
215         if (st0 & FLOPPY_ST0_SEEK_END) success = TRUE;
216     }
217
218     floppy_motor_off(drive);
219     return success;
220 }
221
222 static dword_t floppy_init(void)
223 {
224     init_mutex(&irq_mutex, 0);
225
226     outportb(CMOS_CMD_PORT, CMOS_FLOPPY_REG);
227     byte_t floppy_data = inportb(CMOS_DATA_PORT);
228     if (!floppy_data) return ERR_NOTFOUND;
229
230     dword_t ret = register_irq_handler(FLOPPY_IRQ, floppy_irq_handler, TRUE);
231     if (ret != ERR_SUCCESS) goto cleanup;
232
233     ret = create_system_thread(floppy_motor_thread, 0, THREAD_PRIORITY_LOW, 0, NULL, &floppy_thread);
234     if (ret != ERR_SUCCESS) goto cleanup;
235
236     floppy_reset();
237
238     if ((floppy_data >> 4) == FLOPPY_TYPE_144MB)
239     {
240         floppy_recalibrate(0);
241
242         device_t *device = (device_t*)malloc(sizeof(device_t));
243         if (device == NULL)
244         {
245             ret = ERR_NOMEMORY;
246             goto cleanup;
247         }
248
249         device->driver = &floppy_driver_block;
250         strcpy(device->name, FLOPPY_FIRST_NAME);
251         device->resource = 0;
252         device->capacity = FLOPPY_SECTOR_SIZE * FLOPPY_SECTORS;
253
254         ret = register_block_device(device);
255         if (ret != ERR_SUCCESS) goto cleanup;
256     }
257
258     if ((floppy_data & 0x0F) == FLOPPY_TYPE_144MB)
259     {
260         floppy_recalibrate(1);
261
262         device_t *device = (device_t*)malloc(sizeof(device_t));
263         if (device == NULL)
264         {
265             ret = ERR_NOMEMORY;
266             goto cleanup;
267         }
268
269         device->driver = &floppy_driver_block;
270         strcpy(device->name, FLOPPY_SECOND_NAME);
271         device->resource = 0;
272         device->capacity = FLOPPY_SECTOR_SIZE * FLOPPY_SECTORS;
273
274         ret = register_block_device(device);
275         if (ret != ERR_SUCCESS) goto cleanup;
276     }
277
278 cleanup:
279     if (ret != ERR_SUCCESS)
280     {
281         if (floppy_thread) terminate_thread_internal(floppy_thread, 0);
282
283         device_t *first = get_block_device(FLOPPY_FIRST_NAME);
284         device_t *second = get_block_device(FLOPPY_SECOND_NAME);
285
286         if (first)
287         {
288             unregister_block_device(first);
289             free(first);
290         }
291
292         if (second)
293         {
294             unregister_block_device(second);
295             free(second);
296         }
297
298         unregister_irq_handler(FLOPPY_IRQ, floppy_irq_handler);
299     }
300
301     return ret;
302 }
303
304 static dword_t floppy_cleanup(void)
305 {
306     device_t *first = get_block_device(FLOPPY_FIRST_NAME);
307     device_t *second = get_block_device(FLOPPY_SECOND_NAME);
308
309     if (first)
310     {
311         unregister_block_device(first);
312         free(first);
313     }
314
315     if (second)
316     {
317         unregister_block_device(second);
318         free(second);
319     }
320
321     unregister_irq_handler(FLOPPY_IRQ, floppy_irq_handler);
322     return floppy_thread ? terminate_thread_internal(floppy_thread, 0) : ERR_SUCCESS;
323 }
324
325 static dword_t floppy_read(device_t *device, void *buffer, qword_t offset, size_t length, size_t *bytes_read)
326 {
327     dword_t ret = ERR_SUCCESS;
328     dword_t count = 0;
329     byte_t drive;
330     byte_t cyl, head;
331     dword_t sector_offset = (dword_t)offset % FLOPPY_SECTOR_SIZE;
332     floppy_chs_t start_block = FLOPPY_LBA_TO_CHS((dword_t)offset / FLOPPY_SECTOR_SIZE);
333     floppy_chs_t end_block = FLOPPY_LBA_TO_CHS((dword_t)(offset + length - 1) / FLOPPY_SECTOR_SIZE);
334     byte_t *dma_buffer = NULL;
335
336     if (offset >= (qword_t)(FLOPPY_SECTORS * FLOPPY_SECTOR_SIZE)) return ERR_INVALID;
337
338     if (strcmp(device->name, FLOPPY_FIRST_NAME) == 0) drive = 0;
339     else if (strcmp(device->name, FLOPPY_SECOND_NAME) == 0) drive = 1;
340     else return ERR_INVALID;
341
342     dma_buffer = (byte_t*)isa_dma_alloc();
343     if (dma_buffer == NULL) return ERR_NOMEMORY;
344
345     acquire_lock(&floppy_lock);
346     floppy_select(drive);
347     floppy_motor_on(drive);
348
349     for (cyl = start_block.cylinder; cyl <= end_block.cylinder; cyl++)
350     {
351         byte_t first_head = 0, last_head = 1;
352
353         if (cyl == start_block.cylinder) first_head = start_block.head;
354         if (cyl == end_block.cylinder) last_head = end_block.head;
355
356         for (head = first_head; head <= last_head; head++)
357         {
358             bool_t success = FALSE;
359             byte_t retries = 3;
360             byte_t first_sector = 1;
361             byte_t last_sector = FLOPPY_SPT;
362
363             if ((cyl == start_block.cylinder) && (head == first_head)) first_sector = start_block.sector;
364             if ((cyl == end_block.cylinder) && (head == last_head)) last_sector = end_block.sector;
365             dword_t amount = (last_sector - first_sector + 1) * FLOPPY_SECTOR_SIZE;
366
367             while (retries--)
368             {
369                 byte_t i;
370                 byte_t command_input[FLOPPY_RW_INPUTS] =
371                 {
372                     FLOPPY_CMD_READ | FLOPPY_FLAG_MFM,
373                     drive | (head << 2),
374                     cyl,
375                     head,
376                     first_sector,
377                     0x02,
378                     last_sector,
379                     0x1B,
380                     0xFF
381                 };
382                 byte_t command_output[FLOPPY_RW_OUTPUTS];
383
384                 isa_dma_write(FLOPPY_DMA_CHANNEL, dma_buffer, amount);
385
386                 for (i = 0; i < FLOPPY_RW_INPUTS; i++) if (!floppy_fifo_write(command_input[i])) goto timeout;
387                 if (floppy_wait_irq(FLOPPY_READ_TIMEOUT) == ERR_TIMEOUT) goto timeout;
388                 for (i = 0; i < FLOPPY_RW_OUTPUTS; i++) if (!floppy_fifo_read(&command_output[i])) goto timeout;
389
390                 if ((command_output[0] & 0xC0) || command_output[1] || command_output[2]) continue;
391
392                 success = TRUE;
393                 break;
394
395             timeout:
396                 floppy_reset();
397                 floppy_recalibrate(drive);
398             }
399
400             if (!success)
401             {
402                 ret = ERR_HARDWARE;
403                 goto done;
404             }
405
406             ret = read_physical(&dma_buffer[sector_offset], (void*)((uintptr_t)buffer + count), MIN(length - count, amount - sector_offset));
407             if (ret != ERR_SUCCESS) goto done;
408
409             count += MIN(length - count, amount - sector_offset);
410             sector_offset = 0;
411         }
412     }
413
414 done:
415     floppy_motor_off(drive);
416     release_lock(&floppy_lock);
417     isa_dma_free(dma_buffer);
418
419     if (bytes_read) *bytes_read = count;
420     return ret;
421 }
422
423 static dword_t floppy_write(device_t *device, const void *buffer, qword_t offset, size_t length, size_t *bytes_written)
424 {
425     dword_t ret = ERR_SUCCESS;
426     dword_t count = 0;
427     byte_t drive;
428     byte_t cyl, head;
429     dword_t sector_offset = (dword_t)(offset % FLOPPY_SECTOR_SIZE);
430     floppy_chs_t start_block = FLOPPY_LBA_TO_CHS((dword_t)offset / FLOPPY_SECTOR_SIZE);
431     floppy_chs_t end_block = FLOPPY_LBA_TO_CHS((dword_t)(offset + length - 1) / FLOPPY_SECTOR_SIZE);
432     byte_t first_sector_data[FLOPPY_SECTOR_SIZE];
433     byte_t last_sector_data[FLOPPY_SECTOR_SIZE];
434     byte_t *dma_buffer = NULL;
435
436     if (offset >= (qword_t)(FLOPPY_SECTORS * FLOPPY_SECTOR_SIZE)) return ERR_INVALID;
437
438     if (strcmp(device->name, FLOPPY_FIRST_NAME) == 0) drive = 0;
439     else if (strcmp(device->name, FLOPPY_SECOND_NAME) == 0) drive = 1;
440     else return ERR_INVALID;
441
442     if (sector_offset)
443     {
444         ret = floppy_read(device, first_sector_data, offset & ~(FLOPPY_SECTOR_SIZE - 1), FLOPPY_SECTOR_SIZE, NULL);
445         if (ret != ERR_SUCCESS) return ret;
446     }
447
448     if ((offset + length) % FLOPPY_SECTOR_SIZE)
449     {
450         ret = floppy_read(device, last_sector_data, (offset + length) & ~(FLOPPY_SECTOR_SIZE - 1), FLOPPY_SECTOR_SIZE, NULL);
451         if (ret != ERR_SUCCESS) return ret;
452     }
453
454     dma_buffer = (byte_t*)isa_dma_alloc();
455     if (dma_buffer == NULL) return ERR_NOMEMORY;
456
457     acquire_lock(&floppy_lock);
458     floppy_select(drive);
459     floppy_motor_on(drive);
460
461     for (cyl = start_block.cylinder; cyl <= end_block.cylinder; cyl++)
462     {
463         byte_t first_head = 0, last_head = 1;
464
465         if (cyl == start_block.cylinder) first_head = start_block.head;
466         if (cyl == end_block.cylinder) last_head = end_block.head;
467
468         for (head = first_head; head <= last_head; head++)
469         {
470             bool_t success = FALSE;
471             byte_t retries = 3;
472             byte_t first_sector = 1;
473             byte_t last_sector = FLOPPY_SPT;
474
475             if ((cyl == start_block.cylinder) && (head == first_head)) first_sector = start_block.sector;
476             if ((cyl == end_block.cylinder) && (head == last_head)) last_sector = end_block.sector;
477             dword_t amount = (last_sector - first_sector + 1) * FLOPPY_SECTOR_SIZE;
478
479             if (sector_offset)
480             {
481                 ret = write_physical(dma_buffer, first_sector_data, FLOPPY_SECTOR_SIZE);
482                 if (ret != ERR_SUCCESS) goto done;
483             }
484
485             if ((length - count) < amount)
486             {
487                 ret = write_physical(&dma_buffer[amount - FLOPPY_SECTOR_SIZE], last_sector_data, FLOPPY_SECTOR_SIZE);
488                 if (ret != ERR_SUCCESS) goto done;
489             }
490
491             ret = write_physical(&dma_buffer[sector_offset], (void*)((uintptr_t)buffer + count), MIN(length - count, amount - sector_offset));
492             if (ret != ERR_SUCCESS) goto done;
493
494             while (retries--)
495             {
496                 byte_t i;
497                 byte_t command_input[FLOPPY_RW_INPUTS] =
498                 {
499                     FLOPPY_CMD_WRITE | FLOPPY_FLAG_MFM,
500                     drive | (head << 2),
501                     cyl,
502                     head,
503                     first_sector,
504                     0x02,
505                     last_sector,
506                     0x1B,
507                     0xFF
508                 };
509                 byte_t command_output[FLOPPY_RW_OUTPUTS];
510
511                 isa_dma_read(FLOPPY_DMA_CHANNEL, dma_buffer, amount);
512
513                 for (i = 0; i < FLOPPY_RW_INPUTS; i++) if (!floppy_fifo_write(command_input[i])) goto write_timeout;
514                 if (floppy_wait_irq(FLOPPY_READ_TIMEOUT) == ERR_TIMEOUT) goto write_timeout;
515                 for (i = 0; i < FLOPPY_RW_OUTPUTS; i++) if (!floppy_fifo_read(&command_output[i])) goto write_timeout;
516
517                 if ((command_output[0] & 0xC0) || command_output[1] || command_output[2]) continue;
518
519                 success = TRUE;
520                 break;
521
522             write_timeout:
523                 floppy_reset();
524                 floppy_recalibrate(drive);
525             }
526
527             if (!success)
528             {
529                 ret = ERR_HARDWARE;
530                 goto done;
531             }
532
533             count += MIN(length - count, amount - sector_offset);
534             sector_offset = 0;
535         }
536     }
537
538 done:
539     floppy_motor_off(drive);
540     release_lock(&floppy_lock);
541     isa_dma_free(dma_buffer);
542
543     if (bytes_written) *bytes_written = count;
544     return ret;
545 }
546
547 static dword_t floppy_ioctl(device_t *device,
548                             dword_t control_code,
549                             const void *in_buffer,
550                             size_t in_length,
551                             void *out_buffer,
552                             size_t out_length)
553 {
554     switch (control_code)
555     {
556     case IOCTL_BLOCK_DEV_MEDIA_INFO:
557         if (out_length >= sizeof(block_dev_media_info_t))
558         {
559             ((block_dev_media_info_t*)out_buffer)->heads = 2;
560             ((block_dev_media_info_t*)out_buffer)->tracks = 80;
561             ((block_dev_media_info_t*)out_buffer)->sectors_per_track = 18;
562             ((block_dev_media_info_t*)out_buffer)->bytes_per_sector = 512;
563
564             return ERR_SUCCESS;
565         }
566         else
567         {
568             return ERR_SMALLBUF;
569         }
570
571         break;
572
573     default:
574         return ERR_INVALID;
575     }
576 }
577
578 dword_t floppy_driver_load(void)
579 {
580     return register_block_dev_driver(&floppy_driver_block);
581 }