diskio.cpp 5.3 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184
  1. #include "integer.h"
  2. #include "diskio.h"
  3. static volatile
  4. DSTATUS Stat = STA_NOINIT; /* Disk status */
  5. /* Interface
  6. ** Scratch Buffer
  7. addr 3 byte
  8. size 1 byte
  9. ** Call Interface
  10. cmd 1 byte
  11. sector 4 bytes
  12. count 1 byte
  13. return 1 byte
  14. ** Commands
  15. * disk_init
  16. * disk_read
  17. * disk_write
  18. */
  19. /*-----------------------------------------------------------------------*/
  20. /* Initialize Disk Drive */
  21. /*-----------------------------------------------------------------------*/
  22. #include <stdio.h>
  23. #include <stdlib.h>
  24. #include <sys/types.h>
  25. #include <sys/stat.h>
  26. #include <unistd.h>
  27. #include <fcntl.h>
  28. #include <sys/mman.h>
  29. #include <string.h>
  30. #define IMAGE_NAME "disk00.vfat"
  31. BYTE *image_addr;
  32. DSTATUS disk_initialize (BYTE drv) {
  33. if (drv) return STA_NOINIT; /* Supports only single drive */
  34. Stat |= STA_NOINIT;
  35. int fd = open(IMAGE_NAME, O_RDWR);
  36. if (fd == -1) {
  37. perror("DISKIO::disk_initialize: Error opening file for writing");
  38. exit(EXIT_FAILURE);
  39. }
  40. int size = lseek(fd,0,SEEK_END);
  41. lseek(fd,0,SEEK_SET);
  42. image_addr = (BYTE*)mmap(0, size, PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0);
  43. #ifdef MMIO_DEBUG
  44. printf("DISKIO::disk_initialize: Open Image (size %i) %p\n",size,image_addr);
  45. #endif
  46. if (image_addr == MAP_FAILED) {
  47. close(fd);
  48. perror("DISKIO::disk_initialize: Error mmapping the file");
  49. exit(EXIT_FAILURE);
  50. }
  51. Stat &= ~STA_NOINIT; /* When device goes ready, clear STA_NOINIT */
  52. return Stat;
  53. }
  54. /*-----------------------------------------------------------------------*/
  55. /* Return Disk Status */
  56. /*-----------------------------------------------------------------------*/
  57. DSTATUS disk_status (BYTE drv){
  58. if (drv) return STA_NOINIT; /* Supports only single drive */
  59. return Stat;
  60. }
  61. /*-----------------------------------------------------------------------*/
  62. /* Read Sector(s) */
  63. /*-----------------------------------------------------------------------*/
  64. DRESULT disk_read (
  65. BYTE drv, /* Physical drive nmuber (0) */
  66. BYTE *buff, /* Data buffer to store read data */
  67. DWORD sector, /* Sector number (LBA) */
  68. BYTE count /* Sector count (1..255) */
  69. )
  70. {
  71. BYTE c, iord_l, iord_h;
  72. if (drv || !count) return RES_PARERR;
  73. if (Stat & STA_NOINIT) return RES_NOTRDY;
  74. DWORD offset = sector * 512;
  75. int size = count * 512;
  76. //#ifdef MMIO_DEBUG
  77. printf("DISKIO::disk_read: sector=%li count=%i addr=%p off=%li size=%i\n",sector,count,image_addr + offset,offset,size);
  78. //#endif
  79. memcpy(buff,image_addr + offset,size);
  80. printf("%x %x %x %x\n",buff[0],buff[1],buff[2],buff[3]);
  81. #ifdef MMIO_DEBUG
  82. printf("DISKIO::disk_read: done\n");
  83. #endif
  84. return RES_OK;
  85. }
  86. /*-----------------------------------------------------------------------*/
  87. /* Write Sector(s) */
  88. /*-----------------------------------------------------------------------*/
  89. #if _READONLY == 0
  90. DRESULT disk_write (
  91. BYTE drv, /* Physical drive nmuber (0) */
  92. const BYTE *buff, /* Data to be written */
  93. DWORD sector, /* Sector number (LBA) */
  94. BYTE count /* Sector count (1..255) */
  95. )
  96. {
  97. if (drv || !count) return RES_PARERR;
  98. if (Stat & STA_NOINIT) return RES_NOTRDY;
  99. DWORD offset = sector * 512;
  100. int size = count * 512;
  101. #ifdef MMIO_DEBUG
  102. printf("disk_write: sector=%li count=%i addr=%p off=%li size=%i\n",sector,count,image_addr + offset,offset,size);
  103. #endif
  104. memcpy(image_addr + offset,buff,size);
  105. return RES_OK;
  106. }
  107. #endif /* _READONLY */
  108. /*-----------------------------------------------------------------------*/
  109. /* Miscellaneous Functions */
  110. /*-----------------------------------------------------------------------*/
  111. #if _USE_IOCTL != 0
  112. DRESULT disk_ioctl (
  113. BYTE drv, /* Physical drive nmuber (0) */
  114. BYTE ctrl, /* Control code */
  115. void *buff /* Buffer to send/receive data block */
  116. )
  117. {
  118. BYTE n, w, ofs, dl, dh, *ptr = (BYTE*)buff;
  119. if (drv) return RES_PARERR;
  120. if (Stat & STA_NOINIT) return RES_NOTRDY;
  121. switch (ctrl) {
  122. case GET_SECTOR_COUNT : /* Get number of sectors on the disk (DWORD) */
  123. #ifdef MMIO_DEBUG
  124. printf("disk_ioctl: GET_SECTOR_COUNT\n");
  125. #endif
  126. ofs = 60; w = 2; n = 0;
  127. break;
  128. case GET_SECTOR_SIZE : /* Get sectors on the disk (WORD) */
  129. #ifdef MMIO_DEBUG
  130. printf("disk_ioctl: GET_SECTOR_SIZE\n");
  131. #endif
  132. *(WORD*)buff = 512;
  133. return RES_OK;
  134. case GET_BLOCK_SIZE : /* Get erase block size in sectors (DWORD) */
  135. #ifdef MMIO_DEBUG
  136. printf("disk_ioctl: GET_BLOCK_SIZE\n");
  137. #endif
  138. *(DWORD*)buff = 32;
  139. return RES_OK;
  140. default:
  141. return RES_PARERR;
  142. }
  143. return RES_OK;
  144. }
  145. #endif /* _USE_IOCTL != 0 */