Loading...
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 * @pbuflen: The length in bytes of the packed buffer pointed to by @pbuf.
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 */
89int 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}
211EXPORT_SYMBOL(packing);
212
213MODULE_LICENSE("GPL v2");
214MODULE_DESCRIPTION("Generic bitfield packing and unpacking");
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");