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
get_le_offset(int offset)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
get_reverse_lsw32_offset(int offset,size_t len)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
bit_reverse(u64 val,unsigned int width)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
adjust_for_msb_right_quirk(u64 * to_write,int * box_start_bit,int * box_end_bit,u8 * box_mask)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.
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 */
packing(void * pbuf,u64 * uval,int startbit,int endbit,size_t pbuflen,enum packing_op op,u8 quirks)88 int 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 }
210 EXPORT_SYMBOL(packing);
211
212 MODULE_LICENSE("GPL v2");
213 MODULE_DESCRIPTION("Generic bitfield packing and unpacking");
214