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/bits.h> 9 #include <linux/errno.h> 10 #include <linux/types.h> 11 #include <linux/bitrev.h> 12 13 #define __pack_fields(pbuf, pbuflen, ustruct, fields, num_fields, quirks) \ 14 ({ \ 15 for (size_t i = 0; i < (num_fields); i++) { \ 16 typeof(&(fields)[0]) field = &(fields)[i]; \ 17 u64 uval; \ 18 \ 19 uval = ustruct_field_to_u64(ustruct, field->offset, field->size); \ 20 \ 21 __pack(pbuf, uval, field->startbit, field->endbit, \ 22 pbuflen, quirks); \ 23 } \ 24 }) 25 26 #define __unpack_fields(pbuf, pbuflen, ustruct, fields, num_fields, quirks) \ 27 ({ \ 28 for (size_t i = 0; i < (num_fields); i++) { \ 29 typeof(&(fields)[0]) field = &fields[i]; \ 30 u64 uval; \ 31 \ 32 __unpack(pbuf, &uval, field->startbit, field->endbit, \ 33 pbuflen, quirks); \ 34 \ 35 u64_to_ustruct_field(ustruct, field->offset, field->size, uval); \ 36 } \ 37 }) 38 39 /** 40 * calculate_box_addr - Determine physical location of byte in buffer 41 * @box: Index of byte within buffer seen as a logical big-endian big number 42 * @len: Size of buffer in bytes 43 * @quirks: mask of QUIRK_LSW32_IS_FIRST and QUIRK_LITTLE_ENDIAN 44 * 45 * Function interprets the buffer as a @len byte sized big number, and returns 46 * the physical offset of the @box logical octet within it. Internally, it 47 * treats the big number as groups of 4 bytes. If @len is not a multiple of 4, 48 * the last group may be shorter. 49 * 50 * @QUIRK_LSW32_IS_FIRST gives the ordering of groups of 4 octets relative to 51 * each other. If set, the most significant group of 4 octets is last in the 52 * buffer (and may be truncated if @len is not a multiple of 4). 53 * 54 * @QUIRK_LITTLE_ENDIAN gives the ordering of bytes within each group of 4. 55 * If set, the most significant byte is last in the group. If @len takes the 56 * form of 4k+3, the last group will only be able to represent 24 bits, and its 57 * most significant octet is byte 2. 58 * 59 * Return: the physical offset into the buffer corresponding to the logical box. 60 */ 61 static size_t calculate_box_addr(size_t box, size_t len, u8 quirks) 62 { 63 size_t offset_of_group, offset_in_group, this_group = box / 4; 64 size_t group_size; 65 66 if (quirks & QUIRK_LSW32_IS_FIRST) 67 offset_of_group = this_group * 4; 68 else 69 offset_of_group = len - ((this_group + 1) * 4); 70 71 group_size = min(4, len - offset_of_group); 72 73 if (quirks & QUIRK_LITTLE_ENDIAN) 74 offset_in_group = box - this_group * 4; 75 else 76 offset_in_group = group_size - (box - this_group * 4) - 1; 77 78 return offset_of_group + offset_in_group; 79 } 80 81 static void __pack(void *pbuf, u64 uval, size_t startbit, size_t endbit, 82 size_t pbuflen, u8 quirks) 83 { 84 /* Logical byte indices corresponding to the 85 * start and end of the field. 86 */ 87 int plogical_first_u8 = startbit / BITS_PER_BYTE; 88 int plogical_last_u8 = endbit / BITS_PER_BYTE; 89 int value_width = startbit - endbit + 1; 90 int box; 91 92 /* Check if "uval" fits in "value_width" bits. 93 * The test only works for value_width < 64, but in the latter case, 94 * any 64-bit uval will surely fit. 95 */ 96 WARN(value_width < 64 && uval >= (1ull << value_width), 97 "Cannot store 0x%llx inside bits %zu-%zu - will truncate\n", 98 uval, startbit, endbit); 99 100 /* Iterate through an idealistic view of the pbuf as an u64 with 101 * no quirks, u8 by u8 (aligned at u8 boundaries), from high to low 102 * logical bit significance. "box" denotes the current logical u8. 103 */ 104 for (box = plogical_first_u8; box >= plogical_last_u8; box--) { 105 /* Bit indices into the currently accessed 8-bit box */ 106 size_t box_start_bit, box_end_bit, box_addr; 107 u8 box_mask; 108 /* Corresponding bits from the unpacked u64 parameter */ 109 size_t proj_start_bit, proj_end_bit; 110 u64 proj_mask; 111 u64 pval; 112 113 /* This u8 may need to be accessed in its entirety 114 * (from bit 7 to bit 0), or not, depending on the 115 * input arguments startbit and endbit. 116 */ 117 if (box == plogical_first_u8) 118 box_start_bit = startbit % BITS_PER_BYTE; 119 else 120 box_start_bit = 7; 121 if (box == plogical_last_u8) 122 box_end_bit = endbit % BITS_PER_BYTE; 123 else 124 box_end_bit = 0; 125 126 /* We have determined the box bit start and end. 127 * Now we calculate where this (masked) u8 box would fit 128 * in the unpacked (CPU-readable) u64 - the u8 box's 129 * projection onto the unpacked u64. Though the 130 * box is u8, the projection is u64 because it may fall 131 * anywhere within the unpacked u64. 132 */ 133 proj_start_bit = ((box * BITS_PER_BYTE) + box_start_bit) - endbit; 134 proj_end_bit = ((box * BITS_PER_BYTE) + box_end_bit) - endbit; 135 proj_mask = GENMASK_ULL(proj_start_bit, proj_end_bit); 136 box_mask = GENMASK(box_start_bit, box_end_bit); 137 138 /* Determine the offset of the u8 box inside the pbuf, 139 * adjusted for quirks. The adjusted box_addr will be used for 140 * effective addressing inside the pbuf (so it's not 141 * logical any longer). 142 */ 143 box_addr = calculate_box_addr(box, pbuflen, quirks); 144 145 /* Write to pbuf, read from uval */ 146 pval = uval & proj_mask; 147 pval >>= proj_end_bit; 148 pval <<= box_end_bit; 149 150 if (quirks & QUIRK_MSB_ON_THE_RIGHT) { 151 pval = bitrev8(pval); 152 box_mask = bitrev8(box_mask); 153 } 154 155 ((u8 *)pbuf)[box_addr] &= ~box_mask; 156 ((u8 *)pbuf)[box_addr] |= pval; 157 } 158 } 159 160 /** 161 * pack - Pack u64 number into bitfield of buffer. 162 * 163 * @pbuf: Pointer to a buffer holding the packed value. 164 * @uval: CPU-readable unpacked value to pack. 165 * @startbit: The index (in logical notation, compensated for quirks) where 166 * the packed value starts within pbuf. Must be larger than, or 167 * equal to, endbit. 168 * @endbit: The index (in logical notation, compensated for quirks) where 169 * the packed value ends within pbuf. Must be smaller than, or equal 170 * to, startbit. 171 * @pbuflen: The length in bytes of the packed buffer pointed to by @pbuf. 172 * @quirks: A bit mask of QUIRK_LITTLE_ENDIAN, QUIRK_LSW32_IS_FIRST and 173 * QUIRK_MSB_ON_THE_RIGHT. 174 * 175 * Return: 0 on success, EINVAL or ERANGE if called incorrectly. Assuming 176 * correct usage, return code may be discarded. The @pbuf memory will 177 * be modified on success. 178 */ 179 int pack(void *pbuf, u64 uval, size_t startbit, size_t endbit, size_t pbuflen, 180 u8 quirks) 181 { 182 /* startbit is expected to be larger than endbit, and both are 183 * expected to be within the logically addressable range of the buffer. 184 */ 185 if (unlikely(startbit < endbit || startbit >= BITS_PER_BYTE * pbuflen)) 186 /* Invalid function call */ 187 return -EINVAL; 188 189 if (unlikely(startbit - endbit >= 64)) 190 return -ERANGE; 191 192 __pack(pbuf, uval, startbit, endbit, pbuflen, quirks); 193 194 return 0; 195 } 196 EXPORT_SYMBOL(pack); 197 198 static void __unpack(const void *pbuf, u64 *uval, size_t startbit, size_t endbit, 199 size_t pbuflen, u8 quirks) 200 { 201 /* Logical byte indices corresponding to the 202 * start and end of the field. 203 */ 204 int plogical_first_u8 = startbit / BITS_PER_BYTE; 205 int plogical_last_u8 = endbit / BITS_PER_BYTE; 206 int box; 207 208 /* Initialize parameter */ 209 *uval = 0; 210 211 /* Iterate through an idealistic view of the pbuf as an u64 with 212 * no quirks, u8 by u8 (aligned at u8 boundaries), from high to low 213 * logical bit significance. "box" denotes the current logical u8. 214 */ 215 for (box = plogical_first_u8; box >= plogical_last_u8; box--) { 216 /* Bit indices into the currently accessed 8-bit box */ 217 size_t box_start_bit, box_end_bit, box_addr; 218 u8 box_mask; 219 /* Corresponding bits from the unpacked u64 parameter */ 220 size_t proj_start_bit, proj_end_bit; 221 u64 proj_mask; 222 u64 pval; 223 224 /* This u8 may need to be accessed in its entirety 225 * (from bit 7 to bit 0), or not, depending on the 226 * input arguments startbit and endbit. 227 */ 228 if (box == plogical_first_u8) 229 box_start_bit = startbit % BITS_PER_BYTE; 230 else 231 box_start_bit = 7; 232 if (box == plogical_last_u8) 233 box_end_bit = endbit % BITS_PER_BYTE; 234 else 235 box_end_bit = 0; 236 237 /* We have determined the box bit start and end. 238 * Now we calculate where this (masked) u8 box would fit 239 * in the unpacked (CPU-readable) u64 - the u8 box's 240 * projection onto the unpacked u64. Though the 241 * box is u8, the projection is u64 because it may fall 242 * anywhere within the unpacked u64. 243 */ 244 proj_start_bit = ((box * BITS_PER_BYTE) + box_start_bit) - endbit; 245 proj_end_bit = ((box * BITS_PER_BYTE) + box_end_bit) - endbit; 246 proj_mask = GENMASK_ULL(proj_start_bit, proj_end_bit); 247 box_mask = GENMASK(box_start_bit, box_end_bit); 248 249 /* Determine the offset of the u8 box inside the pbuf, 250 * adjusted for quirks. The adjusted box_addr will be used for 251 * effective addressing inside the pbuf (so it's not 252 * logical any longer). 253 */ 254 box_addr = calculate_box_addr(box, pbuflen, quirks); 255 256 /* Read from pbuf, write to uval */ 257 pval = ((u8 *)pbuf)[box_addr]; 258 259 if (quirks & QUIRK_MSB_ON_THE_RIGHT) 260 pval = bitrev8(pval); 261 262 pval &= box_mask; 263 264 pval >>= box_end_bit; 265 pval <<= proj_end_bit; 266 *uval &= ~proj_mask; 267 *uval |= pval; 268 } 269 } 270 271 /** 272 * unpack - Unpack u64 number from packed buffer. 273 * 274 * @pbuf: Pointer to a buffer holding the packed value. 275 * @uval: Pointer to an u64 holding the unpacked value. 276 * @startbit: The index (in logical notation, compensated for quirks) where 277 * the packed value starts within pbuf. Must be larger than, or 278 * equal to, endbit. 279 * @endbit: The index (in logical notation, compensated for quirks) where 280 * the packed value ends within pbuf. Must be smaller than, or equal 281 * to, startbit. 282 * @pbuflen: The length in bytes of the packed buffer pointed to by @pbuf. 283 * @quirks: A bit mask of QUIRK_LITTLE_ENDIAN, QUIRK_LSW32_IS_FIRST and 284 * QUIRK_MSB_ON_THE_RIGHT. 285 * 286 * Return: 0 on success, EINVAL or ERANGE if called incorrectly. Assuming 287 * correct usage, return code may be discarded. The @uval will be 288 * modified on success. 289 */ 290 int unpack(const void *pbuf, u64 *uval, size_t startbit, size_t endbit, 291 size_t pbuflen, u8 quirks) 292 { 293 /* width of the field to access in the pbuf */ 294 u64 value_width; 295 296 /* startbit is expected to be larger than endbit, and both are 297 * expected to be within the logically addressable range of the buffer. 298 */ 299 if (startbit < endbit || startbit >= BITS_PER_BYTE * pbuflen) 300 /* Invalid function call */ 301 return -EINVAL; 302 303 value_width = startbit - endbit + 1; 304 if (value_width > 64) 305 return -ERANGE; 306 307 __unpack(pbuf, uval, startbit, endbit, pbuflen, quirks); 308 309 return 0; 310 } 311 EXPORT_SYMBOL(unpack); 312 313 /** 314 * packing - Convert numbers (currently u64) between a packed and an unpacked 315 * format. Unpacked means laid out in memory in the CPU's native 316 * understanding of integers, while packed means anything else that 317 * requires translation. 318 * 319 * @pbuf: Pointer to a buffer holding the packed value. 320 * @uval: Pointer to an u64 holding the unpacked value. 321 * @startbit: The index (in logical notation, compensated for quirks) where 322 * the packed value starts within pbuf. Must be larger than, or 323 * equal to, endbit. 324 * @endbit: The index (in logical notation, compensated for quirks) where 325 * the packed value ends within pbuf. Must be smaller than, or equal 326 * to, startbit. 327 * @pbuflen: The length in bytes of the packed buffer pointed to by @pbuf. 328 * @op: If PACK, then uval will be treated as const pointer and copied (packed) 329 * into pbuf, between startbit and endbit. 330 * If UNPACK, then pbuf will be treated as const pointer and the logical 331 * value between startbit and endbit will be copied (unpacked) to uval. 332 * @quirks: A bit mask of QUIRK_LITTLE_ENDIAN, QUIRK_LSW32_IS_FIRST and 333 * QUIRK_MSB_ON_THE_RIGHT. 334 * 335 * Note: this is deprecated, prefer to use pack() or unpack() in new code. 336 * 337 * Return: 0 on success, EINVAL or ERANGE if called incorrectly. Assuming 338 * correct usage, return code may be discarded. 339 * If op is PACK, pbuf is modified. 340 * If op is UNPACK, uval is modified. 341 */ 342 int packing(void *pbuf, u64 *uval, int startbit, int endbit, size_t pbuflen, 343 enum packing_op op, u8 quirks) 344 { 345 if (op == PACK) 346 return pack(pbuf, *uval, startbit, endbit, pbuflen, quirks); 347 348 return unpack(pbuf, uval, startbit, endbit, pbuflen, quirks); 349 } 350 EXPORT_SYMBOL(packing); 351 352 static u64 ustruct_field_to_u64(const void *ustruct, size_t field_offset, 353 size_t field_size) 354 { 355 switch (field_size) { 356 case 1: 357 return *((u8 *)(ustruct + field_offset)); 358 case 2: 359 return *((u16 *)(ustruct + field_offset)); 360 case 4: 361 return *((u32 *)(ustruct + field_offset)); 362 default: 363 return *((u64 *)(ustruct + field_offset)); 364 } 365 } 366 367 static void u64_to_ustruct_field(void *ustruct, size_t field_offset, 368 size_t field_size, u64 uval) 369 { 370 switch (field_size) { 371 case 1: 372 *((u8 *)(ustruct + field_offset)) = uval; 373 break; 374 case 2: 375 *((u16 *)(ustruct + field_offset)) = uval; 376 break; 377 case 4: 378 *((u32 *)(ustruct + field_offset)) = uval; 379 break; 380 default: 381 *((u64 *)(ustruct + field_offset)) = uval; 382 break; 383 } 384 } 385 386 /** 387 * pack_fields_u8 - Pack array of fields 388 * 389 * @pbuf: Pointer to a buffer holding the packed value. 390 * @pbuflen: The length in bytes of the packed buffer pointed to by @pbuf. 391 * @ustruct: Pointer to CPU-readable structure holding the unpacked value. 392 * It is expected (but not checked) that this has the same data type 393 * as all struct packed_field_u8 definitions. 394 * @fields: Array of packed_field_u8 field definition. They must not overlap. 395 * @num_fields: Length of @fields array. 396 * @quirks: A bit mask of QUIRK_LITTLE_ENDIAN, QUIRK_LSW32_IS_FIRST and 397 * QUIRK_MSB_ON_THE_RIGHT. 398 * 399 * Use the pack_fields() macro instead of calling this directly. 400 */ 401 void pack_fields_u8(void *pbuf, size_t pbuflen, const void *ustruct, 402 const struct packed_field_u8 *fields, size_t num_fields, 403 u8 quirks) 404 { 405 __pack_fields(pbuf, pbuflen, ustruct, fields, num_fields, quirks); 406 } 407 EXPORT_SYMBOL(pack_fields_u8); 408 409 /** 410 * pack_fields_u16 - Pack array of fields 411 * 412 * @pbuf: Pointer to a buffer holding the packed value. 413 * @pbuflen: The length in bytes of the packed buffer pointed to by @pbuf. 414 * @ustruct: Pointer to CPU-readable structure holding the unpacked value. 415 * It is expected (but not checked) that this has the same data type 416 * as all struct packed_field_u16 definitions. 417 * @fields: Array of packed_field_u16 field definitions. They must not overlap. 418 * @num_fields: Length of @fields array. 419 * @quirks: A bit mask of QUIRK_LITTLE_ENDIAN, QUIRK_LSW32_IS_FIRST and 420 * QUIRK_MSB_ON_THE_RIGHT. 421 * 422 * Use the pack_fields() macro instead of calling this directly. 423 */ 424 void pack_fields_u16(void *pbuf, size_t pbuflen, const void *ustruct, 425 const struct packed_field_u16 *fields, size_t num_fields, 426 u8 quirks) 427 { 428 __pack_fields(pbuf, pbuflen, ustruct, fields, num_fields, quirks); 429 } 430 EXPORT_SYMBOL(pack_fields_u16); 431 432 /** 433 * unpack_fields_u8 - Unpack array of fields 434 * 435 * @pbuf: Pointer to a buffer holding the packed value. 436 * @pbuflen: The length in bytes of the packed buffer pointed to by @pbuf. 437 * @ustruct: Pointer to CPU-readable structure holding the unpacked value. 438 * It is expected (but not checked) that this has the same data type 439 * as all struct packed_field_u8 definitions. 440 * @fields: Array of packed_field_u8 field definitions. They must not overlap. 441 * @num_fields: Length of @fields array. 442 * @quirks: A bit mask of QUIRK_LITTLE_ENDIAN, QUIRK_LSW32_IS_FIRST and 443 * QUIRK_MSB_ON_THE_RIGHT. 444 * 445 * Use the unpack_fields() macro instead of calling this directly. 446 */ 447 void unpack_fields_u8(const void *pbuf, size_t pbuflen, void *ustruct, 448 const struct packed_field_u8 *fields, size_t num_fields, 449 u8 quirks) 450 { 451 __unpack_fields(pbuf, pbuflen, ustruct, fields, num_fields, quirks); 452 } 453 EXPORT_SYMBOL(unpack_fields_u8); 454 455 /** 456 * unpack_fields_u16 - Unpack array of fields 457 * 458 * @pbuf: Pointer to a buffer holding the packed value. 459 * @pbuflen: The length in bytes of the packed buffer pointed to by @pbuf. 460 * @ustruct: Pointer to CPU-readable structure holding the unpacked value. 461 * It is expected (but not checked) that this has the same data type 462 * as all struct packed_field_u16 definitions. 463 * @fields: Array of packed_field_u16 field definitions. They must not overlap. 464 * @num_fields: Length of @fields array. 465 * @quirks: A bit mask of QUIRK_LITTLE_ENDIAN, QUIRK_LSW32_IS_FIRST and 466 * QUIRK_MSB_ON_THE_RIGHT. 467 * 468 * Use the unpack_fields() macro instead of calling this directly. 469 */ 470 void unpack_fields_u16(const void *pbuf, size_t pbuflen, void *ustruct, 471 const struct packed_field_u16 *fields, size_t num_fields, 472 u8 quirks) 473 { 474 __unpack_fields(pbuf, pbuflen, ustruct, fields, num_fields, quirks); 475 } 476 EXPORT_SYMBOL(unpack_fields_u16); 477 478 MODULE_DESCRIPTION("Generic bitfield packing and unpacking"); 479