Fix crash when SD card is smaller than starting sector of scsi disk
[SCSI2SD-V6.git] / src / firmware / geometry.c
1 //      Copyright (C) 2013 Michael McMaster <michael@codesrc.com>\r
2 //\r
3 //      This file is part of SCSI2SD.\r
4 //\r
5 //      SCSI2SD is free software: you can redistribute it and/or modify\r
6 //      it under the terms of the GNU General Public License as published by\r
7 //      the Free Software Foundation, either version 3 of the License, or\r
8 //      (at your option) any later version.\r
9 //\r
10 //      SCSI2SD is distributed in the hope that it will be useful,\r
11 //      but WITHOUT ANY WARRANTY; without even the implied warranty of\r
12 //      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\r
13 //      GNU General Public License for more details.\r
14 //\r
15 //      You should have received a copy of the GNU General Public License\r
16 //      along with SCSI2SD.  If not, see <http://www.gnu.org/licenses/>.\r
17 \r
18 #include "geometry.h"\r
19 #include "scsi.h"\r
20 #include "sd.h"\r
21 #include "config.h"\r
22 \r
23 #include <string.h>\r
24 \r
25 uint32_t getScsiCapacity(\r
26         uint32_t sdSectorStart,\r
27         uint16_t bytesPerSector,\r
28         uint32_t scsiSectors)\r
29 {\r
30         uint32_t capacity =\r
31                 (sdDev.capacity - sdSectorStart - S2S_CFG_SIZE) /\r
32                         SDSectorsPerSCSISector(bytesPerSector);\r
33 \r
34 \r
35         if (sdDev.capacity == 0)\r
36         {\r
37                 capacity = 0;\r
38         }\r
39         else if (sdSectorStart >= (sdDev.capacity - S2S_CFG_SIZE))\r
40         {\r
41                 capacity = 0;\r
42         }\r
43         else if (scsiSectors && (capacity > scsiSectors))\r
44         {\r
45                 capacity = scsiSectors;\r
46         }\r
47         return capacity;\r
48 }\r
49 \r
50 \r
51 uint32_t SCSISector2SD(\r
52         uint32_t sdSectorStart,\r
53         uint16_t bytesPerSector,\r
54         uint32_t scsiSector)\r
55 {\r
56         return scsiSector * SDSectorsPerSCSISector(bytesPerSector) + sdSectorStart;\r
57 }\r
58 \r
59 // Standard mapping according to ECMA-107 and ISO/IEC 9293:1994\r
60 // Sector always starts at 1. There is no 0 sector.\r
61 uint64_t CHS2LBA(\r
62         uint32_t c,\r
63         uint8_t h,\r
64         uint32_t s,\r
65         uint16_t headsPerCylinder,\r
66         uint16_t sectorsPerTrack)\r
67 {\r
68         return (\r
69                 (((uint64_t)c) * headsPerCylinder + h) *\r
70                         (uint64_t) sectorsPerTrack\r
71                 ) + (s - 1);\r
72 }\r
73 \r
74 \r
75 void LBA2CHS(\r
76         uint32_t lba,\r
77         uint32_t* c,\r
78         uint8_t* h,\r
79         uint32_t* s,\r
80         uint16_t headsPerCylinder,\r
81         uint16_t sectorsPerTrack)\r
82 {\r
83         *c = lba / (((uint32_t) sectorsPerTrack) * headsPerCylinder);\r
84         *h = (lba / sectorsPerTrack) % headsPerCylinder;\r
85         *s = (lba % sectorsPerTrack) + 1;\r
86 }\r
87 \r
88 uint64_t scsiByteAddress(\r
89         uint16_t bytesPerSector,\r
90         uint16_t headsPerCylinder,\r
91         uint16_t sectorsPerTrack,\r
92         int format,\r
93         const uint8_t* addr)\r
94 {\r
95         uint64_t result;\r
96         switch (format)\r
97         {\r
98         case ADDRESS_BLOCK:\r
99         {\r
100                 uint32_t lba =\r
101                         (((uint32_t) addr[0]) << 24) +\r
102                         (((uint32_t) addr[1]) << 16) +\r
103                         (((uint32_t) addr[2]) << 8) +\r
104                         addr[3];\r
105 \r
106                 result = (uint64_t) bytesPerSector * lba;\r
107         } break;\r
108 \r
109         case ADDRESS_PHYSICAL_BYTE:\r
110         {\r
111                 uint32_t cyl =\r
112                         (((uint32_t) addr[0]) << 16) +\r
113                         (((uint32_t) addr[1]) << 8) +\r
114                         addr[2];\r
115 \r
116                 uint8_t head = addr[3];\r
117 \r
118                 uint32_t bytes =\r
119                         (((uint32_t) addr[4]) << 24) +\r
120                         (((uint32_t) addr[5]) << 16) +\r
121                         (((uint32_t) addr[6]) << 8) +\r
122                         addr[7];\r
123 \r
124                 result = CHS2LBA(cyl, head, 1, headsPerCylinder, sectorsPerTrack) *\r
125                         (uint64_t) bytesPerSector + bytes;\r
126         } break;\r
127 \r
128         case ADDRESS_PHYSICAL_SECTOR:\r
129         {\r
130                 uint32_t cyl =\r
131                         (((uint32_t) addr[0]) << 16) +\r
132                         (((uint32_t) addr[1]) << 8) +\r
133                         addr[2];\r
134 \r
135                 uint8_t head = scsiDev.data[3];\r
136 \r
137                 uint32_t sector =\r
138                         (((uint32_t) addr[4]) << 24) +\r
139                         (((uint32_t) addr[5]) << 16) +\r
140                         (((uint32_t) addr[6]) << 8) +\r
141                         addr[7];\r
142 \r
143                 result = CHS2LBA(cyl, head, sector, headsPerCylinder, sectorsPerTrack) * (uint64_t) bytesPerSector;\r
144         } break;\r
145 \r
146         default:\r
147                 result = (uint64_t) -1;\r
148         }\r
149 \r
150         return result;\r
151 }\r
152 \r
153 \r
154 void scsiSaveByteAddress(\r
155         uint16_t bytesPerSector,\r
156         uint16_t headsPerCylinder,\r
157         uint16_t sectorsPerTrack,\r
158         int format,\r
159         uint64_t byteAddr,\r
160         uint8_t* buf)\r
161 {\r
162         uint32_t lba = byteAddr / bytesPerSector;\r
163         uint32_t byteOffset = byteAddr % bytesPerSector;\r
164 \r
165         switch (format)\r
166         {\r
167         case ADDRESS_BLOCK:\r
168         {\r
169                 buf[0] = lba >> 24;\r
170                 buf[1] = lba >> 16;\r
171                 buf[2] = lba >> 8;\r
172                 buf[3] = lba;\r
173 \r
174                 buf[4] = 0;\r
175                 buf[5] = 0;\r
176                 buf[6] = 0;\r
177                 buf[7] = 0;\r
178         } break;\r
179 \r
180         case ADDRESS_PHYSICAL_BYTE:\r
181         {\r
182                 uint32_t cyl;\r
183                 uint8_t head;\r
184                 uint32_t sector;\r
185                 uint32_t bytes;\r
186 \r
187                 LBA2CHS(lba, &cyl, &head, &sector, headsPerCylinder, sectorsPerTrack);\r
188 \r
189                 bytes = sector * bytesPerSector + byteOffset;\r
190 \r
191                 buf[0] = cyl >> 16;\r
192                 buf[1] = cyl >> 8;\r
193                 buf[2] = cyl;\r
194 \r
195                 buf[3] = head;\r
196 \r
197                 buf[4] = bytes >> 24;\r
198                 buf[5] = bytes >> 16;\r
199                 buf[6] = bytes >> 8;\r
200                 buf[7] = bytes;\r
201         } break;\r
202 \r
203         case ADDRESS_PHYSICAL_SECTOR:\r
204         {\r
205                 uint32_t cyl;\r
206                 uint8_t head;\r
207                 uint32_t sector;\r
208 \r
209                 LBA2CHS(lba, &cyl, &head, &sector, headsPerCylinder, sectorsPerTrack);\r
210 \r
211                 buf[0] = cyl >> 16;\r
212                 buf[1] = cyl >> 8;\r
213                 buf[2] = cyl;\r
214 \r
215                 buf[3] = head;\r
216 \r
217                 buf[4] = sector >> 24;\r
218                 buf[5] = sector >> 16;\r
219                 buf[6] = sector >> 8;\r
220                 buf[7] = sector;\r
221         } break;\r
222 \r
223         default:\r
224                 memset(buf, 0, 8);\r
225         }\r
226 \r
227 }\r
228 \r