// SPDX-License-Identifier: BSD-3-Clause OR GPL-2.0/* Copyright 2016-2018 NXP * Copyright (c) 2018-2019, Vladimir Oltean <olteanv@gmail.com> */#include <linux/packing.h>#include <linux/module.h>#include <linux/bitops.h>#include <linux/errno.h>#include <linux/types.h>#include <linux/bitrev.h> static int get_le_offset(int offset){	int closest_multiple_of_4; 	closest_multiple_of_4 = (offset / 4) * 4;	offset -= closest_multiple_of_4;	return closest_multiple_of_4 + (3 - offset);} static int get_reverse_lsw32_offset(int offset, size_t len){	int closest_multiple_of_4;	int word_index; 	word_index = offset / 4;	closest_multiple_of_4 = word_index * 4;	offset -= closest_multiple_of_4;	word_index = (len / 4) - word_index - 1;	return word_index * 4 + offset;} static void adjust_for_msb_right_quirk(u64 *to_write, int *box_start_bit,				       int *box_end_bit, u8 *box_mask){	int box_bit_width = *box_start_bit - *box_end_bit + 1;	int new_box_start_bit, new_box_end_bit; 	*to_write >>= *box_end_bit;	*to_write = bitrev8(*to_write) >> (8 - box_bit_width);	*to_write <<= *box_end_bit; 	new_box_end_bit   = box_bit_width - *box_start_bit - 1;	new_box_start_bit = box_bit_width - *box_end_bit - 1;	*box_mask = GENMASK_ULL(new_box_start_bit, new_box_end_bit);	*box_start_bit = new_box_start_bit;	*box_end_bit   = new_box_end_bit;} /** * packing - Convert numbers (currently u64) between a packed and an unpacked *	     format. Unpacked means laid out in memory in the CPU's native *	     understanding of integers, while packed means anything else that *	     requires translation. * * @pbuf: Pointer to a buffer holding the packed value. * @uval: Pointer to an u64 holding the unpacked value. * @startbit: The index (in logical notation, compensated for quirks) where *	      the packed value starts within pbuf. Must be larger than, or *	      equal to, endbit. * @endbit: The index (in logical notation, compensated for quirks) where *	    the packed value ends within pbuf. Must be smaller than, or equal *	    to, startbit. * @pbuflen: The length in bytes of the packed buffer pointed to by @pbuf. * @op: If PACK, then uval will be treated as const pointer and copied (packed) *	into pbuf, between startbit and endbit. *	If UNPACK, then pbuf will be treated as const pointer and the logical *	value between startbit and endbit will be copied (unpacked) to uval. * @quirks: A bit mask of QUIRK_LITTLE_ENDIAN, QUIRK_LSW32_IS_FIRST and *	    QUIRK_MSB_ON_THE_RIGHT. * * Return: 0 on success, EINVAL or ERANGE if called incorrectly. Assuming *	   correct usage, return code may be discarded. *	   If op is PACK, pbuf is modified. *	   If op is UNPACK, uval is modified. */int packing(void *pbuf, u64 *uval, int startbit, int endbit, size_t pbuflen,	    enum packing_op op, u8 quirks){	/* Number of bits for storing "uval"	 * also width of the field to access in the pbuf	 */	u64 value_width;	/* Logical byte indices corresponding to the	 * start and end of the field.	 */	int plogical_first_u8, plogical_last_u8, box; 	/* startbit is expected to be larger than endbit */	if (startbit < endbit)		/* Invalid function call */		return -EINVAL; 	value_width = startbit - endbit + 1;	if (value_width > 64)		return -ERANGE; 	/* Check if "uval" fits in "value_width" bits.	 * If value_width is 64, the check will fail, but any	 * 64-bit uval will surely fit.	 */	if (op == PACK && value_width < 64 && (*uval >= (1ull << value_width)))		/* Cannot store "uval" inside "value_width" bits.		 * Truncating "uval" is most certainly not desirable,		 * so simply erroring out is appropriate.		 */		return -ERANGE; 	/* Initialize parameter */	if (op == UNPACK)		*uval = 0; 	/* Iterate through an idealistic view of the pbuf as an u64 with	 * no quirks, u8 by u8 (aligned at u8 boundaries), from high to low	 * logical bit significance. "box" denotes the current logical u8.	 */	plogical_first_u8 = startbit / 8;	plogical_last_u8  = endbit / 8; 	for (box = plogical_first_u8; box >= plogical_last_u8; box--) {		/* Bit indices into the currently accessed 8-bit box */		int box_start_bit, box_end_bit, box_addr;		u8  box_mask;		/* Corresponding bits from the unpacked u64 parameter */		int proj_start_bit, proj_end_bit;		u64 proj_mask; 		/* This u8 may need to be accessed in its entirety		 * (from bit 7 to bit 0), or not, depending on the		 * input arguments startbit and endbit.		 */		if (box == plogical_first_u8)			box_start_bit = startbit % 8;		else			box_start_bit = 7;		if (box == plogical_last_u8)			box_end_bit = endbit % 8;		else			box_end_bit = 0; 		/* We have determined the box bit start and end.		 * Now we calculate where this (masked) u8 box would fit		 * in the unpacked (CPU-readable) u64 - the u8 box's		 * projection onto the unpacked u64. Though the		 * box is u8, the projection is u64 because it may fall		 * anywhere within the unpacked u64.		 */		proj_start_bit = ((box * 8) + box_start_bit) - endbit;		proj_end_bit   = ((box * 8) + box_end_bit) - endbit;		proj_mask = GENMASK_ULL(proj_start_bit, proj_end_bit);		box_mask  = GENMASK_ULL(box_start_bit, box_end_bit); 		/* Determine the offset of the u8 box inside the pbuf,		 * adjusted for quirks. The adjusted box_addr will be used for		 * effective addressing inside the pbuf (so it's not		 * logical any longer).		 */		box_addr = pbuflen - box - 1;		if (quirks & QUIRK_LITTLE_ENDIAN)			box_addr = get_le_offset(box_addr);		if (quirks & QUIRK_LSW32_IS_FIRST)			box_addr = get_reverse_lsw32_offset(box_addr,							    pbuflen); 		if (op == UNPACK) {			u64 pval; 			/* Read from pbuf, write to uval */			pval = ((u8 *)pbuf)[box_addr] & box_mask;			if (quirks & QUIRK_MSB_ON_THE_RIGHT)				adjust_for_msb_right_quirk(&pval,							   &box_start_bit,							   &box_end_bit,							   &box_mask); 			pval >>= box_end_bit;			pval <<= proj_end_bit;			*uval &= ~proj_mask;			*uval |= pval;		} else {			u64 pval; 			/* Write to pbuf, read from uval */			pval = (*uval) & proj_mask;			pval >>= proj_end_bit;			if (quirks & QUIRK_MSB_ON_THE_RIGHT)				adjust_for_msb_right_quirk(&pval,							   &box_start_bit,							   &box_end_bit,							   &box_mask); 			pval <<= box_end_bit;			((u8 *)pbuf)[box_addr] &= ~box_mask;			((u8 *)pbuf)[box_addr] |= pval;		}	}	return 0;}EXPORT_SYMBOL(packing); MODULE_LICENSE("GPL v2");MODULE_DESCRIPTION("Generic bitfield packing and unpacking");