1 #ifndef SG_UNALIGNED_H
2 #define SG_UNALIGNED_H
3
4 /*
5 * Copyright (c) 2014-2017 Douglas Gilbert.
6 * All rights reserved.
7 * Use of this source code is governed by a BSD-style
8 * license that can be found in the BSD_LICENSE file.
9 */
10
11 #include <stdint.h>
12
13 #ifdef __cplusplus
14 extern "C" {
15 #endif
16
17 /* Borrowed from the Linux kernel, via mhvtl */
18
19 /* In the first section below, functions that copy unsigned integers in a
20 * computer's native format, to and from an unaligned big endian sequence of
21 * bytes. Big endian byte format "on the wire" is the default used by SCSI
22 * standards (www.t10.org). Big endian is also the network byte order. */
23
__get_unaligned_be16(const uint8_t * p)24 static inline uint16_t __get_unaligned_be16(const uint8_t *p)
25 {
26 return p[0] << 8 | p[1];
27 }
28
__get_unaligned_be32(const uint8_t * p)29 static inline uint32_t __get_unaligned_be32(const uint8_t *p)
30 {
31 return p[0] << 24 | p[1] << 16 | p[2] << 8 | p[3];
32 }
33
34 /* Assume 48 bit value placed in uint64_t */
__get_unaligned_be48(const uint8_t * p)35 static inline uint64_t __get_unaligned_be48(const uint8_t *p)
36 {
37 return (uint64_t)__get_unaligned_be16(p) << 32 |
38 __get_unaligned_be32(p + 2);
39 }
40
__get_unaligned_be64(const uint8_t * p)41 static inline uint64_t __get_unaligned_be64(const uint8_t *p)
42 {
43 return (uint64_t)__get_unaligned_be32(p) << 32 |
44 __get_unaligned_be32(p + 4);
45 }
46
__put_unaligned_be16(uint16_t val,uint8_t * p)47 static inline void __put_unaligned_be16(uint16_t val, uint8_t *p)
48 {
49 *p++ = val >> 8;
50 *p++ = val;
51 }
52
__put_unaligned_be32(uint32_t val,uint8_t * p)53 static inline void __put_unaligned_be32(uint32_t val, uint8_t *p)
54 {
55 __put_unaligned_be16(val >> 16, p);
56 __put_unaligned_be16(val, p + 2);
57 }
58
59 /* Assume 48 bit value placed in uint64_t */
__put_unaligned_be48(uint64_t val,uint8_t * p)60 static inline void __put_unaligned_be48(uint64_t val, uint8_t *p)
61 {
62 __put_unaligned_be16(val >> 32, p);
63 __put_unaligned_be32(val, p + 2);
64 }
65
__put_unaligned_be64(uint64_t val,uint8_t * p)66 static inline void __put_unaligned_be64(uint64_t val, uint8_t *p)
67 {
68 __put_unaligned_be32(val >> 32, p);
69 __put_unaligned_be32(val, p + 4);
70 }
71
sg_get_unaligned_be16(const void * p)72 static inline uint16_t sg_get_unaligned_be16(const void *p)
73 {
74 return __get_unaligned_be16((const uint8_t *)p);
75 }
76
sg_get_unaligned_be24(const void * p)77 static inline uint32_t sg_get_unaligned_be24(const void *p)
78 {
79 return ((const uint8_t *)p)[0] << 16 | ((const uint8_t *)p)[1] << 8 |
80 ((const uint8_t *)p)[2];
81 }
82
sg_get_unaligned_be32(const void * p)83 static inline uint32_t sg_get_unaligned_be32(const void *p)
84 {
85 return __get_unaligned_be32((const uint8_t *)p);
86 }
87
88 /* Assume 48 bit value placed in uint64_t */
sg_get_unaligned_be48(const void * p)89 static inline uint64_t sg_get_unaligned_be48(const void *p)
90 {
91 return __get_unaligned_be48((const uint8_t *)p);
92 }
93
sg_get_unaligned_be64(const void * p)94 static inline uint64_t sg_get_unaligned_be64(const void *p)
95 {
96 return __get_unaligned_be64((const uint8_t *)p);
97 }
98
99 /* Returns 0 if 'num_bytes' is less than or equal to 0 or greater than
100 * 8 (i.e. sizeof(uint64_t)). Else returns result in uint64_t which is
101 * an 8 byte unsigned integer. */
sg_get_unaligned_be(int num_bytes,const void * p)102 static inline uint64_t sg_get_unaligned_be(int num_bytes, const void *p)
103 {
104 if ((num_bytes <= 0) || (num_bytes > (int)sizeof(uint64_t)))
105 return 0;
106 else {
107 const uint8_t * xp = (const uint8_t *)p;
108 uint64_t res = *xp;
109
110 for (++xp; num_bytes > 1; ++xp, --num_bytes)
111 res = (res << 8) | *xp;
112 return res;
113 }
114 }
115
sg_put_unaligned_be16(uint16_t val,void * p)116 static inline void sg_put_unaligned_be16(uint16_t val, void *p)
117 {
118 __put_unaligned_be16(val, (uint8_t *)p);
119 }
120
sg_put_unaligned_be24(uint32_t val,void * p)121 static inline void sg_put_unaligned_be24(uint32_t val, void *p)
122 {
123 ((uint8_t *)p)[0] = (val >> 16) & 0xff;
124 ((uint8_t *)p)[1] = (val >> 8) & 0xff;
125 ((uint8_t *)p)[2] = val & 0xff;
126 }
127
sg_put_unaligned_be32(uint32_t val,void * p)128 static inline void sg_put_unaligned_be32(uint32_t val, void *p)
129 {
130 __put_unaligned_be32(val, (uint8_t *)p);
131 }
132
133 /* Assume 48 bit value placed in uint64_t */
sg_put_unaligned_be48(uint64_t val,void * p)134 static inline void sg_put_unaligned_be48(uint64_t val, void *p)
135 {
136 __put_unaligned_be48(val, (uint8_t *)p);
137 }
138
sg_put_unaligned_be64(uint64_t val,void * p)139 static inline void sg_put_unaligned_be64(uint64_t val, void *p)
140 {
141 __put_unaligned_be64(val, (uint8_t *)p);
142 }
143
144 /* Since cdb and parameter blocks are often memset to zero before these
145 * unaligned function partially fill them, then check for a val of zero
146 * and ignore if it is with these variants. */
sg_nz_put_unaligned_be16(uint16_t val,void * p)147 static inline void sg_nz_put_unaligned_be16(uint16_t val, void *p)
148 {
149 if (val)
150 __put_unaligned_be16(val, (uint8_t *)p);
151 }
152
sg_nz_put_unaligned_be24(uint32_t val,void * p)153 static inline void sg_nz_put_unaligned_be24(uint32_t val, void *p)
154 {
155 if (val) {
156 ((uint8_t *)p)[0] = (val >> 16) & 0xff;
157 ((uint8_t *)p)[1] = (val >> 8) & 0xff;
158 ((uint8_t *)p)[2] = val & 0xff;
159 }
160 }
161
sg_nz_put_unaligned_be32(uint32_t val,void * p)162 static inline void sg_nz_put_unaligned_be32(uint32_t val, void *p)
163 {
164 if (val)
165 __put_unaligned_be32(val, (uint8_t *)p);
166 }
167
sg_nz_put_unaligned_be64(uint64_t val,void * p)168 static inline void sg_nz_put_unaligned_be64(uint64_t val, void *p)
169 {
170 if (val)
171 __put_unaligned_be64(val, (uint8_t *)p);
172 }
173
174
175 /* Below are the little endian equivalents of the big endian functions
176 * above. Little endian is used by ATA, PCI and NVMe.
177 */
178
__get_unaligned_le16(const uint8_t * p)179 static inline uint16_t __get_unaligned_le16(const uint8_t *p)
180 {
181 return p[1] << 8 | p[0];
182 }
183
__get_unaligned_le32(const uint8_t * p)184 static inline uint32_t __get_unaligned_le32(const uint8_t *p)
185 {
186 return p[3] << 24 | p[2] << 16 | p[1] << 8 | p[0];
187 }
188
__get_unaligned_le64(const uint8_t * p)189 static inline uint64_t __get_unaligned_le64(const uint8_t *p)
190 {
191 return (uint64_t)__get_unaligned_le32(p + 4) << 32 |
192 __get_unaligned_le32(p);
193 }
194
__put_unaligned_le16(uint16_t val,uint8_t * p)195 static inline void __put_unaligned_le16(uint16_t val, uint8_t *p)
196 {
197 *p++ = val;
198 *p++ = val >> 8;
199 }
200
__put_unaligned_le32(uint32_t val,uint8_t * p)201 static inline void __put_unaligned_le32(uint32_t val, uint8_t *p)
202 {
203 __put_unaligned_le16(val >> 16, p + 2);
204 __put_unaligned_le16(val, p);
205 }
206
__put_unaligned_le64(uint64_t val,uint8_t * p)207 static inline void __put_unaligned_le64(uint64_t val, uint8_t *p)
208 {
209 __put_unaligned_le32(val >> 32, p + 4);
210 __put_unaligned_le32(val, p);
211 }
212
sg_get_unaligned_le16(const void * p)213 static inline uint16_t sg_get_unaligned_le16(const void *p)
214 {
215 return __get_unaligned_le16((const uint8_t *)p);
216 }
217
sg_get_unaligned_le24(const void * p)218 static inline uint32_t sg_get_unaligned_le24(const void *p)
219 {
220 return (uint32_t)__get_unaligned_le16((const uint8_t *)p) |
221 ((const uint8_t *)p)[2] << 16;
222 }
223
sg_get_unaligned_le32(const void * p)224 static inline uint32_t sg_get_unaligned_le32(const void *p)
225 {
226 return __get_unaligned_le32((const uint8_t *)p);
227 }
228
229 /* Assume 48 bit value placed in uint64_t */
sg_get_unaligned_le48(const void * p)230 static inline uint64_t sg_get_unaligned_le48(const void *p)
231 {
232 return (uint64_t)__get_unaligned_le16((const uint8_t *)p + 4) << 32 |
233 __get_unaligned_le32((const uint8_t *)p);
234 }
235
sg_get_unaligned_le64(const void * p)236 static inline uint64_t sg_get_unaligned_le64(const void *p)
237 {
238 return __get_unaligned_le64((const uint8_t *)p);
239 }
240
241 /* Returns 0 if 'num_bytes' is less than or equal to 0 or greater than
242 * 8 (i.e. sizeof(uint64_t)). Else returns result in uint64_t which is
243 * an 8 byte unsigned integer. */
sg_get_unaligned_le(int num_bytes,const void * p)244 static inline uint64_t sg_get_unaligned_le(int num_bytes, const void *p)
245 {
246 if ((num_bytes <= 0) || (num_bytes > (int)sizeof(uint64_t)))
247 return 0;
248 else {
249 const uint8_t * xp = (const uint8_t *)p + (num_bytes - 1);
250 uint64_t res = *xp;
251
252 for (--xp; num_bytes > 1; --xp, --num_bytes)
253 res = (res << 8) | *xp;
254 return res;
255 }
256 }
257
sg_put_unaligned_le16(uint16_t val,void * p)258 static inline void sg_put_unaligned_le16(uint16_t val, void *p)
259 {
260 __put_unaligned_le16(val, (uint8_t *)p);
261 }
262
sg_put_unaligned_le24(uint32_t val,void * p)263 static inline void sg_put_unaligned_le24(uint32_t val, void *p)
264 {
265 ((uint8_t *)p)[2] = (val >> 16) & 0xff;
266 ((uint8_t *)p)[1] = (val >> 8) & 0xff;
267 ((uint8_t *)p)[0] = val & 0xff;
268 }
269
sg_put_unaligned_le32(uint32_t val,void * p)270 static inline void sg_put_unaligned_le32(uint32_t val, void *p)
271 {
272 __put_unaligned_le32(val, (uint8_t *)p);
273 }
274
275 /* Assume 48 bit value placed in uint64_t */
sg_put_unaligned_le48(uint64_t val,void * p)276 static inline void sg_put_unaligned_le48(uint64_t val, void *p)
277 {
278 ((uint8_t *)p)[5] = (val >> 40) & 0xff;
279 ((uint8_t *)p)[4] = (val >> 32) & 0xff;
280 ((uint8_t *)p)[3] = (val >> 24) & 0xff;
281 ((uint8_t *)p)[2] = (val >> 16) & 0xff;
282 ((uint8_t *)p)[1] = (val >> 8) & 0xff;
283 ((uint8_t *)p)[0] = val & 0xff;
284 }
285
sg_put_unaligned_le64(uint64_t val,void * p)286 static inline void sg_put_unaligned_le64(uint64_t val, void *p)
287 {
288 __put_unaligned_le64(val, (uint8_t *)p);
289 }
290
291 /* Since cdb and parameter blocks are often memset to zero before these
292 * unaligned function partially fill them, then check for a val of zero
293 * and ignore if it is with these variants. */
sg_nz_put_unaligned_le16(uint16_t val,void * p)294 static inline void sg_nz_put_unaligned_le16(uint16_t val, void *p)
295 {
296 if (val)
297 __put_unaligned_le16(val, (uint8_t *)p);
298 }
299
sg_nz_put_unaligned_le24(uint32_t val,void * p)300 static inline void sg_nz_put_unaligned_le24(uint32_t val, void *p)
301 {
302 if (val) {
303 ((uint8_t *)p)[2] = (val >> 16) & 0xff;
304 ((uint8_t *)p)[1] = (val >> 8) & 0xff;
305 ((uint8_t *)p)[0] = val & 0xff;
306 }
307 }
308
sg_nz_put_unaligned_le32(uint32_t val,void * p)309 static inline void sg_nz_put_unaligned_le32(uint32_t val, void *p)
310 {
311 if (val)
312 __put_unaligned_le32(val, (uint8_t *)p);
313 }
314
sg_nz_put_unaligned_le64(uint64_t val,void * p)315 static inline void sg_nz_put_unaligned_le64(uint64_t val, void *p)
316 {
317 if (val)
318 __put_unaligned_le64(val, (uint8_t *)p);
319 }
320
321 #ifdef __cplusplus
322 }
323 #endif
324
325 #endif /* SG_UNALIGNED_H */
326