From e0a7d9e2c220e5544ee8982c78851ff40ac425bc Mon Sep 17 00:00:00 2001
From: Max <msuraev@sysmocom.de>
Date: Fri, 17 Jun 2016 17:58:52 +0200
Subject: Add octet-aligned/unaligned shift functions

The actual code is from OsmoBTS' tch.c by Harald Welte. Add unit tests,
doxygen annotation and extra memory safety check. Those functions are
used in several BTS implementations but seems generic enough to be
generally useful.

Change-Id: I2b1901c4161e8035f059585901dca593b661556d
---
 src/bits.c | 48 ++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 48 insertions(+)

(limited to 'src/bits.c')

diff --git a/src/bits.c b/src/bits.c
index 569a10f0..0c77b279 100644
--- a/src/bits.c
+++ b/src/bits.c
@@ -61,6 +61,54 @@ int osmo_ubit2pbit(pbit_t *out, const ubit_t *in, unsigned int num_bits)
 	return outptr - out;
 }
 
+/*! \brief Shift unaligned input to octet-aligned output
+ *  \param[out] out output buffer, unaligned
+ *  \param[in] in input buffer, octet-aligned
+ *  \param[in] num_nibbles number of nibbles
+ */
+void osmo_nibble_shift_right(uint8_t *out, const uint8_t *in,
+			     unsigned int num_nibbles)
+{
+	unsigned int i, num_whole_bytes = num_nibbles / 2;
+	if (!num_whole_bytes)
+		return;
+
+	/* first byte: upper nibble empty, lower nibble from src */
+	out[0] = (in[0] >> 4);
+
+	/* bytes 1.. */
+	for (i = 1; i < num_whole_bytes; i++)
+		out[i] = ((in[i - 1] & 0xF) << 4) | (in[i] >> 4);
+
+	/* shift the last nibble, in case there's an odd count */
+	i = num_whole_bytes;
+	if (num_nibbles & 1)
+		out[i] = ((in[i - 1] & 0xF) << 4) | (in[i] >> 4);
+	else
+		out[i] = (in[i - 1] & 0xF) << 4;
+}
+
+/*! \brief Shift unaligned input to octet-aligned output
+ *  \param[out] out output buffer, octet-aligned
+ *  \param[in] in input buffer, unaligned
+ *  \param[in] num_nibbles number of nibbles
+ */
+void osmo_nibble_shift_left_unal(uint8_t *out, const uint8_t *in,
+				unsigned int num_nibbles)
+{
+	unsigned int i, num_whole_bytes = num_nibbles / 2;
+	if (!num_whole_bytes)
+		return;
+
+	for (i = 0; i < num_whole_bytes; i++)
+		out[i] = ((in[i] & 0xF) << 4) | (in[i + 1] >> 4);
+
+	/* shift the last nibble, in case there's an odd count */
+	i = num_whole_bytes;
+	if (num_nibbles & 1)
+		out[i] = (in[i] & 0xF) << 4;
+}
+
 /*! \brief convert unpacked bits to soft bits
  *  \param[out] out output buffer of soft bits
  *  \param[in] in input buffer of unpacked bits
-- 
cgit v1.2.3