1*554aae35SVladimir Oltean // SPDX-License-Identifier: BSD-3-Clause OR GPL-2.0 2*554aae35SVladimir Oltean /* Copyright (c) 2016-2018, NXP Semiconductors 3*554aae35SVladimir Oltean * Copyright (c) 2018-2019, Vladimir Oltean <olteanv@gmail.com> 4*554aae35SVladimir Oltean */ 5*554aae35SVladimir Oltean #include <linux/packing.h> 6*554aae35SVladimir Oltean #include <linux/module.h> 7*554aae35SVladimir Oltean #include <linux/bitops.h> 8*554aae35SVladimir Oltean #include <linux/errno.h> 9*554aae35SVladimir Oltean #include <linux/types.h> 10*554aae35SVladimir Oltean 11*554aae35SVladimir Oltean static int get_le_offset(int offset) 12*554aae35SVladimir Oltean { 13*554aae35SVladimir Oltean int closest_multiple_of_4; 14*554aae35SVladimir Oltean 15*554aae35SVladimir Oltean closest_multiple_of_4 = (offset / 4) * 4; 16*554aae35SVladimir Oltean offset -= closest_multiple_of_4; 17*554aae35SVladimir Oltean return closest_multiple_of_4 + (3 - offset); 18*554aae35SVladimir Oltean } 19*554aae35SVladimir Oltean 20*554aae35SVladimir Oltean static int get_reverse_lsw32_offset(int offset, size_t len) 21*554aae35SVladimir Oltean { 22*554aae35SVladimir Oltean int closest_multiple_of_4; 23*554aae35SVladimir Oltean int word_index; 24*554aae35SVladimir Oltean 25*554aae35SVladimir Oltean word_index = offset / 4; 26*554aae35SVladimir Oltean closest_multiple_of_4 = word_index * 4; 27*554aae35SVladimir Oltean offset -= closest_multiple_of_4; 28*554aae35SVladimir Oltean word_index = (len / 4) - word_index - 1; 29*554aae35SVladimir Oltean return word_index * 4 + offset; 30*554aae35SVladimir Oltean } 31*554aae35SVladimir Oltean 32*554aae35SVladimir Oltean static u64 bit_reverse(u64 val, unsigned int width) 33*554aae35SVladimir Oltean { 34*554aae35SVladimir Oltean u64 new_val = 0; 35*554aae35SVladimir Oltean unsigned int bit; 36*554aae35SVladimir Oltean unsigned int i; 37*554aae35SVladimir Oltean 38*554aae35SVladimir Oltean for (i = 0; i < width; i++) { 39*554aae35SVladimir Oltean bit = (val & (1 << i)) != 0; 40*554aae35SVladimir Oltean new_val |= (bit << (width - i - 1)); 41*554aae35SVladimir Oltean } 42*554aae35SVladimir Oltean return new_val; 43*554aae35SVladimir Oltean } 44*554aae35SVladimir Oltean 45*554aae35SVladimir Oltean static void adjust_for_msb_right_quirk(u64 *to_write, int *box_start_bit, 46*554aae35SVladimir Oltean int *box_end_bit, u8 *box_mask) 47*554aae35SVladimir Oltean { 48*554aae35SVladimir Oltean int box_bit_width = *box_start_bit - *box_end_bit + 1; 49*554aae35SVladimir Oltean int new_box_start_bit, new_box_end_bit; 50*554aae35SVladimir Oltean 51*554aae35SVladimir Oltean *to_write >>= *box_end_bit; 52*554aae35SVladimir Oltean *to_write = bit_reverse(*to_write, box_bit_width); 53*554aae35SVladimir Oltean *to_write <<= *box_end_bit; 54*554aae35SVladimir Oltean 55*554aae35SVladimir Oltean new_box_end_bit = box_bit_width - *box_start_bit - 1; 56*554aae35SVladimir Oltean new_box_start_bit = box_bit_width - *box_end_bit - 1; 57*554aae35SVladimir Oltean *box_mask = GENMASK_ULL(new_box_start_bit, new_box_end_bit); 58*554aae35SVladimir Oltean *box_start_bit = new_box_start_bit; 59*554aae35SVladimir Oltean *box_end_bit = new_box_end_bit; 60*554aae35SVladimir Oltean } 61*554aae35SVladimir Oltean 62*554aae35SVladimir Oltean /** 63*554aae35SVladimir Oltean * packing - Convert numbers (currently u64) between a packed and an unpacked 64*554aae35SVladimir Oltean * format. Unpacked means laid out in memory in the CPU's native 65*554aae35SVladimir Oltean * understanding of integers, while packed means anything else that 66*554aae35SVladimir Oltean * requires translation. 67*554aae35SVladimir Oltean * 68*554aae35SVladimir Oltean * @pbuf: Pointer to a buffer holding the packed value. 69*554aae35SVladimir Oltean * @uval: Pointer to an u64 holding the unpacked value. 70*554aae35SVladimir Oltean * @startbit: The index (in logical notation, compensated for quirks) where 71*554aae35SVladimir Oltean * the packed value starts within pbuf. Must be larger than, or 72*554aae35SVladimir Oltean * equal to, endbit. 73*554aae35SVladimir Oltean * @endbit: The index (in logical notation, compensated for quirks) where 74*554aae35SVladimir Oltean * the packed value ends within pbuf. Must be smaller than, or equal 75*554aae35SVladimir Oltean * to, startbit. 76*554aae35SVladimir Oltean * @op: If PACK, then uval will be treated as const pointer and copied (packed) 77*554aae35SVladimir Oltean * into pbuf, between startbit and endbit. 78*554aae35SVladimir Oltean * If UNPACK, then pbuf will be treated as const pointer and the logical 79*554aae35SVladimir Oltean * value between startbit and endbit will be copied (unpacked) to uval. 80*554aae35SVladimir Oltean * @quirks: A bit mask of QUIRK_LITTLE_ENDIAN, QUIRK_LSW32_IS_FIRST and 81*554aae35SVladimir Oltean * QUIRK_MSB_ON_THE_RIGHT. 82*554aae35SVladimir Oltean * 83*554aae35SVladimir Oltean * Return: 0 on success, EINVAL or ERANGE if called incorrectly. Assuming 84*554aae35SVladimir Oltean * correct usage, return code may be discarded. 85*554aae35SVladimir Oltean * If op is PACK, pbuf is modified. 86*554aae35SVladimir Oltean * If op is UNPACK, uval is modified. 87*554aae35SVladimir Oltean */ 88*554aae35SVladimir Oltean int packing(void *pbuf, u64 *uval, int startbit, int endbit, size_t pbuflen, 89*554aae35SVladimir Oltean enum packing_op op, u8 quirks) 90*554aae35SVladimir Oltean { 91*554aae35SVladimir Oltean /* Number of bits for storing "uval" 92*554aae35SVladimir Oltean * also width of the field to access in the pbuf 93*554aae35SVladimir Oltean */ 94*554aae35SVladimir Oltean u64 value_width; 95*554aae35SVladimir Oltean /* Logical byte indices corresponding to the 96*554aae35SVladimir Oltean * start and end of the field. 97*554aae35SVladimir Oltean */ 98*554aae35SVladimir Oltean int plogical_first_u8, plogical_last_u8, box; 99*554aae35SVladimir Oltean 100*554aae35SVladimir Oltean /* startbit is expected to be larger than endbit */ 101*554aae35SVladimir Oltean if (startbit < endbit) 102*554aae35SVladimir Oltean /* Invalid function call */ 103*554aae35SVladimir Oltean return -EINVAL; 104*554aae35SVladimir Oltean 105*554aae35SVladimir Oltean value_width = startbit - endbit + 1; 106*554aae35SVladimir Oltean if (value_width > 64) 107*554aae35SVladimir Oltean return -ERANGE; 108*554aae35SVladimir Oltean 109*554aae35SVladimir Oltean /* Check if "uval" fits in "value_width" bits. 110*554aae35SVladimir Oltean * If value_width is 64, the check will fail, but any 111*554aae35SVladimir Oltean * 64-bit uval will surely fit. 112*554aae35SVladimir Oltean */ 113*554aae35SVladimir Oltean if (op == PACK && value_width < 64 && (*uval >= (1ull << value_width))) 114*554aae35SVladimir Oltean /* Cannot store "uval" inside "value_width" bits. 115*554aae35SVladimir Oltean * Truncating "uval" is most certainly not desirable, 116*554aae35SVladimir Oltean * so simply erroring out is appropriate. 117*554aae35SVladimir Oltean */ 118*554aae35SVladimir Oltean return -ERANGE; 119*554aae35SVladimir Oltean 120*554aae35SVladimir Oltean /* Initialize parameter */ 121*554aae35SVladimir Oltean if (op == UNPACK) 122*554aae35SVladimir Oltean *uval = 0; 123*554aae35SVladimir Oltean 124*554aae35SVladimir Oltean /* Iterate through an idealistic view of the pbuf as an u64 with 125*554aae35SVladimir Oltean * no quirks, u8 by u8 (aligned at u8 boundaries), from high to low 126*554aae35SVladimir Oltean * logical bit significance. "box" denotes the current logical u8. 127*554aae35SVladimir Oltean */ 128*554aae35SVladimir Oltean plogical_first_u8 = startbit / 8; 129*554aae35SVladimir Oltean plogical_last_u8 = endbit / 8; 130*554aae35SVladimir Oltean 131*554aae35SVladimir Oltean for (box = plogical_first_u8; box >= plogical_last_u8; box--) { 132*554aae35SVladimir Oltean /* Bit indices into the currently accessed 8-bit box */ 133*554aae35SVladimir Oltean int box_start_bit, box_end_bit, box_addr; 134*554aae35SVladimir Oltean u8 box_mask; 135*554aae35SVladimir Oltean /* Corresponding bits from the unpacked u64 parameter */ 136*554aae35SVladimir Oltean int proj_start_bit, proj_end_bit; 137*554aae35SVladimir Oltean u64 proj_mask; 138*554aae35SVladimir Oltean 139*554aae35SVladimir Oltean /* This u8 may need to be accessed in its entirety 140*554aae35SVladimir Oltean * (from bit 7 to bit 0), or not, depending on the 141*554aae35SVladimir Oltean * input arguments startbit and endbit. 142*554aae35SVladimir Oltean */ 143*554aae35SVladimir Oltean if (box == plogical_first_u8) 144*554aae35SVladimir Oltean box_start_bit = startbit % 8; 145*554aae35SVladimir Oltean else 146*554aae35SVladimir Oltean box_start_bit = 7; 147*554aae35SVladimir Oltean if (box == plogical_last_u8) 148*554aae35SVladimir Oltean box_end_bit = endbit % 8; 149*554aae35SVladimir Oltean else 150*554aae35SVladimir Oltean box_end_bit = 0; 151*554aae35SVladimir Oltean 152*554aae35SVladimir Oltean /* We have determined the box bit start and end. 153*554aae35SVladimir Oltean * Now we calculate where this (masked) u8 box would fit 154*554aae35SVladimir Oltean * in the unpacked (CPU-readable) u64 - the u8 box's 155*554aae35SVladimir Oltean * projection onto the unpacked u64. Though the 156*554aae35SVladimir Oltean * box is u8, the projection is u64 because it may fall 157*554aae35SVladimir Oltean * anywhere within the unpacked u64. 158*554aae35SVladimir Oltean */ 159*554aae35SVladimir Oltean proj_start_bit = ((box * 8) + box_start_bit) - endbit; 160*554aae35SVladimir Oltean proj_end_bit = ((box * 8) + box_end_bit) - endbit; 161*554aae35SVladimir Oltean proj_mask = GENMASK_ULL(proj_start_bit, proj_end_bit); 162*554aae35SVladimir Oltean box_mask = GENMASK_ULL(box_start_bit, box_end_bit); 163*554aae35SVladimir Oltean 164*554aae35SVladimir Oltean /* Determine the offset of the u8 box inside the pbuf, 165*554aae35SVladimir Oltean * adjusted for quirks. The adjusted box_addr will be used for 166*554aae35SVladimir Oltean * effective addressing inside the pbuf (so it's not 167*554aae35SVladimir Oltean * logical any longer). 168*554aae35SVladimir Oltean */ 169*554aae35SVladimir Oltean box_addr = pbuflen - box - 1; 170*554aae35SVladimir Oltean if (quirks & QUIRK_LITTLE_ENDIAN) 171*554aae35SVladimir Oltean box_addr = get_le_offset(box_addr); 172*554aae35SVladimir Oltean if (quirks & QUIRK_LSW32_IS_FIRST) 173*554aae35SVladimir Oltean box_addr = get_reverse_lsw32_offset(box_addr, 174*554aae35SVladimir Oltean pbuflen); 175*554aae35SVladimir Oltean 176*554aae35SVladimir Oltean if (op == UNPACK) { 177*554aae35SVladimir Oltean u64 pval; 178*554aae35SVladimir Oltean 179*554aae35SVladimir Oltean /* Read from pbuf, write to uval */ 180*554aae35SVladimir Oltean pval = ((u8 *)pbuf)[box_addr] & box_mask; 181*554aae35SVladimir Oltean if (quirks & QUIRK_MSB_ON_THE_RIGHT) 182*554aae35SVladimir Oltean adjust_for_msb_right_quirk(&pval, 183*554aae35SVladimir Oltean &box_start_bit, 184*554aae35SVladimir Oltean &box_end_bit, 185*554aae35SVladimir Oltean &box_mask); 186*554aae35SVladimir Oltean 187*554aae35SVladimir Oltean pval >>= box_end_bit; 188*554aae35SVladimir Oltean pval <<= proj_end_bit; 189*554aae35SVladimir Oltean *uval &= ~proj_mask; 190*554aae35SVladimir Oltean *uval |= pval; 191*554aae35SVladimir Oltean } else { 192*554aae35SVladimir Oltean u64 pval; 193*554aae35SVladimir Oltean 194*554aae35SVladimir Oltean /* Write to pbuf, read from uval */ 195*554aae35SVladimir Oltean pval = (*uval) & proj_mask; 196*554aae35SVladimir Oltean pval >>= proj_end_bit; 197*554aae35SVladimir Oltean if (quirks & QUIRK_MSB_ON_THE_RIGHT) 198*554aae35SVladimir Oltean adjust_for_msb_right_quirk(&pval, 199*554aae35SVladimir Oltean &box_start_bit, 200*554aae35SVladimir Oltean &box_end_bit, 201*554aae35SVladimir Oltean &box_mask); 202*554aae35SVladimir Oltean 203*554aae35SVladimir Oltean pval <<= box_end_bit; 204*554aae35SVladimir Oltean ((u8 *)pbuf)[box_addr] &= ~box_mask; 205*554aae35SVladimir Oltean ((u8 *)pbuf)[box_addr] |= pval; 206*554aae35SVladimir Oltean } 207*554aae35SVladimir Oltean } 208*554aae35SVladimir Oltean return 0; 209*554aae35SVladimir Oltean } 210*554aae35SVladimir Oltean EXPORT_SYMBOL(packing); 211*554aae35SVladimir Oltean 212*554aae35SVladimir Oltean MODULE_LICENSE("GPL v2"); 213*554aae35SVladimir Oltean MODULE_DESCRIPTION("Generic bitfield packing and unpacking"); 214