/*
 * dectmon DLC message reassembly
 *
 * Copyright (c) 2010 Patrick McHardy <kaber@trash.net>
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 */

#include <assert.h>
#include <stdio.h>
#include <dect/libdect.h>
#include <dectmon.h>
#include <mac.h>
#include <dlc.h>

#define dlc_print(fmt, args...)				\
	do {						\
		if (dumpopts & DECTMON_DUMP_DLC)	\
			printf(fmt, ## args);		\
	} while (0)

#if 1
#define lc_debug(lc, fmt, args...)	dlc_print(fmt, ## args)
#else
#define lc_debug(lc, fmt, args...)
#endif

static void dect_fa_parse_len(struct dect_fa_len *len,
			      const struct dect_msg_buf *mb)
{
	uint8_t l;

	l = mb->data[DECT_FA_LI_OFF];
	len->len  = (l & DECT_FA_LI_LENGTH_MASK) >> DECT_FA_LI_LENGTH_SHIFT;
	len->more = (l & DECT_FA_LI_M_FLAG);
}

static struct dect_lc *dect_lc_init(struct dect_mac_con *mc)
{
	struct dect_lc *lc;

	lc = calloc(1, sizeof(*lc));
	if ((mc->pmid & 0xf0000) != 0xe0000)
		lc->lsig = mc->pmid;
	return lc;
}

static bool dect_fa_frame_csum_verify(const struct dect_lc *lc,
				      struct dect_msg_buf *mb)
{
	uint8_t *data = mb->data;
	unsigned int i;
	uint8_t c0 = 0, c1 = 0;
	uint16_t t;

	data[mb->len - 2] ^= lc->lsig >> 8;
	data[mb->len - 1] ^= lc->lsig & 0xff;

	for (i = 0; i < mb->len; i++) {
		t = c0 + data[i];
		c0 = (t & 0xffU) + ((t >> 8) & 0x1U);
		t = c1 + c0;
		c1 = (t & 0xffU) + ((t >> 8) & 0x1U);
	}

	lc_debug(lc, "csum verify: lsig %.4x c0: %.2x c1: %.2x\n",
		 lc->lsig, c0, c1);
	return c0 == (uint8_t)~0 && c1 == (uint8_t)~0;
}

static const uint8_t channel_sdu_size[] = {
        [DECT_MC_C_S]	= DECT_C_S_SDU_SIZE,
        [DECT_MC_C_F]	= DECT_C_F_SDU_SIZE,
};

#define roundup(x, y) ((((x) + ((y) - 1)) / (y)) * (y))

static struct dect_msg_buf *dect_lc_reassemble(struct dect_lc *lc,
					       enum dect_data_channels chan,
					       struct dect_msg_buf *mb)
{
	struct dect_fa_len fl;
	uint8_t sdu_len, len;

	sdu_len = channel_sdu_size[chan];
	if (lc->rx_buf == NULL) {
		dect_fa_parse_len(&fl, mb);
		len = fl.len;
		len += DECT_FA_HDR_SIZE + DECT_FA_CSUM_SIZE;

		lc->rx_len = roundup(len, sdu_len);
		lc->rx_buf = dect_mbuf_alloc(dh);
		if (lc->rx_buf == NULL)
			goto err;
	}

	memcpy(dect_mbuf_put(lc->rx_buf, sdu_len), mb->data, sdu_len);
	mb = NULL;

	if (lc->rx_buf->len >= lc->rx_len) {
		assert(lc->rx_buf->len == lc->rx_len);
		mb = lc->rx_buf;
		lc->rx_buf = NULL;

		if (!dect_fa_frame_csum_verify(lc, mb))
			goto err;

		/* Trim checksum and filling */
		dect_fa_parse_len(&fl, mb);
		mb->len = fl.len + DECT_FA_HDR_SIZE;
		lc_debug(lc, "reassembled SDU len %u\n", mb->len);
	}

	return mb;

err:
	lc_debug(lc, "reassembly failed\n");
	return NULL;
}

void dect_mac_co_data_ind(struct dect_mac_con *mc, enum dect_data_channels chan,
			  struct dect_msg_buf *mb)
{
	struct dect_lc *lc;

	//printf("MAC_CO_DATA-ind\n");
	if (mc->lc == NULL) {
		lc = dect_lc_init(mc);
		if (lc == NULL)
			return;
		mc->lc = lc;
	}

	mb = dect_lc_reassemble(mc->lc, chan, mb);
	if (mb != NULL && mb->len > DECT_FA_HDR_SIZE) {
		dect_mbuf_pull(mb, DECT_FA_HDR_SIZE);
		dect_dl_data_ind(&mc->tbc->dl, mb);
	}
}