52e87b9e680ab047212477fbdd55385532b22a59
[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         if (sdDev.capacity == 0)\r
34         {\r
35                 capacity = 0;\r
36         }\r
37         else if (scsiSectors && (capacity > scsiSectors))\r
38         {\r
39                 capacity = scsiSectors;\r
40         }\r
41         return capacity;\r
42 }\r
43 \r
44 \r
45 uint32_t SCSISector2SD(\r
46         uint32_t sdSectorStart,\r
47         uint16_t bytesPerSector,\r
48         uint32_t scsiSector)\r
49 {\r
50         return scsiSector * SDSectorsPerSCSISector(bytesPerSector) + sdSectorStart;\r
51 }\r
52 \r
53 // Standard mapping according to ECMA-107 and ISO/IEC 9293:1994\r
54 // Sector always starts at 1. There is no 0 sector.\r
55 uint64_t CHS2LBA(\r
56         uint32_t c,\r
57         uint8_t h,\r
58         uint32_t s,\r
59         uint16_t headsPerCylinder,\r
60         uint16_t sectorsPerTrack)\r
61 {\r
62         return (\r
63                 (((uint64_t)c) * headsPerCylinder + h) *\r
64                         (uint64_t) sectorsPerTrack\r
65                 ) + (s - 1);\r
66 }\r
67 \r
68 \r
69 void LBA2CHS(\r
70         uint32_t lba,\r
71         uint32_t* c,\r
72         uint8_t* h,\r
73         uint32_t* s,\r
74         uint16_t headsPerCylinder,\r
75         uint16_t sectorsPerTrack)\r
76 {\r
77         *c = lba / (((uint32_t) sectorsPerTrack) * headsPerCylinder);\r
78         *h = (lba / sectorsPerTrack) % headsPerCylinder;\r
79         *s = (lba % sectorsPerTrack) + 1;\r
80 }\r
81 \r
82 uint64_t scsiByteAddress(\r
83         uint16_t bytesPerSector,\r
84         uint16_t headsPerCylinder,\r
85         uint16_t sectorsPerTrack,\r
86         int format,\r
87         const uint8_t* addr)\r
88 {\r
89         uint64_t result;\r
90         switch (format)\r
91         {\r
92         case ADDRESS_BLOCK:\r
93         {\r
94                 uint32_t lba =\r
95                         (((uint32_t) addr[0]) << 24) +\r
96                         (((uint32_t) addr[1]) << 16) +\r
97                         (((uint32_t) addr[2]) << 8) +\r
98                         addr[3];\r
99 \r
100                 result = (uint64_t) bytesPerSector * lba;\r
101         } break;\r
102 \r
103         case ADDRESS_PHYSICAL_BYTE:\r
104         {\r
105                 uint32_t cyl =\r
106                         (((uint32_t) addr[0]) << 16) +\r
107                         (((uint32_t) addr[1]) << 8) +\r
108                         addr[2];\r
109 \r
110                 uint8_t head = addr[3];\r
111 \r
112                 uint32_t bytes =\r
113                         (((uint32_t) addr[4]) << 24) +\r
114                         (((uint32_t) addr[5]) << 16) +\r
115                         (((uint32_t) addr[6]) << 8) +\r
116                         addr[7];\r
117 \r
118                 result = CHS2LBA(cyl, head, 1, headsPerCylinder, sectorsPerTrack) *\r
119                         (uint64_t) bytesPerSector + bytes;\r
120         } break;\r
121 \r
122         case ADDRESS_PHYSICAL_SECTOR:\r
123         {\r
124                 uint32_t cyl =\r
125                         (((uint32_t) addr[0]) << 16) +\r
126                         (((uint32_t) addr[1]) << 8) +\r
127                         addr[2];\r
128 \r
129                 uint8_t head = scsiDev.data[3];\r
130 \r
131                 uint32_t sector =\r
132                         (((uint32_t) addr[4]) << 24) +\r
133                         (((uint32_t) addr[5]) << 16) +\r
134                         (((uint32_t) addr[6]) << 8) +\r
135                         addr[7];\r
136 \r
137                 result = CHS2LBA(cyl, head, sector, headsPerCylinder, sectorsPerTrack) * (uint64_t) bytesPerSector;\r
138         } break;\r
139 \r
140         default:\r
141                 result = (uint64_t) -1;\r
142         }\r
143 \r
144         return result;\r
145 }\r
146 \r
147 \r
148 void scsiSaveByteAddress(\r
149         uint16_t bytesPerSector,\r
150         uint16_t headsPerCylinder,\r
151         uint16_t sectorsPerTrack,\r
152         int format,\r
153         uint64_t byteAddr,\r
154         uint8_t* buf)\r
155 {\r
156         uint32_t lba = byteAddr / bytesPerSector;\r
157         uint32_t byteOffset = byteAddr % bytesPerSector;\r
158 \r
159         switch (format)\r
160         {\r
161         case ADDRESS_BLOCK:\r
162         {\r
163                 buf[0] = lba >> 24;\r
164                 buf[1] = lba >> 16;\r
165                 buf[2] = lba >> 8;\r
166                 buf[3] = lba;\r
167 \r
168                 buf[4] = 0;\r
169                 buf[5] = 0;\r
170                 buf[6] = 0;\r
171                 buf[7] = 0;\r
172         } break;\r
173 \r
174         case ADDRESS_PHYSICAL_BYTE:\r
175         {\r
176                 uint32_t cyl;\r
177                 uint8_t head;\r
178                 uint32_t sector;\r
179                 uint32_t bytes;\r
180 \r
181                 LBA2CHS(lba, &cyl, &head, &sector, headsPerCylinder, sectorsPerTrack);\r
182 \r
183                 bytes = sector * bytesPerSector + byteOffset;\r
184 \r
185                 buf[0] = cyl >> 16;\r
186                 buf[1] = cyl >> 8;\r
187                 buf[2] = cyl;\r
188 \r
189                 buf[3] = head;\r
190 \r
191                 buf[4] = bytes >> 24;\r
192                 buf[5] = bytes >> 16;\r
193                 buf[6] = bytes >> 8;\r
194                 buf[7] = bytes;\r
195         } break;\r
196 \r
197         case ADDRESS_PHYSICAL_SECTOR:\r
198         {\r
199                 uint32_t cyl;\r
200                 uint8_t head;\r
201                 uint32_t sector;\r
202 \r
203                 LBA2CHS(lba, &cyl, &head, &sector, headsPerCylinder, sectorsPerTrack);\r
204 \r
205                 buf[0] = cyl >> 16;\r
206                 buf[1] = cyl >> 8;\r
207                 buf[2] = cyl;\r
208 \r
209                 buf[3] = head;\r
210 \r
211                 buf[4] = sector >> 24;\r
212                 buf[5] = sector >> 16;\r
213                 buf[6] = sector >> 8;\r
214                 buf[7] = sector;\r
215         } break;\r
216 \r
217         default:\r
218                 memset(buf, 0, 8);\r
219         }\r
220 \r
221 }\r
222 \r