packing.c 6.6 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214
  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. static int get_le_offset(int offset)
  11. {
  12. int closest_multiple_of_4;
  13. closest_multiple_of_4 = (offset / 4) * 4;
  14. offset -= closest_multiple_of_4;
  15. return closest_multiple_of_4 + (3 - offset);
  16. }
  17. static int get_reverse_lsw32_offset(int offset, size_t len)
  18. {
  19. int closest_multiple_of_4;
  20. int word_index;
  21. word_index = offset / 4;
  22. closest_multiple_of_4 = word_index * 4;
  23. offset -= closest_multiple_of_4;
  24. word_index = (len / 4) - word_index - 1;
  25. return word_index * 4 + offset;
  26. }
  27. static u64 bit_reverse(u64 val, unsigned int width)
  28. {
  29. u64 new_val = 0;
  30. unsigned int bit;
  31. unsigned int i;
  32. for (i = 0; i < width; i++) {
  33. bit = (val & (1 << i)) != 0;
  34. new_val |= (bit << (width - i - 1));
  35. }
  36. return new_val;
  37. }
  38. static void adjust_for_msb_right_quirk(u64 *to_write, int *box_start_bit,
  39. int *box_end_bit, u8 *box_mask)
  40. {
  41. int box_bit_width = *box_start_bit - *box_end_bit + 1;
  42. int new_box_start_bit, new_box_end_bit;
  43. *to_write >>= *box_end_bit;
  44. *to_write = bit_reverse(*to_write, box_bit_width);
  45. *to_write <<= *box_end_bit;
  46. new_box_end_bit = box_bit_width - *box_start_bit - 1;
  47. new_box_start_bit = box_bit_width - *box_end_bit - 1;
  48. *box_mask = GENMASK_ULL(new_box_start_bit, new_box_end_bit);
  49. *box_start_bit = new_box_start_bit;
  50. *box_end_bit = new_box_end_bit;
  51. }
  52. /**
  53. * packing - Convert numbers (currently u64) between a packed and an unpacked
  54. * format. Unpacked means laid out in memory in the CPU's native
  55. * understanding of integers, while packed means anything else that
  56. * requires translation.
  57. *
  58. * @pbuf: Pointer to a buffer holding the packed value.
  59. * @uval: Pointer to an u64 holding the unpacked value.
  60. * @startbit: The index (in logical notation, compensated for quirks) where
  61. * the packed value starts within pbuf. Must be larger than, or
  62. * equal to, endbit.
  63. * @endbit: The index (in logical notation, compensated for quirks) where
  64. * the packed value ends within pbuf. Must be smaller than, or equal
  65. * to, startbit.
  66. * @pbuflen: The length in bytes of the packed buffer pointed to by @pbuf.
  67. * @op: If PACK, then uval will be treated as const pointer and copied (packed)
  68. * into pbuf, between startbit and endbit.
  69. * If UNPACK, then pbuf will be treated as const pointer and the logical
  70. * value between startbit and endbit will be copied (unpacked) to uval.
  71. * @quirks: A bit mask of QUIRK_LITTLE_ENDIAN, QUIRK_LSW32_IS_FIRST and
  72. * QUIRK_MSB_ON_THE_RIGHT.
  73. *
  74. * Return: 0 on success, EINVAL or ERANGE if called incorrectly. Assuming
  75. * correct usage, return code may be discarded.
  76. * If op is PACK, pbuf is modified.
  77. * If op is UNPACK, uval is modified.
  78. */
  79. int packing(void *pbuf, u64 *uval, int startbit, int endbit, size_t pbuflen,
  80. enum packing_op op, u8 quirks)
  81. {
  82. /* Number of bits for storing "uval"
  83. * also width of the field to access in the pbuf
  84. */
  85. u64 value_width;
  86. /* Logical byte indices corresponding to the
  87. * start and end of the field.
  88. */
  89. int plogical_first_u8, plogical_last_u8, box;
  90. /* startbit is expected to be larger than endbit */
  91. if (startbit < endbit)
  92. /* Invalid function call */
  93. return -EINVAL;
  94. value_width = startbit - endbit + 1;
  95. if (value_width > 64)
  96. return -ERANGE;
  97. /* Check if "uval" fits in "value_width" bits.
  98. * If value_width is 64, the check will fail, but any
  99. * 64-bit uval will surely fit.
  100. */
  101. if (op == PACK && value_width < 64 && (*uval >= (1ull << value_width)))
  102. /* Cannot store "uval" inside "value_width" bits.
  103. * Truncating "uval" is most certainly not desirable,
  104. * so simply erroring out is appropriate.
  105. */
  106. return -ERANGE;
  107. /* Initialize parameter */
  108. if (op == UNPACK)
  109. *uval = 0;
  110. /* Iterate through an idealistic view of the pbuf as an u64 with
  111. * no quirks, u8 by u8 (aligned at u8 boundaries), from high to low
  112. * logical bit significance. "box" denotes the current logical u8.
  113. */
  114. plogical_first_u8 = startbit / 8;
  115. plogical_last_u8 = endbit / 8;
  116. for (box = plogical_first_u8; box >= plogical_last_u8; box--) {
  117. /* Bit indices into the currently accessed 8-bit box */
  118. int box_start_bit, box_end_bit, box_addr;
  119. u8 box_mask;
  120. /* Corresponding bits from the unpacked u64 parameter */
  121. int proj_start_bit, proj_end_bit;
  122. u64 proj_mask;
  123. /* This u8 may need to be accessed in its entirety
  124. * (from bit 7 to bit 0), or not, depending on the
  125. * input arguments startbit and endbit.
  126. */
  127. if (box == plogical_first_u8)
  128. box_start_bit = startbit % 8;
  129. else
  130. box_start_bit = 7;
  131. if (box == plogical_last_u8)
  132. box_end_bit = endbit % 8;
  133. else
  134. box_end_bit = 0;
  135. /* We have determined the box bit start and end.
  136. * Now we calculate where this (masked) u8 box would fit
  137. * in the unpacked (CPU-readable) u64 - the u8 box's
  138. * projection onto the unpacked u64. Though the
  139. * box is u8, the projection is u64 because it may fall
  140. * anywhere within the unpacked u64.
  141. */
  142. proj_start_bit = ((box * 8) + box_start_bit) - endbit;
  143. proj_end_bit = ((box * 8) + box_end_bit) - endbit;
  144. proj_mask = GENMASK_ULL(proj_start_bit, proj_end_bit);
  145. box_mask = GENMASK_ULL(box_start_bit, box_end_bit);
  146. /* Determine the offset of the u8 box inside the pbuf,
  147. * adjusted for quirks. The adjusted box_addr will be used for
  148. * effective addressing inside the pbuf (so it's not
  149. * logical any longer).
  150. */
  151. box_addr = pbuflen - box - 1;
  152. if (quirks & QUIRK_LITTLE_ENDIAN)
  153. box_addr = get_le_offset(box_addr);
  154. if (quirks & QUIRK_LSW32_IS_FIRST)
  155. box_addr = get_reverse_lsw32_offset(box_addr,
  156. pbuflen);
  157. if (op == UNPACK) {
  158. u64 pval;
  159. /* Read from pbuf, write to uval */
  160. pval = ((u8 *)pbuf)[box_addr] & box_mask;
  161. if (quirks & QUIRK_MSB_ON_THE_RIGHT)
  162. adjust_for_msb_right_quirk(&pval,
  163. &box_start_bit,
  164. &box_end_bit,
  165. &box_mask);
  166. pval >>= box_end_bit;
  167. pval <<= proj_end_bit;
  168. *uval &= ~proj_mask;
  169. *uval |= pval;
  170. } else {
  171. u64 pval;
  172. /* Write to pbuf, read from uval */
  173. pval = (*uval) & proj_mask;
  174. pval >>= proj_end_bit;
  175. if (quirks & QUIRK_MSB_ON_THE_RIGHT)
  176. adjust_for_msb_right_quirk(&pval,
  177. &box_start_bit,
  178. &box_end_bit,
  179. &box_mask);
  180. pval <<= box_end_bit;
  181. ((u8 *)pbuf)[box_addr] &= ~box_mask;
  182. ((u8 *)pbuf)[box_addr] |= pval;
  183. }
  184. }
  185. return 0;
  186. }
  187. EXPORT_SYMBOL(packing);
  188. MODULE_LICENSE("GPL v2");
  189. MODULE_DESCRIPTION("Generic bitfield packing and unpacking");