[Resend v1 1/5] linux/bitqueue.h: add the bit queue implementation

[Alexander Potapenko]

struct bitq represents a bit queue with external storage.

Its purpose is to easily pack sub-byte values, which can be used, for
example, to implement RLE algorithms.

Signed-off-by: Alexander Potapenko <glider@xxxxxxxxxx>
---
 include/linux/bitqueue.h | 144 +++++++++++++++++++++++++++++++++++++++
 1 file changed, 144 insertions(+)
 create mode 100644 include/linux/bitqueue.h

diff --git a/include/linux/bitqueue.h b/include/linux/bitqueue.h
new file mode 100644
index 0000000000000..c4393f703c697
--- /dev/null
+++ b/include/linux/bitqueue.h
@@ -0,0 +1,144 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * A simple bit queue which supports enqueueing/dequeueing of sub-byte values.
+ *
+ * This can be used to pack complex bitfields into byte arrays.
+ */
+#ifndef _LINUX_BITQUEUE_H
+#define _LINUX_BITQUEUE_H
+
+#include <linux/string.h>
+#include <linux/types.h>
+
+/**
+ * struct bitq - represents a bit queue with external storage.
+ * @data:	data buffer used by the queue.
+ * @size:	size of @data in bytes.
+ * @bit_pos:	current bit position.
+ */
+struct bitq {
+	u8 *data;
+	int size, bit_pos;
+};
+
+/**
+ * bitq_init - initialize an empty bit queue.
+ * @q:		struct bitq to be initialized.
+ * @data:	external data buffer to use.
+ * @size:	capacity in bytes.
+ *
+ * Return:	0 in the case of success, -1 if either of the pointers is NULL.
+ */
+static inline int bitq_init(struct bitq *q, u8 *data, int size)
+{
+	if (!q || !data)
+		return -1;
+	q->data = data;
+	q->size = size;
+	memset(data, 0, size);
+	q->bit_pos = 0;
+	return 0;
+}
+
+/**
+ * bitq_init_full - make a bit queue from an initialized byte array.
+ * @q:		struct bitq to be initialized.
+ * @data:	external data buffer to use.
+ * @size:	capacity in bytes.
+ *
+ * Return:	0 in the case of success, -1 if either of the pointers is NULL.
+ */
+static inline int bitq_init_full(struct bitq *q, u8 *data, int size)
+{
+	if (!q || !data)
+		return -1;
+	q->data = data;
+	q->size = size;
+	q->bit_pos = q->size * 8;
+	return 0;
+}
+
+/**
+ * bitq_enqueue - push up to 8 bits to the end of the queue.
+ * @q:		struct bitq.
+ * @value:	byte containing the value to be pushed.
+ * @bits:	number of bits (1 to 8) to push.
+ *
+ * Return:	number of bits pushed, or -1 in the case of an error.
+ */
+static inline int bitq_enqueue(struct bitq *q, u8 value, int bits)
+{
+	int byte_pos, left_in_byte, max_pos;
+	u8 hi, lo;
+
+	if (!q || (bits < 1) || (bits > 8))
+		return -1;
+
+	max_pos = q->size * 8;
+	if ((max_pos - q->bit_pos) < bits)
+		return -1;
+
+	left_in_byte = 8 - (q->bit_pos % 8);
+	byte_pos = q->bit_pos / 8;
+	/* Clamp @value. */
+	value %= (1 << bits);
+	if (left_in_byte >= bits) {
+		/* @value fits into the current byte. */
+		value <<= (left_in_byte - bits);
+		q->data[byte_pos] |= value;
+	} else {
+		/*
+		 * @value needs to be split between the current and the
+		 * following bytes.
+		 */
+		hi = value >> (bits - left_in_byte);
+		q->data[byte_pos] |= hi;
+		byte_pos++;
+		lo = value << (8 - (bits - left_in_byte));
+		q->data[byte_pos] |= lo;
+	}
+	q->bit_pos += bits;
+	return bits;
+}
+
+/**
+ * bitq_dequeue - pop up to 8 bits from the beginning of the queue.
+ * @q:		struct bitq.
+ * @value:	u8* to store the popped value (can be NULL).
+ * @bits:	number of bits (1 to 8) to pop.
+ *
+ * Return:	number of bits popped, or -1 in the case of an error.
+ */
+
+#include <linux/printk.h>
+static inline int bitq_dequeue(struct bitq *q, u8 *value, int bits)
+{
+	int rem_bits = 8 - bits, i;
+	u8 output;
+
+	/* Invalid arguments. */
+	if (!q || (bits < 1) || (bits > 8))
+		return -1;
+	/* Not enough space to insert @bits. */
+	if (q->bit_pos < bits)
+		return -1;
+	/* Take the first @bits bits from the first byte. */
+	output = q->data[0];
+	output >>= rem_bits;
+	if (value)
+		*value = output;
+
+	/*
+	 * Shift every byte in the queue to the left by @bits, carrying over to
+	 * the previous byte.
+	 */
+	for (i = 0; i < q->size - 1; i++) {
+		q->data[i] = (q->data[i] << bits) |
+			     (q->data[i + 1] >> rem_bits);
+	}
+	q->data[q->size - 1] <<= bits;
+	q->bit_pos -= bits;
+	return bits;
+}
+
+#endif // _LINUX_BITQUEUE_H
-- 
2.41.0.255.g8b1d071c50-goog