1// SPDX-License-Identifier: BSD-3-Clause OR GPL-2.0
2/* Copyright 2016-2018 NXP
3 * Copyright (c) 2018-2019, Vladimir Oltean <olteanv@gmail.com>
4 */
5#include <linux/packing.h>
6#include <linux/module.h>
7#include <linux/bitops.h>
8#include <linux/bits.h>
9#include <linux/errno.h>
10#include <linux/types.h>
11#include <linux/bitrev.h>
12
13#define __pack_fields(pbuf, pbuflen, ustruct, fields, num_fields, quirks) \
14 ({ \
15 for (size_t i = 0; i < (num_fields); i++) { \
16 typeof(&(fields)[0]) field = &(fields)[i]; \
17 u64 uval; \
18 \
19 uval = ustruct_field_to_u64(ustruct, field->offset, field->size); \
20 \
21 __pack(pbuf, uval, field->startbit, field->endbit, \
22 pbuflen, quirks); \
23 } \
24 })
25
26#define __unpack_fields(pbuf, pbuflen, ustruct, fields, num_fields, quirks) \
27 ({ \
28 for (size_t i = 0; i < (num_fields); i++) { \
29 typeof(&(fields)[0]) field = &fields[i]; \
30 u64 uval; \
31 \
32 __unpack(pbuf, &uval, field->startbit, field->endbit, \
33 pbuflen, quirks); \
34 \
35 u64_to_ustruct_field(ustruct, field->offset, field->size, uval); \
36 } \
37 })
38
39/**
40 * calculate_box_addr - Determine physical location of byte in buffer
41 * @box: Index of byte within buffer seen as a logical big-endian big number
42 * @len: Size of buffer in bytes
43 * @quirks: mask of QUIRK_LSW32_IS_FIRST and QUIRK_LITTLE_ENDIAN
44 *
45 * Function interprets the buffer as a @len byte sized big number, and returns
46 * the physical offset of the @box logical octet within it. Internally, it
47 * treats the big number as groups of 4 bytes. If @len is not a multiple of 4,
48 * the last group may be shorter.
49 *
50 * @QUIRK_LSW32_IS_FIRST gives the ordering of groups of 4 octets relative to
51 * each other. If set, the most significant group of 4 octets is last in the
52 * buffer (and may be truncated if @len is not a multiple of 4).
53 *
54 * @QUIRK_LITTLE_ENDIAN gives the ordering of bytes within each group of 4.
55 * If set, the most significant byte is last in the group. If @len takes the
56 * form of 4k+3, the last group will only be able to represent 24 bits, and its
57 * most significant octet is byte 2.
58 *
59 * Return: the physical offset into the buffer corresponding to the logical box.
60 */
61static size_t calculate_box_addr(size_t box, size_t len, u8 quirks)
62{
63 size_t offset_of_group, offset_in_group, this_group = box / 4;
64 size_t group_size;
65
66 if (quirks & QUIRK_LSW32_IS_FIRST)
67 offset_of_group = this_group * 4;
68 else
69 offset_of_group = len - ((this_group + 1) * 4);
70
71 group_size = min(4, len - offset_of_group);
72
73 if (quirks & QUIRK_LITTLE_ENDIAN)
74 offset_in_group = box - this_group * 4;
75 else
76 offset_in_group = group_size - (box - this_group * 4) - 1;
77
78 return offset_of_group + offset_in_group;
79}
80
81static void __pack(void *pbuf, u64 uval, size_t startbit, size_t endbit,
82 size_t pbuflen, u8 quirks)
83{
84 /* Logical byte indices corresponding to the
85 * start and end of the field.
86 */
87 int plogical_first_u8 = startbit / BITS_PER_BYTE;
88 int plogical_last_u8 = endbit / BITS_PER_BYTE;
89 int value_width = startbit - endbit + 1;
90 int box;
91
92 /* Check if "uval" fits in "value_width" bits.
93 * The test only works for value_width < 64, but in the latter case,
94 * any 64-bit uval will surely fit.
95 */
96 WARN(value_width < 64 && uval >= (1ull << value_width),
97 "Cannot store 0x%llx inside bits %zu-%zu - will truncate\n",
98 uval, startbit, endbit);
99
100 /* Iterate through an idealistic view of the pbuf as an u64 with
101 * no quirks, u8 by u8 (aligned at u8 boundaries), from high to low
102 * logical bit significance. "box" denotes the current logical u8.
103 */
104 for (box = plogical_first_u8; box >= plogical_last_u8; box--) {
105 /* Bit indices into the currently accessed 8-bit box */
106 size_t box_start_bit, box_end_bit, box_addr;
107 u8 box_mask;
108 /* Corresponding bits from the unpacked u64 parameter */
109 size_t proj_start_bit, proj_end_bit;
110 u64 proj_mask;
111 u64 pval;
112
113 /* This u8 may need to be accessed in its entirety
114 * (from bit 7 to bit 0), or not, depending on the
115 * input arguments startbit and endbit.
116 */
117 if (box == plogical_first_u8)
118 box_start_bit = startbit % BITS_PER_BYTE;
119 else
120 box_start_bit = 7;
121 if (box == plogical_last_u8)
122 box_end_bit = endbit % BITS_PER_BYTE;
123 else
124 box_end_bit = 0;
125
126 /* We have determined the box bit start and end.
127 * Now we calculate where this (masked) u8 box would fit
128 * in the unpacked (CPU-readable) u64 - the u8 box's
129 * projection onto the unpacked u64. Though the
130 * box is u8, the projection is u64 because it may fall
131 * anywhere within the unpacked u64.
132 */
133 proj_start_bit = ((box * BITS_PER_BYTE) + box_start_bit) - endbit;
134 proj_end_bit = ((box * BITS_PER_BYTE) + box_end_bit) - endbit;
135 proj_mask = GENMASK_ULL(proj_start_bit, proj_end_bit);
136 box_mask = GENMASK(box_start_bit, box_end_bit);
137
138 /* Determine the offset of the u8 box inside the pbuf,
139 * adjusted for quirks. The adjusted box_addr will be used for
140 * effective addressing inside the pbuf (so it's not
141 * logical any longer).
142 */
143 box_addr = calculate_box_addr(box, len: pbuflen, quirks);
144
145 /* Write to pbuf, read from uval */
146 pval = uval & proj_mask;
147 pval >>= proj_end_bit;
148 pval <<= box_end_bit;
149
150 if (quirks & QUIRK_MSB_ON_THE_RIGHT) {
151 pval = bitrev8(pval);
152 box_mask = bitrev8(box_mask);
153 }
154
155 ((u8 *)pbuf)[box_addr] &= ~box_mask;
156 ((u8 *)pbuf)[box_addr] |= pval;
157 }
158}
159
160/**
161 * pack - Pack u64 number into bitfield of buffer.
162 *
163 * @pbuf: Pointer to a buffer holding the packed value.
164 * @uval: CPU-readable unpacked value to pack.
165 * @startbit: The index (in logical notation, compensated for quirks) where
166 * the packed value starts within pbuf. Must be larger than, or
167 * equal to, endbit.
168 * @endbit: The index (in logical notation, compensated for quirks) where
169 * the packed value ends within pbuf. Must be smaller than, or equal
170 * to, startbit.
171 * @pbuflen: The length in bytes of the packed buffer pointed to by @pbuf.
172 * @quirks: A bit mask of QUIRK_LITTLE_ENDIAN, QUIRK_LSW32_IS_FIRST and
173 * QUIRK_MSB_ON_THE_RIGHT.
174 *
175 * Return: 0 on success, EINVAL or ERANGE if called incorrectly. Assuming
176 * correct usage, return code may be discarded. The @pbuf memory will
177 * be modified on success.
178 */
179int pack(void *pbuf, u64 uval, size_t startbit, size_t endbit, size_t pbuflen,
180 u8 quirks)
181{
182 /* startbit is expected to be larger than endbit, and both are
183 * expected to be within the logically addressable range of the buffer.
184 */
185 if (unlikely(startbit < endbit || startbit >= BITS_PER_BYTE * pbuflen))
186 /* Invalid function call */
187 return -EINVAL;
188
189 if (unlikely(startbit - endbit >= 64))
190 return -ERANGE;
191
192 __pack(pbuf, uval, startbit, endbit, pbuflen, quirks);
193
194 return 0;
195}
196EXPORT_SYMBOL(pack);
197
198static void __unpack(const void *pbuf, u64 *uval, size_t startbit, size_t endbit,
199 size_t pbuflen, u8 quirks)
200{
201 /* Logical byte indices corresponding to the
202 * start and end of the field.
203 */
204 int plogical_first_u8 = startbit / BITS_PER_BYTE;
205 int plogical_last_u8 = endbit / BITS_PER_BYTE;
206 int box;
207
208 /* Initialize parameter */
209 *uval = 0;
210
211 /* Iterate through an idealistic view of the pbuf as an u64 with
212 * no quirks, u8 by u8 (aligned at u8 boundaries), from high to low
213 * logical bit significance. "box" denotes the current logical u8.
214 */
215 for (box = plogical_first_u8; box >= plogical_last_u8; box--) {
216 /* Bit indices into the currently accessed 8-bit box */
217 size_t box_start_bit, box_end_bit, box_addr;
218 u8 box_mask;
219 /* Corresponding bits from the unpacked u64 parameter */
220 size_t proj_start_bit, proj_end_bit;
221 u64 proj_mask;
222 u64 pval;
223
224 /* This u8 may need to be accessed in its entirety
225 * (from bit 7 to bit 0), or not, depending on the
226 * input arguments startbit and endbit.
227 */
228 if (box == plogical_first_u8)
229 box_start_bit = startbit % BITS_PER_BYTE;
230 else
231 box_start_bit = 7;
232 if (box == plogical_last_u8)
233 box_end_bit = endbit % BITS_PER_BYTE;
234 else
235 box_end_bit = 0;
236
237 /* We have determined the box bit start and end.
238 * Now we calculate where this (masked) u8 box would fit
239 * in the unpacked (CPU-readable) u64 - the u8 box's
240 * projection onto the unpacked u64. Though the
241 * box is u8, the projection is u64 because it may fall
242 * anywhere within the unpacked u64.
243 */
244 proj_start_bit = ((box * BITS_PER_BYTE) + box_start_bit) - endbit;
245 proj_end_bit = ((box * BITS_PER_BYTE) + box_end_bit) - endbit;
246 proj_mask = GENMASK_ULL(proj_start_bit, proj_end_bit);
247 box_mask = GENMASK(box_start_bit, box_end_bit);
248
249 /* Determine the offset of the u8 box inside the pbuf,
250 * adjusted for quirks. The adjusted box_addr will be used for
251 * effective addressing inside the pbuf (so it's not
252 * logical any longer).
253 */
254 box_addr = calculate_box_addr(box, len: pbuflen, quirks);
255
256 /* Read from pbuf, write to uval */
257 pval = ((u8 *)pbuf)[box_addr];
258
259 if (quirks & QUIRK_MSB_ON_THE_RIGHT)
260 pval = bitrev8(pval);
261
262 pval &= box_mask;
263
264 pval >>= box_end_bit;
265 pval <<= proj_end_bit;
266 *uval &= ~proj_mask;
267 *uval |= pval;
268 }
269}
270
271/**
272 * unpack - Unpack u64 number from packed buffer.
273 *
274 * @pbuf: Pointer to a buffer holding the packed value.
275 * @uval: Pointer to an u64 holding the unpacked value.
276 * @startbit: The index (in logical notation, compensated for quirks) where
277 * the packed value starts within pbuf. Must be larger than, or
278 * equal to, endbit.
279 * @endbit: The index (in logical notation, compensated for quirks) where
280 * the packed value ends within pbuf. Must be smaller than, or equal
281 * to, startbit.
282 * @pbuflen: The length in bytes of the packed buffer pointed to by @pbuf.
283 * @quirks: A bit mask of QUIRK_LITTLE_ENDIAN, QUIRK_LSW32_IS_FIRST and
284 * QUIRK_MSB_ON_THE_RIGHT.
285 *
286 * Return: 0 on success, EINVAL or ERANGE if called incorrectly. Assuming
287 * correct usage, return code may be discarded. The @uval will be
288 * modified on success.
289 */
290int unpack(const void *pbuf, u64 *uval, size_t startbit, size_t endbit,
291 size_t pbuflen, u8 quirks)
292{
293 /* width of the field to access in the pbuf */
294 u64 value_width;
295
296 /* startbit is expected to be larger than endbit, and both are
297 * expected to be within the logically addressable range of the buffer.
298 */
299 if (startbit < endbit || startbit >= BITS_PER_BYTE * pbuflen)
300 /* Invalid function call */
301 return -EINVAL;
302
303 value_width = startbit - endbit + 1;
304 if (value_width > 64)
305 return -ERANGE;
306
307 __unpack(pbuf, uval, startbit, endbit, pbuflen, quirks);
308
309 return 0;
310}
311EXPORT_SYMBOL(unpack);
312
313/**
314 * packing - Convert numbers (currently u64) between a packed and an unpacked
315 * format. Unpacked means laid out in memory in the CPU's native
316 * understanding of integers, while packed means anything else that
317 * requires translation.
318 *
319 * @pbuf: Pointer to a buffer holding the packed value.
320 * @uval: Pointer to an u64 holding the unpacked value.
321 * @startbit: The index (in logical notation, compensated for quirks) where
322 * the packed value starts within pbuf. Must be larger than, or
323 * equal to, endbit.
324 * @endbit: The index (in logical notation, compensated for quirks) where
325 * the packed value ends within pbuf. Must be smaller than, or equal
326 * to, startbit.
327 * @pbuflen: The length in bytes of the packed buffer pointed to by @pbuf.
328 * @op: If PACK, then uval will be treated as const pointer and copied (packed)
329 * into pbuf, between startbit and endbit.
330 * If UNPACK, then pbuf will be treated as const pointer and the logical
331 * value between startbit and endbit will be copied (unpacked) to uval.
332 * @quirks: A bit mask of QUIRK_LITTLE_ENDIAN, QUIRK_LSW32_IS_FIRST and
333 * QUIRK_MSB_ON_THE_RIGHT.
334 *
335 * Note: this is deprecated, prefer to use pack() or unpack() in new code.
336 *
337 * Return: 0 on success, EINVAL or ERANGE if called incorrectly. Assuming
338 * correct usage, return code may be discarded.
339 * If op is PACK, pbuf is modified.
340 * If op is UNPACK, uval is modified.
341 */
342int packing(void *pbuf, u64 *uval, int startbit, int endbit, size_t pbuflen,
343 enum packing_op op, u8 quirks)
344{
345 if (op == PACK)
346 return pack(pbuf, *uval, startbit, endbit, pbuflen, quirks);
347
348 return unpack(pbuf, uval, startbit, endbit, pbuflen, quirks);
349}
350EXPORT_SYMBOL(packing);
351
352static u64 ustruct_field_to_u64(const void *ustruct, size_t field_offset,
353 size_t field_size)
354{
355 switch (field_size) {
356 case 1:
357 return *((u8 *)(ustruct + field_offset));
358 case 2:
359 return *((u16 *)(ustruct + field_offset));
360 case 4:
361 return *((u32 *)(ustruct + field_offset));
362 default:
363 return *((u64 *)(ustruct + field_offset));
364 }
365}
366
367static void u64_to_ustruct_field(void *ustruct, size_t field_offset,
368 size_t field_size, u64 uval)
369{
370 switch (field_size) {
371 case 1:
372 *((u8 *)(ustruct + field_offset)) = uval;
373 break;
374 case 2:
375 *((u16 *)(ustruct + field_offset)) = uval;
376 break;
377 case 4:
378 *((u32 *)(ustruct + field_offset)) = uval;
379 break;
380 default:
381 *((u64 *)(ustruct + field_offset)) = uval;
382 break;
383 }
384}
385
386/**
387 * pack_fields_u8 - Pack array of fields
388 *
389 * @pbuf: Pointer to a buffer holding the packed value.
390 * @pbuflen: The length in bytes of the packed buffer pointed to by @pbuf.
391 * @ustruct: Pointer to CPU-readable structure holding the unpacked value.
392 * It is expected (but not checked) that this has the same data type
393 * as all struct packed_field_u8 definitions.
394 * @fields: Array of packed_field_u8 field definition. They must not overlap.
395 * @num_fields: Length of @fields array.
396 * @quirks: A bit mask of QUIRK_LITTLE_ENDIAN, QUIRK_LSW32_IS_FIRST and
397 * QUIRK_MSB_ON_THE_RIGHT.
398 *
399 * Use the pack_fields() macro instead of calling this directly.
400 */
401void pack_fields_u8(void *pbuf, size_t pbuflen, const void *ustruct,
402 const struct packed_field_u8 *fields, size_t num_fields,
403 u8 quirks)
404{
405 __pack_fields(pbuf, pbuflen, ustruct, fields, num_fields, quirks);
406}
407EXPORT_SYMBOL(pack_fields_u8);
408
409/**
410 * pack_fields_u16 - Pack array of fields
411 *
412 * @pbuf: Pointer to a buffer holding the packed value.
413 * @pbuflen: The length in bytes of the packed buffer pointed to by @pbuf.
414 * @ustruct: Pointer to CPU-readable structure holding the unpacked value.
415 * It is expected (but not checked) that this has the same data type
416 * as all struct packed_field_u16 definitions.
417 * @fields: Array of packed_field_u16 field definitions. They must not overlap.
418 * @num_fields: Length of @fields array.
419 * @quirks: A bit mask of QUIRK_LITTLE_ENDIAN, QUIRK_LSW32_IS_FIRST and
420 * QUIRK_MSB_ON_THE_RIGHT.
421 *
422 * Use the pack_fields() macro instead of calling this directly.
423 */
424void pack_fields_u16(void *pbuf, size_t pbuflen, const void *ustruct,
425 const struct packed_field_u16 *fields, size_t num_fields,
426 u8 quirks)
427{
428 __pack_fields(pbuf, pbuflen, ustruct, fields, num_fields, quirks);
429}
430EXPORT_SYMBOL(pack_fields_u16);
431
432/**
433 * unpack_fields_u8 - Unpack array of fields
434 *
435 * @pbuf: Pointer to a buffer holding the packed value.
436 * @pbuflen: The length in bytes of the packed buffer pointed to by @pbuf.
437 * @ustruct: Pointer to CPU-readable structure holding the unpacked value.
438 * It is expected (but not checked) that this has the same data type
439 * as all struct packed_field_u8 definitions.
440 * @fields: Array of packed_field_u8 field definitions. They must not overlap.
441 * @num_fields: Length of @fields array.
442 * @quirks: A bit mask of QUIRK_LITTLE_ENDIAN, QUIRK_LSW32_IS_FIRST and
443 * QUIRK_MSB_ON_THE_RIGHT.
444 *
445 * Use the unpack_fields() macro instead of calling this directly.
446 */
447void unpack_fields_u8(const void *pbuf, size_t pbuflen, void *ustruct,
448 const struct packed_field_u8 *fields, size_t num_fields,
449 u8 quirks)
450{
451 __unpack_fields(pbuf, pbuflen, ustruct, fields, num_fields, quirks);
452}
453EXPORT_SYMBOL(unpack_fields_u8);
454
455/**
456 * unpack_fields_u16 - Unpack array of fields
457 *
458 * @pbuf: Pointer to a buffer holding the packed value.
459 * @pbuflen: The length in bytes of the packed buffer pointed to by @pbuf.
460 * @ustruct: Pointer to CPU-readable structure holding the unpacked value.
461 * It is expected (but not checked) that this has the same data type
462 * as all struct packed_field_u16 definitions.
463 * @fields: Array of packed_field_u16 field definitions. They must not overlap.
464 * @num_fields: Length of @fields array.
465 * @quirks: A bit mask of QUIRK_LITTLE_ENDIAN, QUIRK_LSW32_IS_FIRST and
466 * QUIRK_MSB_ON_THE_RIGHT.
467 *
468 * Use the unpack_fields() macro instead of calling this directly.
469 */
470void unpack_fields_u16(const void *pbuf, size_t pbuflen, void *ustruct,
471 const struct packed_field_u16 *fields, size_t num_fields,
472 u8 quirks)
473{
474 __unpack_fields(pbuf, pbuflen, ustruct, fields, num_fields, quirks);
475}
476EXPORT_SYMBOL(unpack_fields_u16);
477
478MODULE_DESCRIPTION("Generic bitfield packing and unpacking");
479

source code of linux/lib/packing.c