/*
 * The utility function implemented in this module constructs a 33-byte
 * frame for loading into Calypso DSP TCH UL buffer from a standard
 * 244-bit EFR codec frame in the RTP encoding format of ETSI TS 101 318.
 */

#include <sys/types.h>
#include <string.h>
#include <strings.h>

static const u_short efr_to_tidsp_table[244] = {
	 50,  24,  25,  37,  38,  43,  44,  26,  42,  27,  39,  40,
	 45,  51,  52,  41,  46,  28,  29,  47,  53,  77,  78,  30,
	 54,  55,  79,  56,  80, 109, 110, 111, 112, 113, 114, 115,
	180, 181,   0,   1,   2,   3,   4,   5,  20,  31,  32,  16,
	 73, 105, 157,  81, 118, 138, 165,  82, 119, 139, 166,  83,
	120, 140, 167,  84, 121, 141, 168,  85, 122, 186, 200, 204,
	224, 244, 205, 225, 245, 206, 226, 246, 207, 227, 247, 208,
	228, 248,  17,  61, 101, 153, 161,  12,  13,  22,  35,  48,
	116,  18,  74, 106, 158,  86, 123, 142, 169,  87, 124, 143,
	170,  88, 125, 144, 171,  89, 126, 145, 172,  90, 127, 189,
	201, 209, 229, 249, 210, 230, 250, 211, 231, 251, 212, 232,
	252, 213, 233, 253,  19,  62, 102, 154, 162,   6,   7,   8,
	  9,  10,  11,  21,  33,  34,  57,  75, 107, 159,  91, 128,
	146, 173,  92, 129, 147, 174,  93, 130, 148, 175,  94, 131,
	149, 176,  95, 132, 192, 202, 214, 234, 254, 215, 235, 255,
	216, 236, 256, 217, 237, 257, 218, 238, 258,  59,  63, 103,
	155, 163,  14,  15,  23,  36,  49, 117,  58,  76, 108, 160,
	 96, 133, 150, 177,  97, 134, 198, 199,  98, 135, 151, 178,
	 99, 136, 152, 179, 100, 137, 195, 203, 219, 239, 259, 220,
	240, 260, 221, 241, 261, 222, 242, 262, 223, 243, 263,  60,
	 64, 104, 156, 164
};

static int
msb_get_bit(buf, bn)
	u_char *buf;
{
	int pos_byte = bn >> 3;
	int pos_bit  = 7 - (bn & 7);

	return (buf[pos_byte] >> pos_bit) & 1;
}

static void
msb_set_bit(buf, bn)
	u_char *buf;
{
	int pos_byte = bn >> 3;
	int pos_bit  = 7 - (bn & 7);

	buf[pos_byte] |= (1 << pos_bit);
}

void
efr_std_to_tidsp(inbytes, outbytes)
	u_char *inbytes, *outbytes;
{
	int i;

	bzero(outbytes, 33);
	for (i = 0; i < 244; i++) {
		if (msb_get_bit(inbytes, i + 4))
			msb_set_bit(outbytes, efr_to_tidsp_table[i]);
	}
	/* set repetition bits */
	if (outbytes[23] & 0x20)
		outbytes[23] |= 0x18;
	if (outbytes[23] & 0x04)
		outbytes[23] |= 0x03;
	if (outbytes[24] & 0x80)
		outbytes[24] |= 0x60;
	if (outbytes[24] & 0x10)
		outbytes[24] |= 0x0C;
}