lib/packing.c

   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");