/*
 * This module holds our gsmfr_unpack_to_array() function: a drop-in
 * replacement for gsm_explode() from classic libgsm.
 */

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

void gsmfr_unpack_to_array(const uint8_t *frame, int16_t *params)
{
	const uint8_t *c = frame;
	unsigned sub;

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