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 11 static 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 20 static 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 32 static 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 45 static 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 * @op: If PACK, then uval will be treated as const pointer and copied (packed) 77 * into pbuf, between startbit and endbit. 78 * If UNPACK, then pbuf will be treated as const pointer and the logical 79 * value between startbit and endbit will be copied (unpacked) to uval. 80 * @quirks: A bit mask of QUIRK_LITTLE_ENDIAN, QUIRK_LSW32_IS_FIRST and 81 * QUIRK_MSB_ON_THE_RIGHT. 82 * 83 * Return: 0 on success, EINVAL or ERANGE if called incorrectly. Assuming 84 * correct usage, return code may be discarded. 85 * If op is PACK, pbuf is modified. 86 * If op is UNPACK, uval is modified. 87 */ 88 int packing(void *pbuf, u64 *uval, int startbit, int endbit, size_t pbuflen, 89 enum packing_op op, u8 quirks) 90 { 91 /* Number of bits for storing "uval" 92 * also width of the field to access in the pbuf 93 */ 94 u64 value_width; 95 /* Logical byte indices corresponding to the 96 * start and end of the field. 97 */ 98 int plogical_first_u8, plogical_last_u8, box; 99 100 /* startbit is expected to be larger than endbit */ 101 if (startbit < endbit) 102 /* Invalid function call */ 103 return -EINVAL; 104 105 value_width = startbit - endbit + 1; 106 if (value_width > 64) 107 return -ERANGE; 108 109 /* Check if "uval" fits in "value_width" bits. 110 * If value_width is 64, the check will fail, but any 111 * 64-bit uval will surely fit. 112 */ 113 if (op == PACK && value_width < 64 && (*uval >= (1ull << value_width))) 114 /* Cannot store "uval" inside "value_width" bits. 115 * Truncating "uval" is most certainly not desirable, 116 * so simply erroring out is appropriate. 117 */ 118 return -ERANGE; 119 120 /* Initialize parameter */ 121 if (op == UNPACK) 122 *uval = 0; 123 124 /* Iterate through an idealistic view of the pbuf as an u64 with 125 * no quirks, u8 by u8 (aligned at u8 boundaries), from high to low 126 * logical bit significance. "box" denotes the current logical u8. 127 */ 128 plogical_first_u8 = startbit / 8; 129 plogical_last_u8 = endbit / 8; 130 131 for (box = plogical_first_u8; box >= plogical_last_u8; box--) { 132 /* Bit indices into the currently accessed 8-bit box */ 133 int box_start_bit, box_end_bit, box_addr; 134 u8 box_mask; 135 /* Corresponding bits from the unpacked u64 parameter */ 136 int proj_start_bit, proj_end_bit; 137 u64 proj_mask; 138 139 /* This u8 may need to be accessed in its entirety 140 * (from bit 7 to bit 0), or not, depending on the 141 * input arguments startbit and endbit. 142 */ 143 if (box == plogical_first_u8) 144 box_start_bit = startbit % 8; 145 else 146 box_start_bit = 7; 147 if (box == plogical_last_u8) 148 box_end_bit = endbit % 8; 149 else 150 box_end_bit = 0; 151 152 /* We have determined the box bit start and end. 153 * Now we calculate where this (masked) u8 box would fit 154 * in the unpacked (CPU-readable) u64 - the u8 box's 155 * projection onto the unpacked u64. Though the 156 * box is u8, the projection is u64 because it may fall 157 * anywhere within the unpacked u64. 158 */ 159 proj_start_bit = ((box * 8) + box_start_bit) - endbit; 160 proj_end_bit = ((box * 8) + box_end_bit) - endbit; 161 proj_mask = GENMASK_ULL(proj_start_bit, proj_end_bit); 162 box_mask = GENMASK_ULL(box_start_bit, box_end_bit); 163 164 /* Determine the offset of the u8 box inside the pbuf, 165 * adjusted for quirks. The adjusted box_addr will be used for 166 * effective addressing inside the pbuf (so it's not 167 * logical any longer). 168 */ 169 box_addr = pbuflen - box - 1; 170 if (quirks & QUIRK_LITTLE_ENDIAN) 171 box_addr = get_le_offset(box_addr); 172 if (quirks & QUIRK_LSW32_IS_FIRST) 173 box_addr = get_reverse_lsw32_offset(box_addr, 174 pbuflen); 175 176 if (op == UNPACK) { 177 u64 pval; 178 179 /* Read from pbuf, write to uval */ 180 pval = ((u8 *)pbuf)[box_addr] & box_mask; 181 if (quirks & QUIRK_MSB_ON_THE_RIGHT) 182 adjust_for_msb_right_quirk(&pval, 183 &box_start_bit, 184 &box_end_bit, 185 &box_mask); 186 187 pval >>= box_end_bit; 188 pval <<= proj_end_bit; 189 *uval &= ~proj_mask; 190 *uval |= pval; 191 } else { 192 u64 pval; 193 194 /* Write to pbuf, read from uval */ 195 pval = (*uval) & proj_mask; 196 pval >>= proj_end_bit; 197 if (quirks & QUIRK_MSB_ON_THE_RIGHT) 198 adjust_for_msb_right_quirk(&pval, 199 &box_start_bit, 200 &box_end_bit, 201 &box_mask); 202 203 pval <<= box_end_bit; 204 ((u8 *)pbuf)[box_addr] &= ~box_mask; 205 ((u8 *)pbuf)[box_addr] |= pval; 206 } 207 } 208 return 0; 209 } 210 EXPORT_SYMBOL(packing); 211 212 MODULE_LICENSE("GPL v2"); 213 MODULE_DESCRIPTION("Generic bitfield packing and unpacking"); 214