/* NMT DMS (data service) processing
 *
 * (C) 2016 by Andreas Eversberg <jolly@eversberg.eu>
 * All Rights Reserved
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include "../common/debug.h"
#include "../common/timer.h"
#include "nmt.h"
#include "dsp.h"

#define MUTE_DURATION		0.300	/* 200ms, and about 95ms for the frame itself */

#define DMS_DOTTING		"101010101010101"
#define DMS_SYNC		"00101000111"

int dms_allow_loopback = 0;

/*
 * support
 */

/* calculate CRC from the bits of label and data and 16 zeroes.
 * the result is the remainder of the polynomial division and
 * conforms to DMS standard.
 */
static uint16_t crc16(uint8_t *bits, int len)
{
	uint16_t generator = 0x1021;
	uint16_t crc = 0; /* init crc register with 0 */
	int i;

	for (i = 0; i < len; i++) {
		/* check if MSB is set */
		if ((crc & 0x8000)) {   /* MSB set, shift it out of the register */
			/* shift in next bit of input stream */
			crc = (crc << 1) | bits[i];
			/* Perform the 'division' by XORing the crc register with the generator polynomial */
			crc = crc ^ generator;
		} else {   /* MSB not set, shift it out and shift in next bit of input stream. Same as above, just no division */
			crc = (crc << 1) | bits[i];
		}
	}

	return crc;
}

/*
 * frame handling
 */

/* print CT/DT frame in 8-bit or 7-bit mode. */
static const char *print_ct_dt(uint8_t s, uint8_t n, uint8_t *data, int eight_bits)
{
	static char text[128], *ct;

	if (s)
		ct = "    ";
	else switch (data[0]) {
	case 0:
		ct = "IDLE";
		break;
	case 73:
		ct = "ID  ";
		break;
	case 82:
		ct = "RAND";
		break;
	case 84:
		ct = "CT84";
		break;
	default:
		ct = "????";
	}

	if (!eight_bits || s == 0)
		sprintf(text, "%cT(%d) = %s %3d %3d %3d %3d %3d %3d %3d %3d", 'C' + s, n, ct, data[0], data[1], data[2], data[3], data[4], data[5], data[6], data[7]);
	else
		sprintf(text, "%cT(%d) = %s 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x", 'C' + s, n, ct,
			((data[0] << 1) & 0x80) | data[1],
			((data[0] << 2) & 0x80) | data[2],
			((data[0] << 3) & 0x80) | data[3],
			((data[0] << 4) & 0x80) | data[4],
			((data[0] << 5) & 0x80) | data[5],
			((data[0] << 6) & 0x80) | data[6],
			((data[0] << 7) & 0x80) | data[7]);

	return text;
}

/* link DMS frame to list of TX frames */
void link_dms_frame(nmt_t *nmt, struct dms_frame *frame)
{
	dms_t *dms = &nmt->dms;
	struct dms_frame **framep;

	PDEBUG(DDMS, DEBUG_DEBUG, "link DMS frame\n");

	/* attach to end of list */
	framep = &dms->state.frame_list;
	while (*framep)
		framep = &((*framep)->next);
	*framep = frame;
}

/* unlink DMS frame from list of TX frames */
void unlink_dms_frame(nmt_t *nmt, struct dms_frame *frame)
{
	dms_t *dms = &nmt->dms;
	struct dms_frame **framep;

	PDEBUG(DDMS, DEBUG_DEBUG, "unlink DMS frame\n");

	/* unlink */
	framep = &dms->state.frame_list;
	while (*framep && *framep != frame)
		framep = &((*framep)->next);
	if (!(*framep)) {
		PDEBUG(DTRANS, DEBUG_ERROR, "Frame not in list, please fix!!\n");
		abort();
	}
	*framep = frame->next;
}

/* add DMS frame to list of TX frames */
static void dms_frame_add(nmt_t *nmt, int s, const uint8_t *data)
{
	dms_t *dms = &nmt->dms;
	struct dms_frame *dms_frame;

	dms_frame = calloc(1, sizeof(*dms_frame));
	if (!dms_frame) {
		PDEBUG(DDMS, DEBUG_ERROR, "No memory!\n");
			return;
	}

	dms_frame->s = s;
	dms_frame->n = dms->state.n_count;
	dms->state.n_count = (dms->state.n_count + 1) & 7;
	memcpy(dms_frame->data, data, 8);

	PDEBUG(DDMS, DEBUG_DEBUG, "add DMS %cT(%d) frame to queue\n", dms_frame->s + 'C', dms_frame->n);

	link_dms_frame(nmt, dms_frame);
}

/* delete DMS frame from list of TX frames */
static void dms_frame_delete(nmt_t *nmt, struct dms_frame *dms_frame)
{
	PDEBUG(DDMS, DEBUG_DEBUG, "delete DMS frame %cT(%d) from queue\n", dms_frame->s + 'C', dms_frame->n);

	unlink_dms_frame(nmt, dms_frame);

	free(dms_frame);
}

/* add DT frame */
static void dms_frame_add_dt(nmt_t *nmt, const uint8_t *data)
{
	dms_frame_add(nmt, 1, data);
}

/* add CT frame */
static void dms_frame_add_ct(nmt_t *nmt, const uint8_t *data)
{
	dms_frame_add(nmt, 0, data);
}

/* add ID frame */
static void dms_frame_add_id(nmt_t *nmt)
{
	uint8_t frame[8];

	frame[0] = 73; /* ID */
	frame[1] = 3; // FIXME: add real id
	frame[2] = 0;
	frame[3] = 0;
	frame[4] = 0;
	frame[5] = 0;
	frame[6] = 0;
	frame[7] = 0;
	dms_frame_add_ct(nmt, frame);
}

/* add RAND frame */
static void dms_frame_add_rand(nmt_t *nmt, int eight_bits)
{
	uint32_t rand = random();
	uint8_t frame[8];

	frame[0] = 82; /* RAND */
	frame[1] = (rand >> 17) & 0x40;
	frame[2] = (rand >> 16) & 0x7f;
	frame[3] = (rand >> 9) & 0x40;
	frame[4] = (rand >> 8) & 0x7f;
	frame[5] = (rand >> 1) & 0x40;
	frame[6] = rand & 0x7f;
	frame[7] = eight_bits ? '8' : '7';
	dms_frame_add_ct(nmt, frame);
}

/*
 * init and exit
 */

/* init instance */
int dms_init_sender(nmt_t *nmt)
{
	/* we need some simple random */
	srandom((unsigned int)(get_time() * 1000));

	return 0;
}

/* Cleanup transceiver instance. */
void dms_cleanup_sender(nmt_t *nmt)
{
	dms_reset(nmt);
}

/*
 * transmission of frames
 */

/* encode DT frame and schedule for next transmission */
static void dms_encode_dt(nmt_t *nmt, uint8_t d, uint8_t s, uint8_t n, uint8_t *_data)
{
	dms_t *dms = &nmt->dms;
	char frame[127];
	uint8_t data[12];
	uint8_t bits[63 + 16];
	uint16_t crc;
	int i, j;

	PDEBUG(DDMS, DEBUG_INFO, "Sending DMS frame: %s\n", print_ct_dt(s, n, _data, dms->state.eight_bits));

	/* generate label */
	data[0] = (d << 6) | (s << 5) | (3 << 3) | n;
	memcpy(data + 1, _data, 8);
	for (i = 0; i < 9; i++) {
		for (j = 0; j < 7; j++)
			bits[i * 7 + j] = (data[i] >> (6 - j)) & 1;
	}
	for (i = 0; i < 16; i++)
		bits[63 + i] = 0;
	crc = crc16(bits, 63 + 16);
	data[9] = crc >> 9;
	data[10] = crc >> 2;
	data[11] = crc & 0x3;

	/* create RR frame */
	// FIXME: no dotting on consecutive frames
	memcpy(frame, DMS_DOTTING, 15);
	memcpy(frame + 15, DMS_SYNC, 11);
	for (i = 0; i < 11; i++) {
		for (j = 0; j < 7; j++)
			frame[26 + j + i*9] = ((data[i] >> (6 - j)) & 1) | '0';
		frame[26 + 7 + i*9] = '1';
		frame[26 + 8 + i*9] = '1';
	}
	frame[125] = ((data[11] >> 1) & 1) | '0';
	frame[126] = (data[11] & 1) | '0';
#if 0
	for (i = 0; i < 127; i++) {
		if (i == 15 || i == 26 || (i - 26) % 9 == 6 || (i - 26) % 9 == 8)
			printf(" ");
		printf("%c", frame[i]);
	}
	printf("\n");
#endif

	/* render wave form */
	fsk_render_frame(nmt, frame, 127, dms->frame_spl);
	dms->frame_valid = 1;
	dms->frame_pos = 0;
	dms->frame_length = nmt->samples_per_bit * 127;
}

/* encode RR frame and schedule for next transmission */
static void dms_encode_rr(nmt_t *nmt, uint8_t d, uint8_t s, uint8_t n)
{
	dms_t *dms = &nmt->dms;
	uint8_t data;
	char frame[77], label[9];
	int parity, i;

	/* generate label */
	data = (d << 6) | (s << 5) | (1 << 3) | n;
	parity = '0';
	for (i = 0; i < 7; i++) {
		label[i] = ((data >> (6 - i)) & 1) | '0';
		if (label[i] == '1')
			parity ^= 1;
	}
	label[7] = '1';
	label[8] = '1';

	/* create RR frame */
	memcpy(frame, DMS_DOTTING, 15);
	memcpy(frame + 15, DMS_SYNC, 11);
	memcpy(frame + 26, label, 9);
	memcpy(frame + 35, label, 9);
	frame[44] = parity; frame[45] = parity;
	memcpy(frame + 46, frame + 15, 31);
#if 0
	for (i = 0; i < 77; i++) {
		if (i == 15 || i == 26 || i == 33 || i == 35
		 || i == 42 || i == 44 || i == 46 || i == 57
		 || i == 64 || i == 66 || i == 73 || i == 75)
			printf(" ");
		printf("%c", frame[i]);
	}
	printf("\n");
#endif

	/* render wave form */
	fsk_render_frame(nmt, frame, 77, dms->frame_spl);
	dms->frame_valid = 1;
	dms->frame_pos = 0;
	dms->frame_length = nmt->samples_per_bit * 77;
}

/* check if we have to transmit a frame and render it
 * also do nothing until a currently transmitted frame is completely
 * transmitted.
 */
static void trigger_frame_transmission(nmt_t *nmt)
{
	dms_t *dms = &nmt->dms;
	struct dms_frame *dms_frame;
	int i;

	/* ongoing transmission, so we wait */
	if (dms->frame_valid)
		return;

	/* check for RR first, because high priority */
	if (dms->state.send_rr) {
		PDEBUG(DDMS, DEBUG_DEBUG, "Found pending RR(%d) frame, sending.\n", dms->state.n_r);
		dms->state.send_rr = 0;
		dms_encode_rr(nmt, dms->state.dir ^ 1, 1, dms->state.n_r);
		return;
	}

	/* get next frame to send */
	/* loop 4 times, because only 4 unacked frames may be transmitted */
	dms_frame = dms->state.frame_list;
	for (i = 0; i < 4 && dms_frame; i++) {
		/* stop before DT frame, if RAND was not acked */
		if (dms_frame->next && dms_frame->next->s == 1 && !dms->state.established)
			break;
		if (dms_frame->n == dms->state.n_s)
			break;
		dms_frame = dms_frame->next;
	}

	/* check if outstanding frame */
	if (!dms_frame) {
		PDEBUG(DDMS, DEBUG_DEBUG, "No pending RR/CT/DT frame found.\n");
		if (dms->state.tx_pending) {
			dms->state.tx_pending = 0;
			dms_all_sent(nmt);
		}
		return;
	}

	PDEBUG(DDMS, DEBUG_DEBUG, "Found pending %cT(%d) frame, sending.\n", dms_frame->s + 'C', dms_frame->n);

	/* sent next send state to next frame in buffer.
	 * if there is no next frame, set it to the first frame (cycle).
	 * also if RAND was not acked, but next frame is DT, send first frame.
	 */
	if (!dms_frame->next) {
		dms->state.n_s = dms->state.frame_list->n;
		PDEBUG(DDMS, DEBUG_DEBUG, " -> Next sequence number is %d, because this was the last frame in queue.\n", dms->state.n_s);
	} else if (!dms->state.established && dms_frame->next->s == 1) {
		dms->state.n_s = dms->state.frame_list->n;
		PDEBUG(DDMS, DEBUG_DEBUG, " -> Next sequence number is %d, because this was the last frame before DT queue, and RAND has not been acked yet.\n", dms->state.n_s);
	} else if (i == 3) {
		dms->state.n_s = dms->state.frame_list->n;
		PDEBUG(DDMS, DEBUG_DEBUG, " -> Next sequence number is %d, because we reached max number of unacknowledged frames.\n", dms->state.n_s);
	} else if (!dms->state.established && dms_frame->next->s == 0) {
		dms->state.n_s = dms_frame->next->n;
		PDEBUG(DDMS, DEBUG_DEBUG, " -> Next sequence number is %d, because this is the next CT frame in queue.\n", dms->state.n_s);
	} else {
		dms->state.n_s = dms_frame->next->n;
		PDEBUG(DDMS, DEBUG_DEBUG, " -> Next sequence number is %d, because this is the next frame in queue.\n", dms->state.n_s);
	}

	dms_encode_dt(nmt, dms->state.dir ^ 1, dms_frame->s, dms_frame->n, dms_frame->data);
}

/* send data using FSK */
int fsk_dms_frame(nmt_t *nmt, int16_t *samples, int length)
{
	dms_t *dms = &nmt->dms;
	int16_t *spl;
	int i;
	int count, max;

next_frame:
	/* check if no frame is currently transmitted */
	if (dms->frame_length == 0) {
		dms->frame_valid = 0;
		trigger_frame_transmission(nmt);
		if (!dms->frame_valid)
			return length;
	}
	/* send audio from frame */
	max = dms->frame_length;
	count = max - dms->frame_pos;
//printf("length = %d count=%d\n", length, count);
	if (count > length)
		count = length;
	spl = dms->frame_spl + dms->frame_pos;
	for (i = 0; i < count; i++) {
		*samples++ = *spl++;
	}
	dms->frame_pos += count;
	/* check for end of frame and stop */
	if (dms->frame_pos == max) {
		dms->frame_length = 0;
		/* we need more ? */
		if (length)
			goto next_frame;
	}

	return length;
}

/*
 * reception of frames
 */

/* decode DT frame from mobile */
static void dms_rx_dt(nmt_t *nmt, uint8_t d, uint8_t s, uint8_t n, uint8_t *data)
{
	dms_t *dms = &nmt->dms;
	int length;

	/* start transfer */
	if (!dms->state.started) {
		PDEBUG(DDMS, DEBUG_INFO, "Starting DMS transfer (mobile originated)\n");
		dms->state.started = 1;
		dms->state.established = 0;
		dms->state.dir = d;
		dms->state.n_r = 0;
		dms->state.n_s = 0;
		dms->state.n_a = 0;
		dms->state.n_count = 0;
		dms->state.rand_sent = 0;
	}

	if (dms->state.dir != d && !dms_allow_loopback) {
		/* drop frames with wrong direction indicator */
		PDEBUG(DDMS, DEBUG_INFO, "DMS frame ignored, direction indicator missmatch!\n");
		return;
	}

	if (dms->state.n_r != n) {
		/* ignore out of sequence frames */
		PDEBUG(DDMS, DEBUG_DEBUG, "DMS frame number missmatch (due to resending)\n");
	} else {
		PDEBUG(DDMS, DEBUG_INFO, "Received valid DMS frame: %s\n", print_ct_dt(s, n, data, dms->state.eight_bits));

		/* cycle sequence */
		dms->state.n_r = (n + 1) % 8;

		/* CT frames */
		if (s == 0) {
			switch (data[0]) {
			case 73: /* ID */
				break;
			case 82: /* RAND */
				PDEBUG(DDMS, DEBUG_DEBUG, "RAND frame has been received, so we can send/receive DT frame\n");
				/* when we sent RAND, we do not resend it again, this would be wrong */
				if (!dms->state.rand_sent) {
					dms_frame_add_rand(nmt, data[7]);
					dms->state.rand_sent = 1;
				}
				dms->state.established = 1;
				dms->state.eight_bits = (data[7] == '8') ? 1 : 0;
				break;
			default:
				;
			}
		} else {
			if (!dms->state.established)
				PDEBUG(DDMS, DEBUG_NOTICE, "Received DT frame, but RAND frame has not been received yet\n");
			else {
				if (!dms->state.eight_bits)
					length = 8;
				else {
					int i;

					for (i = 1; i < 8; i++)
						data[i] |= ((data[0] << i) & 0x80);
					length = 7;
					data++;
				}
				/* according to NMT Doc 450-3 10.8 remove trailing zeroes */
				while (length > 1) {
					if (data[length - 1] == 0)
						length--;
					else
						break;
				}
				dms_receive(nmt, data, length, dms->state.eight_bits);
			}
		}
	}

	/* schedule sending of RR frame */
	dms->state.send_rr = 1;
	/* now trigger frame transmission */
	trigger_frame_transmission(nmt);
}

/* decode RR frame from mobile */
static void dms_rx_rr(nmt_t *nmt, uint8_t d, uint8_t s, uint8_t n)
{
	dms_t *dms = &nmt->dms;
	struct dms_frame *dms_frame, *dms_frame_next;
	int i, j;

	if (!dms->state.started)
		return;

	if (dms->state.dir != d && !dms_allow_loopback) {
		/* drop frames with wrong direction indicator */
		PDEBUG(DDMS, DEBUG_INFO, "DMS frame ignored, direction indicator missmatch!\n");
		return;
	}

	/* check to which entry in the list of frames this ack belongs to */
	/* loop 4 times, because only 4 unacked frames may have been transmitted */
	dms_frame = dms->state.frame_list;
	for (i = 0; i < 4 && dms_frame; i++) {
		if (dms_frame->n == ((n - 1) & 7))
			break;
		dms_frame = dms_frame->next;
	}

	/* if we don't find a frame, it must have been already acked, so we igore RR */
	if (!dms_frame || i == 4) {
		PDEBUG(DDMS, DEBUG_DEBUG, "Received already acked DMS frame: RR(%d) (s = %d), ignoring\n", n, s);
		return;
	}

	PDEBUG(DDMS, DEBUG_INFO, "Received valid DMS frame: RR(%d) (s = %d)\n", n, s);

	/* flush all acked frames. */
	dms_frame = dms->state.frame_list;
	for (j = 0; j <= i; j++) {
		if (dms_frame->data[0] == 82) { /* RAND */
			PDEBUG(DDMS, DEBUG_DEBUG, "RAND frame has been acknowledged, so we can continue to send DT frame\n");
			dms->state.established = 1;
		}
		/* increment ack counter */
		dms->state.n_a = (dms_frame->n + 1) & 7;
		/* raise send counter if required */
		if (dms->state.n_s == dms_frame->n) {
			dms->state.n_s = dms->state.n_a;
			PDEBUG(DDMS, DEBUG_DEBUG, "Raising next frame to send to #%d\n", dms->state.n_s);
		}
		PDEBUG(DDMS, DEBUG_DEBUG, "Removing acked frame #%d\n", dms_frame->n);
		dms_frame_next = dms_frame->next;
		dms_frame_delete(nmt, dms_frame);
		dms_frame = dms_frame_next;
	}

	/* now trigger frame transmission */
	trigger_frame_transmission(nmt);
}

/* decode NR frame from mobile */
static void dms_rx_nr(nmt_t *nmt, uint8_t d, uint8_t s, uint8_t n)
{
	dms_t *dms = &nmt->dms;

	if (!dms->state.started)
		return;

	if (dms->state.dir != d && !dms_allow_loopback) {
		/* drop frames with wrong direction indicator */
		PDEBUG(DDMS, DEBUG_INFO, "DMS frame ignored, direction indicator missmatch!\n");
		return;
	}

	PDEBUG(DDMS, DEBUG_INFO, "Received valid DMS frame: NR(%d) (s = %d)\n", n, s);

	// FIXME: support NR

	/* now trigger frame transmission */
	trigger_frame_transmission(nmt);
}

/* Check for DMS SYNC bits, then collect data bits */
void fsk_receive_bit_dms(nmt_t *nmt, int bit, double quality, double level)
{
	dms_t *dms = &nmt->dms;
//	double frames_elapsed;
	int i;

//	printf("bit=%d quality=%.4f\n", bit, quality);
	/* we always search for sync, because the sync cannot show up inside the message itself */
	dms->rx_sync = (dms->rx_sync << 1) | bit;

	/* sync level and quality */
	dms->rx_sync_level[dms->rx_sync_count & 0xff] = level;
	dms->rx_sync_quality[dms->rx_sync_count & 0xff] = quality;
	dms->rx_sync_count++;

	/* check if pattern 00101000111 matches */
	if ((dms->rx_sync & 0x07ff) == 0x0147) {
		/* average level and quality */
		level = quality = 0;
		for (i = 0; i < 16; i++) {
			level += dms->rx_sync_level[(dms->rx_sync_count - 1 - i) & 0xff];
			quality += dms->rx_sync_quality[(dms->rx_sync_count - 1 - i) & 0xff];
		}
		level /= 16.0; quality /= 16.0;
//		printf("DMS sync (level = %.2f, quality = %.2f\n", level, quality);

		/* do not accept garbage */
		if (quality < 0.65)
			return;

		PDEBUG(DDSP, DEBUG_DEBUG, "DMS sync  RX Level: %.0f%% Quality=%.0f\n", level * 100.0 + 0.5, quality * 100.0 + 0.5);

		/* rest sync register */
		dms->rx_sync = 0;
		dms->rx_in_sync = 1;
		dms->rx_frame_count = 0;
		dms->rx_bit_count = 0;
		memset(dms->rx_frame, 0, sizeof(dms->rx_frame));
		memset(dms->rx_frame_level, 0, sizeof(dms->rx_frame_level));
		memset(dms->rx_frame_quality, 0, sizeof(dms->rx_frame_quality));

		/* set muting of receive path */
		nmt->fsk_filter_mute = (int)((double)nmt->sender.samplerate * MUTE_DURATION);
		return;
	}

	if (!dms->rx_in_sync)
		return;

	/* read bits */
	if (++dms->rx_bit_count <= 7)
		dms->rx_frame[dms->rx_frame_count] = (dms->rx_frame[dms->rx_frame_count] << 1) | bit;
	dms->rx_frame_level[dms->rx_frame_count] += level;
	dms->rx_frame_quality[dms->rx_frame_count] += quality;

	/* check label */
	if (dms->rx_frame_count == 0) {
		if (dms->rx_bit_count == 9) {
			dms->rx_bit_count = 0;
			dms->rx_label.d = (dms->rx_frame[0] >> 6) & 0x1;
			dms->rx_label.s = (dms->rx_frame[0] >> 5) & 0x1;
			dms->rx_label.p = (dms->rx_frame[0] >> 3) & 0x3;
			dms->rx_label.n = dms->rx_frame[0] & 0x7;
			PDEBUG(DDMS, DEBUG_DEBUG, "Got DMS label (d = %d, s = %d, p = %d, n = %d)\n", dms->rx_label.d,dms->rx_label.s,dms->rx_label.p,dms->rx_label.n);
			dms->rx_frame_count++;
			if (dms->rx_label.p == 0) {
				PDEBUG(DDMS, DEBUG_DEBUG, "Spare prefix '00' ignoring!\n");
				dms->rx_in_sync = 0;
			}
		}
		return;
	}

	if (dms->rx_label.p == 3) {
		/* read DT frame */
		if (dms->rx_frame_count <= 8) {
			if (dms->rx_bit_count == 9) {
				dms->rx_bit_count = 0;
				uint8_t c = dms->rx_frame[dms->rx_frame_count];
				PDEBUG(DDMS, DEBUG_DEBUG, "Got DMS word 0x%02x (%c)\n", c, (c >= 32 && c <= 126) ? c : '.');
				dms->rx_frame_count++;
			}
			return;
		}
		if (dms->rx_frame_count <= 10) {
			if (dms->rx_bit_count == 9) {
				dms->rx_bit_count = 0;
				PDEBUG(DDMS, DEBUG_DEBUG, "Got DMS CRC 0x%02x\n", dms->rx_frame[dms->rx_frame_count]);
				dms->rx_frame_count++;
			}
			return;
		}
		if (dms->rx_bit_count == 2) {
			uint16_t crc_got, crc_calc;
			uint8_t bits[63 + 16];
			int i, j;
			dms->rx_bit_count = 0;
			PDEBUG(DDMS, DEBUG_DEBUG, "Got DMS CRC 0x%x\n", dms->rx_frame[dms->rx_frame_count]);
			crc_got = (dms->rx_frame[9] << 9) | (dms->rx_frame[10] << 2) | dms->rx_frame[11];
			for (i = 0; i < 9; i++) {
				for (j = 0; j < 7; j++)
					bits[i * 7 + j] = (dms->rx_frame[i] >> (6 - j)) & 1;
			}
			for (i = 0; i < 16; i++)
				bits[63 + i] = 0;
			crc_calc = crc16(bits, 63 + 16);
			PDEBUG(DDMS, DEBUG_DEBUG, "DMS CRC = 0x%04x %s\n", crc_got, (crc_calc == crc_got) ? "(OK)" : "(CRC error)");
			if (crc_calc == crc_got)
				dms_rx_dt(nmt, dms->rx_label.d, dms->rx_label.s, dms->rx_label.n, dms->rx_frame + 1);
			dms->rx_in_sync = 0;
			return;
		}
		return;
	} else {
		/* read RR/NR frame */
		if (dms->rx_frame_count <= 1) {
			if (dms->rx_bit_count == 9) {
				dms->rx_bit_count = 0;
				if (dms->rx_frame[0] != dms->rx_frame[1]) {
					PDEBUG(DDMS, DEBUG_DEBUG, "Repeated DMS label missmatches!\n");
					dms->rx_in_sync = 0;
					return;
				}
				dms->rx_frame_count++;
				PDEBUG(DDMS, DEBUG_DEBUG, "Repeated label matches\n");
			}
			return;
		}
		if (dms->rx_bit_count == 2) {
			uint8_t parity_got, parity_calc = 0, bit;
			int i;
			dms->rx_bit_count = 0;
			parity_got = dms->rx_frame[2];
			PDEBUG(DDMS, DEBUG_DEBUG, "Got DMS parity 0x%x\n", dms->rx_frame[dms->rx_frame_count]);
			for (i = 0; i < 7; i++) {
				bit = (dms->rx_frame[0] >> i) & 1;
				if (bit)
					parity_calc ^= 0x3;
			}
			PDEBUG(DDMS, DEBUG_DEBUG, "DMS parity %s\n", (parity_calc == parity_got) ? "(OK)" : "(parity error)");
			if (parity_calc == parity_got) {
				if (dms->rx_label.p == 1)
					dms_rx_rr(nmt, dms->rx_label.d, dms->rx_label.s, dms->rx_label.n);
				else
					dms_rx_nr(nmt, dms->rx_label.d, dms->rx_label.s, dms->rx_label.n);
			}
			dms->rx_in_sync = 0;
			return;
		}
	}
}

/*
 * calls from upper layer
 */

/* recive data from upper layer to be sent as DT frames
 * the DT frames are generated */
void dms_send(nmt_t *nmt, const uint8_t *data, int length, int eight_bits)
{
	dms_t *dms = &nmt->dms;
	uint8_t frame[8];
	int i, copied;

	PDEBUG(DDMS, DEBUG_DEBUG, "Received message with %d digits of %d bits\n", length, (eight_bits) ? 8 : 7);

	/* active connection */
	if (dms->state.started) {
		if (dms->state.eight_bits != eight_bits) {
			PDEBUG(DDMS, DEBUG_ERROR, "DMS session active, but upper layer sends wrong bit format!\n");
			return;
		}
	}

	if (!dms->state.started) {
		PDEBUG(DDMS, DEBUG_DEBUG, "Transfer not started, so we send ID + RAND first\n");
		dms->state.started = 1;
		dms->state.established = 0;
		dms->state.eight_bits = eight_bits;
		dms->state.dir = 1; /* we send 0, we expect 1 */
		dms->state.n_r = 0;
		dms->state.n_s = 0;
		dms->state.n_a = 0;
		dms->state.n_count = 0;
		dms_frame_add_id(nmt);
		dms_frame_add_rand(nmt, eight_bits);
		dms->state.rand_sent = 1;
	}

	PDEBUG(DDMS, DEBUG_DEBUG, "Queueing message data as DT frames...\n");
	while (length) {
		if (eight_bits) {
			/* copy what we have */
			for (i = 1; i < 8 && length; i++) {
				frame[i] = *data++;
				length--;
			}
			copied = i - 1;
			/* padd with 0, if required */
			for (; i < 8; i++)
				frame[i] = 0;
			/* move the 8th bits to first character */
			frame[0] = 0;
			for (i = 1; i < 8; i++) {
				frame[0] |= (frame[i] & 0x80) >> i;
				frame[i] &= 0x7f;
			}
		} else {
			/* copy what we have */
			for (i = 0; i < 8 && length; i++) {
				frame[i] = (*data++) & 0x7f;
				length--;
			}
			copied = i;
			/* padd with 0, if required */
			for (; i < 8; i++)
				frame[i] = 0;
		}
		/* according to NMT Doc 450-3 10.8 trailing zeros are ignored.
		 * we put back the trailing zeros to the data buffer.
		 * except for a single zero, we keep it, because all digits
		 * 0 means a single zero is transmitted.
		 */
		for (i = 0; i < copied - 1; i++) {
			if (data[-1] == 0) {
				/* put back last take byte */
				data--;
				length++;
			}
		}
		dms_frame_add_dt(nmt, frame);
		/* indicate that we have pending data */
		dms->state.tx_pending = 1;
	}

	/* now trigger frame transmission */
	trigger_frame_transmission(nmt);
}

/* reset DMS instance */
void dms_reset(nmt_t *nmt)
{
	dms_t *dms = &nmt->dms;
	PDEBUG(DDMS, DEBUG_DEBUG, "Resetting DMS states\n");

	dms->rx_in_sync = 0;
	memset(&dms->state, 0, sizeof(dms->state));

	dms->frame_valid = 0;

	while (dms->state.frame_list)
		dms_frame_delete(nmt, dms->state.frame_list);
}