LXR linux/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
  11static 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
  20static 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
  32static 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
  45static 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 */
  88int 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}
 210EXPORT_SYMBOL(packing);
 211
 212MODULE_LICENSE("GPL v2");
 213MODULE_DESCRIPTION("Generic bitfield packing and unpacking");
 214