/*
 * In this module we implement comfort noise generation per GSM 06.12
 * or 3GPP TS 46.012.
 */

#include <stdint.h>
#include <string.h>
#include "gsm_fr_preproc.h"
#include "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};

/* pseudonoise() function is based on ETSI EFR code */

static uint16_t pseudonoise(struct gsmfr_preproc_state *st, uint16_t no_bits)
{
    uint16_t noise_bits, Sn, i;

    noise_bits = 0;
    for (i = 0; i < no_bits; i++)
    {
        /* State n == 31 */
        if ((st->cn_random_lfsr & 0x00000001L) != 0)
        {
            Sn = 1;
        }
        else
        {
            Sn = 0;
        }

        /* State n == 3 */
        if ((st->cn_random_lfsr & 0x10000000L) != 0)
        {
            Sn = Sn ^ 1;
        }
        else
        {
            Sn = Sn ^ 0;
        }

        noise_bits = noise_bits << 1;
        noise_bits = noise_bits | st->cn_random_lfsr & 1;

        st->cn_random_lfsr >>= 1;
        if (Sn & 1)
        {
            st->cn_random_lfsr |= 0x40000000L;
        }
    }

    return noise_bits;
}

static uint8_t random_1to6(struct gsmfr_preproc_state *st)
{
	uint8_t range8, range6;

	range8 = pseudonoise(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, gsm_byte *frame)
{
	unsigned sub, pulse;
	uint8_t Mc, xmc[13];
	gsm_byte *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 = pseudonoise(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[sub] >> 1) & 0x1F);
		*c++ =   ((st->sid_xmaxc[sub] & 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);
	}
}