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