Fix crash when SD card is smaller than starting sector of scsi disk
[SCSI2SD.git] / software / SCSI2SD / src / geometry.c
index 01478e3..c3c6bb7 100755 (executable)
 \r
 #include <string.h>\r
 \r
-uint32_t getScsiCapacity()\r
+uint32_t getScsiCapacity(\r
+       uint32_t sdSectorStart,\r
+       uint16_t bytesPerSector,\r
+       uint32_t scsiSectors)\r
 {\r
-       uint32_t capacity = sdDev.capacity / SDSectorsPerSCSISector();\r
-       if (config->maxSectors && (capacity > config->maxSectors))\r
+       uint32_t capacity =\r
+               (sdDev.capacity - sdSectorStart) /\r
+                       SDSectorsPerSCSISector(bytesPerSector);\r
+\r
+       if (sdDev.capacity == 0)\r
+       {\r
+               capacity = 0;\r
+       }\r
+       else if (sdSectorStart >= sdDev.capacity)\r
        {\r
-               capacity = config->maxSectors;\r
+               capacity = 0;\r
+       }\r
+       else if (scsiSectors && (capacity > scsiSectors))\r
+       {\r
+               capacity = scsiSectors;\r
        }\r
        return capacity;\r
 }\r
 \r
 \r
-uint32_t SCSISector2SD(uint32_t scsiSector)\r
+uint32_t SCSISector2SD(\r
+       uint32_t sdSectorStart,\r
+       uint16_t bytesPerSector,\r
+       uint32_t scsiSector)\r
 {\r
-       return scsiSector * SDSectorsPerSCSISector();\r
+       return scsiSector * SDSectorsPerSCSISector(bytesPerSector) + sdSectorStart;\r
 }\r
 \r
 // Standard mapping according to ECMA-107 and ISO/IEC 9293:1994\r
 // Sector always starts at 1. There is no 0 sector.\r
-uint64 CHS2LBA(uint32 c, uint8 h, uint32 s)\r
+uint64_t CHS2LBA(\r
+       uint32_t c,\r
+       uint8_t h,\r
+       uint32_t s,\r
+       uint16_t headsPerCylinder,\r
+       uint16_t sectorsPerTrack)\r
 {\r
        return (\r
-               (((uint64)c) * SCSI_HEADS_PER_CYLINDER + h) *\r
-                       (uint64) SCSI_SECTORS_PER_TRACK\r
+               (((uint64_t)c) * headsPerCylinder + h) *\r
+                       (uint64_t) sectorsPerTrack\r
                ) + (s - 1);\r
 }\r
 \r
 \r
-void LBA2CHS(uint32 lba, uint32* c, uint8* h, uint32* s)\r
+void LBA2CHS(\r
+       uint32_t lba,\r
+       uint32_t* c,\r
+       uint8_t* h,\r
+       uint32_t* s,\r
+       uint16_t headsPerCylinder,\r
+       uint16_t sectorsPerTrack)\r
 {\r
-       *c = lba / (SCSI_SECTORS_PER_TRACK * SCSI_HEADS_PER_CYLINDER);\r
-       *h = (lba / SCSI_SECTORS_PER_TRACK) % SCSI_HEADS_PER_CYLINDER;\r
-       *s = (lba % SCSI_SECTORS_PER_TRACK) + 1;\r
+       *c = lba / (((uint32_t) sectorsPerTrack) * headsPerCylinder);\r
+       *h = (lba / sectorsPerTrack) % headsPerCylinder;\r
+       *s = (lba % sectorsPerTrack) + 1;\r
 }\r
 \r
-uint64 scsiByteAddress(int format, const uint8* addr)\r
+uint64_t scsiByteAddress(\r
+       uint16_t bytesPerSector,\r
+       uint16_t headsPerCylinder,\r
+       uint16_t sectorsPerTrack,\r
+       int format,\r
+       const uint8_t* addr)\r
 {\r
-       uint64 result;\r
+       uint64_t result;\r
        switch (format)\r
        {\r
        case ADDRESS_BLOCK:\r
        {\r
-               uint32 lba =\r
-                       (((uint32) addr[0]) << 24) +\r
-                       (((uint32) addr[1]) << 16) +\r
-                       (((uint32) addr[2]) << 8) +\r
+               uint32_t lba =\r
+                       (((uint32_t) addr[0]) << 24) +\r
+                       (((uint32_t) addr[1]) << 16) +\r
+                       (((uint32_t) addr[2]) << 8) +\r
                        addr[3];\r
 \r
-               result = (uint64_t) config->bytesPerSector * lba;\r
+               result = (uint64_t) bytesPerSector * lba;\r
        } break;\r
 \r
        case ADDRESS_PHYSICAL_BYTE:\r
        {\r
-               uint32 cyl =\r
-                       (((uint32) addr[0]) << 16) +\r
-                       (((uint32) addr[1]) << 8) +\r
+               uint32_t cyl =\r
+                       (((uint32_t) addr[0]) << 16) +\r
+                       (((uint32_t) addr[1]) << 8) +\r
                        addr[2];\r
 \r
-               uint8 head = addr[3];\r
+               uint8_t head = addr[3];\r
 \r
-               uint32 bytes =\r
-                       (((uint32) addr[4]) << 24) +\r
-                       (((uint32) addr[5]) << 16) +\r
-                       (((uint32) addr[6]) << 8) +\r
+               uint32_t bytes =\r
+                       (((uint32_t) addr[4]) << 24) +\r
+                       (((uint32_t) addr[5]) << 16) +\r
+                       (((uint32_t) addr[6]) << 8) +\r
                        addr[7];\r
 \r
-               result = CHS2LBA(cyl, head, 1) * (uint64_t) config->bytesPerSector + bytes;\r
+               result = CHS2LBA(cyl, head, 1, headsPerCylinder, sectorsPerTrack) *\r
+                       (uint64_t) bytesPerSector + bytes;\r
        } break;\r
 \r
        case ADDRESS_PHYSICAL_SECTOR:\r
        {\r
                uint32 cyl =\r
-                       (((uint32) addr[0]) << 16) +\r
-                       (((uint32) addr[1]) << 8) +\r
+                       (((uint32_t) addr[0]) << 16) +\r
+                       (((uint32_t) addr[1]) << 8) +\r
                        addr[2];\r
 \r
                uint8 head = scsiDev.data[3];\r
 \r
                uint32 sector =\r
-                       (((uint32) addr[4]) << 24) +\r
-                       (((uint32) addr[5]) << 16) +\r
-                       (((uint32) addr[6]) << 8) +\r
+                       (((uint32_t) addr[4]) << 24) +\r
+                       (((uint32_t) addr[5]) << 16) +\r
+                       (((uint32_t) addr[6]) << 8) +\r
                        addr[7];\r
 \r
-               result = CHS2LBA(cyl, head, sector) * (uint64_t) config->bytesPerSector;\r
+               result = CHS2LBA(cyl, head, sector, headsPerCylinder, sectorsPerTrack) * (uint64_t) bytesPerSector;\r
        } break;\r
 \r
        default:\r
@@ -114,10 +148,16 @@ uint64 scsiByteAddress(int format, const uint8* addr)
 }\r
 \r
 \r
-void scsiSaveByteAddress(int format, uint64 byteAddr, uint8* buf)\r
+void scsiSaveByteAddress(\r
+       uint16_t bytesPerSector,\r
+       uint16_t headsPerCylinder,\r
+       uint16_t sectorsPerTrack,\r
+       int format,\r
+       uint64_t byteAddr,\r
+       uint8_t* buf)\r
 {\r
-       uint32 lba = byteAddr / config->bytesPerSector;\r
-       uint32 byteOffset = byteAddr % config->bytesPerSector;\r
+       uint32_t lba = byteAddr / bytesPerSector;\r
+       uint32_t byteOffset = byteAddr % bytesPerSector;\r
 \r
        switch (format)\r
        {\r
@@ -136,14 +176,14 @@ void scsiSaveByteAddress(int format, uint64 byteAddr, uint8* buf)
 \r
        case ADDRESS_PHYSICAL_BYTE:\r
        {\r
-               uint32 cyl;\r
-               uint8 head;\r
-               uint32 sector;\r
-               uint32 bytes;\r
+               uint32_t cyl;\r
+               uint8_t head;\r
+               uint32_t sector;\r
+               uint32_t bytes;\r
 \r
-               LBA2CHS(lba, &cyl, &head, &sector);\r
+               LBA2CHS(lba, &cyl, &head, &sector, headsPerCylinder, sectorsPerTrack);\r
 \r
-               bytes = sector * config->bytesPerSector + byteOffset;\r
+               bytes = sector * bytesPerSector + byteOffset;\r
 \r
                buf[0] = cyl >> 16;\r
                buf[1] = cyl >> 8;\r
@@ -159,11 +199,11 @@ void scsiSaveByteAddress(int format, uint64 byteAddr, uint8* buf)
 \r
        case ADDRESS_PHYSICAL_SECTOR:\r
        {\r
-               uint32 cyl;\r
-               uint8 head;\r
-               uint32 sector;\r
+               uint32_t cyl;\r
+               uint8_t head;\r
+               uint32_t sector;\r
 \r
-               LBA2CHS(lba, &cyl, &head, &sector);\r
+               LBA2CHS(lba, &cyl, &head, &sector, headsPerCylinder, sectorsPerTrack);\r
 \r
                buf[0] = cyl >> 16;\r
                buf[1] = cyl >> 8;\r