Fix crash when SD card is smaller than starting sector of scsi disk
[SCSI2SD.git] / software / SCSI2SD / src / geometry.c
index cbf71ce..c3c6bb7 100755 (executable)
@@ -28,7 +28,16 @@ uint32_t getScsiCapacity(
        uint32_t capacity =\r
                (sdDev.capacity - sdSectorStart) /\r
                        SDSectorsPerSCSISector(bytesPerSector);\r
-       if (scsiSectors && (capacity > scsiSectors))\r
+\r
+       if (sdDev.capacity == 0)\r
+       {\r
+               capacity = 0;\r
+       }\r
+       else if (sdSectorStart >= sdDev.capacity)\r
+       {\r
+               capacity = 0;\r
+       }\r
+       else if (scsiSectors && (capacity > scsiSectors))\r
        {\r
                capacity = scsiSectors;\r
        }\r
@@ -46,36 +55,49 @@ uint32_t SCSISector2SD(
 \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(\r
+uint64_t scsiByteAddress(\r
        uint16_t bytesPerSector,\r
+       uint16_t headsPerCylinder,\r
+       uint16_t sectorsPerTrack,\r
        int format,\r
-       const uint8* addr)\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) bytesPerSector * lba;\r
@@ -83,38 +105,39 @@ uint64 scsiByteAddress(
 \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) 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) bytesPerSector;\r
+               result = CHS2LBA(cyl, head, sector, headsPerCylinder, sectorsPerTrack) * (uint64_t) bytesPerSector;\r
        } break;\r
 \r
        default:\r
@@ -127,12 +150,14 @@ uint64 scsiByteAddress(
 \r
 void scsiSaveByteAddress(\r
        uint16_t bytesPerSector,\r
+       uint16_t headsPerCylinder,\r
+       uint16_t sectorsPerTrack,\r
        int format,\r
-       uint64 byteAddr,\r
-       uint8* buf)\r
+       uint64_t byteAddr,\r
+       uint8_t* buf)\r
 {\r
-       uint32 lba = byteAddr / bytesPerSector;\r
-       uint32 byteOffset = byteAddr % bytesPerSector;\r
+       uint32_t lba = byteAddr / bytesPerSector;\r
+       uint32_t byteOffset = byteAddr % bytesPerSector;\r
 \r
        switch (format)\r
        {\r
@@ -151,12 +176,12 @@ void scsiSaveByteAddress(
 \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 * bytesPerSector + byteOffset;\r
 \r
@@ -174,11 +199,11 @@ void scsiSaveByteAddress(
 \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