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(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 } 199 EXPORT_SYMBOL(packing); 200 201 MODULE_LICENSE("GPL v2"); 202 MODULE_DESCRIPTION("Generic bitfield packing and unpacking"); 203