lib/packing.c

   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 /**
  13  * calculate_box_addr - Determine physical location of byte in buffer
  14  * @box: Index of byte within buffer seen as a logical big-endian big number
  15  * @len: Size of buffer in bytes
  16  * @quirks: mask of QUIRK_LSW32_IS_FIRST and QUIRK_LITTLE_ENDIAN
  17  *
  18  * Function interprets the buffer as a @len byte sized big number, and returns
  19  * the physical offset of the @box logical octet within it. Internally, it
  20  * treats the big number as groups of 4 bytes. If @len is not a multiple of 4,
  21  * the last group may be shorter.
  22  *
  23  * @QUIRK_LSW32_IS_FIRST gives the ordering of groups of 4 octets relative to
  24  * each other. If set, the most significant group of 4 octets is last in the
  25  * buffer (and may be truncated if @len is not a multiple of 4).
  26  *
  27  * @QUIRK_LITTLE_ENDIAN gives the ordering of bytes within each group of 4.
  28  * If set, the most significant byte is last in the group. If @len takes the
  29  * form of 4k+3, the last group will only be able to represent 24 bits, and its
  30  * most significant octet is byte 2.
  31  *
  32  * Return: the physical offset into the buffer corresponding to the logical box.
  33  */
  34 static size_t calculate_box_addr(size_t box, size_t len, u8 quirks)
  35 {
  36         size_t offset_of_group, offset_in_group, this_group = box / 4;
  37         size_t group_size;
  38
  39         if (quirks & QUIRK_LSW32_IS_FIRST)
  40                 offset_of_group = this_group * 4;
  41         else
  42                 offset_of_group = len - ((this_group + 1) * 4);
  43
  44         group_size = min(4, len - offset_of_group);
  45
  46         if (quirks & QUIRK_LITTLE_ENDIAN)
  47                 offset_in_group = box - this_group * 4;
  48         else
  49                 offset_in_group = group_size - (box - this_group * 4) - 1;
  50
  51         return offset_of_group + offset_in_group;
  52 }
  53
  54 /**
  55  * pack - Pack u64 number into bitfield of buffer.
  56  *
  57  * @pbuf: Pointer to a buffer holding the packed value.
  58  * @uval: CPU-readable unpacked value to pack.
  59  * @startbit: The index (in logical notation, compensated for quirks) where
  60  *            the packed value starts within pbuf. Must be larger than, or
  61  *            equal to, endbit.
  62  * @endbit: The index (in logical notation, compensated for quirks) where
  63  *          the packed value ends within pbuf. Must be smaller than, or equal
  64  *          to, startbit.
  65  * @pbuflen: The length in bytes of the packed buffer pointed to by @pbuf.
  66  * @quirks: A bit mask of QUIRK_LITTLE_ENDIAN, QUIRK_LSW32_IS_FIRST and
  67  *          QUIRK_MSB_ON_THE_RIGHT.
  68  *
  69  * Return: 0 on success, EINVAL or ERANGE if called incorrectly. Assuming
  70  *         correct usage, return code may be discarded. The @pbuf memory will
  71  *         be modified on success.
  72  */
  73 int pack(void *pbuf, u64 uval, size_t startbit, size_t endbit, size_t pbuflen,
  74          u8 quirks)
  75 {
  76         /* Logical byte indices corresponding to the
  77          * start and end of the field.
  78          */
  79         int plogical_first_u8, plogical_last_u8, box;
  80         /* width of the field to access in the pbuf */
  81         u64 value_width;
  82
  83         /* startbit is expected to be larger than endbit, and both are
  84          * expected to be within the logically addressable range of the buffer.
  85          */
  86         if (unlikely(startbit < endbit || startbit >= BITS_PER_BYTE * pbuflen))
  87                 /* Invalid function call */
  88                 return -EINVAL;
  89
  90         value_width = startbit - endbit + 1;
  91         if (unlikely(value_width > 64))
  92                 return -ERANGE;
  93
  94         /* Check if "uval" fits in "value_width" bits.
  95          * If value_width is 64, the check will fail, but any
  96          * 64-bit uval will surely fit.
  97          */
  98         if (unlikely(value_width < 64 && uval >= (1ull << value_width)))
  99                 /* Cannot store "uval" inside "value_width" bits.
 100                  * Truncating "uval" is most certainly not desirable,
 101                  * so simply erroring out is appropriate.
 102                  */
 103                 return -ERANGE;
 104
 105         /* Iterate through an idealistic view of the pbuf as an u64 with
 106          * no quirks, u8 by u8 (aligned at u8 boundaries), from high to low
 107          * logical bit significance. "box" denotes the current logical u8.
 108          */
 109         plogical_first_u8 = startbit / BITS_PER_BYTE;
 110         plogical_last_u8  = endbit / BITS_PER_BYTE;
 111
 112         for (box = plogical_first_u8; box >= plogical_last_u8; box--) {
 113                 /* Bit indices into the currently accessed 8-bit box */
 114                 size_t box_start_bit, box_end_bit, box_addr;
 115                 u8  box_mask;
 116                 /* Corresponding bits from the unpacked u64 parameter */
 117                 size_t proj_start_bit, proj_end_bit;
 118                 u64 proj_mask;
 119                 u64 pval;
 120
 121                 /* This u8 may need to be accessed in its entirety
 122                  * (from bit 7 to bit 0), or not, depending on the
 123                  * input arguments startbit and endbit.
 124                  */
 125                 if (box == plogical_first_u8)
 126                         box_start_bit = startbit % BITS_PER_BYTE;
 127                 else
 128                         box_start_bit = 7;
 129                 if (box == plogical_last_u8)
 130                         box_end_bit = endbit % BITS_PER_BYTE;
 131                 else
 132                         box_end_bit = 0;
 133
 134                 /* We have determined the box bit start and end.
 135                  * Now we calculate where this (masked) u8 box would fit
 136                  * in the unpacked (CPU-readable) u64 - the u8 box's
 137                  * projection onto the unpacked u64. Though the
 138                  * box is u8, the projection is u64 because it may fall
 139                  * anywhere within the unpacked u64.
 140                  */
 141                 proj_start_bit = ((box * BITS_PER_BYTE) + box_start_bit) - endbit;
 142                 proj_end_bit = ((box * BITS_PER_BYTE) + box_end_bit) - endbit;
 143                 proj_mask = GENMASK_ULL(proj_start_bit, proj_end_bit);
 144                 box_mask = GENMASK(box_start_bit, box_end_bit);
 145
 146                 /* Determine the offset of the u8 box inside the pbuf,
 147                  * adjusted for quirks. The adjusted box_addr will be used for
 148                  * effective addressing inside the pbuf (so it's not
 149                  * logical any longer).
 150                  */
 151                 box_addr = calculate_box_addr(box, pbuflen, quirks);
 152
 153                 /* Write to pbuf, read from uval */
 154                 pval = uval & proj_mask;
 155                 pval >>= proj_end_bit;
 156                 pval <<= box_end_bit;
 157
 158                 if (quirks & QUIRK_MSB_ON_THE_RIGHT) {
 159                         pval = bitrev8(pval);
 160                         box_mask = bitrev8(box_mask);
 161                 }
 162
 163                 ((u8 *)pbuf)[box_addr] &= ~box_mask;
 164                 ((u8 *)pbuf)[box_addr] |= pval;
 165         }
 166         return 0;
 167 }
 168 EXPORT_SYMBOL(pack);
 169
 170 /**
 171  * unpack - Unpack u64 number from packed buffer.
 172  *
 173  * @pbuf: Pointer to a buffer holding the packed value.
 174  * @uval: Pointer to an u64 holding the unpacked value.
 175  * @startbit: The index (in logical notation, compensated for quirks) where
 176  *            the packed value starts within pbuf. Must be larger than, or
 177  *            equal to, endbit.
 178  * @endbit: The index (in logical notation, compensated for quirks) where
 179  *          the packed value ends within pbuf. Must be smaller than, or equal
 180  *          to, startbit.
 181  * @pbuflen: The length in bytes of the packed buffer pointed to by @pbuf.
 182  * @quirks: A bit mask of QUIRK_LITTLE_ENDIAN, QUIRK_LSW32_IS_FIRST and
 183  *          QUIRK_MSB_ON_THE_RIGHT.
 184  *
 185  * Return: 0 on success, EINVAL or ERANGE if called incorrectly. Assuming
 186  *         correct usage, return code may be discarded. The @uval will be
 187  *         modified on success.
 188  */
 189 int unpack(const void *pbuf, u64 *uval, size_t startbit, size_t endbit,
 190            size_t pbuflen, u8 quirks)
 191 {
 192         /* Logical byte indices corresponding to the
 193          * start and end of the field.
 194          */
 195         int plogical_first_u8, plogical_last_u8, box;
 196         /* width of the field to access in the pbuf */
 197         u64 value_width;
 198
 199         /* startbit is expected to be larger than endbit, and both are
 200          * expected to be within the logically addressable range of the buffer.
 201          */
 202         if (unlikely(startbit < endbit || startbit >= BITS_PER_BYTE * pbuflen))
 203                 /* Invalid function call */
 204                 return -EINVAL;
 205
 206         value_width = startbit - endbit + 1;
 207         if (unlikely(value_width > 64))
 208                 return -ERANGE;
 209
 210         /* Initialize parameter */
 211         *uval = 0;
 212
 213         /* Iterate through an idealistic view of the pbuf as an u64 with
 214          * no quirks, u8 by u8 (aligned at u8 boundaries), from high to low
 215          * logical bit significance. "box" denotes the current logical u8.
 216          */
 217         plogical_first_u8 = startbit / BITS_PER_BYTE;
 218         plogical_last_u8  = endbit / BITS_PER_BYTE;
 219
 220         for (box = plogical_first_u8; box >= plogical_last_u8; box--) {
 221                 /* Bit indices into the currently accessed 8-bit box */
 222                 size_t box_start_bit, box_end_bit, box_addr;
 223                 u8  box_mask;
 224                 /* Corresponding bits from the unpacked u64 parameter */
 225                 size_t proj_start_bit, proj_end_bit;
 226                 u64 proj_mask;
 227                 u64 pval;
 228
 229                 /* This u8 may need to be accessed in its entirety
 230                  * (from bit 7 to bit 0), or not, depending on the
 231                  * input arguments startbit and endbit.
 232                  */
 233                 if (box == plogical_first_u8)
 234                         box_start_bit = startbit % BITS_PER_BYTE;
 235                 else
 236                         box_start_bit = 7;
 237                 if (box == plogical_last_u8)
 238                         box_end_bit = endbit % BITS_PER_BYTE;
 239                 else
 240                         box_end_bit = 0;
 241
 242                 /* We have determined the box bit start and end.
 243                  * Now we calculate where this (masked) u8 box would fit
 244                  * in the unpacked (CPU-readable) u64 - the u8 box's
 245                  * projection onto the unpacked u64. Though the
 246                  * box is u8, the projection is u64 because it may fall
 247                  * anywhere within the unpacked u64.
 248                  */
 249                 proj_start_bit = ((box * BITS_PER_BYTE) + box_start_bit) - endbit;
 250                 proj_end_bit = ((box * BITS_PER_BYTE) + box_end_bit) - endbit;
 251                 proj_mask = GENMASK_ULL(proj_start_bit, proj_end_bit);
 252                 box_mask = GENMASK(box_start_bit, box_end_bit);
 253
 254                 /* Determine the offset of the u8 box inside the pbuf,
 255                  * adjusted for quirks. The adjusted box_addr will be used for
 256                  * effective addressing inside the pbuf (so it's not
 257                  * logical any longer).
 258                  */
 259                 box_addr = calculate_box_addr(box, pbuflen, quirks);
 260
 261                 /* Read from pbuf, write to uval */
 262                 pval = ((u8 *)pbuf)[box_addr];
 263
 264                 if (quirks & QUIRK_MSB_ON_THE_RIGHT)
 265                         pval = bitrev8(pval);
 266
 267                 pval &= box_mask;
 268
 269                 pval >>= box_end_bit;
 270                 pval <<= proj_end_bit;
 271                 *uval &= ~proj_mask;
 272                 *uval |= pval;
 273         }
 274         return 0;
 275 }
 276 EXPORT_SYMBOL(unpack);
 277
 278 /**
 279  * packing - Convert numbers (currently u64) between a packed and an unpacked
 280  *           format. Unpacked means laid out in memory in the CPU's native
 281  *           understanding of integers, while packed means anything else that
 282  *           requires translation.
 283  *
 284  * @pbuf: Pointer to a buffer holding the packed value.
 285  * @uval: Pointer to an u64 holding the unpacked value.
 286  * @startbit: The index (in logical notation, compensated for quirks) where
 287  *            the packed value starts within pbuf. Must be larger than, or
 288  *            equal to, endbit.
 289  * @endbit: The index (in logical notation, compensated for quirks) where
 290  *          the packed value ends within pbuf. Must be smaller than, or equal
 291  *          to, startbit.
 292  * @pbuflen: The length in bytes of the packed buffer pointed to by @pbuf.
 293  * @op: If PACK, then uval will be treated as const pointer and copied (packed)
 294  *      into pbuf, between startbit and endbit.
 295  *      If UNPACK, then pbuf will be treated as const pointer and the logical
 296  *      value between startbit and endbit will be copied (unpacked) to uval.
 297  * @quirks: A bit mask of QUIRK_LITTLE_ENDIAN, QUIRK_LSW32_IS_FIRST and
 298  *          QUIRK_MSB_ON_THE_RIGHT.
 299  *
 300  * Note: this is deprecated, prefer to use pack() or unpack() in new code.
 301  *
 302  * Return: 0 on success, EINVAL or ERANGE if called incorrectly. Assuming
 303  *         correct usage, return code may be discarded.
 304  *         If op is PACK, pbuf is modified.
 305  *         If op is UNPACK, uval is modified.
 306  */
 307 int packing(void *pbuf, u64 *uval, int startbit, int endbit, size_t pbuflen,
 308             enum packing_op op, u8 quirks)
 309 {
 310         if (op == PACK)
 311                 return pack(pbuf, *uval, startbit, endbit, pbuflen, quirks);
 312
 313         return unpack(pbuf, uval, startbit, endbit, pbuflen, quirks);
 314 }
 315 EXPORT_SYMBOL(packing);
 316
 317 MODULE_DESCRIPTION("Generic bitfield packing and unpacking");