--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/libgsmfr2/comfort_noise.c	Fri Apr 12 20:49:53 2024 +0000
@@ -0,0 +1,111 @@
+/*
+ * In this module we implement comfort noise generation per GSM 06.12
+ * or 3GPP TS 46.012.
+ */
+
+#include <stdint.h>
+#include <string.h>
+#include "tw_gsmfr.h"
+#include "pp_internal.h"
+
+static const uint8_t fold_table_8to6[24] = {
+	1, 2, 3, 4, 5, 6, 1, 2,
+	1, 2, 3, 4, 5, 6, 3, 4,
+	1, 2, 3, 4, 5, 6, 5, 6,
+};
+
+static const uint8_t bc[4] = {0, 0, 0, 0};
+static const uint8_t Nc[4] = {40, 120, 40, 120};
+
+static uint8_t random_1to6(struct gsmfr_preproc_state *st)
+{
+	uint8_t range8, range6;
+
+	range8 = gsmfr_preproc_prng(st, 3);
+	range6 = fold_table_8to6[(st->cn_random_6fold << 3) | range8];
+	st->cn_random_6fold++;
+	if (st->cn_random_6fold >= 3)
+		st->cn_random_6fold = 0;
+	return range6;
+}
+
+void gsmfr_preproc_gen_cn(struct gsmfr_preproc_state *st, uint8_t *frame)
+{
+	unsigned sub, pulse;
+	uint8_t Mc, xmc[13];
+	uint8_t *c;
+
+	/* global bytes (magic and LARc) are fixed */
+	memcpy(frame, st->sid_prefix, 5);
+	c = frame + 5;
+	/* now do the 4 subframes, mostly PRNG output */
+	for (sub = 0; sub < 4; sub++) {
+		Mc = gsmfr_preproc_prng(st, 2);
+		for (pulse = 0; pulse < 13; pulse++)
+			xmc[pulse] = random_1to6(st);
+		/* packing code from libgsm */
+		*c++ =   ((Nc[sub] & 0x7F) << 1)
+		       | ((bc[sub] >> 1) & 0x1);
+		*c++ =   ((bc[sub] & 0x1) << 7)
+		       | ((Mc & 0x3) << 5)
+		       | ((st->sid_xmaxc >> 1) & 0x1F);
+		*c++ =   ((st->sid_xmaxc & 0x1) << 7)
+		       | ((xmc[0] & 0x7) << 4)
+		       | ((xmc[1] & 0x7) << 1)
+		       | ((xmc[2] >> 2) & 0x1);
+		*c++ =   ((xmc[2] & 0x3) << 6)
+		       | ((xmc[3] & 0x7) << 3)
+		       | (xmc[4] & 0x7);
+		*c++ =   ((xmc[5] & 0x7) << 5)
+		       | ((xmc[6] & 0x7) << 2)
+		       | ((xmc[7] >> 1) & 0x3);
+		*c++ =   ((xmc[7] & 0x1) << 7)
+		       | ((xmc[8] & 0x7) << 4)
+		       | ((xmc[9] & 0x7) << 1)
+		       | ((xmc[10] >> 2) & 0x1);
+		*c++ =   ((xmc[10] & 0x3) << 6)
+		       | ((xmc[11] & 0x7) << 3)
+		       | (xmc[12] & 0x7);
+	}
+}
+
+void gsmfr_preproc_sid2cn(struct gsmfr_preproc_state *st, uint8_t *frame)
+{
+	unsigned sub, pulse;
+	uint8_t Mc, xmc[13];
+	uint8_t *c;
+
+	/* save LARc and Xmaxc from the last subframe for subsequent CN gen */
+	memcpy(st->sid_prefix, frame, 5);
+	st->sid_xmaxc = ((frame[27] & 0x1F) << 1) | (frame[28] >> 7);
+	/* ... and turn *this* frame into very first CN output */
+	c = frame + 5;
+	for (sub = 0; sub < 4; sub++) {
+		Mc = gsmfr_preproc_prng(st, 2);
+		for (pulse = 0; pulse < 13; pulse++)
+			xmc[pulse] = random_1to6(st);
+		/* keep each of Xmaxc and replace the rest with CN */
+		*c++ =   ((Nc[sub] & 0x7F) << 1)
+		       | ((bc[sub] >> 1) & 0x1);
+		*c &= 0x1F;
+		*c++ |=  ((bc[sub] & 0x1) << 7)
+		       | ((Mc & 0x3) << 5);
+		*c &= 0x80;
+		*c++ |=  ((xmc[0] & 0x7) << 4)
+		       | ((xmc[1] & 0x7) << 1)
+		       | ((xmc[2] >> 2) & 0x1);
+		*c++ =   ((xmc[2] & 0x3) << 6)
+		       | ((xmc[3] & 0x7) << 3)
+		       | (xmc[4] & 0x7);
+		*c++ =   ((xmc[5] & 0x7) << 5)
+		       | ((xmc[6] & 0x7) << 2)
+		       | ((xmc[7] >> 1) & 0x3);
+		*c++ =   ((xmc[7] & 0x1) << 7)
+		       | ((xmc[8] & 0x7) << 4)
+		       | ((xmc[9] & 0x7) << 1)
+		       | ((xmc[10] >> 2) & 0x1);
+		*c++ =   ((xmc[10] & 0x3) << 6)
+		       | ((xmc[11] & 0x7) << 3)
+		       | (xmc[12] & 0x7);
+	}
+}