202 lines · c
1// SPDX-License-Identifier: BSD-3-Clause OR GPL-2.02/* Copyright 2016-2018 NXP3 * 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 12static 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 21static 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 33static 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 unpacked52 * format. Unpacked means laid out in memory in the CPU's native53 * understanding of integers, while packed means anything else that54 * 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) where59 * the packed value starts within pbuf. Must be larger than, or60 * equal to, endbit.61 * @endbit: The index (in logical notation, compensated for quirks) where62 * the packed value ends within pbuf. Must be smaller than, or equal63 * 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 logical68 * value between startbit and endbit will be copied (unpacked) to uval.69 * @quirks: A bit mask of QUIRK_LITTLE_ENDIAN, QUIRK_LSW32_IS_FIRST and70 * QUIRK_MSB_ON_THE_RIGHT.71 *72 * Return: 0 on success, EINVAL or ERANGE if called incorrectly. Assuming73 * correct usage, return code may be discarded.74 * If op is PACK, pbuf is modified.75 * If op is UNPACK, uval is modified.76 */77int 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 pbuf82 */83 u64 value_width;84 /* Logical byte indices corresponding to the85 * 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 any100 * 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 with114 * no quirks, u8 by u8 (aligned at u8 boundaries), from high to low115 * 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 entirety129 * (from bit 7 to bit 0), or not, depending on the130 * input arguments startbit and endbit.131 */132 if (box == plogical_first_u8)133 box_start_bit = startbit % 8;134 else135 box_start_bit = 7;136 if (box == plogical_last_u8)137 box_end_bit = endbit % 8;138 else139 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 fit143 * in the unpacked (CPU-readable) u64 - the u8 box's144 * projection onto the unpacked u64. Though the145 * box is u8, the projection is u64 because it may fall146 * 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 for155 * effective addressing inside the pbuf (so it's not156 * 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}199EXPORT_SYMBOL(packing);200 201MODULE_DESCRIPTION("Generic bitfield packing and unpacking");202