/*
 * dectmon MAC layer message tracing
 *
 * 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 <stdlib.h>
#include <stdio.h>
#include <asm/byteorder.h>

#include <dect/libdect.h>
#include <dectmon.h>
#include <phl.h>
#include <mac.h>
#include <dsc.h>

#define BITS_PER_BYTE	8

#define mac_print(fmt, args...)				\
	do {						\
		if (dumpopts & DECTMON_DUMP_MAC)	\
			dectmon_log(fmt, ## args);	\
	} while (0)

/*
 * Tail message parsing/construction
 */

static enum dect_tail_identifications dect_parse_tail(const struct dect_msg_buf *mb)
{
	return mb->data[DECT_HDR_TA_OFF] & DECT_HDR_TA_MASK;
}

static uint8_t dect_parse_ari(struct dect_ari *ari, uint64_t a)
{
	ari->arc = (a & DECT_ARI_ARC_MASK) >> DECT_ARI_ARC_SHIFT;
	switch (ari->arc) {
	case DECT_ARC_A:
		ari->emc = (a & DECT_ARI_A_EMC_MASK) >> DECT_ARI_A_EMC_SHIFT;
		ari->fpn = (a & DECT_ARI_A_FPN_MASK) >> DECT_ARI_A_FPN_SHIFT;
		return DECT_ARC_A_LEN;
	case DECT_ARC_B:
		ari->eic = (a & DECT_ARI_B_EIC_MASK) >> DECT_ARI_B_EIC_SHIFT;
		ari->fpn = (a & DECT_ARI_B_FPN_MASK) >> DECT_ARI_B_FPN_SHIFT;
		ari->fps = (a & DECT_ARI_B_FPS_MASK) >> DECT_ARI_B_FPS_SHIFT;
		return DECT_ARC_B_LEN;
	case DECT_ARC_C:
		ari->poc = (a & DECT_ARI_C_POC_MASK) >> DECT_ARI_C_POC_SHIFT;
		ari->fpn = (a & DECT_ARI_C_FPN_MASK) >> DECT_ARI_C_FPN_SHIFT;
		ari->fps = (a & DECT_ARI_C_FPS_MASK) >> DECT_ARI_C_FPS_SHIFT;
		return DECT_ARC_C_LEN;
	case DECT_ARC_D:
		ari->gop = (a & DECT_ARI_D_GOP_MASK) >> DECT_ARI_D_GOP_SHIFT;
		ari->fpn = (a & DECT_ARI_D_FPN_MASK) >> DECT_ARI_D_FPN_SHIFT;
		return DECT_ARC_D_LEN;
	case DECT_ARC_E:
		ari->fil = (a & DECT_ARI_E_FIL_MASK) >> DECT_ARI_E_FIL_SHIFT;
		ari->fpn = (a & DECT_ARI_E_FPN_MASK) >> DECT_ARI_E_FPN_SHIFT;
		return DECT_ARC_E_LEN;
	default:
		return 0;
        }
}

static int dect_parse_identities_information(struct dect_tail_msg *tm, uint64_t t)
{
	struct dect_idi *idi = &tm->idi;
	uint8_t ari_len, rpn_len;

	ari_len = dect_parse_ari(&idi->pari, t << DECT_RFPI_ARI_SHIFT);
	if (ari_len == 0)
		return -1;
	rpn_len = BITS_PER_BYTE * DECT_NT_ID_RFPI_LEN - 1 - ari_len;

	idi->e   = (t & DECT_RFPI_E_FLAG);
	idi->rpn = (t >> DECT_RFPI_RPN_SHIFT) & ((1 << rpn_len) - 1);
	tm->type = DECT_TM_TYPE_ID;

	mac_print("identities information: E: %u class: %u EMC: %.4x "
		 "FPN: %.5x RPN: %x\n", idi->e, idi->pari.arc,
		 idi->pari.emc, idi->pari.fpn, idi->rpn);
	return 0;
}

static int dect_parse_static_system_information(struct dect_tail_msg *tm, uint64_t t)
{
	struct dect_ssi *ssi = &tm->ssi;

	ssi->nr	    = (t & DECT_QT_SSI_NR_FLAG);
	ssi->sn     = (t & DECT_QT_SSI_SN_MASK) >> DECT_QT_SSI_SN_SHIFT;
	ssi->sp     = (t & DECT_QT_SSI_SP_MASK) >> DECT_QT_SSI_SP_SHIFT;
	ssi->txs    = (t & DECT_QT_SSI_TXS_MASK) >> DECT_QT_SSI_TXS_SHIFT;
	ssi->mc     = (t & DECT_QT_SSI_MC_FLAG);
	ssi->rfcars = (t & DECT_QT_SSI_RFCARS_MASK) >> DECT_QT_SSI_RFCARS_SHIFT;
	ssi->cn     = (t & DECT_QT_SSI_CN_MASK) >> DECT_QT_SSI_CN_SHIFT;
	ssi->pscn   = (t & DECT_QT_SSI_PSCN_MASK) >> DECT_QT_SSI_PSCN_SHIFT;

	if (ssi->sn > 11 || ssi->cn > 9 || ssi->pscn > 9 || ssi->rfcars == 0)
		return -1;
	tm->type = DECT_TM_TYPE_SSI;

	mac_print("static system information: SN: %u CN: %u PSCN: %u NR: %u "
		  "Txs: %u Mc: %u RF-carriers: %x\n",
		  ssi->sn, ssi->cn, ssi->pscn, ssi->nr, ssi->txs, ssi->mc,
		  ssi->rfcars);
	return 0;
}

static int dect_parse_extended_rf_carrier_information(struct dect_tail_msg *tm, uint64_t t)
{
	struct dect_erfc *erfc = &tm->erfc;

	erfc->rfcars	 = (t & DECT_QT_ERFC_RFCARS_MASK) >>
			   DECT_QT_ERFC_RFCARS_SHIFT;
	erfc->band	 = (t & DECT_QT_ERFC_RFBAND_MASK) >>
			   DECT_QT_ERFC_RFBAND_SHIFT;
	erfc->num_rfcars = (t & DECT_QT_ERFC_NUM_RFCARS_MASK) >
			   DECT_QT_ERFC_NUM_RFCARS_SHIFT;
	tm->type = DECT_TM_TYPE_ERFC;

	mac_print("extended rf carrier information: RF-carriers: %.6x band: %u num: %u\n",
		  erfc->rfcars, erfc->band, erfc->num_rfcars);
	return 0;
}

static int dect_parse_fixed_part_capabilities(struct dect_tail_msg *tm, uint64_t t)
{
	struct dect_fpc *fpc = &tm->fpc;

	fpc->fpc = (t & DECT_QT_FPC_CAPABILITY_MASK) >>
		   DECT_QT_FPC_CAPABILITY_SHIFT;
	fpc->hlc = (t & DECT_QT_FPC_HLC_MASK) >> DECT_QT_FPC_HLC_SHIFT;
	tm->type = DECT_TM_TYPE_FPC;

	mac_print("fixed part capabilities: FPC: %.5x HLC: %.4x\n",
		  fpc->fpc, fpc->hlc);
	return 0;
}

static int dect_parse_extended_fixed_part_capabilities(struct dect_tail_msg *tm, uint64_t t)
{
	struct dect_efpc *efpc = &tm->efpc;

	efpc->fpc = (t & DECT_QT_EFPC_EFPC_MASK) >> DECT_QT_EFPC_EFPC_SHIFT;
	efpc->hlc = (t & DECT_QT_EFPC_EHLC_MASK) >> DECT_QT_EFPC_EHLC_SHIFT;
	tm->type  = DECT_TM_TYPE_EFPC;

	mac_print("extended fixed part capabilities: FPC: %.5x HLC: %.6x\n",
		  efpc->fpc, efpc->hlc);
	return 0;
}

static int dect_parse_extended_fixed_part_capabilities2(struct dect_tail_msg *tm, uint64_t t)
{
	struct dect_efpc2 *efpc2 = &tm->efpc2;

	efpc2->fpc = (t & DECT_QT_EFPC2_FPC_MASK) >> DECT_QT_EFPC2_FPC_SHIFT;
	efpc2->hlc = (t & DECT_QT_EFPC2_HLC_MASK) >> DECT_QT_EFPC2_HLC_SHIFT;
	tm->type   = DECT_TM_TYPE_EFPC2;

	mac_print("extended fixed part capabilities2: FPC: %x HLC: %x\n",
		  efpc2->fpc, efpc2->hlc);
	return 0;
}

static int dect_parse_sari(struct dect_tail_msg *tm, uint64_t t)
{
	struct dect_sari *sari = &tm->sari;

	sari->list_cycle = (((t & DECT_QT_SARI_LIST_CYCLE_MASK) >>
			     DECT_QT_SARI_LIST_CYCLE_SHIFT) + 1) * 2;
	sari->tari  = (t & DECT_QT_SARI_TARI_FLAG);
	sari->black = (t & DECT_QT_SARI_BLACK_FLAG);
	//dect_parse_ari(&sari->ari, t << DECT_QT_SARI_ARI_SHIFT);
	tm->type = DECT_TM_TYPE_SARI;

	mac_print("sari: cycle %u TARI: %u black: %u\n",
		  sari->list_cycle, sari->tari, sari->black);
	return 0;
}

static int dect_parse_multiframe_number(struct dect_tail_msg *tm, uint64_t t)
{
	tm->mfn.num = (t & DECT_QT_MFN_MASK) >> DECT_QT_MFN_SHIFT;
	tm->type = DECT_TM_TYPE_MFN;

	mac_print("multiframe number: %u\n", tm->mfn.num);
	return 0;
}

static int dect_parse_system_information(struct dect_tail_msg *tm, uint64_t t)
{
	/* clear of memcmp */
	memset(((void *)tm) + offsetof(struct dect_tail_msg, ssi), 0,
	       sizeof(*tm) - offsetof(struct dect_tail_msg, ssi));

	switch (t & DECT_QT_H_MASK) {
	case DECT_QT_SI_SSI:
	case DECT_QT_SI_SSI2:
		return dect_parse_static_system_information(tm, t);
	case DECT_QT_SI_ERFC:
		return dect_parse_extended_rf_carrier_information(tm, t);
	case DECT_QT_SI_FPC:
		return dect_parse_fixed_part_capabilities(tm, t);
	case DECT_QT_SI_EFPC:
		return dect_parse_extended_fixed_part_capabilities(tm, t);
	case DECT_QT_SI_EFPC2:
		return dect_parse_extended_fixed_part_capabilities2(tm, t);
	case DECT_QT_SI_SARI:
		return dect_parse_sari(tm, t);
	case DECT_QT_SI_MFN:
		return dect_parse_multiframe_number(tm, t);
	default:
		mac_print("unknown system information type %llx\n",
			  (unsigned long long)t & DECT_QT_H_MASK);
		return -1;
	}
}

static int dect_parse_blind_full_slots(struct dect_tail_msg *tm, uint64_t t)
{
	struct dect_bfs *bfs = &tm->bfs;

	bfs->mask = (t & DECT_PT_BFS_MASK) >> DECT_PT_BFS_SHIFT;
	tm->type = DECT_TM_TYPE_BFS;

	mac_print("page: RFPI: %.3x blind full slots: %.3x\n",
		  tm->page.rfpi, bfs->mask);
	return 0;
}

static int dect_parse_bearer_description(struct dect_tail_msg *tm, uint64_t t)
{
	struct dect_bearer_desc *bd = &tm->bd;

	bd->bt = (t & DECT_PT_INFO_TYPE_MASK);
	bd->sn = (t & DECT_PT_BEARER_SN_MASK) >> DECT_PT_BEARER_SN_SHIFT;
	bd->sp = (t & DECT_PT_BEARER_SP_MASK) >> DECT_PT_BEARER_SP_SHIFT;
	bd->cn = (t & DECT_PT_BEARER_CN_MASK) >> DECT_PT_BEARER_CN_SHIFT;
	if (bd->sn >= DECT_HALF_FRAME_SIZE)
		return -1;
	tm->type = DECT_TM_TYPE_BD;

	mac_print("page: RFPI: %.3x bearer description: BT: %llx SN: %u SP: %u CN: %u\n",
		  tm->page.rfpi, (unsigned long long)bd->bt, bd->sn, bd->sp, bd->cn);
	return 0;
}

static int dect_parse_rfp_identity(struct dect_tail_msg *tm, uint64_t t)
{
	struct dect_rfp_id *id = &tm->rfp_id;

	id->id = (t & DECT_PT_RFP_ID_MASK) >> DECT_PT_RFP_ID_SHIFT;
	tm->type = DECT_TM_TYPE_RFP_ID;

	mac_print("page: RFPI: %.3x RFP identity: %.3x\n",
		  tm->page.rfpi, id->id);
	return 0;
}

static int dect_parse_rfp_status(struct dect_tail_msg *tm, uint64_t t)
{
	struct dect_rfp_status *st = &tm->rfp_status;

	st->rfp_busy = t & DECT_PT_RFPS_RFP_BUSY_FLAG;
	st->sys_busy = t & DECT_PT_RFPS_SYS_BUSY_FLAG;
	tm->type = DECT_TM_TYPE_RFP_STATUS;

	mac_print("page: RFPI: %.3x RFP status: rfp_busy: %d sys_busy: %d\n",
		  tm->page.rfpi, st->rfp_busy, st->sys_busy);
	return 0;
}

static int dect_parse_active_carriers(struct dect_tail_msg *tm, uint64_t t)
{
	struct dect_active_carriers *ac = &tm->active_carriers;

	ac->active = (t & DECT_PT_ACTIVE_CARRIERS_MASK) >>
		     DECT_PT_ACTIVE_CARRIERS_SHIFT;
	tm->type = DECT_TM_TYPE_ACTIVE_CARRIERS;

	mac_print("page: RFPI: %.3x active carriers: %.3x\n",
		  tm->page.rfpi, ac->active);
	return 0;
}

static int dect_parse_paging_info(struct dect_tail_msg *tm, uint64_t t)
{
	switch (t & DECT_PT_INFO_TYPE_MASK) {
	case DECT_PT_IT_BLIND_FULL_SLOT:
		return dect_parse_blind_full_slots(tm, t);
	case DECT_PT_IT_OTHER_BEARER:
	case DECT_PT_IT_RECOMMENDED_OTHER_BEARER:
	case DECT_PT_IT_GOOD_RFP_BEARER:
	case DECT_PT_IT_DUMMY_OR_CL_BEARER_POSITION:
	case DECT_PT_IT_CL_BEARER_POSITION:
		return dect_parse_bearer_description(tm, t);
	case DECT_PT_IT_RFP_IDENTITY:
		return dect_parse_rfp_identity(tm, t);
	case DECT_PT_IT_DUMMY_OR_CL_BEARER_MARKER:
		mac_print("dummy or connectionless bearer marker\n");
		return 0;
	case DECT_PT_IT_RFP_STATUS:
		return dect_parse_rfp_status(tm, t);
	case DECT_PT_IT_ACTIVE_CARRIERS:
		return dect_parse_active_carriers(tm, t);
	default:
		mac_print("unknown paging info %llx\n",
			  (unsigned long long)t);
		return -1;
	}
}

static int dect_parse_paging_msg(struct dect_tail_msg *tm, uint64_t t)
{
	tm->page.extend = t & DECT_PT_HDR_EXTEND_FLAG;
	tm->page.length = t & DECT_PT_HDR_LENGTH_MASK;

	switch (tm->page.length) {
	case DECT_PT_ZERO_PAGE:
		tm->page.rfpi = (t & DECT_PT_ZP_RFPI_MASK) >>
				DECT_PT_ZP_RFPI_SHIFT;

		return dect_parse_paging_info(tm, t);
	case DECT_PT_SHORT_PAGE:
		tm->page.rfpi = 0;
		return dect_parse_paging_info(tm, t);
	case DECT_PT_FULL_PAGE:
	case DECT_PT_LONG_PAGE:
	case DECT_PT_LONG_PAGE_FIRST:
	case DECT_PT_LONG_PAGE_LAST:
	case DECT_PT_LONG_PAGE_ALL:
		tm->type = DECT_TM_TYPE_PAGE;
		mac_print("full/long page: extend: %u length: %llx\n",
			  tm->page.extend, (unsigned long long)tm->page.length);
		return 0;
	default:
		mac_print("invalid page length %llx\n",
			  (unsigned long long)tm->page.length);
		return -1;
	}
}

static int dect_parse_cctrl_common(struct dect_cctrl *cctl, uint64_t t)
{
	cctl->fmid = (t & DECT_CCTRL_FMID_MASK) >> DECT_CCTRL_FMID_SHIFT;
	cctl->pmid = (t & DECT_CCTRL_PMID_MASK) >> DECT_CCTRL_PMID_SHIFT;

	mac_print("cctrl: command: %llx FMID: %.3x PMID: %.5x\n",
		  (unsigned long long)cctl->cmd, cctl->fmid, cctl->pmid);
	return 0;
}

static int dect_parse_cctrl_attr(struct dect_cctrl *cctl, uint64_t t)
{
	cctl->ecn        = (t & DECT_CCTRL_ATTR_ECN_MASK) >> DECT_CCTRL_ATTR_ECN_SHIFT;
	cctl->lbn        = (t & DECT_CCTRL_ATTR_LBN_MASK) >> DECT_CCTRL_ATTR_LBN_SHIFT;
	cctl->type       = (t & DECT_CCTRL_ATTR_TYPE_MASK) >> DECT_CCTRL_ATTR_TYPE_SHIFT;
	cctl->service    = (t & DECT_CCTRL_ATTR_SERVICE_MASK) >> DECT_CCTRL_ATTR_SERVICE_SHIFT;
	cctl->slot       = (t & DECT_CCTRL_ATTR_SLOT_MASK) >> DECT_CCTRL_ATTR_SLOT_SHIFT;
	cctl->cf         = (t & DECT_CCTRL_ATTR_CF_FLAG);
	cctl->a_mod      = (t & DECT_CCTRL_ATTR_A_MOD_MASK) >> DECT_CCTRL_ATTR_A_MOD_SHIFT;
	cctl->bz_mod     = (t & DECT_CCTRL_ATTR_BZ_MOD_MASK) >> DECT_CCTRL_ATTR_BZ_MOD_SHIFT;
	cctl->bz_ext_mod = (t & DECT_CCTRL_ATTR_BZ_EXT_MOD_MASK) >> DECT_CCTRL_ATTR_BZ_EXT_MOD_SHIFT;
	cctl->acr        = (t & DECT_CCTRL_ATTR_ACR_MASK) >> DECT_CCTRL_ATTR_ACR_SHIFT;

	mac_print("cctrl: command: %llx ECN: %x LBN: %x type: %x "
		  "service: %x slot type: %x CF: %d A-modulation: %x "
		  "B/Z-modulation: %x B/Z extended modulation: %x ACR: %x\n",
		  (unsigned long long)cctl->cmd, cctl->ecn, cctl->lbn,
		  cctl->type, cctl->service, cctl->slot, cctl->cf,
		  cctl->a_mod, cctl->bz_mod, cctl->bz_ext_mod, cctl->acr);

	return 0;
}

static int dect_parse_cctrl_release(struct dect_cctrl *cctl, uint64_t t)
{
	cctl->lbn    = (t & DECT_CCTRL_RELEASE_LBN_MASK) >>
		       DECT_CCTRL_RELEASE_LBN_SHIFT;
	cctl->reason = (t & DECT_CCTRL_RELEASE_REASON_MASK) >>
		       DECT_CCTRL_RELEASE_REASON_SHIFT;
	cctl->pmid   = (t & DECT_CCTRL_RELEASE_PMID_MASK) >>
		       DECT_CCTRL_RELEASE_PMID_SHIFT;

	mac_print("cctrl: release: PMID: %.5x LBN: %x reason: %x\n",
		  cctl->pmid, cctl->lbn, cctl->reason);
	return 0;
}

static int dect_parse_basic_cctrl(struct dect_tail_msg *tm, uint64_t t)
{
	struct dect_cctrl *cctl = &tm->cctl;

	cctl->cmd = t & DECT_MT_CMD_MASK;
	switch (cctl->cmd) {
	case DECT_CCTRL_ACCESS_REQ:
	case DECT_CCTRL_BEARER_HANDOVER_REQ:
	case DECT_CCTRL_CONNECTION_HANDOVER_REQ:
	case DECT_CCTRL_UNCONFIRMED_ACCESS_REQ:
	case DECT_CCTRL_BEARER_CONFIRM:
	case DECT_CCTRL_WAIT:
		return dect_parse_cctrl_common(cctl, t);
	case DECT_CCTRL_ATTRIBUTES_T_REQUEST:
	case DECT_CCTRL_ATTRIBUTES_T_CONFIRM:
		return dect_parse_cctrl_attr(cctl, t);
	case DECT_CCTRL_RELEASE:
		return dect_parse_cctrl_release(cctl, t);
	default:
		mac_print("unknown basic cctrl command: %llx\n",
			  (unsigned long long)cctl->cmd);
		return -1;
	}
}

static int dect_parse_advanced_cctrl(struct dect_tail_msg *tm, uint64_t t)
{
	struct dect_cctrl *cctl = &tm->cctl;

	cctl->cmd = t & DECT_MT_CMD_MASK;
	switch (cctl->cmd) {
	case DECT_CCTRL_ACCESS_REQ:
	case DECT_CCTRL_BEARER_HANDOVER_REQ:
	case DECT_CCTRL_CONNECTION_HANDOVER_REQ:
	case DECT_CCTRL_UNCONFIRMED_ACCESS_REQ:
	case DECT_CCTRL_BEARER_CONFIRM:
	case DECT_CCTRL_WAIT:
	case DECT_CCTRL_UNCONFIRMED_DUMMY:
	case DECT_CCTRL_UNCONFIRMED_HANDOVER:
		return dect_parse_cctrl_common(cctl, t);
	case DECT_CCTRL_ATTRIBUTES_T_REQUEST:
	case DECT_CCTRL_ATTRIBUTES_T_CONFIRM:
		return dect_parse_cctrl_attr(cctl, t);
	case DECT_CCTRL_BANDWIDTH_T_REQUEST:
	case DECT_CCTRL_BANDWIDTH_T_CONFIRM:
		return -1;
	case DECT_CCTRL_RELEASE:
		return dect_parse_cctrl_release(cctl, t);
	default:
		mac_print("unknown advanced cctrl command: %llx\n",
			  (unsigned long long)cctl->cmd);
		return -1;
	}
}

static int dect_parse_encryption_ctrl(struct dect_tail_msg *tm, uint64_t t)
{
	struct dect_encctrl *ectl = &tm->encctl;

	ectl->cmd  = (t & DECT_ENCCTRL_CMD_MASK) >> DECT_ENCCTRL_CMD_SHIFT;
	ectl->fmid = (t & DECT_ENCCTRL_FMID_MASK) >> DECT_ENCCTRL_FMID_SHIFT;
	ectl->pmid = (t & DECT_ENCCTRL_PMID_MASK) >> DECT_ENCCTRL_PMID_SHIFT;
	mac_print("encctrl: command: %x FMID: %.4x PMID: %.5x\n",
		  ectl->cmd, ectl->fmid, ectl->pmid);
	return 0;
}

static int dect_parse_mac_ctrl(struct dect_tail_msg *tm, uint64_t t)
{
	switch (t & DECT_MT_HDR_MASK) {
	case DECT_MT_BASIC_CCTRL:
		if (dect_parse_basic_cctrl(tm, t) < 0)
			return -1;
		tm->type = DECT_TM_TYPE_BCCTRL;
		return 0;
	case DECT_MT_ADV_CCTRL:
		if (dect_parse_advanced_cctrl(tm, t) < 0)
			return -1;
		tm->type = DECT_TM_TYPE_ACCTRL;
		return 0;
	case DECT_MT_ENC_CTRL:
		if (dect_parse_encryption_ctrl(tm, t) < 0)
			return -1;
		tm->type = DECT_TM_TYPE_ENCCTRL;
		return 0;
	default:
		mac_print("Unknown MAC control %llx\n",
			  (unsigned long long)t & DECT_MT_HDR_MASK);
		return -1;
	}
}

static int dect_parse_ct_data(struct dect_tail_msg *tm, uint64_t t, uint8_t seq)
{
	struct dect_ct_data *ctd = &tm->ctd;

	ctd->seq = seq;
	tm->type = DECT_TM_TYPE_CT;
	mac_print("CS tail: sequence number: %u\n", seq);
	return 0;
}

static int dect_parse_tail_msg(struct dect_tail_msg *tm,
			       const struct dect_msg_buf *mb)
{
	uint64_t t;

	tm->type = DECT_TM_TYPE_INVALID;
	t = __be64_to_cpu(*(uint64_t *)&mb->data[DECT_T_FIELD_OFF]);

	switch (dect_parse_tail(mb)) {
	case DECT_TI_CT_PKT_0:
		return dect_parse_ct_data(tm, t, 0);
	case DECT_TI_CT_PKT_1:
		return dect_parse_ct_data(tm, t, 1);
	case DECT_TI_NT_CL:
		mac_print("connectionless: ");
	case DECT_TI_NT:
		return dect_parse_identities_information(tm, t);
	case DECT_TI_QT:
		return dect_parse_system_information(tm, t);
	case DECT_TI_PT:
		/* Paging tail in direction FP->PP, MAC control otherwise */
		if (mb->slot < 12)
			return dect_parse_paging_msg(tm, t);
	case DECT_TI_MT:
		return dect_parse_mac_ctrl(tm, t);
	default:
		mac_print("unknown tail %x\n", dect_parse_tail(mb));
		return -1;
	}
}

/*
 * TBC
 */

#define tbc_log(tbc, fmt, args...) \
	dectmon_log("TBC: PMID: %.5x FMID: %.3x: " fmt, \
		    (tbc)->pmid, (tbc)->fmid, ## args)

static void dect_tbc_release(struct dect_handle *dh, struct dect_tbc *tbc)
{
	struct dect_handle_priv *priv = dect_handle_priv(dh);

	tbc_log(tbc, "release\n");
	if (dect_timer_running(tbc->timer))
		dect_timer_stop(dh, tbc->timer);
	priv->slots[tbc->slot1] = NULL;
	priv->slots[tbc->slot2] = NULL;
	free(tbc);
}

static void dect_tbc_timeout(struct dect_handle *dh, struct dect_timer *timer)
{
	struct dect_tbc *tbc = dect_timer_data(timer);

	tbc_log(tbc, "timeout\n");
	dect_tbc_release(dh, tbc);
}

static struct dect_tbc *dect_tbc_init(struct dect_handle *dh,
				      const struct dect_tail_msg *tm,
				      uint8_t slot)
{
	struct dect_handle_priv *priv = dect_handle_priv(dh);
	uint8_t slot2 = dect_tdd_slot(slot);
	struct dect_tbc *tbc;

	tbc = calloc(1, sizeof(*tbc));
	if (tbc == NULL)
		goto err1;

	tbc->slot1 = slot;
	tbc->slot2 = slot2;
	tbc->fmid  = tm->cctl.fmid;
	tbc->pmid  = tm->cctl.pmid;

	tbc->timer = dect_timer_alloc(dh);
	if (tbc->timer == NULL)
		goto err2;
	dect_timer_setup(tbc->timer, dect_tbc_timeout, tbc);
	dect_timer_start(dh, tbc->timer, 5);

	tbc->mbc[DECT_MODE_FP].cs_seq  = 1;
	tbc->mbc[DECT_MODE_FP].cf_seq  = 1;
	tbc->mbc[DECT_MODE_FP].mc.tbc  = tbc;

	tbc->mbc[DECT_MODE_PP].cs_seq  = 1;
	tbc->mbc[DECT_MODE_PP].cf_seq  = 1;
	tbc->mbc[DECT_MODE_PP].mc.tbc  = tbc;

	tbc->dl.tbc		       = tbc;

	priv->slots[slot]  = tbc;
	priv->slots[slot2] = tbc;
	tbc_log(tbc, "establish: slot %u/%u\n", slot, slot2);

	return tbc;

err2:
	free(tbc);
err1:
	return NULL;
}

static void dect_dsc_cipher(struct dect_tbc *tbc, struct dect_msg_buf *mb)
{
	unsigned int i;
	uint8_t *ks;

	if (mb->slot < DECT_HALF_FRAME_SIZE)
		ks = tbc->ks;
	else
		ks = tbc->ks + 45;

	switch (mb->data[0] & DECT_HDR_TA_MASK) {
	case DECT_TI_CT_PKT_0:
	case DECT_TI_CT_PKT_1:
		for (i = 0; i < 5; i++)
			mb->data[i + 1] ^= ks[i];
	default:
		break;
	}

	for (i = 0; i < DECT_B_FIELD_SIZE; i++)
		mb->data[i + 8] ^= ks[i + 5];
}

static void dect_tbc_rcv(struct dect_handle *dh, struct dect_tbc *tbc,
			 struct dect_msg_buf *mb, struct dect_tail_msg *tm)
{
	enum dect_b_identifications b_id;
	struct dect_mbc *mbc;
	unsigned int i;
	uint8_t slot = mb->slot;
	bool cf;

	if (tbc->ciphered) {
		if (slot < DECT_HALF_FRAME_SIZE)
			dect_dsc_keystream(dect_dsc_iv(mb->mfn, mb->frame),
					   tbc->dl.pt->dck,
					   tbc->ks, sizeof(tbc->ks));
		dect_dsc_cipher(tbc, mb);
	}

	if (tm->type == DECT_TM_TYPE_ID) {
		dect_timer_stop(dh, tbc->timer);
		dect_timer_start(dh, tbc->timer, 5);
	}

	mbc = &tbc->mbc[slot < DECT_HALF_FRAME_SIZE ? DECT_MODE_FP : DECT_MODE_PP];
	b_id = (mb->data[0] & DECT_HDR_BA_MASK);

	if (tm->type == DECT_TM_TYPE_CT) {
		if (tm->ctd.seq != mbc->cs_seq) {
			tbc_log(tbc, "CS: incorrect seq: %u\n", tm->ctd.seq);
			return;
		}
		mbc->cs_seq = !mbc->cs_seq;

		dect_mbuf_pull(mb, 1);
		dect_mac_co_data_ind(dh, &mbc->mc, DECT_MC_C_S, mb);
		dect_mbuf_pull(mb, 7);
	} else
		dect_mbuf_pull(mb, 8);

	cf = true;
	switch (b_id) {
	case DECT_BI_ETYPE_NOT_ALL_CF_0:
	case DECT_BI_ETYPE_NOT_ALL_CF_1:
		mac_print("Not all CF\n");
		cf = false;
	case DECT_BI_ETYPE_CF_0:
	case DECT_BI_ETYPE_CF_1:
		if (((b_id >> DECT_HDR_BA_SHIFT) & 0x1) != mbc->cf_seq) {
			tbc_log(tbc, "CF: incorrect seq: %u\n", b_id & 0x1);
			return;
		}
		mbc->cf_seq = !mbc->cf_seq;

		for (i = 0; i < mb->len / 10; i++) {
			if (cf) {
				tbc_log(tbc, "CF: seq: %u\n", i);
				dect_mac_co_data_ind(dh, &mbc->mc, DECT_MC_C_F, mb);
			} else if (!(mb->data[0] & 0x80))
				cf = true;

			dect_mbuf_pull(mb, 10);
		}
		break;
	case DECT_BI_UTYPE_0:
		dect_dl_u_data_ind(dh, &tbc->dl, slot < 12, mb);
		break;
	default:
		break;
	}

	switch (tm->type) {
	case DECT_TM_TYPE_BCCTRL:
	case DECT_TM_TYPE_ACCTRL:
		if (tm->cctl.cmd == DECT_CCTRL_RELEASE)
			dect_tbc_release(dh, tbc);
		break;
	case DECT_TM_TYPE_ENCCTRL:
		switch (tm->encctl.cmd) {
		case DECT_ENCCTRL_START_REQUEST:
			break;
		case DECT_ENCCTRL_START_CONFIRM:
		case DECT_ENCCTRL_START_GRANT:
			tbc_log(tbc, "ciphering enabled: %s\n",
			        slot < 12 ? "FP->PP" : "PP->FP");
			break;
		default:
			break;
		}
		break;
	default:
		break;
	}
}

void dect_mac_rcv(struct dect_handle *dh, struct dect_msg_buf *mb)
{
	struct dect_handle_priv *priv = dect_handle_priv(dh);
	struct dect_tbc *tbc = priv->slots[mb->slot];
	enum dect_tail_identifications a_id;
	enum dect_b_identifications b_id;
	struct dect_tail_msg tm;

	a_id = (mb->data[0] & DECT_HDR_TA_MASK) >> DECT_HDR_TA_SHIFT;
	b_id = (mb->data[0] & DECT_HDR_BA_MASK) >> DECT_HDR_BA_SHIFT;
	mac_print("slot: %02u A: %x B: %x ", mb->slot, a_id, b_id);

	dect_parse_tail_msg(&tm, mb);
	//dect_hexdump("MAC RCV", mb->data, mb->len);

	if (tbc != NULL)
		return dect_tbc_rcv(dh, tbc, mb, &tm);

	switch (tm.type) {
	case DECT_TM_TYPE_BCCTRL:
	case DECT_TM_TYPE_ACCTRL:
		switch (tm.cctl.cmd) {
		case DECT_CCTRL_ACCESS_REQ:
		case DECT_CCTRL_BEARER_HANDOVER_REQ:
		case DECT_CCTRL_CONNECTION_HANDOVER_REQ:
			dect_tbc_init(dh, &tm, mb->slot);
			break;
		default:
			break;
		}
		break;
	default:
		break;
	}
}