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 /** 13 * calculate_box_addr - Determine physical location of byte in buffer 14 * @box: Index of byte within buffer seen as a logical big-endian big number 15 * @len: Size of buffer in bytes 16 * @quirks: mask of QUIRK_LSW32_IS_FIRST and QUIRK_LITTLE_ENDIAN 17 * 18 * Function interprets the buffer as a @len byte sized big number, and returns 19 * the physical offset of the @box logical octet within it. Internally, it 20 * treats the big number as groups of 4 bytes. If @len is not a multiple of 4, 21 * the last group may be shorter. 22 * 23 * @QUIRK_LSW32_IS_FIRST gives the ordering of groups of 4 octets relative to 24 * each other. If set, the most significant group of 4 octets is last in the 25 * buffer (and may be truncated if @len is not a multiple of 4). 26 * 27 * @QUIRK_LITTLE_ENDIAN gives the ordering of bytes within each group of 4. 28 * If set, the most significant byte is last in the group. If @len takes the 29 * form of 4k+3, the last group will only be able to represent 24 bits, and its 30 * most significant octet is byte 2. 31 * 32 * Return: the physical offset into the buffer corresponding to the logical box. 33 */ 34 static size_t calculate_box_addr(size_t box, size_t len, u8 quirks) 35 { 36 size_t offset_of_group, offset_in_group, this_group = box / 4; 37 size_t group_size; 38 39 if (quirks & QUIRK_LSW32_IS_FIRST) 40 offset_of_group = this_group * 4; 41 else 42 offset_of_group = len - ((this_group + 1) * 4); 43 44 group_size = min(4, len - offset_of_group); 45 46 if (quirks & QUIRK_LITTLE_ENDIAN) 47 offset_in_group = box - this_group * 4; 48 else 49 offset_in_group = group_size - (box - this_group * 4) - 1; 50 51 return offset_of_group + offset_in_group; 52 } 53 54 /** 55 * pack - Pack u64 number into bitfield of buffer. 56 * 57 * @pbuf: Pointer to a buffer holding the packed value. 58 * @uval: CPU-readable unpacked value to pack. 59 * @startbit: The index (in logical notation, compensated for quirks) where 60 * the packed value starts within pbuf. Must be larger than, or 61 * equal to, endbit. 62 * @endbit: The index (in logical notation, compensated for quirks) where 63 * the packed value ends within pbuf. Must be smaller than, or equal 64 * to, startbit. 65 * @pbuflen: The length in bytes of the packed buffer pointed to by @pbuf. 66 * @quirks: A bit mask of QUIRK_LITTLE_ENDIAN, QUIRK_LSW32_IS_FIRST and 67 * QUIRK_MSB_ON_THE_RIGHT. 68 * 69 * Return: 0 on success, EINVAL or ERANGE if called incorrectly. Assuming 70 * correct usage, return code may be discarded. The @pbuf memory will 71 * be modified on success. 72 */ 73 int pack(void *pbuf, u64 uval, size_t startbit, size_t endbit, size_t pbuflen, 74 u8 quirks) 75 { 76 /* Logical byte indices corresponding to the 77 * start and end of the field. 78 */ 79 int plogical_first_u8, plogical_last_u8, box; 80 /* width of the field to access in the pbuf */ 81 u64 value_width; 82 83 /* startbit is expected to be larger than endbit, and both are 84 * expected to be within the logically addressable range of the buffer. 85 */ 86 if (unlikely(startbit < endbit || startbit >= BITS_PER_BYTE * pbuflen)) 87 /* Invalid function call */ 88 return -EINVAL; 89 90 value_width = startbit - endbit + 1; 91 if (unlikely(value_width > 64)) 92 return -ERANGE; 93 94 /* Check if "uval" fits in "value_width" bits. 95 * If value_width is 64, the check will fail, but any 96 * 64-bit uval will surely fit. 97 */ 98 if (unlikely(value_width < 64 && uval >= (1ull << value_width))) 99 /* Cannot store "uval" inside "value_width" bits. 100 * Truncating "uval" is most certainly not desirable, 101 * so simply erroring out is appropriate. 102 */ 103 return -ERANGE; 104 105 /* Iterate through an idealistic view of the pbuf as an u64 with 106 * no quirks, u8 by u8 (aligned at u8 boundaries), from high to low 107 * logical bit significance. "box" denotes the current logical u8. 108 */ 109 plogical_first_u8 = startbit / BITS_PER_BYTE; 110 plogical_last_u8 = endbit / BITS_PER_BYTE; 111 112 for (box = plogical_first_u8; box >= plogical_last_u8; box--) { 113 /* Bit indices into the currently accessed 8-bit box */ 114 size_t box_start_bit, box_end_bit, box_addr; 115 u8 box_mask; 116 /* Corresponding bits from the unpacked u64 parameter */ 117 size_t proj_start_bit, proj_end_bit; 118 u64 proj_mask; 119 u64 pval; 120 121 /* This u8 may need to be accessed in its entirety 122 * (from bit 7 to bit 0), or not, depending on the 123 * input arguments startbit and endbit. 124 */ 125 if (box == plogical_first_u8) 126 box_start_bit = startbit % BITS_PER_BYTE; 127 else 128 box_start_bit = 7; 129 if (box == plogical_last_u8) 130 box_end_bit = endbit % BITS_PER_BYTE; 131 else 132 box_end_bit = 0; 133 134 /* We have determined the box bit start and end. 135 * Now we calculate where this (masked) u8 box would fit 136 * in the unpacked (CPU-readable) u64 - the u8 box's 137 * projection onto the unpacked u64. Though the 138 * box is u8, the projection is u64 because it may fall 139 * anywhere within the unpacked u64. 140 */ 141 proj_start_bit = ((box * BITS_PER_BYTE) + box_start_bit) - endbit; 142 proj_end_bit = ((box * BITS_PER_BYTE) + box_end_bit) - endbit; 143 proj_mask = GENMASK_ULL(proj_start_bit, proj_end_bit); 144 box_mask = GENMASK(box_start_bit, box_end_bit); 145 146 /* Determine the offset of the u8 box inside the pbuf, 147 * adjusted for quirks. The adjusted box_addr will be used for 148 * effective addressing inside the pbuf (so it's not 149 * logical any longer). 150 */ 151 box_addr = calculate_box_addr(box, pbuflen, quirks); 152 153 /* Write to pbuf, read from uval */ 154 pval = uval & proj_mask; 155 pval >>= proj_end_bit; 156 pval <<= box_end_bit; 157 158 if (quirks & QUIRK_MSB_ON_THE_RIGHT) { 159 pval = bitrev8(pval); 160 box_mask = bitrev8(box_mask); 161 } 162 163 ((u8 *)pbuf)[box_addr] &= ~box_mask; 164 ((u8 *)pbuf)[box_addr] |= pval; 165 } 166 return 0; 167 } 168 EXPORT_SYMBOL(pack); 169 170 /** 171 * unpack - Unpack u64 number from packed buffer. 172 * 173 * @pbuf: Pointer to a buffer holding the packed value. 174 * @uval: Pointer to an u64 holding the unpacked value. 175 * @startbit: The index (in logical notation, compensated for quirks) where 176 * the packed value starts within pbuf. Must be larger than, or 177 * equal to, endbit. 178 * @endbit: The index (in logical notation, compensated for quirks) where 179 * the packed value ends within pbuf. Must be smaller than, or equal 180 * to, startbit. 181 * @pbuflen: The length in bytes of the packed buffer pointed to by @pbuf. 182 * @quirks: A bit mask of QUIRK_LITTLE_ENDIAN, QUIRK_LSW32_IS_FIRST and 183 * QUIRK_MSB_ON_THE_RIGHT. 184 * 185 * Return: 0 on success, EINVAL or ERANGE if called incorrectly. Assuming 186 * correct usage, return code may be discarded. The @uval will be 187 * modified on success. 188 */ 189 int unpack(const void *pbuf, u64 *uval, size_t startbit, size_t endbit, 190 size_t pbuflen, u8 quirks) 191 { 192 /* Logical byte indices corresponding to the 193 * start and end of the field. 194 */ 195 int plogical_first_u8, plogical_last_u8, box; 196 /* width of the field to access in the pbuf */ 197 u64 value_width; 198 199 /* startbit is expected to be larger than endbit, and both are 200 * expected to be within the logically addressable range of the buffer. 201 */ 202 if (unlikely(startbit < endbit || startbit >= BITS_PER_BYTE * pbuflen)) 203 /* Invalid function call */ 204 return -EINVAL; 205 206 value_width = startbit - endbit + 1; 207 if (unlikely(value_width > 64)) 208 return -ERANGE; 209 210 /* Initialize parameter */ 211 *uval = 0; 212 213 /* Iterate through an idealistic view of the pbuf as an u64 with 214 * no quirks, u8 by u8 (aligned at u8 boundaries), from high to low 215 * logical bit significance. "box" denotes the current logical u8. 216 */ 217 plogical_first_u8 = startbit / BITS_PER_BYTE; 218 plogical_last_u8 = endbit / BITS_PER_BYTE; 219 220 for (box = plogical_first_u8; box >= plogical_last_u8; box--) { 221 /* Bit indices into the currently accessed 8-bit box */ 222 size_t box_start_bit, box_end_bit, box_addr; 223 u8 box_mask; 224 /* Corresponding bits from the unpacked u64 parameter */ 225 size_t proj_start_bit, proj_end_bit; 226 u64 proj_mask; 227 u64 pval; 228 229 /* This u8 may need to be accessed in its entirety 230 * (from bit 7 to bit 0), or not, depending on the 231 * input arguments startbit and endbit. 232 */ 233 if (box == plogical_first_u8) 234 box_start_bit = startbit % BITS_PER_BYTE; 235 else 236 box_start_bit = 7; 237 if (box == plogical_last_u8) 238 box_end_bit = endbit % BITS_PER_BYTE; 239 else 240 box_end_bit = 0; 241 242 /* We have determined the box bit start and end. 243 * Now we calculate where this (masked) u8 box would fit 244 * in the unpacked (CPU-readable) u64 - the u8 box's 245 * projection onto the unpacked u64. Though the 246 * box is u8, the projection is u64 because it may fall 247 * anywhere within the unpacked u64. 248 */ 249 proj_start_bit = ((box * BITS_PER_BYTE) + box_start_bit) - endbit; 250 proj_end_bit = ((box * BITS_PER_BYTE) + box_end_bit) - endbit; 251 proj_mask = GENMASK_ULL(proj_start_bit, proj_end_bit); 252 box_mask = GENMASK(box_start_bit, box_end_bit); 253 254 /* Determine the offset of the u8 box inside the pbuf, 255 * adjusted for quirks. The adjusted box_addr will be used for 256 * effective addressing inside the pbuf (so it's not 257 * logical any longer). 258 */ 259 box_addr = calculate_box_addr(box, pbuflen, quirks); 260 261 /* Read from pbuf, write to uval */ 262 pval = ((u8 *)pbuf)[box_addr]; 263 264 if (quirks & QUIRK_MSB_ON_THE_RIGHT) 265 pval = bitrev8(pval); 266 267 pval &= box_mask; 268 269 pval >>= box_end_bit; 270 pval <<= proj_end_bit; 271 *uval &= ~proj_mask; 272 *uval |= pval; 273 } 274 return 0; 275 } 276 EXPORT_SYMBOL(unpack); 277 278 /** 279 * packing - Convert numbers (currently u64) between a packed and an unpacked 280 * format. Unpacked means laid out in memory in the CPU's native 281 * understanding of integers, while packed means anything else that 282 * requires translation. 283 * 284 * @pbuf: Pointer to a buffer holding the packed value. 285 * @uval: Pointer to an u64 holding the unpacked value. 286 * @startbit: The index (in logical notation, compensated for quirks) where 287 * the packed value starts within pbuf. Must be larger than, or 288 * equal to, endbit. 289 * @endbit: The index (in logical notation, compensated for quirks) where 290 * the packed value ends within pbuf. Must be smaller than, or equal 291 * to, startbit. 292 * @pbuflen: The length in bytes of the packed buffer pointed to by @pbuf. 293 * @op: If PACK, then uval will be treated as const pointer and copied (packed) 294 * into pbuf, between startbit and endbit. 295 * If UNPACK, then pbuf will be treated as const pointer and the logical 296 * value between startbit and endbit will be copied (unpacked) to uval. 297 * @quirks: A bit mask of QUIRK_LITTLE_ENDIAN, QUIRK_LSW32_IS_FIRST and 298 * QUIRK_MSB_ON_THE_RIGHT. 299 * 300 * Note: this is deprecated, prefer to use pack() or unpack() in new code. 301 * 302 * Return: 0 on success, EINVAL or ERANGE if called incorrectly. Assuming 303 * correct usage, return code may be discarded. 304 * If op is PACK, pbuf is modified. 305 * If op is UNPACK, uval is modified. 306 */ 307 int packing(void *pbuf, u64 *uval, int startbit, int endbit, size_t pbuflen, 308 enum packing_op op, u8 quirks) 309 { 310 if (op == PACK) 311 return pack(pbuf, *uval, startbit, endbit, pbuflen, quirks); 312 313 return unpack(pbuf, uval, startbit, endbit, pbuflen, quirks); 314 } 315 EXPORT_SYMBOL(packing); 316 317 MODULE_DESCRIPTION("Generic bitfield packing and unpacking"); 318