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
 11static 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
 20static 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
 32static 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
 45static 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.
 76 * @op: If PACK, then uval will be treated as const pointer and copied (packed)
 77 *	into pbuf, between startbit and endbit.
 78 *	If UNPACK, then pbuf will be treated as const pointer and the logical
 79 *	value between startbit and endbit will be copied (unpacked) to uval.
 80 * @quirks: A bit mask of QUIRK_LITTLE_ENDIAN, QUIRK_LSW32_IS_FIRST and
 81 *	    QUIRK_MSB_ON_THE_RIGHT.
 82 *
 83 * Return: 0 on success, EINVAL or ERANGE if called incorrectly. Assuming
 84 *	   correct usage, return code may be discarded.
 85 *	   If op is PACK, pbuf is modified.
 86 *	   If op is UNPACK, uval is modified.
 87 */
 88int packing(void *pbuf, u64 *uval, int startbit, int endbit, size_t pbuflen,
 89	    enum packing_op op, u8 quirks)
 90{
 91	/* Number of bits for storing "uval"
 92	 * also width of the field to access in the pbuf
 93	 */
 94	u64 value_width;
 95	/* Logical byte indices corresponding to the
 96	 * start and end of the field.
 97	 */
 98	int plogical_first_u8, plogical_last_u8, box;
 99
100	/* startbit is expected to be larger than endbit */
101	if (startbit < endbit)
102		/* Invalid function call */
103		return -EINVAL;
104
105	value_width = startbit - endbit + 1;
106	if (value_width > 64)
107		return -ERANGE;
108
109	/* Check if "uval" fits in "value_width" bits.
110	 * If value_width is 64, the check will fail, but any
111	 * 64-bit uval will surely fit.
112	 */
113	if (op == PACK && value_width < 64 && (*uval >= (1ull << value_width)))
114		/* Cannot store "uval" inside "value_width" bits.
115		 * Truncating "uval" is most certainly not desirable,
116		 * so simply erroring out is appropriate.
117		 */
118		return -ERANGE;
119
120	/* Initialize parameter */
121	if (op == UNPACK)
122		*uval = 0;
123
124	/* Iterate through an idealistic view of the pbuf as an u64 with
125	 * no quirks, u8 by u8 (aligned at u8 boundaries), from high to low
126	 * logical bit significance. "box" denotes the current logical u8.
127	 */
128	plogical_first_u8 = startbit / 8;
129	plogical_last_u8  = endbit / 8;
130
131	for (box = plogical_first_u8; box >= plogical_last_u8; box--) {
132		/* Bit indices into the currently accessed 8-bit box */
133		int box_start_bit, box_end_bit, box_addr;
134		u8  box_mask;
135		/* Corresponding bits from the unpacked u64 parameter */
136		int proj_start_bit, proj_end_bit;
137		u64 proj_mask;
138
139		/* This u8 may need to be accessed in its entirety
140		 * (from bit 7 to bit 0), or not, depending on the
141		 * input arguments startbit and endbit.
142		 */
143		if (box == plogical_first_u8)
144			box_start_bit = startbit % 8;
145		else
146			box_start_bit = 7;
147		if (box == plogical_last_u8)
148			box_end_bit = endbit % 8;
149		else
150			box_end_bit = 0;
151
152		/* We have determined the box bit start and end.
153		 * Now we calculate where this (masked) u8 box would fit
154		 * in the unpacked (CPU-readable) u64 - the u8 box's
155		 * projection onto the unpacked u64. Though the
156		 * box is u8, the projection is u64 because it may fall
157		 * anywhere within the unpacked u64.
158		 */
159		proj_start_bit = ((box * 8) + box_start_bit) - endbit;
160		proj_end_bit   = ((box * 8) + box_end_bit) - endbit;
161		proj_mask = GENMASK_ULL(proj_start_bit, proj_end_bit);
162		box_mask  = GENMASK_ULL(box_start_bit, box_end_bit);
163
164		/* Determine the offset of the u8 box inside the pbuf,
165		 * adjusted for quirks. The adjusted box_addr will be used for
166		 * effective addressing inside the pbuf (so it's not
167		 * logical any longer).
168		 */
169		box_addr = pbuflen - box - 1;
170		if (quirks & QUIRK_LITTLE_ENDIAN)
171			box_addr = get_le_offset(box_addr);
172		if (quirks & QUIRK_LSW32_IS_FIRST)
173			box_addr = get_reverse_lsw32_offset(box_addr,
174							    pbuflen);
175
176		if (op == UNPACK) {
177			u64 pval;
178
179			/* Read from pbuf, write to uval */
180			pval = ((u8 *)pbuf)[box_addr] & box_mask;
181			if (quirks & QUIRK_MSB_ON_THE_RIGHT)
182				adjust_for_msb_right_quirk(&pval,
183							   &box_start_bit,
184							   &box_end_bit,
185							   &box_mask);
186
187			pval >>= box_end_bit;
188			pval <<= proj_end_bit;
189			*uval &= ~proj_mask;
190			*uval |= pval;
191		} else {
192			u64 pval;
193
194			/* Write to pbuf, read from uval */
195			pval = (*uval) & proj_mask;
196			pval >>= proj_end_bit;
197			if (quirks & QUIRK_MSB_ON_THE_RIGHT)
198				adjust_for_msb_right_quirk(&pval,
199							   &box_start_bit,
200							   &box_end_bit,
201							   &box_mask);
202
203			pval <<= box_end_bit;
204			((u8 *)pbuf)[box_addr] &= ~box_mask;
205			((u8 *)pbuf)[box_addr] |= pval;
206		}
207	}
208	return 0;
209}
210EXPORT_SYMBOL(packing);
211
212MODULE_LICENSE("GPL v2");
213MODULE_DESCRIPTION("Generic bitfield packing and unpacking");