/*
 * The function in this development-only module is a copy of
 * gsmfr_pack_from_array() from work-in-progress libgsmfr2, which in turn
 * was created to serve as a drop-in replacement for gsm_implode()
 * from classic libgsm.  The function is copied here to avoid the
 * philosophical problem of libgsmfr2 being a dependency for developer aid
 * programs used in the development of that library.
 */

#include <stdint.h>

#define	GSM_FR_MAGIC	0xD

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

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