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