/*
 * This module holds our gsmfr_pack_frame() function: packing a single GSM-FR
 * frame from a struct of broken-down parameters into libgsm/RTP format.
 */

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

#define	GSM_FR_MAGIC	0xD

void gsmfr_pack_frame(const struct gsmfr_param_frame *param, uint8_t *frame)
{
	uint8_t *c = frame;
	unsigned sub;

	*c++ =   (GSM_FR_MAGIC << 4)
	       | ((param->LARc[0] >> 2) & 0xF);
	*c++ =   ((param->LARc[0] & 0x3) << 6)
	       | (param->LARc[1] & 0x3F);
	*c++ =   ((param->LARc[2] & 0x1F) << 3)
	       | ((param->LARc[3] >> 2) & 0x7);
	*c++ =   ((param->LARc[3] & 0x3) << 6)
	       | ((param->LARc[4] & 0xF) << 2)
	       | ((param->LARc[5] >> 2) & 0x3);
	*c++ =   ((param->LARc[5] & 0x3) << 6)
	       | ((param->LARc[6] & 0x7) << 3)
	       | (param->LARc[7] & 0x7);
	for (sub = 0; sub < 4; sub++) {
		*c++ =   ((param->Nc[sub] & 0x7F) << 1)
		       | ((param->bc[sub] >> 1) & 0x1);
		*c++ =   ((param->bc[sub] & 0x1) << 7)
		       | ((param->Mc[sub] & 0x3) << 5)
		       | ((param->xmaxc[sub] >> 1) & 0x1F);
		*c++ =   ((param->xmaxc[sub] & 0x1) << 7)
		       | ((param->xMc[sub][0] & 0x7) << 4)
		       | ((param->xMc[sub][1] & 0x7) << 1)
		       | ((param->xMc[sub][2] >> 2) & 0x1);
		*c++ =   ((param->xMc[sub][2] & 0x3) << 6)
		       | ((param->xMc[sub][3] & 0x7) << 3)
		       | (param->xMc[sub][4] & 0x7);
		*c++ =   ((param->xMc[sub][5] & 0x7) << 5)
		       | ((param->xMc[sub][6] & 0x7) << 2)
		       | ((param->xMc[sub][7] >> 1) & 0x3);
		*c++ =   ((param->xMc[sub][7] & 0x1) << 7)
		       | ((param->xMc[sub][8] & 0x7) << 4)
		       | ((param->xMc[sub][9] & 0x7) << 1)
		       | ((param->xMc[sub][10] >> 2) & 0x1);
		*c++ =   ((param->xMc[sub][10] & 0x3) << 6)
		       | ((param->xMc[sub][11] & 0x7) << 3)
		       | (param->xMc[sub][12] & 0x7);
	}
}