/*
 * This module has been copied from WIP libgsmhr1: it holds the function that
 * unpacks a single GSM-HR codec frame from TS 101 318 packed format into
 * the codec's native array of parameters.
 */

#include <stdint.h>
#include "gsmhr_unpack.h"

void gsmhr_unpack_ts101318(const uint8_t *payload, int16_t *params)
{
	const uint8_t *c = payload;

	params[0]  = (*c >> 3) & 0x1F;
	params[1]  = (*c++ & 0x7) << 8;
	params[1] |= *c++;
	params[2]  = (*c++ & 0xFF) << 1;
	params[2] |= (*c >> 7) & 0x1;
	params[3]  = (*c++ & 0x7F) << 1;
	params[3] |= (*c >> 7) & 0x1;
	params[4]  = (*c >> 6) & 0x1;
	params[5]  = (*c >> 4) & 0x3;
	if (params[5]) {
		/* voiced modes */
		params[6]  = (*c++ & 0xF) << 4;
		params[6] |= (*c >> 4) & 0xF;
		params[7]  = (*c++ & 0xF) << 5;
		params[7] |= (*c >> 3) & 0x1F;
		params[8]  = (*c++ & 0x7) << 2;
		params[8] |= (*c >> 6) & 0x3;
		params[9]  = (*c >> 2) & 0xF;
		params[10]  = (*c++ & 0x3) << 7;
		params[10] |= (*c >> 1) & 0x7F;
		params[11]  = (*c++ & 0x1) << 4;
		params[11] |= (*c >> 4) & 0xF;
		params[12]  = *c++ & 0xF;
		params[13]  = (*c++ & 0xFF) << 1;
		params[13] |= (*c >> 7) & 0x1;
		params[14]  = (*c >> 2) & 0x1F;
		params[15]  = (*c++ & 0x3) << 2;
		params[15] |= (*c >> 6) & 0x3;
		params[16]  = (*c++ & 0x3F) << 3;
		params[16] |= (*c >> 5) & 0x7;
		params[17]  = *c++ & 0x1F;
	} else {
		/* unvoiced mode */
		params[6]  = (*c++ & 0xF) << 3;
		params[6] |= (*c >> 5) & 0x7;
		params[7]  = (*c++ & 0x1F) << 2;
		params[7] |= (*c >> 6) & 0x3;
		params[8]  = (*c >> 1) & 0x1F;
		params[9]  = (*c++ & 0x1) << 6;
		params[9] |= (*c >> 2) & 0x3F;
		params[10]  = (*c++ & 0x3) << 5;
		params[10] |= (*c >> 3) & 0x1F;
		params[11]  = (*c++ & 0x7) << 2;
		params[11] |= (*c >> 6) & 0x3;
		params[12]  = (*c++ & 0x3F) << 1;
		params[12] |= (*c >> 7) & 0x1;
		params[13]  = *c++ & 0x7F;
		params[14]  = (*c >> 3) & 0x1F;
		params[15]  = (*c++ & 0x7) << 4;
		params[15] |= (*c >> 4) & 0xF;
		params[16]  = (*c++ & 0xF) << 3;
		params[16] |= (*c >> 5) & 0x7;
		params[17]  = *c++ & 0x1F;
	}
}