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 | |
12 | static 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 | |
21 | static 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 | |
33 | static 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 | */ |
77 | int 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(offset: box_addr); |
161 | if (quirks & QUIRK_LSW32_IS_FIRST) |
162 | box_addr = get_reverse_lsw32_offset(offset: box_addr, |
163 | len: 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(to_write: &pval, |
172 | box_start_bit: &box_start_bit, |
173 | box_end_bit: &box_end_bit, |
174 | box_mask: &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(to_write: &pval, |
188 | box_start_bit: &box_start_bit, |
189 | box_end_bit: &box_end_bit, |
190 | box_mask: &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 | } |
199 | EXPORT_SYMBOL(packing); |
200 | |
201 | MODULE_DESCRIPTION("Generic bitfield packing and unpacking" ); |
202 | |