[mirror_ubuntu-hirsute-kernel.git] / lib / packing.c

// SPDX-License-Identifier: BSD-3-Clause OR GPL-2.0
/* Copyright (c) 2016-2018, NXP Semiconductors
 * Copyright (c) 2018-2019, Vladimir Oltean <olteanv@gmail.com>
 */
#include <linux/packing.h>
#include <linux/module.h>
#include <linux/bitops.h>
#include <linux/errno.h>
#include <linux/types.h>

static int get_le_offset(int offset)
{
	int closest_multiple_of_4;

	closest_multiple_of_4 = (offset / 4) * 4;
	offset -= closest_multiple_of_4;
	return closest_multiple_of_4 + (3 - offset);
}

static int get_reverse_lsw32_offset(int offset, size_t len)
{
	int closest_multiple_of_4;
	int word_index;

	word_index = offset / 4;
	closest_multiple_of_4 = word_index * 4;
	offset -= closest_multiple_of_4;
	word_index = (len / 4) - word_index - 1;
	return word_index * 4 + offset;
}

static u64 bit_reverse(u64 val, unsigned int width)
{
	u64 new_val = 0;
	unsigned int bit;
	unsigned int i;

	for (i = 0; i < width; i++) {
		bit = (val & (1 << i)) != 0;
		new_val |= (bit << (width - i - 1));
	}
	return new_val;
}

static void adjust_for_msb_right_quirk(u64 *to_write, int *box_start_bit,
				       int *box_end_bit, u8 *box_mask)
{
	int box_bit_width = *box_start_bit - *box_end_bit + 1;
	int new_box_start_bit, new_box_end_bit;

	*to_write >>= *box_end_bit;
	*to_write = bit_reverse(*to_write, box_bit_width);
	*to_write <<= *box_end_bit;

	new_box_end_bit   = box_bit_width - *box_start_bit - 1;
	new_box_start_bit = box_bit_width - *box_end_bit - 1;
	*box_mask = GENMASK_ULL(new_box_start_bit, new_box_end_bit);
	*box_start_bit = new_box_start_bit;
	*box_end_bit   = new_box_end_bit;
}

/**
 * packing - Convert numbers (currently u64) between a packed and an unpacked
 *	     format. Unpacked means laid out in memory in the CPU's native
 *	     understanding of integers, while packed means anything else that
 *	     requires translation.
 *
 * @pbuf: Pointer to a buffer holding the packed value.
 * @uval: Pointer to an u64 holding the unpacked value.
 * @startbit: The index (in logical notation, compensated for quirks) where
 *	      the packed value starts within pbuf. Must be larger than, or
 *	      equal to, endbit.
 * @endbit: The index (in logical notation, compensated for quirks) where
 *	    the packed value ends within pbuf. Must be smaller than, or equal
 *	    to, startbit.
 * @pbuflen: The length in bytes of the packed buffer pointed to by @pbuf.
 * @op: If PACK, then uval will be treated as const pointer and copied (packed)
 *	into pbuf, between startbit and endbit.
 *	If UNPACK, then pbuf will be treated as const pointer and the logical
 *	value between startbit and endbit will be copied (unpacked) to uval.
 * @quirks: A bit mask of QUIRK_LITTLE_ENDIAN, QUIRK_LSW32_IS_FIRST and
 *	    QUIRK_MSB_ON_THE_RIGHT.
 *
 * Return: 0 on success, EINVAL or ERANGE if called incorrectly. Assuming
 *	   correct usage, return code may be discarded.
 *	   If op is PACK, pbuf is modified.
 *	   If op is UNPACK, uval is modified.
 */
int packing(void *pbuf, u64 *uval, int startbit, int endbit, size_t pbuflen,
	    enum packing_op op, u8 quirks)
{
	/* Number of bits for storing "uval"
	 * also width of the field to access in the pbuf
	 */
	u64 value_width;
	/* Logical byte indices corresponding to the
	 * start and end of the field.
	 */
	int plogical_first_u8, plogical_last_u8, box;

	/* startbit is expected to be larger than endbit */
	if (startbit < endbit)
		/* Invalid function call */
		return -EINVAL;

	value_width = startbit - endbit + 1;
	if (value_width > 64)
		return -ERANGE;

	/* Check if "uval" fits in "value_width" bits.
	 * If value_width is 64, the check will fail, but any
	 * 64-bit uval will surely fit.
	 */
	if (op == PACK && value_width < 64 && (*uval >= (1ull << value_width)))
		/* Cannot store "uval" inside "value_width" bits.
		 * Truncating "uval" is most certainly not desirable,
		 * so simply erroring out is appropriate.
		 */
		return -ERANGE;

	/* Initialize parameter */
	if (op == UNPACK)
		*uval = 0;

	/* Iterate through an idealistic view of the pbuf as an u64 with
	 * no quirks, u8 by u8 (aligned at u8 boundaries), from high to low
	 * logical bit significance. "box" denotes the current logical u8.
	 */
	plogical_first_u8 = startbit / 8;
	plogical_last_u8  = endbit / 8;

	for (box = plogical_first_u8; box >= plogical_last_u8; box--) {
		/* Bit indices into the currently accessed 8-bit box */
		int box_start_bit, box_end_bit, box_addr;
		u8  box_mask;
		/* Corresponding bits from the unpacked u64 parameter */
		int proj_start_bit, proj_end_bit;
		u64 proj_mask;

		/* This u8 may need to be accessed in its entirety
		 * (from bit 7 to bit 0), or not, depending on the
		 * input arguments startbit and endbit.
		 */
		if (box == plogical_first_u8)
			box_start_bit = startbit % 8;
		else
			box_start_bit = 7;
		if (box == plogical_last_u8)
			box_end_bit = endbit % 8;
		else
			box_end_bit = 0;

		/* We have determined the box bit start and end.
		 * Now we calculate where this (masked) u8 box would fit
		 * in the unpacked (CPU-readable) u64 - the u8 box's
		 * projection onto the unpacked u64. Though the
		 * box is u8, the projection is u64 because it may fall
		 * anywhere within the unpacked u64.
		 */
		proj_start_bit = ((box * 8) + box_start_bit) - endbit;
		proj_end_bit   = ((box * 8) + box_end_bit) - endbit;
		proj_mask = GENMASK_ULL(proj_start_bit, proj_end_bit);
		box_mask  = GENMASK_ULL(box_start_bit, box_end_bit);

		/* Determine the offset of the u8 box inside the pbuf,
		 * adjusted for quirks. The adjusted box_addr will be used for
		 * effective addressing inside the pbuf (so it's not
		 * logical any longer).
		 */
		box_addr = pbuflen - box - 1;
		if (quirks & QUIRK_LITTLE_ENDIAN)
			box_addr = get_le_offset(box_addr);
		if (quirks & QUIRK_LSW32_IS_FIRST)
			box_addr = get_reverse_lsw32_offset(box_addr,
							    pbuflen);

		if (op == UNPACK) {
			u64 pval;

			/* Read from pbuf, write to uval */
			pval = ((u8 *)pbuf)[box_addr] & box_mask;
			if (quirks & QUIRK_MSB_ON_THE_RIGHT)
				adjust_for_msb_right_quirk(&pval,
							   &box_start_bit,
							   &box_end_bit,
							   &box_mask);

			pval >>= box_end_bit;
			pval <<= proj_end_bit;
			*uval &= ~proj_mask;
			*uval |= pval;
		} else {
			u64 pval;

			/* Write to pbuf, read from uval */
			pval = (*uval) & proj_mask;
			pval >>= proj_end_bit;
			if (quirks & QUIRK_MSB_ON_THE_RIGHT)
				adjust_for_msb_right_quirk(&pval,
							   &box_start_bit,
							   &box_end_bit,
							   &box_mask);

			pval <<= box_end_bit;
			((u8 *)pbuf)[box_addr] &= ~box_mask;
			((u8 *)pbuf)[box_addr] |= pval;
		}
	}
	return 0;
}
EXPORT_SYMBOL(packing);

MODULE_LICENSE("GPL v2");
MODULE_DESCRIPTION("Generic bitfield packing and unpacking");
Commit	Line	Data
554aae35 VO	1	// SPDX-License-Identifier: BSD-3-Clause OR GPL-2.0
	2	/* Copyright (c) 2016-2018, NXP Semiconductors
	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/errno.h>
	9	#include <linux/types.h>
	10
	11	static int get_le_offset(int offset)
	12	{
	13	int closest_multiple_of_4;
	14
	15	closest_multiple_of_4 = (offset / 4) * 4;
	16	offset -= closest_multiple_of_4;
	17	return closest_multiple_of_4 + (3 - offset);
	18	}
	19
	20	static int get_reverse_lsw32_offset(int offset, size_t len)
	21	{
	22	int closest_multiple_of_4;
	23	int word_index;
	24
	25	word_index = offset / 4;
	26	closest_multiple_of_4 = word_index * 4;
	27	offset -= closest_multiple_of_4;
	28	word_index = (len / 4) - word_index - 1;
	29	return word_index * 4 + offset;
	30	}
	31
	32	static u64 bit_reverse(u64 val, unsigned int width)
	33	{
	34	u64 new_val = 0;
	35	unsigned int bit;
	36	unsigned int i;
	37
	38	for (i = 0; i < width; i++) {
	39	bit = (val & (1 << i)) != 0;
	40	new_val \|= (bit << (width - i - 1));
	41	}
	42	return new_val;
	43	}
	44
	45	static void adjust_for_msb_right_quirk(u64 to_write, int box_start_bit,
	46	int box_end_bit, u8 box_mask)
	47	{
	48	int box_bit_width = box_start_bit - box_end_bit + 1;
	49	int new_box_start_bit, new_box_end_bit;
	50
	51	to_write >>= box_end_bit;
	52	to_write = bit_reverse(to_write, box_bit_width);
	53	to_write <<= box_end_bit;
	54
	55	new_box_end_bit = box_bit_width - *box_start_bit - 1;
	56	new_box_start_bit = box_bit_width - *box_end_bit - 1;
	57	*box_mask = GENMASK_ULL(new_box_start_bit, new_box_end_bit);
	58	*box_start_bit = new_box_start_bit;
	59	*box_end_bit = new_box_end_bit;
	60	}
	61
	62	/**
	63	* packing - Convert numbers (currently u64) between a packed and an unpacked
	64	* format. Unpacked means laid out in memory in the CPU's native
65	* understanding of integers, while packed means anything else that
66	* requires translation.
67	*
68	* @pbuf: Pointer to a buffer holding the packed value.
69	* @uval: Pointer to an u64 holding the unpacked value.
70	* @startbit: The index (in logical notation, compensated for quirks) where
71	* the packed value starts within pbuf. Must be larger than, or
72	* equal to, endbit.
73	* @endbit: The index (in logical notation, compensated for quirks) where
74	* the packed value ends within pbuf. Must be smaller than, or equal
75	* to, startbit.
7dea927f	76	* @pbuflen: The length in bytes of the packed buffer pointed to by @pbuf.
554aae35 VO	77	* @op: If PACK, then uval will be treated as const pointer and copied (packed)
	78	* into pbuf, between startbit and endbit.
	79	* If UNPACK, then pbuf will be treated as const pointer and the logical
	80	* value between startbit and endbit will be copied (unpacked) to uval.
	81	* @quirks: A bit mask of QUIRK_LITTLE_ENDIAN, QUIRK_LSW32_IS_FIRST and
	82	* QUIRK_MSB_ON_THE_RIGHT.
	83	*
	84	* Return: 0 on success, EINVAL or ERANGE if called incorrectly. Assuming
	85	* correct usage, return code may be discarded.
	86	* If op is PACK, pbuf is modified.
	87	* If op is UNPACK, uval is modified.
	88	*/
	89	int packing(void pbuf, u64 uval, int startbit, int endbit, size_t pbuflen,
	90	enum packing_op op, u8 quirks)
	91	{
	92	/* Number of bits for storing "uval"
	93	* also width of the field to access in the pbuf
	94	*/
	95	u64 value_width;
	96	/* Logical byte indices corresponding to the
	97	* start and end of the field.
	98	*/
	99	int plogical_first_u8, plogical_last_u8, box;
	100
	101	/* startbit is expected to be larger than endbit */
	102	if (startbit < endbit)
	103	/* Invalid function call */
	104	return -EINVAL;
	105
	106	value_width = startbit - endbit + 1;
	107	if (value_width > 64)
	108	return -ERANGE;
	109
	110	/* Check if "uval" fits in "value_width" bits.
	111	* If value_width is 64, the check will fail, but any
	112	* 64-bit uval will surely fit.
	113	*/
	114	if (op == PACK && value_width < 64 && (*uval >= (1ull << value_width)))
	115	/* Cannot store "uval" inside "value_width" bits.
	116	* Truncating "uval" is most certainly not desirable,
	117	* so simply erroring out is appropriate.
	118	*/
	119	return -ERANGE;
	120
	121	/* Initialize parameter */
	122	if (op == UNPACK)
	123	*uval = 0;
	124
	125	/* Iterate through an idealistic view of the pbuf as an u64 with
	126	* no quirks, u8 by u8 (aligned at u8 boundaries), from high to low
	127	* logical bit significance. "box" denotes the current logical u8.
	128	*/
	129	plogical_first_u8 = startbit / 8;
	130	plogical_last_u8 = endbit / 8;
	131
	132	for (box = plogical_first_u8; box >= plogical_last_u8; box--) {
	133	/* Bit indices into the currently accessed 8-bit box */
	134	int box_start_bit, box_end_bit, box_addr;
	135	u8 box_mask;
	136	/* Corresponding bits from the unpacked u64 parameter */
	137	int proj_start_bit, proj_end_bit;
	138	u64 proj_mask;
	139
	140	/* This u8 may need to be accessed in its entirety
141	* (from bit 7 to bit 0), or not, depending on the
142	* input arguments startbit and endbit.
143	*/
144	if (box == plogical_first_u8)
145	box_start_bit = startbit % 8;
146	else
147	box_start_bit = 7;
148	if (box == plogical_last_u8)
149	box_end_bit = endbit % 8;
150	else
151	box_end_bit = 0;
152
153	/* We have determined the box bit start and end.
154	* Now we calculate where this (masked) u8 box would fit
155	* in the unpacked (CPU-readable) u64 - the u8 box's
156	* projection onto the unpacked u64. Though the
157	* box is u8, the projection is u64 because it may fall
158	* anywhere within the unpacked u64.
159	*/
160	proj_start_bit = ((box * 8) + box_start_bit) - endbit;
161	proj_end_bit = ((box * 8) + box_end_bit) - endbit;
162	proj_mask = GENMASK_ULL(proj_start_bit, proj_end_bit);
163	box_mask = GENMASK_ULL(box_start_bit, box_end_bit);
164
165	/* Determine the offset of the u8 box inside the pbuf,
166	* adjusted for quirks. The adjusted box_addr will be used for
167	* effective addressing inside the pbuf (so it's not
168	* logical any longer).
169	*/
170	box_addr = pbuflen - box - 1;
171	if (quirks & QUIRK_LITTLE_ENDIAN)
172	box_addr = get_le_offset(box_addr);
173	if (quirks & QUIRK_LSW32_IS_FIRST)
174	box_addr = get_reverse_lsw32_offset(box_addr,
175	pbuflen);
176
177	if (op == UNPACK) {
178	u64 pval;
179
180	/* Read from pbuf, write to uval */
181	pval = ((u8 *)pbuf)[box_addr] & box_mask;
182	if (quirks & QUIRK_MSB_ON_THE_RIGHT)
183	adjust_for_msb_right_quirk(&pval,
184	&box_start_bit,
185	&box_end_bit,
186	&box_mask);
187
188	pval >>= box_end_bit;
189	pval <<= proj_end_bit;
190	*uval &= ~proj_mask;
191	*uval \|= pval;
192	} else {
193	u64 pval;
194
195	/* Write to pbuf, read from uval */
196	pval = (*uval) & proj_mask;
197	pval >>= proj_end_bit;
198	if (quirks & QUIRK_MSB_ON_THE_RIGHT)
199	adjust_for_msb_right_quirk(&pval,
200	&box_start_bit,
201	&box_end_bit,
202	&box_mask);
203
204	pval <<= box_end_bit;
205	((u8 *)pbuf)[box_addr] &= ~box_mask;
206	((u8 *)pbuf)[box_addr] \|= pval;
207	}
208	}
209	return 0;
210	}
211	EXPORT_SYMBOL(packing);
212
213	MODULE_LICENSE("GPL v2");
214	MODULE_DESCRIPTION("Generic bitfield packing and unpacking");