the-tree/lib/packing.c

0001 // SPDX-License-Identifier: BSD-3-Clause OR GPL-2.0
0002 /* Copyright 2016-2018 NXP
0003  * Copyright (c) 2018-2019, Vladimir Oltean <olteanv@gmail.com>
0004  */
0005 #include <linux/packing.h>
0006 #include <linux/module.h>
0007 #include <linux/bitops.h>
0008 #include <linux/errno.h>
0009 #include <linux/types.h>
0010
0011 static int get_le_offset(int offset)
0012 {
0013     int closest_multiple_of_4;
0014
0015     closest_multiple_of_4 = (offset / 4) * 4;
0016     offset -= closest_multiple_of_4;
0017     return closest_multiple_of_4 + (3 - offset);
0018 }
0019
0020 static int get_reverse_lsw32_offset(int offset, size_t len)
0021 {
0022     int closest_multiple_of_4;
0023     int word_index;
0024
0025     word_index = offset / 4;
0026     closest_multiple_of_4 = word_index * 4;
0027     offset -= closest_multiple_of_4;
0028     word_index = (len / 4) - word_index - 1;
0029     return word_index * 4 + offset;
0030 }
0031
0032 static u64 bit_reverse(u64 val, unsigned int width)
0033 {
0034     u64 new_val = 0;
0035     unsigned int bit;
0036     unsigned int i;
0037
0038     for (i = 0; i < width; i++) {
0039         bit = (val & (1 << i)) != 0;
0040         new_val |= (bit << (width - i - 1));
0041     }
0042     return new_val;
0043 }
0044
0045 static void adjust_for_msb_right_quirk(u64 *to_write, int *box_start_bit,
0046                        int *box_end_bit, u8 *box_mask)
0047 {
0048     int box_bit_width = *box_start_bit - *box_end_bit + 1;
0049     int new_box_start_bit, new_box_end_bit;
0050
0051     *to_write >>= *box_end_bit;
0052     *to_write = bit_reverse(*to_write, box_bit_width);
0053     *to_write <<= *box_end_bit;
0054
0055     new_box_end_bit   = box_bit_width - *box_start_bit - 1;
0056     new_box_start_bit = box_bit_width - *box_end_bit - 1;
0057     *box_mask = GENMASK_ULL(new_box_start_bit, new_box_end_bit);
0058     *box_start_bit = new_box_start_bit;
0059     *box_end_bit   = new_box_end_bit;
0060 }
0061
0062 /**
0063  * packing - Convert numbers (currently u64) between a packed and an unpacked
0064  *       format. Unpacked means laid out in memory in the CPU's native
0065  *       understanding of integers, while packed means anything else that
0066  *       requires translation.
0067  *
0068  * @pbuf: Pointer to a buffer holding the packed value.
0069  * @uval: Pointer to an u64 holding the unpacked value.
0070  * @startbit: The index (in logical notation, compensated for quirks) where
0071  *        the packed value starts within pbuf. Must be larger than, or
0072  *        equal to, endbit.
0073  * @endbit: The index (in logical notation, compensated for quirks) where
0074  *      the packed value ends within pbuf. Must be smaller than, or equal
0075  *      to, startbit.
0076  * @pbuflen: The length in bytes of the packed buffer pointed to by @pbuf.
0077  * @op: If PACK, then uval will be treated as const pointer and copied (packed)
0078  *  into pbuf, between startbit and endbit.
0079  *  If UNPACK, then pbuf will be treated as const pointer and the logical
0080  *  value between startbit and endbit will be copied (unpacked) to uval.
0081  * @quirks: A bit mask of QUIRK_LITTLE_ENDIAN, QUIRK_LSW32_IS_FIRST and
0082  *      QUIRK_MSB_ON_THE_RIGHT.
0083  *
0084  * Return: 0 on success, EINVAL or ERANGE if called incorrectly. Assuming
0085  *     correct usage, return code may be discarded.
0086  *     If op is PACK, pbuf is modified.
0087  *     If op is UNPACK, uval is modified.
0088  */
0089 int packing(void *pbuf, u64 *uval, int startbit, int endbit, size_t pbuflen,
0090         enum packing_op op, u8 quirks)
0091 {
0092     /* Number of bits for storing "uval"
0093      * also width of the field to access in the pbuf
0094      */
0095     u64 value_width;
0096     /* Logical byte indices corresponding to the
0097      * start and end of the field.
0098      */
0099     int plogical_first_u8, plogical_last_u8, box;
0100
0101     /* startbit is expected to be larger than endbit */
0102     if (startbit < endbit)
0103         /* Invalid function call */
0104         return -EINVAL;
0105
0106     value_width = startbit - endbit + 1;
0107     if (value_width > 64)
0108         return -ERANGE;
0109
0110     /* Check if "uval" fits in "value_width" bits.
0111      * If value_width is 64, the check will fail, but any
0112      * 64-bit uval will surely fit.
0113      */
0114     if (op == PACK && value_width < 64 && (*uval >= (1ull << value_width)))
0115         /* Cannot store "uval" inside "value_width" bits.
0116          * Truncating "uval" is most certainly not desirable,
0117          * so simply erroring out is appropriate.
0118          */
0119         return -ERANGE;
0120
0121     /* Initialize parameter */
0122     if (op == UNPACK)
0123         *uval = 0;
0124
0125     /* Iterate through an idealistic view of the pbuf as an u64 with
0126      * no quirks, u8 by u8 (aligned at u8 boundaries), from high to low
0127      * logical bit significance. "box" denotes the current logical u8.
0128      */
0129     plogical_first_u8 = startbit / 8;
0130     plogical_last_u8  = endbit / 8;
0131
0132     for (box = plogical_first_u8; box >= plogical_last_u8; box--) {
0133         /* Bit indices into the currently accessed 8-bit box */
0134         int box_start_bit, box_end_bit, box_addr;
0135         u8  box_mask;
0136         /* Corresponding bits from the unpacked u64 parameter */
0137         int proj_start_bit, proj_end_bit;
0138         u64 proj_mask;
0139
0140         /* This u8 may need to be accessed in its entirety
0141          * (from bit 7 to bit 0), or not, depending on the
0142          * input arguments startbit and endbit.
0143          */
0144         if (box == plogical_first_u8)
0145             box_start_bit = startbit % 8;
0146         else
0147             box_start_bit = 7;
0148         if (box == plogical_last_u8)
0149             box_end_bit = endbit % 8;
0150         else
0151             box_end_bit = 0;
0152
0153         /* We have determined the box bit start and end.
0154          * Now we calculate where this (masked) u8 box would fit
0155          * in the unpacked (CPU-readable) u64 - the u8 box's
0156          * projection onto the unpacked u64. Though the
0157          * box is u8, the projection is u64 because it may fall
0158          * anywhere within the unpacked u64.
0159          */
0160         proj_start_bit = ((box * 8) + box_start_bit) - endbit;
0161         proj_end_bit   = ((box * 8) + box_end_bit) - endbit;
0162         proj_mask = GENMASK_ULL(proj_start_bit, proj_end_bit);
0163         box_mask  = GENMASK_ULL(box_start_bit, box_end_bit);
0164
0165         /* Determine the offset of the u8 box inside the pbuf,
0166          * adjusted for quirks. The adjusted box_addr will be used for
0167          * effective addressing inside the pbuf (so it's not
0168          * logical any longer).
0169          */
0170         box_addr = pbuflen - box - 1;
0171         if (quirks & QUIRK_LITTLE_ENDIAN)
0172             box_addr = get_le_offset(box_addr);
0173         if (quirks & QUIRK_LSW32_IS_FIRST)
0174             box_addr = get_reverse_lsw32_offset(box_addr,
0175                                 pbuflen);
0176
0177         if (op == UNPACK) {
0178             u64 pval;
0179
0180             /* Read from pbuf, write to uval */
0181             pval = ((u8 *)pbuf)[box_addr] & box_mask;
0182             if (quirks & QUIRK_MSB_ON_THE_RIGHT)
0183                 adjust_for_msb_right_quirk(&pval,
0184                                &box_start_bit,
0185                                &box_end_bit,
0186                                &box_mask);
0187
0188             pval >>= box_end_bit;
0189             pval <<= proj_end_bit;
0190             *uval &= ~proj_mask;
0191             *uval |= pval;
0192         } else {
0193             u64 pval;
0194
0195             /* Write to pbuf, read from uval */
0196             pval = (*uval) & proj_mask;
0197             pval >>= proj_end_bit;
0198             if (quirks & QUIRK_MSB_ON_THE_RIGHT)
0199                 adjust_for_msb_right_quirk(&pval,
0200                                &box_start_bit,
0201                                &box_end_bit,
0202                                &box_mask);
0203
0204             pval <<= box_end_bit;
0205             ((u8 *)pbuf)[box_addr] &= ~box_mask;
0206             ((u8 *)pbuf)[box_addr] |= pval;
0207         }
0208     }
0209     return 0;
0210 }
0211 EXPORT_SYMBOL(packing);
0212
0213 MODULE_LICENSE("GPL v2");
0214 MODULE_DESCRIPTION("Generic bitfield packing and unpacking");