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