packing.c

  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, 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, 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");