gsm: TS 44.021 modified V.110 frame encoding/decoding support

3GPP TS 44.021 specifies the format for modified V.110 frames as used
on the GSM air (radio) interface.  Implement encoders and decoders for
this modified V.110 format.

Related: OS#1572
Change-Id: I60a2f2690459359437df20cf4da9043fa7c3ad11
diff --git a/include/osmocom/gsm/Makefile.am b/include/osmocom/gsm/Makefile.am
index 2c59e55..f840a27 100644
--- a/include/osmocom/gsm/Makefile.am
+++ b/include/osmocom/gsm/Makefile.am
@@ -29,6 +29,7 @@
 	gsm23003.h \
 	gsm23236.h \
 	gsm29118.h \
+	gsm44021.h \
 	gsm48.h \
 	gsm48_arfcn_range_encode.h \
 	gsm48_ie.h \
diff --git a/include/osmocom/gsm/gsm44021.h b/include/osmocom/gsm/gsm44021.h
new file mode 100644
index 0000000..8f89c56
--- /dev/null
+++ b/include/osmocom/gsm/gsm44021.h
@@ -0,0 +1,8 @@
+#pragma once
+#include <osmocom/isdn/v110.h>
+
+int osmo_csd_12k_6k_decode_frame(struct osmo_v110_decoded_frame *fr, const ubit_t *ra_bits, size_t n_bits);
+int osmo_csd_12k_6k_encode_frame(ubit_t *ra_bits, size_t ra_bits_size, const struct osmo_v110_decoded_frame *fr);
+int osmo_csd_3k6_decode_frame(struct osmo_v110_decoded_frame *fr, const ubit_t *ra_bits, size_t n_bits);
+int osmo_csd_3k6_encode_frame(ubit_t *ra_bits, size_t ra_bits_size, const struct osmo_v110_decoded_frame *fr);
+void osmo_csd_ubit_dump(FILE *outf, const ubit_t *fr, size_t in_len);
diff --git a/src/gsm/Makefile.am b/src/gsm/Makefile.am
index 79c5f04..ce0b1f4 100644
--- a/src/gsm/Makefile.am
+++ b/src/gsm/Makefile.am
@@ -33,7 +33,7 @@
 			gsup.c gsup_sms.c gprs_gea.c gsm0503_conv.c oap.c gsm0808_utils.c \
 			gsm23003.c gsm23236.c mncc.c bts_features.c oap_client.c \
 			gsm29118.c gsm48_rest_octets.c cbsp.c gsm48049.c \
-			gad.c bsslap.c bssmap_le.c kdf.c iuup.c
+			gad.c bsslap.c bssmap_le.c kdf.c iuup.c gsm44021.c
 
 libgsmint_la_LDFLAGS = -no-undefined
 libgsmint_la_LIBADD = $(top_builddir)/src/core/libosmocore.la $(top_builddir)/src/isdn/libosmoisdn.la
diff --git a/src/gsm/gsm44021.c b/src/gsm/gsm44021.c
new file mode 100644
index 0000000..5e9d5ab
--- /dev/null
+++ b/src/gsm/gsm44021.c
@@ -0,0 +1,303 @@
+/*************************************************************************
+ * GSM CSD modified V.110 frame decoding/encoding (ubits <-> struct with D/S/X/E bits)
+ *************************************************************************/
+
+/* (C) 2022 by Harald Welte <laforge@osmocom.org>
+ *
+ * SPDX-License-Identifier: GPL-2.0+
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ */
+
+#include <errno.h>
+#include <osmocom/core/bits.h>
+#include <osmocom/isdn/v110.h>
+
+/*! Decode a 60-bit GSM 12kbit/s CSD frame present as 60 ubits into a struct osmo_v110_decoded_frame.
+ *  \param[out] caller-allocated output data structure, filled by this function
+ *  \param[in] ra_bits One V.110 frame as 60 unpacked bits. */
+int osmo_csd_12k_6k_decode_frame(struct osmo_v110_decoded_frame *fr, const ubit_t *ra_bits, size_t n_bits)
+{
+	/* 3GPP TS 44.021 Section 8.1.2 / 8.1.3
+	D1	D2	D3	D4	D5	D6	S1
+	D7	D8	D9	D10	D11	D12	X
+	D13	D14	D15	D16	D17	D18	S3
+	D19	D20	D21	D22	D23	D24	S4
+	E4	E5	E6	E7	D25	D26	D27
+	D28	D29	D30	S6	D31	D32	D33
+	D34	D35	D36	X	D37	D38	D39
+	D40	D41	D42	S8	D43	D44	D45
+	D46	D47	D48	S9 */
+
+	if (n_bits < 60)
+		return -EINVAL;
+
+	/* X1 .. X2 */
+	fr->x_bits[0] = ra_bits[1 * 7 + 6];
+	fr->x_bits[1] = ra_bits[6 * 7 + 3];
+
+	/* S1, S3, S4, S6, S8, S9 */
+	fr->s_bits[0] = ra_bits[0 * 7 + 6];
+	fr->s_bits[2] = ra_bits[2 * 7 + 6];
+	fr->s_bits[3] = ra_bits[3 * 7 + 6];
+	fr->s_bits[5] = ra_bits[5 * 7 + 3];
+	fr->s_bits[7] = ra_bits[7 * 7 + 3];
+	fr->s_bits[8] = ra_bits[8 * 7 + 3];
+
+	/* E1 .. E3 must be set by out-of-band knowledge! */
+
+	/* E4 .. E7 */
+	memcpy(fr->e_bits+3, ra_bits + 4 * 7 + 0, 4);
+
+	/* D-bits */
+	memcpy(fr->d_bits + 0 * 6 + 0, ra_bits + 0 * 7 + 0, 6);
+	memcpy(fr->d_bits + 1 * 6 + 0, ra_bits + 1 * 7 + 0, 6);
+	memcpy(fr->d_bits + 2 * 6 + 0, ra_bits + 2 * 7 + 0, 6);
+	memcpy(fr->d_bits + 3 * 6 + 0, ra_bits + 3 * 7 + 0, 6);
+	memcpy(fr->d_bits + 4 * 6 + 0, ra_bits + 4 * 7 + 4, 3);
+	memcpy(fr->d_bits + 4 * 6 + 3, ra_bits + 5 * 7 + 0, 3);
+	memcpy(fr->d_bits + 5 * 6 + 0, ra_bits + 5 * 7 + 4, 3);
+	memcpy(fr->d_bits + 5 * 6 + 3, ra_bits + 6 * 7 + 0, 3);
+	memcpy(fr->d_bits + 6 * 6 + 0, ra_bits + 6 * 7 + 4, 3);
+	memcpy(fr->d_bits + 6 * 6 + 3, ra_bits + 7 * 7 + 0, 3);
+	memcpy(fr->d_bits + 7 * 6 + 0, ra_bits + 7 * 7 + 4, 3);
+	memcpy(fr->d_bits + 7 * 6 + 3, ra_bits + 8 * 7 + 0, 3);
+
+	return 0;
+}
+
+int osmo_csd_12k_6k_encode_frame(ubit_t *ra_bits, size_t ra_bits_size, const struct osmo_v110_decoded_frame *fr)
+{
+	if (ra_bits_size < 60)
+		return -EINVAL;
+
+	/* X1 .. X2 */
+	ra_bits[1 * 7 + 6] = fr->x_bits[0];
+	ra_bits[6 * 7 + 3] = fr->x_bits[1];
+
+	/* S1, S3, S4, S6, S8, S9 */
+	ra_bits[0 * 7 + 6] = fr->s_bits[0];
+	ra_bits[2 * 7 + 6] = fr->s_bits[2];
+	ra_bits[3 * 7 + 6] = fr->s_bits[3];
+	ra_bits[5 * 7 + 3] = fr->s_bits[5];
+	ra_bits[7 * 7 + 3] = fr->s_bits[7];
+	ra_bits[8 * 7 + 3] = fr->s_bits[8];
+
+	/* E1 .. E3 are dropped */
+
+	/* E4 .. E7 */
+	memcpy(ra_bits + 4 * 7 + 0, fr->e_bits+3, 4);
+
+	/* D-bits */
+	memcpy(ra_bits + 0 * 7 + 0, fr->d_bits + 0 * 6 + 0, 6);
+	memcpy(ra_bits + 1 * 7 + 0, fr->d_bits + 1 * 6 + 0, 6);
+	memcpy(ra_bits + 2 * 7 + 0, fr->d_bits + 2 * 6 + 0, 6);
+	memcpy(ra_bits + 3 * 7 + 0, fr->d_bits + 3 * 6 + 0, 6);
+	memcpy(ra_bits + 4 * 7 + 4, fr->d_bits + 4 * 6 + 0, 3);
+	memcpy(ra_bits + 5 * 7 + 0, fr->d_bits + 4 * 6 + 3, 3);
+	memcpy(ra_bits + 5 * 7 + 4, fr->d_bits + 5 * 6 + 0, 3);
+	memcpy(ra_bits + 6 * 7 + 0, fr->d_bits + 5 * 6 + 3, 3);
+	memcpy(ra_bits + 6 * 7 + 4, fr->d_bits + 6 * 6 + 0, 3);
+	memcpy(ra_bits + 7 * 7 + 0, fr->d_bits + 6 * 6 + 3, 3);
+	memcpy(ra_bits + 7 * 7 + 4, fr->d_bits + 7 * 6 + 0, 3);
+	memcpy(ra_bits + 8 * 7 + 0, fr->d_bits + 7 * 6 + 3, 3);
+
+	return 60;
+}
+
+/*! Decode a 36-bit GSM 3k6kbit/s CSD frame present as 36 ubits into a struct osmo_v110_decoded_frame.
+ *  \param[out] caller-allocated output data structure, filled by this function
+ *  \param[in] ra_bits One V.110 frame as 36 unpacked bits. */
+int osmo_csd_3k6_decode_frame(struct osmo_v110_decoded_frame *fr, const ubit_t *ra_bits, size_t n_bits)
+{
+
+	/* 3GPP TS 44.021 Section 8.1.4
+	D1	D2	D3	S1	D4	D5	D6	X
+	D7	D8	D9	S3	D10	D11	D12	S4
+	E4	E5	E6	E7	D13	D14	D15	S6
+	D16	D17	D18	X	D19	D20	D21	S8
+	D22	D23	D24	S9
+	*/
+
+	if (n_bits < 36)
+		return -EINVAL;
+
+	/* X1 .. X2 */
+	fr->x_bits[0] = ra_bits[0 * 8 + 7];
+	fr->x_bits[1] = ra_bits[3 * 8 + 3];
+
+	/* S1, S3, S4, S6, S8, S9 */
+	fr->s_bits[0] = ra_bits[0 * 8 + 3];
+	fr->s_bits[2] = ra_bits[1 * 8 + 3];
+	fr->s_bits[3] = ra_bits[1 * 8 + 7];
+	fr->s_bits[5] = ra_bits[2 * 8 + 7];
+	fr->s_bits[7] = ra_bits[3 * 8 + 7];
+	fr->s_bits[8] = ra_bits[4 * 8 + 3];
+
+	/* E1 .. E3 must be set by out-of-band knowledge! */
+
+	/* E4 .. E7 */
+	memcpy(fr->e_bits+3, ra_bits + 2 * 8 + 0, 4);
+
+	/* D-bits */
+	unsigned int d_idx = 0;
+	fr->d_bits[d_idx++] = ra_bits[0 * 8 + 0];	/* D1 */
+	fr->d_bits[d_idx++] = ra_bits[0 * 8 + 0];	/* D1 */
+	fr->d_bits[d_idx++] = ra_bits[0 * 8 + 1];	/* D2 */
+	fr->d_bits[d_idx++] = ra_bits[0 * 8 + 1];	/* D2 */
+	fr->d_bits[d_idx++] = ra_bits[0 * 8 + 2];	/* D3 */
+	fr->d_bits[d_idx++] = ra_bits[0 * 8 + 2];	/* D3 */
+	fr->d_bits[d_idx++] = ra_bits[0 * 8 + 4];	/* D4 */
+	fr->d_bits[d_idx++] = ra_bits[0 * 8 + 4];	/* D4 */
+	fr->d_bits[d_idx++] = ra_bits[0 * 8 + 5];	/* D5 */
+	fr->d_bits[d_idx++] = ra_bits[0 * 8 + 5];	/* D5 */
+	fr->d_bits[d_idx++] = ra_bits[0 * 8 + 6];	/* D6 */
+	fr->d_bits[d_idx++] = ra_bits[0 * 8 + 6];	/* D6 */
+
+	fr->d_bits[d_idx++] = ra_bits[1 * 8 + 0];	/* D7 */
+	fr->d_bits[d_idx++] = ra_bits[1 * 8 + 0];	/* D7 */
+	fr->d_bits[d_idx++] = ra_bits[1 * 8 + 1];	/* D8 */
+	fr->d_bits[d_idx++] = ra_bits[1 * 8 + 1];	/* D8 */
+	fr->d_bits[d_idx++] = ra_bits[1 * 8 + 2];	/* D9 */
+	fr->d_bits[d_idx++] = ra_bits[1 * 8 + 2];	/* D9 */
+	fr->d_bits[d_idx++] = ra_bits[1 * 8 + 4];	/* D10 */
+	fr->d_bits[d_idx++] = ra_bits[1 * 8 + 4];	/* D10 */
+	fr->d_bits[d_idx++] = ra_bits[1 * 8 + 5];	/* D11 */
+	fr->d_bits[d_idx++] = ra_bits[1 * 8 + 5];	/* D11 */
+	fr->d_bits[d_idx++] = ra_bits[1 * 8 + 6];	/* D12 */
+	fr->d_bits[d_idx++] = ra_bits[1 * 8 + 6];	/* D12 */
+
+	fr->d_bits[d_idx++] = ra_bits[2 * 8 + 4];	/* D13 */
+	fr->d_bits[d_idx++] = ra_bits[2 * 8 + 4];	/* D13 */
+	fr->d_bits[d_idx++] = ra_bits[2 * 8 + 5];	/* D14 */
+	fr->d_bits[d_idx++] = ra_bits[2 * 8 + 5];	/* D14 */
+	fr->d_bits[d_idx++] = ra_bits[2 * 8 + 6];	/* D15 */
+	fr->d_bits[d_idx++] = ra_bits[2 * 8 + 6];	/* D15 */
+
+	fr->d_bits[d_idx++] = ra_bits[3 * 8 + 0];	/* D16 */
+	fr->d_bits[d_idx++] = ra_bits[3 * 8 + 0];	/* D16 */
+	fr->d_bits[d_idx++] = ra_bits[3 * 8 + 1];	/* D17 */
+	fr->d_bits[d_idx++] = ra_bits[3 * 8 + 1];	/* D17 */
+	fr->d_bits[d_idx++] = ra_bits[3 * 8 + 2];	/* D18 */
+	fr->d_bits[d_idx++] = ra_bits[3 * 8 + 2];	/* D18 */
+	fr->d_bits[d_idx++] = ra_bits[3 * 8 + 4];	/* D19 */
+	fr->d_bits[d_idx++] = ra_bits[3 * 8 + 4];	/* D19 */
+	fr->d_bits[d_idx++] = ra_bits[3 * 8 + 5];	/* D20 */
+	fr->d_bits[d_idx++] = ra_bits[3 * 8 + 5];	/* D20 */
+	fr->d_bits[d_idx++] = ra_bits[3 * 8 + 6];	/* D21 */
+	fr->d_bits[d_idx++] = ra_bits[3 * 8 + 6];	/* D21 */
+
+	fr->d_bits[d_idx++] = ra_bits[4 * 8 + 0];	/* D22 */
+	fr->d_bits[d_idx++] = ra_bits[4 * 8 + 0];	/* D22 */
+	fr->d_bits[d_idx++] = ra_bits[4 * 8 + 1];	/* D23 */
+	fr->d_bits[d_idx++] = ra_bits[4 * 8 + 1];	/* D23 */
+	fr->d_bits[d_idx++] = ra_bits[4 * 8 + 2];	/* D24 */
+	fr->d_bits[d_idx++] = ra_bits[4 * 8 + 2];	/* D24 */
+
+	OSMO_ASSERT(d_idx == 48);
+
+	return 0;
+}
+
+int osmo_csd_3k6_encode_frame(ubit_t *ra_bits, size_t ra_bits_size, const struct osmo_v110_decoded_frame *fr)
+{
+	if (ra_bits_size < 36)
+		return -EINVAL;
+
+	/* X1 .. X2 */
+	ra_bits[0 * 8 + 7] = fr->x_bits[0];
+	ra_bits[3 * 8 + 3] = fr->x_bits[1];
+
+	/* S1, S3, S4, S6, S8, S9 */
+	ra_bits[0 * 8 + 3] = fr->s_bits[0];
+	ra_bits[1 * 8 + 3] = fr->s_bits[2];
+	ra_bits[1 * 8 + 7] = fr->s_bits[3];
+	ra_bits[2 * 8 + 7] = fr->s_bits[5];
+	ra_bits[3 * 8 + 7] = fr->s_bits[7];
+	ra_bits[4 * 8 + 3] = fr->s_bits[8];
+
+	/* E1 .. E3 are ignored */
+
+	/* E4 .. E7 */
+	memcpy(ra_bits + 2 * 8 + 0, fr->e_bits+3, 4);
+
+	/* D-bits */
+	unsigned int d_idx = 0;
+	ra_bits[0 * 8 + 0] = fr->d_bits[d_idx]; d_idx += 2;
+	ra_bits[0 * 8 + 1] = fr->d_bits[d_idx]; d_idx += 2;
+	ra_bits[0 * 8 + 2] = fr->d_bits[d_idx]; d_idx += 2;
+	ra_bits[0 * 8 + 4] = fr->d_bits[d_idx]; d_idx += 2;
+	ra_bits[0 * 8 + 5] = fr->d_bits[d_idx]; d_idx += 2;
+	ra_bits[0 * 8 + 6] = fr->d_bits[d_idx]; d_idx += 2;
+
+	ra_bits[1 * 8 + 0] = fr->d_bits[d_idx]; d_idx += 2;
+	ra_bits[1 * 8 + 1] = fr->d_bits[d_idx]; d_idx += 2;
+	ra_bits[1 * 8 + 2] = fr->d_bits[d_idx]; d_idx += 2;
+	ra_bits[1 * 8 + 4] = fr->d_bits[d_idx]; d_idx += 2;
+	ra_bits[1 * 8 + 5] = fr->d_bits[d_idx]; d_idx += 2;
+	ra_bits[1 * 8 + 6] = fr->d_bits[d_idx]; d_idx += 2;
+
+	ra_bits[2 * 8 + 4] = fr->d_bits[d_idx]; d_idx += 2;
+	ra_bits[2 * 8 + 5] = fr->d_bits[d_idx]; d_idx += 2;
+	ra_bits[2 * 8 + 6] = fr->d_bits[d_idx]; d_idx += 2;
+
+	ra_bits[3 * 8 + 0] = fr->d_bits[d_idx]; d_idx += 2;
+	ra_bits[3 * 8 + 1] = fr->d_bits[d_idx]; d_idx += 2;
+	ra_bits[3 * 8 + 2] = fr->d_bits[d_idx]; d_idx += 2;
+	ra_bits[3 * 8 + 4] = fr->d_bits[d_idx]; d_idx += 2;
+	ra_bits[3 * 8 + 5] = fr->d_bits[d_idx]; d_idx += 2;
+	ra_bits[3 * 8 + 6] = fr->d_bits[d_idx]; d_idx += 2;
+
+	ra_bits[4 * 8 + 0] = fr->d_bits[d_idx]; d_idx += 2;
+	ra_bits[4 * 8 + 1] = fr->d_bits[d_idx]; d_idx += 2;
+	ra_bits[4 * 8 + 2] = fr->d_bits[d_idx]; d_idx += 2;
+
+	OSMO_ASSERT(d_idx == 48);
+
+	return 36;
+}
+
+/*! Print a encoded "CSD modififed V.110" frame in the same table-like structure as the spec.
+ *  \param outf output FILE stream to which to dump
+ *  \param[in] fr unpacked bits to dump
+ *  \param[in] in_len length of unpacked bits available at fr. */
+void osmo_csd_ubit_dump(FILE *outf, const ubit_t *fr, size_t in_len)
+{
+	switch (in_len) {
+	case 60:
+		for (unsigned int septet = 0; septet < 9; septet++) {
+			if (septet < 8) {
+				fprintf(outf, "%d\t%d\t%d\t%d\t%d\t%d\t%d\n", fr[septet * 7 + 0],
+					fr[septet * 7 + 1], fr[septet * 7 + 2], fr[septet * 7 + 3],
+					fr[septet * 7 + 4], fr[septet * 7 + 5], fr[septet*7 + 6]);
+			} else {
+				fprintf(outf, "%d\t%d\t%d\t%d\n", fr[septet * 7 + 0],
+					fr[septet * 7 + 1], fr[septet * 7 + 2], fr[septet * 7 + 3]);
+			}
+		}
+		break;
+	case 36:
+		for (unsigned int octet = 0; octet < 5; octet++) {
+			if (octet < 4) {
+				fprintf(outf, "%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\n",
+					fr[octet * 8 + 0], fr[octet * 8 + 1], fr[octet * 8 + 2],
+					fr[octet * 8 + 3], fr[octet * 8 + 4], fr[octet * 8 + 5],
+					fr[octet * 8 + 6], fr[octet * 8 + 7]);
+			} else {
+				fprintf(outf, "%d\t%d\t%d\t%d\n", fr[octet * 8 + 0],
+					fr[octet * 8 + 1], fr[octet * 8 + 2], fr[octet * 8 + 3]);
+			}
+		}
+		break;
+	default:
+		fprintf(outf, "invalid input data length: %zu\n", in_len);
+	}
+}
diff --git a/src/gsm/libosmogsm.map b/src/gsm/libosmogsm.map
index 6ad363f..6795c57 100644
--- a/src/gsm/libosmogsm.map
+++ b/src/gsm/libosmogsm.map
@@ -791,5 +791,11 @@
 osmo_iuup_rnl_prim_alloc;
 osmo_iuup_tnl_prim_alloc;
 
+osmo_csd_12k_6k_decode_frame;
+osmo_csd_12k_6k_encode_frame;
+osmo_csd_3k6_decode_frame;
+osmo_csd_3k6_encode_frame;
+osmo_csd_ubit_dump;
+
 local: *;
 };
diff --git a/tests/Makefile.am b/tests/Makefile.am
index f0c80d5..275cf5f 100644
--- a/tests/Makefile.am
+++ b/tests/Makefile.am
@@ -52,6 +52,7 @@
 		 auth/xor2g_test                                        \
 		 v110/test_frame                                        \
 		 v110/test_ra1                                          \
+		 gsm44021/test_frame_csd                                \
 		 $(NULL)
 
 if ENABLE_MSGFILE
@@ -347,6 +348,11 @@
 v110_test_ra1_SOURCES = v110/test_ra1.c
 v110_test_ra1_LDADD = $(LDADD) $(top_builddir)/src/isdn/libosmoisdn.la
 
+gsm44021_test_frame_csd_SOURCES = gsm44021/test_frame_csd.c
+gsm44021_test_frame_csd_LDADD = $(LDADD) $(top_builddir)/src/isdn/libosmoisdn.la \
+				$(top_builddir)/src/gsm/libosmogsm.la
+
+
 # The `:;' works around a Bash 3.2 bug when the output is not writeable.
 $(srcdir)/package.m4: $(top_srcdir)/configure.ac
 	:;{ \
@@ -447,6 +453,7 @@
 	     smscb/cbsp_test.ok \
 	     v110/test_frame.ok \
 	     v110/test_ra1.ok \
+	     gsm44021/test_frame_csd.ok \
 	     $(NULL)
 
 if ENABLE_LIBSCTP
@@ -654,6 +661,8 @@
 		>$(srcdir)/v110/test_frame.ok
 	v110/test_ra1 \
 		>$(srcdir)/v110/test_ra1.ok
+	gsm44021/test_frame_csd \
+		>$(srcdir)/gsm44021/test_frame_csd.ok
 
 check-local: atconfig $(TESTSUITE)
 	[ -e /proc/cpuinfo ] && cat /proc/cpuinfo
diff --git a/tests/gsm44021/test_frame_csd.c b/tests/gsm44021/test_frame_csd.c
new file mode 100644
index 0000000..98efdac
--- /dev/null
+++ b/tests/gsm44021/test_frame_csd.c
@@ -0,0 +1,93 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <unistd.h>
+
+#include <osmocom/core/bits.h>
+#include <osmocom/isdn/v110.h>
+#include <osmocom/gsm/gsm44021.h>
+
+
+static void fill_v110_frame(struct osmo_v110_decoded_frame *fr)
+{
+	unsigned int i;
+
+	memset(fr, 0, sizeof(*fr));
+
+	/* we abuse the fact that ubit_t is 8bit so we can actually
+	 * store integer values to clearly identify which bit ends up where */
+
+	/* D1..D48: 101..148 */
+	for (i = 0; i < ARRAY_SIZE(fr->d_bits); i++)
+		fr->d_bits[i] = 101 + i;
+	/* E1..E7: 201..207 */
+	for (i = 0; i < ARRAY_SIZE(fr->e_bits); i++)
+		fr->e_bits[i] = 201 + i;
+	/* S1..S9: 211..219 */
+	for (i = 0; i < ARRAY_SIZE(fr->s_bits); i++)
+		fr->s_bits[i] = 211 + i;
+	/* X1..X2: 221..222 */
+	for (i = 0; i < ARRAY_SIZE(fr->x_bits); i++)
+		fr->x_bits[i] = 221 + i;
+}
+
+
+static void test_frame_enc_12k_6k(void)
+{
+	struct osmo_v110_decoded_frame fr;
+	ubit_t bits[60];
+
+	printf("Testing Frame Encoding for 12k/6k radio interface rate\n");
+
+	fill_v110_frame(&fr);
+
+	/* run encoder and dump to stdout */
+	memset(bits, 0xff, sizeof(bits));
+	osmo_csd_12k_6k_encode_frame(bits, sizeof(bits), &fr);
+	osmo_csd_ubit_dump(stdout, bits, sizeof(bits));
+
+	/* run decoder on what we just encoded */
+	memset(&fr, 0, sizeof(fr));
+	osmo_csd_12k_6k_decode_frame(&fr, bits, sizeof(bits));
+
+	/* re-encode and dump again 'expout' will match it. */
+	memset(bits, 0xff, sizeof(bits));
+	osmo_csd_12k_6k_encode_frame(bits, sizeof(bits), &fr);
+	osmo_csd_ubit_dump(stdout, bits, sizeof(bits));
+}
+
+static void test_frame_enc_3k6(void)
+{
+	struct osmo_v110_decoded_frame fr;
+	ubit_t bits[36];
+
+	printf("Testing Frame Encoding for 3.6k radio interface rate\n");
+
+	fill_v110_frame(&fr);
+	/* different D-bit numbering for 3k6, see TS 44.021 Section 8.1.4 */
+	for (unsigned int i = 0; i < ARRAY_SIZE(fr.d_bits); i++)
+		fr.d_bits[i] = 101 + i/2;
+
+	/* run encoder and dump to stdout */
+	memset(bits, 0xff, sizeof(bits));
+	osmo_csd_3k6_encode_frame(bits, sizeof(bits), &fr);
+	osmo_csd_ubit_dump(stdout, bits, sizeof(bits));
+
+	/* run decoder on what we just encoded */
+	memset(&fr, 0, sizeof(fr));
+	osmo_csd_3k6_decode_frame(&fr, bits, sizeof(bits));
+
+	/* re-encode and dump again 'expout' will match it. */
+	memset(bits, 0xff, sizeof(bits));
+	osmo_csd_3k6_encode_frame(bits, sizeof(bits), &fr);
+	osmo_csd_ubit_dump(stdout, bits, sizeof(bits));
+}
+
+
+int main(int argc, char **argv)
+{
+	test_frame_enc_12k_6k();
+	printf("\n");
+	test_frame_enc_3k6();
+}
+
diff --git a/tests/gsm44021/test_frame_csd.ok b/tests/gsm44021/test_frame_csd.ok
new file mode 100644
index 0000000..1a5d3f2
--- /dev/null
+++ b/tests/gsm44021/test_frame_csd.ok
@@ -0,0 +1,31 @@
+Testing Frame Encoding for 12k/6k radio interface rate
+101	102	103	104	105	106	211
+107	108	109	110	111	112	221
+113	114	115	116	117	118	213
+119	120	121	122	123	124	214
+204	205	206	207	125	126	127
+128	129	130	216	131	132	133
+134	135	136	222	137	138	139
+140	141	142	218	143	144	145
+146	147	148	219
+101	102	103	104	105	106	211
+107	108	109	110	111	112	221
+113	114	115	116	117	118	213
+119	120	121	122	123	124	214
+204	205	206	207	125	126	127
+128	129	130	216	131	132	133
+134	135	136	222	137	138	139
+140	141	142	218	143	144	145
+146	147	148	219
+
+Testing Frame Encoding for 3.6k radio interface rate
+101	102	103	211	104	105	106	221
+107	108	109	213	110	111	112	214
+204	205	206	207	113	114	115	216
+116	117	118	222	119	120	121	218
+122	123	124	219
+101	102	103	211	104	105	106	221
+107	108	109	213	110	111	112	214
+204	205	206	207	113	114	115	216
+116	117	118	222	119	120	121	218
+122	123	124	219
diff --git a/tests/testsuite.at b/tests/testsuite.at
index 39cf54a..60aa74d 100644
--- a/tests/testsuite.at
+++ b/tests/testsuite.at
@@ -502,3 +502,9 @@
 cat $abs_srcdir/v110/test_ra1.ok > expout
 AT_CHECK([$abs_top_builddir/tests/v110/test_ra1], [], [expout],[])
 AT_CLEANUP
+
+AT_SETUP([gsm44021_test_frame_csd])
+AT_KEYWORDS([gsm44021_test_frame_csd])
+cat $abs_srcdir/gsm44021/test_frame_csd.ok > expout
+AT_CHECK([$abs_top_builddir/tests/gsm44021/test_frame_csd], [], [expout],[])
+AT_CLEANUP