LXR linux/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
  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 * @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 */
  89int 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}
 211EXPORT_SYMBOL(packing);
 212
 213MODULE_LICENSE("GPL v2");
 214MODULE_DESCRIPTION("Generic bitfield packing and unpacking");
 215