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 static 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
  21 static 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
  33 static 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 unpacked
  52  *           format. Unpacked means laid out in memory in the CPU's native
  53  *           understanding of integers, while packed means anything else that
  54  *           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) where
  59  *            the packed value starts within pbuf. Must be larger than, or
  60  *            equal to, endbit.
  61  * @endbit: The index (in logical notation, compensated for quirks) where
  62  *          the packed value ends within pbuf. Must be smaller than, or equal
  63  *          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 logical
  68  *      value between startbit and endbit will be copied (unpacked) to uval.
  69  * @quirks: A bit mask of QUIRK_LITTLE_ENDIAN, QUIRK_LSW32_IS_FIRST and
  70  *          QUIRK_MSB_ON_THE_RIGHT.
  71  *
  72  * Return: 0 on success, EINVAL or ERANGE if called incorrectly. Assuming
  73  *         correct usage, return code may be discarded.
  74  *         If op is PACK, pbuf is modified.
  75  *         If op is UNPACK, uval is modified.
  76  */
  77 int 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 pbuf
  82          */
  83         u64 value_width;
  84         /* Logical byte indices corresponding to the
  85          * 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 any
 100          * 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 with
 114          * no quirks, u8 by u8 (aligned at u8 boundaries), from high to low
 115          * 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 entirety
 129                  * (from bit 7 to bit 0), or not, depending on the
 130                  * input arguments startbit and endbit.
 131                  */
 132                 if (box == plogical_first_u8)
 133                         box_start_bit = startbit % 8;
 134                 else
 135                         box_start_bit = 7;
 136                 if (box == plogical_last_u8)
 137                         box_end_bit = endbit % 8;
 138                 else
 139                         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 fit
 143                  * in the unpacked (CPU-readable) u64 - the u8 box's
 144                  * projection onto the unpacked u64. Though the
 145                  * box is u8, the projection is u64 because it may fall
 146                  * 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 for
 155                  * effective addressing inside the pbuf (so it's not
 156                  * 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 }
 199 EXPORT_SYMBOL(packing);
 200
 201 MODULE_DESCRIPTION("Generic bitfield packing and unpacking");