Merge tag 'clk-fixes-for-linus' of git://git.kernel.org/pub/scm/linux/kernel/git...
[platform/kernel/linux-starfive.git] / 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");