/*
 * This module implements the EFR decoding part of trau-parse.
 */

#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include <strings.h>
#include <gsm_efr.h>
#include "osmo_bits.h"

/*
 * EFR TRAU parity
 *
 * g(x) = x^3 + x^1 + 1
 */
static const struct osmo_crc8gen_code gsm0860_efr_crc3 = {
	.bits = 3,
	.poly = 0x3,
	.init = 0x0,
	.remainder = 0x7,
};

/* re-combine EFR parity bits */
static inline void efr_parity_bits_1(ubit_t *check_bits, const ubit_t *d_bits)
{
	memcpy(check_bits + 0 , d_bits + 0, 22);
	memcpy(check_bits + 22 , d_bits + 24, 3);
	check_bits[25] = d_bits[28];
}

static inline void efr_parity_bits_2(ubit_t *check_bits, const ubit_t *d_bits)
{
	memcpy(check_bits + 0 , d_bits + 42, 10);
	memcpy(check_bits + 10 , d_bits + 90, 2);
}

static inline void efr_parity_bits_3(ubit_t *check_bits, const ubit_t *d_bits)
{
	memcpy(check_bits + 0 , d_bits + 98, 5);
	check_bits[5] = d_bits[104];
	memcpy(check_bits + 6 , d_bits + 143, 2);
}

static inline void efr_parity_bits_4(ubit_t *check_bits, const ubit_t *d_bits)
{
	memcpy(check_bits + 0 , d_bits + 151, 10);
	memcpy(check_bits + 10 , d_bits + 199, 2);
}

static inline void efr_parity_bits_5(ubit_t *check_bits, const ubit_t *d_bits)
{
	memcpy(check_bits + 0 , d_bits + 207, 5);
	check_bits[5] = d_bits[213];
	memcpy(check_bits + 6 , d_bits + 252, 2);
}

void
check_efr_crc(d_bits)
	ubit_t *d_bits;
{
	ubit_t check_bits[26];
	int rc1, rc2, rc3, rc4, rc5;

	efr_parity_bits_1(check_bits, d_bits);
	rc1 = osmo_crc8gen_check_bits(&gsm0860_efr_crc3, check_bits, 26,
					d_bits + 39);
	efr_parity_bits_2(check_bits, d_bits);
	rc2 = osmo_crc8gen_check_bits(&gsm0860_efr_crc3, check_bits, 12,
					d_bits + 95);
	efr_parity_bits_3(check_bits, d_bits);
	rc3 = osmo_crc8gen_check_bits(&gsm0860_efr_crc3, check_bits, 8,
					d_bits + 148);
	efr_parity_bits_4(check_bits, d_bits);
	rc4 = osmo_crc8gen_check_bits(&gsm0860_efr_crc3, check_bits, 12,
					d_bits + 204);
	efr_parity_bits_5(check_bits, d_bits);
	rc5 = osmo_crc8gen_check_bits(&gsm0860_efr_crc3, check_bits, 8,
					d_bits + 257);
	printf("  D1=%u CRC: %s %s %s %s %s\n", d_bits[0],
		rc1 ? "bad" : "good", rc2 ? "bad" : "good",
		rc3 ? "bad" : "good", rc4 ? "bad" : "good",
		rc5 ? "bad" : "good");
}

static void
dbits_to_frame(d_bits, frame)
	ubit_t *d_bits;
	uint8_t *frame;
{
	ubit_t intermed[248], *ip;
	uint8_t *op, mask;
	unsigned nb;

	intermed[0] = 1;
	intermed[1] = 1;
	intermed[2] = 0;
	intermed[3] = 0;
	bcopy(d_bits + 1, intermed + 4, 38);
	bcopy(d_bits + 42, intermed + 42, 53);
	bcopy(d_bits + 98, intermed + 95, 50);
	bcopy(d_bits + 151, intermed + 145, 53);
	bcopy(d_bits + 207, intermed + 198, 50);
	ip = intermed;
	op = frame;
	for (nb = 0; nb < EFR_RTP_FRAME_LEN; nb++) {
		*op = 0;
		for (mask = 0x80; mask; mask >>= 1) {
			if (*ip)
				*op |= mask;
			ip++;
		}
		op++;
	}
}

void
print_efr_frame(d_bits)
	ubit_t *d_bits;
{
	uint8_t frame[EFR_RTP_FRAME_LEN];
	int16_t params[EFR_NUM_PARAMS];
	int i, j, n, sid;

	dbits_to_frame(d_bits, frame);
	EFR_frame2params(frame, params);
	fputs("  EFR frame:\n    LPC", stdout);
	n = 0;
	for (i = 0; i < 5; i++)
		printf(" %d", params[n++]);
	putchar('\n');
	for (i = 0; i < 4; i++) {
		fputs("   ", stdout);
		for (j = 0; j < 13; j++)
			printf(" %d", params[n++]);
		putchar('\n');
	}
	sid = EFR_sid_classify(frame);
	printf("    SID recompute: %d\n", sid);
	if (!bcmp(frame, EFR_decoder_homing_frame, EFR_RTP_FRAME_LEN))
		puts("    Matches DHF");
}