Merge remote-tracking branch 'origin/jerlbeck/master'

This adds EDGE support and at the same time is changing a lot of code
on GPRS support as well. Due my business decision of completing as much
as possible during the time we had the unit test coverage is not as
extensive as I had hoped for.

This is just the beginning. We do not support mixed GPRS/EDGE support
and have plenty of things to improve throughput.

Thanks to On-Waves for supporting a project with so many unknowns and
uncertainty and Jacob for leading the effort at sysmocom.

30 files changed, 3212 insertions, 1019 deletions

diff --git a/src/bitvector.cpp b/src/bitvector.cpp
index 43feebc5..10284073 100644
--- a/src/bitvector.cpp
+++ b/src/bitvector.cpp
@@ -117,3 +117,21 @@ int bitvec_write_field(struct bitvec *bv, unsigned& write_index, uint64_t val, u
 	write_index += len;
 	return 0;
 }
+
+int bitvec_write_field_lh(struct bitvec *bv, unsigned& write_index,
+		uint64_t val, unsigned len)
+{
+	unsigned int i;
+	int rc;
+	bv->cur_bit = write_index;
+	for (i = 0; i < len; i++) {
+		bit_value bit = L;
+		if (val & ((uint64_t)1 << (len - i - 1)))
+			bit = H;
+		rc = bitvec_set_bit(bv, bit);
+		if (rc)
+			return rc;
+	}
+	write_index += len;
+	return 0;
+}
diff --git a/src/bitvector.h b/src/bitvector.h
index 36bdbaba..b14d2040 100644
--- a/src/bitvector.h
+++ b/src/bitvector.h
@@ -39,6 +39,7 @@ unsigned int bitvec_pack(struct bitvec *bv, uint8_t *buffer);
 unsigned int bitvec_unpack(struct bitvec *bv, uint8_t *buffer);
 uint64_t bitvec_read_field(struct bitvec *bv, unsigned& read_index, unsigned len);
 int bitvec_write_field(struct bitvec *bv, unsigned& write_index, uint64_t val, unsigned len);
+int bitvec_write_field_lh(struct bitvec *bv, unsigned& write_index, uint64_t val, unsigned len);
 
 /*! }@ */
 
diff --git a/src/bts.cpp b/src/bts.cpp
index d1d738c6..715fb515 100644
--- a/src/bts.cpp
+++ b/src/bts.cpp
@@ -52,11 +52,14 @@ static BTS s_bts;
 static const struct rate_ctr_desc bts_ctr_description[] = {
 	{ "tbf.dl.alloc",		"TBF DL Allocated     "},
 	{ "tbf.dl.freed",		"TBF DL Freed         "},
+	{ "tbf.dl.aborted",		"TBF DL Aborted       "},
 	{ "tbf.ul.alloc",		"TBF UL Allocated     "},
 	{ "tbf.ul.freed",		"TBF UL Freed         "},
+	{ "tbf.ul.aborted",		"TBF UL Aborted       "},
 	{ "tbf.reused",			"TBF Reused           "},
 	{ "tbf.alloc.algo-a",		"TBF Alloc Algo A     "},
 	{ "tbf.alloc.algo-b",		"TBF Alloc Algo B     "},
+	{ "tbf.failed.egprs-only",	"TBF Failed EGPRS-only"},
 	{ "rlc.sent",			"RLC Sent             "},
 	{ "rlc.resent",			"RLC Resent           "},
 	{ "rlc.restarted",		"RLC Restarted        "},
@@ -127,8 +130,6 @@ BTS::BTS()
 	, m_ms_store(this)
 {
 	memset(&m_bts, 0, sizeof(m_bts));
-	INIT_LLIST_HEAD(&m_bts.ul_tbfs);
-	INIT_LLIST_HEAD(&m_bts.dl_tbfs);
 	m_bts.bts = this;
 
 	/* initialize back pointers */
@@ -214,14 +215,14 @@ int BTS::add_paging(uint8_t chan_needed, uint8_t *identity_lv)
 {
 	uint8_t l, trx, ts, any_tbf = 0;
 	struct gprs_rlcmac_tbf *tbf;
-	struct llist_pods *lpods;
+	LListHead<gprs_rlcmac_tbf> *pos;
 	struct gprs_rlcmac_paging *pag;
 	uint8_t slot_mask[8];
 	int8_t first_ts; /* must be signed */
 
-	llist_head *tbfs_lists[] = {
-		&m_bts.ul_tbfs,
-		&m_bts.dl_tbfs,
+	LListHead<gprs_rlcmac_tbf> *tbfs_lists[] = {
+		&m_ul_tbfs,
+		&m_dl_tbfs,
 		NULL
 	};
 
@@ -235,7 +236,8 @@ int BTS::add_paging(uint8_t chan_needed, uint8_t *identity_lv)
 	 * Don't mark, if TBF uses a different slot that is already marked. */
 	memset(slot_mask, 0, sizeof(slot_mask));
 	for (l = 0; tbfs_lists[l]; l++) {
-		llist_pods_for_each_entry(tbf, tbfs_lists[l], list, lpods) {
+		llist_for_each(pos, tbfs_lists[l]) {
+			tbf = pos->entry();
 			first_ts = -1;
 			for (ts = 0; ts < 8; ts++) {
 				if (tbf->pdch[ts]) {
@@ -296,16 +298,17 @@ int BTS::add_paging(uint8_t chan_needed, uint8_t *identity_lv)
 gprs_rlcmac_dl_tbf *BTS::dl_tbf_by_poll_fn(uint32_t fn, uint8_t trx, uint8_t ts)
 {
 	struct gprs_rlcmac_dl_tbf *tbf;
-	struct llist_pods *lpods;
+	LListHead<gprs_rlcmac_tbf> *pos;
 
 	/* only one TBF can poll on specific TS/FN, because scheduler can only
 	 * schedule one downlink control block (with polling) at a FN per TS */
-	llist_pods_for_each_entry(tbf, &m_bts.dl_tbfs, list, lpods) {
+	llist_for_each(pos, &m_dl_tbfs) {
+		tbf = as_dl_tbf(pos->entry());
 		if (tbf->state_is_not(GPRS_RLCMAC_RELEASING)
 		 && tbf->poll_state == GPRS_RLCMAC_POLL_SCHED
 		 && tbf->poll_fn == fn && tbf->trx->trx_no == trx
-		 && tbf->control_ts == ts) {
-			return static_cast<gprs_rlcmac_dl_tbf *>(tbf);
+		 && tbf->poll_ts == ts) {
+			return tbf;
 		}
 	}
 	return NULL;
@@ -313,16 +316,17 @@ gprs_rlcmac_dl_tbf *BTS::dl_tbf_by_poll_fn(uint32_t fn, uint8_t trx, uint8_t ts)
 gprs_rlcmac_ul_tbf *BTS::ul_tbf_by_poll_fn(uint32_t fn, uint8_t trx, uint8_t ts)
 {
 	struct gprs_rlcmac_ul_tbf *tbf;
-	struct llist_pods *lpods;
+	LListHead<gprs_rlcmac_tbf> *pos;
 
 	/* only one TBF can poll on specific TS/FN, because scheduler can only
 	 * schedule one downlink control block (with polling) at a FN per TS */
-	llist_pods_for_each_entry(tbf, &m_bts.ul_tbfs, list, lpods) {
+	llist_for_each(pos, &m_ul_tbfs) {
+		tbf = as_ul_tbf(pos->entry());
 		if (tbf->state_is_not(GPRS_RLCMAC_RELEASING)
 		 && tbf->poll_state == GPRS_RLCMAC_POLL_SCHED
 		 && tbf->poll_fn == fn && tbf->trx->trx_no == trx
-		 && tbf->control_ts == ts) {
-			return static_cast<gprs_rlcmac_ul_tbf *>(tbf);
+		 && tbf->poll_ts == ts) {
+			return tbf;
 		}
 	}
 	return NULL;
@@ -462,7 +466,10 @@ int BTS::rcv_rach(uint8_t ra, uint32_t Fn, int16_t qta)
 	uint8_t sb = 0;
 	uint32_t sb_fn = 0;
 	int rc;
-	uint8_t plen;
+	int plen;
+	uint8_t usf = 7;
+	uint8_t tsc;
+	uint16_t ta;
 
 	rach_frame();
 
@@ -481,8 +488,11 @@ int BTS::rcv_rach(uint8_t ra, uint32_t Fn, int16_t qta)
 		qta = 0;
 	if (qta > 252)
 		qta = 252;
+
+	ta = qta >> 2;
+
 	if (sb) {
-		rc = sba()->alloc(&trx_no, &ts_no, &sb_fn, qta >> 2);
+		rc = sba()->alloc(&trx_no, &ts_no, &sb_fn, ta);
 		if (rc < 0)
 			return rc;
 		LOGP(DRLCMAC, LOGL_DEBUG, "RX: [PCU <- BTS] RACH qbit-ta=%d "
@@ -492,6 +502,7 @@ int BTS::rcv_rach(uint8_t ra, uint32_t Fn, int16_t qta)
 			sb_fn);
 		LOGP(DRLCMAC, LOGL_INFO, "TX: Immediate Assignment Uplink "
 			"(AGCH)\n");
+		tsc = m_bts.trx[trx_no].pdch[ts_no].tsc;
 	} else {
 		// Create new TBF
 		#warning "Copy and pate with other routines.."
@@ -502,7 +513,7 @@ int BTS::rcv_rach(uint8_t ra, uint32_t Fn, int16_t qta)
 			/* FIXME: send reject */
 			return -EBUSY;
 		}
-		tbf->set_ta(qta >> 2);
+		tbf->set_ta(ta);
 		tbf->set_state(GPRS_RLCMAC_FLOW);
 		tbf->state_flags |= (1 << GPRS_RLCMAC_FLAG_CCCH);
 		tbf_timer_start(tbf, 3169, m_bts.t3169, 0);
@@ -514,21 +525,31 @@ int BTS::rcv_rach(uint8_t ra, uint32_t Fn, int16_t qta)
 			qta, ra, Fn, (Fn / (26 * 51)) % 32, Fn % 51, Fn % 26);
 		LOGP(DRLCMAC, LOGL_INFO, "%s TX: START Immediate "
 			"Assignment Uplink (AGCH)\n", tbf_name(tbf));
+		trx_no = tbf->trx->trx_no;
+		ts_no = tbf->first_ts;
+		usf = tbf->m_usf[ts_no];
+		tsc = tbf->tsc();
 	}
 	bitvec *immediate_assignment = bitvec_alloc(22) /* without plen */;
 	bitvec_unhex(immediate_assignment,
 		"2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b");
-	if (sb)
-		plen = Encoding::write_immediate_assignment(&m_bts, immediate_assignment, 0, ra,
-			Fn, qta >> 2, m_bts.trx[trx_no].arfcn, ts_no,
-			m_bts.trx[trx_no].pdch[ts_no].tsc, 0, 0, 0, 0, sb_fn, 1,
-			m_bts.alpha, m_bts.gamma, -1);
-	else
-		plen = Encoding::write_immediate_assignment(&m_bts, immediate_assignment, 0, ra,
-			Fn, tbf->ta(), tbf->trx->arfcn, tbf->first_ts, tbf->tsc(),
-			tbf->tfi(), tbf->m_usf[tbf->first_ts], 0, 0, 0, 0,
-			m_bts.alpha, m_bts.gamma, -1);
-	pcu_l1if_tx_agch(immediate_assignment, plen);
+
+	LOGP(DRLCMAC, LOGL_DEBUG,
+		" - TRX=%d (%d) TS=%d TA=%d TSC=%d TFI=%d USF=%d\n",
+		trx_no, m_bts.trx[trx_no].arfcn, ts_no, ta, tsc,
+		tbf ? tbf->tfi() : -1, usf);
+
+	plen = Encoding::write_immediate_assignment(
+		tbf, immediate_assignment, 0, ra, Fn, ta,
+		m_bts.trx[trx_no].arfcn, ts_no, tsc, usf, 0, sb_fn,
+		m_bts.alpha, m_bts.gamma, -1);
+
+	if (plen >= 0) {
+		pcu_l1if_tx_agch(immediate_assignment, plen);
+		if (tbf)
+			tbf->set_state(GPRS_RLCMAC_WAIT_ASSIGN);
+	}
+
 	bitvec_free(immediate_assignment);
 
 	return 0;
@@ -552,7 +573,8 @@ void BTS::trigger_dl_ass(
 
 		/* change state */
 		dl_tbf->set_state(GPRS_RLCMAC_ASSIGN);
-		dl_tbf->state_flags |= (1 << GPRS_RLCMAC_FLAG_PACCH);
+		if (!(dl_tbf->state_flags & (1 << GPRS_RLCMAC_FLAG_CCCH)))
+			dl_tbf->state_flags |= (1 << GPRS_RLCMAC_FLAG_PACCH);
 		/* start timer */
 		tbf_timer_start(dl_tbf, 0, Tassign_pacch);
 	} else {
@@ -570,17 +592,24 @@ void BTS::trigger_dl_ass(
 void BTS::snd_dl_ass(gprs_rlcmac_tbf *tbf, uint8_t poll, const char *imsi)
 {
 	int plen;
+	unsigned int ts = tbf->first_ts;
 
 	LOGP(DRLCMAC, LOGL_INFO, "TX: START %s Immediate Assignment Downlink (PCH)\n", tbf_name(tbf));
 	bitvec *immediate_assignment = bitvec_alloc(22); /* without plen */
 	bitvec_unhex(immediate_assignment, "2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b");
 	/* use request reference that has maximum distance to current time,
 	 * so the assignment will not conflict with possible RACH requests. */
-	plen = Encoding::write_immediate_assignment(&m_bts, immediate_assignment, 1, 125,
-		(tbf->pdch[tbf->first_ts]->last_rts_fn + 21216) % 2715648, tbf->ta(),
-		tbf->trx->arfcn, tbf->first_ts, tbf->tsc(), tbf->tfi(), 0, tbf->tlli(), poll,
-		tbf->poll_fn, 0, m_bts.alpha, m_bts.gamma, -1);
-	pcu_l1if_tx_pch(immediate_assignment, plen, imsi);
+	LOGP(DRLCMAC, LOGL_DEBUG, " - TRX=%d (%d) TS=%d TA=%d pollFN=%d\n",
+		tbf->trx->trx_no, tbf->trx->arfcn,
+		ts, tbf->ta(), poll ? tbf->poll_fn : -1);
+	plen = Encoding::write_immediate_assignment(tbf, immediate_assignment, 1, 125,
+		(tbf->pdch[ts]->last_rts_fn + 21216) % 2715648, tbf->ta(),
+		tbf->trx->arfcn, ts, tbf->tsc(), 7, poll,
+		tbf->poll_fn, m_bts.alpha, m_bts.gamma, -1);
+	if (plen >= 0) {
+		pcu_l1if_tx_pch(immediate_assignment, plen, imsi);
+		tbf->set_state(GPRS_RLCMAC_WAIT_ASSIGN);
+	}
 	bitvec_free(immediate_assignment);
 }
 
@@ -775,9 +804,19 @@ void gprs_rlcmac_pdch::rcv_control_ack(Packet_Control_Acknowledgement_t *packet,
 				"TBF is gone TLLI=0x%08x\n", tlli);
 			return;
 		}
-		if (tbf->state_is(GPRS_RLCMAC_WAIT_RELEASE))
+		if (tbf->state_is(GPRS_RLCMAC_WAIT_RELEASE) &&
+				tbf->direction == new_tbf->direction)
 			tbf_free(tbf);
 
+		if ((new_tbf->state_flags & (1 << GPRS_RLCMAC_FLAG_CCCH))) {
+			/* We now know that the PACCH really existed */
+			LOGP(DRLCMAC, LOGL_INFO,
+				"The TBF has been confirmed on the PACCH, "
+				"changed type from CCCH to PACCH for %s\n",
+				tbf_name(new_tbf));
+			new_tbf->state_flags &= ~(1 << GPRS_RLCMAC_FLAG_CCCH);
+			new_tbf->state_flags |= (1 << GPRS_RLCMAC_FLAG_PACCH);
+		}
 		new_tbf->set_state(GPRS_RLCMAC_FLOW);
 		/* stop pending assignment timer */
 		new_tbf->stop_timer();
@@ -803,7 +842,8 @@ void gprs_rlcmac_pdch::rcv_control_ack(Packet_Control_Acknowledgement_t *packet,
 				"TBF is gone TLLI=0x%08x\n", tlli);
 			return;
 		}
-		if (tbf->state_is(GPRS_RLCMAC_WAIT_RELEASE))
+		if (tbf->state_is(GPRS_RLCMAC_WAIT_RELEASE) &&
+				tbf->direction == new_tbf->direction)
 			tbf_free(tbf);
 
 		new_tbf->set_state(GPRS_RLCMAC_FLOW);
@@ -891,6 +931,11 @@ void gprs_rlcmac_pdch::rcv_control_dl_ack_nack(Packet_Downlink_Ack_Nack_t *ack_n
 	struct gprs_rlcmac_dl_tbf *tbf;
 	int rc;
 	struct pcu_l1_meas meas;
+	int num_blocks;
+	uint8_t bits_data[RLC_GPRS_WS/8];
+	bitvec bits;
+	int bsn_begin, bsn_end;
+	char show_bits[RLC_GPRS_WS + 1];
 
 	tfi = ack_nack->DOWNLINK_TFI;
 	tbf = bts()->dl_tbf_by_poll_fn(fn, trx_no(), ts_no);
@@ -917,10 +962,24 @@ void gprs_rlcmac_pdch::rcv_control_dl_ack_nack(Packet_Downlink_Ack_Nack_t *ack_n
 	LOGP(DRLCMAC, LOGL_DEBUG, "RX: [PCU <- BTS] %s Packet Downlink Ack/Nack\n", tbf_name(tbf));
 	tbf->poll_state = GPRS_RLCMAC_POLL_NONE;
 
+	bits.data = bits_data;
+	bits.data_len = sizeof(bits_data);
+	bits.cur_bit = 0;
+
+	num_blocks = Decoding::decode_gprs_acknack_bits(
+		&ack_nack->Ack_Nack_Description, &bits,
+		&bsn_begin, &bsn_end, &tbf->m_window);
+
+	LOGP(DRLCMAC, LOGL_DEBUG,
+		"Got GPRS DL ACK bitmap: SSN: %d, BSN %d to %d - 1 (%d blocks), "
+		"\"%s\"\n",
+		ack_nack->Ack_Nack_Description.STARTING_SEQUENCE_NUMBER,
+		bsn_begin, bsn_end, num_blocks,
+		(Decoding::extract_rbb(&bits, show_bits), show_bits));
+
 	rc = tbf->rcvd_dl_ack(
 		ack_nack->Ack_Nack_Description.FINAL_ACK_INDICATION,
-		ack_nack->Ack_Nack_Description.STARTING_SEQUENCE_NUMBER,
-		ack_nack->Ack_Nack_Description.RECEIVED_BLOCK_BITMAP);
+		bsn_begin, &bits);
 	if (rc == 1) {
 		tbf_free(tbf);
 		return;
@@ -945,6 +1004,112 @@ void gprs_rlcmac_pdch::rcv_control_dl_ack_nack(Packet_Downlink_Ack_Nack_t *ack_n
 	}
 }
 
+void gprs_rlcmac_pdch::rcv_control_egprs_dl_ack_nack(EGPRS_PD_AckNack_t *ack_nack, uint32_t fn)
+{
+	int8_t tfi = 0; /* must be signed */
+	struct gprs_rlcmac_dl_tbf *tbf;
+	struct pcu_l1_meas meas;
+	int rc;
+	int num_blocks;
+	uint8_t bits_data[RLC_EGPRS_MAX_WS/8];
+	char show_bits[RLC_EGPRS_MAX_WS + 1];
+	bitvec bits;
+	int bsn_begin, bsn_end;
+
+	tfi = ack_nack->DOWNLINK_TFI;
+	tbf = bts()->dl_tbf_by_poll_fn(fn, trx_no(), ts_no);
+	if (!tbf) {
+		LOGP(DRLCMAC, LOGL_NOTICE, "EGPRS PACKET DOWNLINK ACK with "
+			"unknown FN=%u TFI=%d (TRX %d TS %d)\n",
+			fn, tfi, trx_no(), ts_no);
+		return;
+	}
+	if (tbf->tfi() != tfi) {
+		LOGP(DRLCMAC, LOGL_NOTICE, "EGPRS PACKET DOWNLINK ACK with "
+			"wrong TFI=%d, ignoring!\n", tfi);
+		return;
+	}
+	tbf->state_flags |= (1 << GPRS_RLCMAC_FLAG_DL_ACK);
+	if ((tbf->state_flags & (1 << GPRS_RLCMAC_FLAG_TO_DL_ACK))) {
+		tbf->state_flags &= ~(1 << GPRS_RLCMAC_FLAG_TO_DL_ACK);
+		LOGP(DRLCMAC, LOGL_NOTICE, "Recovered EGPRS downlink ack "
+			"for %s\n", tbf_name(tbf));
+	}
+	/* reset N3105 */
+	tbf->n3105 = 0;
+	tbf->stop_t3191();
+	LOGP(DRLCMAC, LOGL_DEBUG,
+		"RX: [PCU <- BTS] %s EGPRS Packet Downlink Ack/Nack\n",
+		tbf_name(tbf));
+	tbf->poll_state = GPRS_RLCMAC_POLL_NONE;
+
+	LOGP(DRLCMAC, LOGL_DEBUG, "EGPRS ACK/NACK: "
+		"ut: %d, final: %d, bow: %d, eow: %d, ssn: %d, have_crbb: %d, "
+		"urbb_len:%d, %p, %p, %d, %d, win: %d-%d, urbb: %s\n",
+		(int)ack_nack->EGPRS_AckNack.UnionType,
+		(int)ack_nack->EGPRS_AckNack.Desc.FINAL_ACK_INDICATION,
+		(int)ack_nack->EGPRS_AckNack.Desc.BEGINNING_OF_WINDOW,
+		(int)ack_nack->EGPRS_AckNack.Desc.END_OF_WINDOW,
+		(int)ack_nack->EGPRS_AckNack.Desc.STARTING_SEQUENCE_NUMBER,
+		(int)ack_nack->EGPRS_AckNack.Desc.Exist_CRBB,
+		(int)ack_nack->EGPRS_AckNack.Desc.URBB_LENGTH,
+		(void *)&ack_nack->EGPRS_AckNack.UnionType,
+		(void *)&ack_nack->EGPRS_AckNack.Desc,
+		(int)offsetof(EGPRS_AckNack_t, Desc),
+		(int)offsetof(EGPRS_AckNack_w_len_t, Desc),
+		tbf->m_window.v_a(),
+		tbf->m_window.v_s(),
+		osmo_hexdump((const uint8_t *)&ack_nack->EGPRS_AckNack.Desc.URBB,
+			sizeof(ack_nack->EGPRS_AckNack.Desc.URBB)));
+
+	bits.data = bits_data;
+	bits.data_len = sizeof(bits_data);
+	bits.cur_bit = 0;
+
+	num_blocks = Decoding::decode_egprs_acknack_bits(
+		&ack_nack->EGPRS_AckNack.Desc, &bits,
+		&bsn_begin, &bsn_end, &tbf->m_window);
+
+	LOGP(DRLCMAC, LOGL_DEBUG,
+		"Got EGPRS DL ACK bitmap: SSN: %d, BSN %d to %d - 1 (%d blocks), "
+		"\"%s\"\n",
+		ack_nack->EGPRS_AckNack.Desc.STARTING_SEQUENCE_NUMBER,
+		bsn_begin, bsn_end, num_blocks,
+		(Decoding::extract_rbb(&bits, show_bits), show_bits)
+	    );
+
+	rc = tbf->rcvd_dl_ack(
+		ack_nack->EGPRS_AckNack.Desc.FINAL_ACK_INDICATION,
+		bsn_begin, &bits);
+	if (rc == 1) {
+		tbf_free(tbf);
+		return;
+	}
+
+	/* check for channel request */
+	if (ack_nack->Exist_ChannelRequestDescription) {
+		LOGP(DRLCMAC, LOGL_DEBUG, "MS requests UL TBF in ack "
+			"message, so we provide one:\n");
+
+		/* This call will register the new TBF with the MS on success */
+		tbf_alloc_ul(bts_data(), tbf->trx->trx_no,
+			tbf->ms_class(), tbf->ms()->egprs_ms_class(),
+			tbf->tlli(), tbf->ta(), tbf->ms());
+
+		/* schedule uplink assignment */
+		tbf->ul_ass_state = GPRS_RLCMAC_UL_ASS_SEND_ASS;
+	}
+
+	/* get measurements */
+	if (tbf->ms()) {
+		/* TODO: Implement Measurements parsing for EGPRS */
+		/*
+		get_meas(&meas, &ack_nack->Channel_Quality_Report);
+		tbf->ms()->update_l1_meas(&meas);
+		*/
+	}
+}
+
 void gprs_rlcmac_pdch::rcv_resource_request(Packet_Resource_Request_t *request, uint32_t fn)
 {
 	struct gprs_rlcmac_sba *sba;
@@ -968,28 +1133,24 @@ void gprs_rlcmac_pdch::rcv_resource_request(Packet_Resource_Request_t *request,
 			ta = ms->ta();
 		}
 
+		/* We got a RACH so the MS was in packet idle mode and thus
+		 * didn't have any active TBFs */
 		if (ul_tbf) {
 			LOGP(DRLCMACUL, LOGL_NOTICE, "Got RACH from "
 				"TLLI=0x%08x while %s still "
 				"exists. Killing pending UL TBF\n",
 				tlli, tbf_name(ul_tbf));
-			/* The MS will not use the old TBF again, so we can
-			 * safely throw it away immediately */
 			tbf_free(ul_tbf);
 			ul_tbf = NULL;
 		}
 
 		if (dl_tbf) {
-			/* TODO: There a chance that releasing dl_tbf can be
-			 * avoided if this PDCH is the control TS of dl_tbf,
-			 * but this needs to be checked with the spec. If an MS
-			 * losed the DL TBF because of PDCH mismatches only,
-			 * this check would make sense. */
 			LOGP(DRLCMACUL, LOGL_NOTICE, "Got RACH from "
 				"TLLI=0x%08x while %s still exists. "
 				"Release pending DL TBF\n", tlli,
 				tbf_name(dl_tbf));
-			dl_tbf->release();
+			tbf_free(dl_tbf);
+			dl_tbf = NULL;
 		}
 		LOGP(DRLCMAC, LOGL_DEBUG, "MS requests UL TBF "
 			"in packet resource request of single "
@@ -1104,6 +1265,9 @@ int gprs_rlcmac_pdch::rcv_control_block(
 	case MT_PACKET_DOWNLINK_ACK_NACK:
 		rcv_control_dl_ack_nack(&ul_control_block->u.Packet_Downlink_Ack_Nack, fn);
 		break;
+	case MT_EGPRS_PACKET_DOWNLINK_ACK_NACK:
+		rcv_control_egprs_dl_ack_nack(&ul_control_block->u.Egprs_Packet_Downlink_Ack_Nack, fn);
+		break;
 	case MT_PACKET_RESOURCE_REQUEST:
 		rcv_resource_request(&ul_control_block->u.Packet_Resource_Request, fn);
 		break;
@@ -1137,7 +1301,7 @@ int gprs_rlcmac_pdch::rcv_block(uint8_t *data, uint8_t len, uint32_t fn,
 	}
 
 	LOGP(DRLCMACUL, LOGL_DEBUG, "Got RLC block, coding scheme: %s, "
-		"length: %d (%d))\n", cs.name(), len, cs.maxBytesUL());
+		"length: %d (%d))\n", cs.name(), len, cs.usedSizeUL());
 
 	if (cs.isGprs())
 		return rcv_block_gprs(data, fn, meas, cs);
@@ -1155,9 +1319,9 @@ int gprs_rlcmac_pdch::rcv_data_block(uint8_t *data, uint32_t fn,
 	struct pcu_l1_meas *meas, GprsCodingScheme cs)
 {
 	int rc;
-	struct gprs_rlc_ul_header_egprs rlc_dec;
+	struct gprs_rlc_data_info rlc_dec;
 	struct gprs_rlcmac_ul_tbf *tbf;
-	unsigned len = cs.maxBytesUL();
+	unsigned len = cs.sizeUL();
 
 	/* These are always data blocks, since EGPRS still uses CS-1 for
 	 * control blocks (see 44.060, section 10.3, 1st par.)
@@ -1180,6 +1344,8 @@ int gprs_rlcmac_pdch::rcv_data_block(uint8_t *data, uint32_t fn,
 		}
 	}
 
+	LOGP(DRLCMACUL, LOGL_DEBUG, "  UL data: %s\n", osmo_hexdump(data, len));
+
 	rc = Decoding::rlc_parse_ul_data_header(&rlc_dec, data, cs);
 	if (rc < 0) {
 		LOGP(DRLCMACUL, LOGL_ERROR,
@@ -1205,7 +1371,7 @@ int gprs_rlcmac_pdch::rcv_data_block(uint8_t *data, uint32_t fn,
 		return 0;
 	}
 
-	return tbf->rcv_data_block_acknowledged(&rlc_dec, data, len, meas);
+	return tbf->rcv_data_block_acknowledged(&rlc_dec, data, meas);
 }
 
 int gprs_rlcmac_pdch::rcv_block_gprs(uint8_t *data, uint32_t fn,
@@ -1239,13 +1405,15 @@ int gprs_rlcmac_pdch::rcv_block_gprs(uint8_t *data, uint32_t fn,
 	return rc;
 }
 
-gprs_rlcmac_tbf *gprs_rlcmac_pdch::tbf_from_list_by_tfi(struct llist_head *tbf_list, uint8_t tfi,
+gprs_rlcmac_tbf *gprs_rlcmac_pdch::tbf_from_list_by_tfi(
+		LListHead<gprs_rlcmac_tbf> *tbf_list, uint8_t tfi,
 		enum gprs_rlcmac_tbf_direction dir)
 {
 	gprs_rlcmac_tbf *tbf;
-	struct llist_pods *lpods;
+	LListHead<gprs_rlcmac_tbf> *pos;
 
-	llist_pods_for_each_entry(tbf, tbf_list, list, lpods) {
+	llist_for_each(pos, tbf_list) {
+		tbf = pos->entry();
 		if (tbf->tfi() != tfi)
 			continue;
 		if (!tbf->pdch[ts_no])
@@ -1257,14 +1425,12 @@ gprs_rlcmac_tbf *gprs_rlcmac_pdch::tbf_from_list_by_tfi(struct llist_head *tbf_l
 
 gprs_rlcmac_ul_tbf *gprs_rlcmac_pdch::ul_tbf_by_tfi(uint8_t tfi)
 {
-	return static_cast<gprs_rlcmac_ul_tbf *>(
-		tbf_by_tfi(tfi, GPRS_RLCMAC_UL_TBF));
+	return as_ul_tbf(tbf_by_tfi(tfi, GPRS_RLCMAC_UL_TBF));
 }
 
 gprs_rlcmac_dl_tbf *gprs_rlcmac_pdch::dl_tbf_by_tfi(uint8_t tfi)
 {
-	return static_cast<gprs_rlcmac_dl_tbf *>(
-		tbf_by_tfi(tfi, GPRS_RLCMAC_DL_TBF));
+	return as_dl_tbf(tbf_by_tfi(tfi, GPRS_RLCMAC_DL_TBF));
 }
 
 /* lookup TBF Entity (by TFI) */
@@ -1300,7 +1466,7 @@ void gprs_rlcmac_pdch::attach_tbf(gprs_rlcmac_tbf *tbf)
 
 	m_num_tbfs[tbf->direction] += 1;
 	if (tbf->direction == GPRS_RLCMAC_UL_TBF) {
-		ul_tbf = static_cast<gprs_rlcmac_ul_tbf *>(tbf);
+		ul_tbf = as_ul_tbf(tbf);
 		m_assigned_usf |= 1 << ul_tbf->m_usf[ts_no];
 	}
 	m_assigned_tfi[tbf->direction] |= 1UL << tbf->tfi();
@@ -1320,7 +1486,7 @@ void gprs_rlcmac_pdch::detach_tbf(gprs_rlcmac_tbf *tbf)
 
 	m_num_tbfs[tbf->direction] -= 1;
 	if (tbf->direction == GPRS_RLCMAC_UL_TBF) {
-		ul_tbf = static_cast<gprs_rlcmac_ul_tbf *>(tbf);
+		ul_tbf = as_ul_tbf(tbf);
 		m_assigned_usf &= ~(1 << ul_tbf->m_usf[ts_no]);
 	}
 	m_assigned_tfi[tbf->direction] &= ~(1UL << tbf->tfi());
diff --git a/src/bts.h b/src/bts.h
index 45432eaa..119f6b2c 100644
--- a/src/bts.h
+++ b/src/bts.h
@@ -107,9 +107,11 @@ private:
 
 	void rcv_control_ack(Packet_Control_Acknowledgement_t *, uint32_t fn);
 	void rcv_control_dl_ack_nack(Packet_Downlink_Ack_Nack_t *, uint32_t fn);
+	void rcv_control_egprs_dl_ack_nack(EGPRS_PD_AckNack_t *, uint32_t fn);
 	void rcv_resource_request(Packet_Resource_Request_t *t, uint32_t fn);
 	void rcv_measurement_report(Packet_Measurement_Report_t *t, uint32_t fn);
-	gprs_rlcmac_tbf *tbf_from_list_by_tfi(struct llist_head *tbf_list, uint8_t tfi,
+	gprs_rlcmac_tbf *tbf_from_list_by_tfi(
+		LListHead<gprs_rlcmac_tbf> *tbf_list, uint8_t tfi,
 		enum gprs_rlcmac_tbf_direction dir);
 	gprs_rlcmac_tbf *tbf_by_tfi(uint8_t tfi,
 		enum gprs_rlcmac_tbf_direction dir);
@@ -154,7 +156,9 @@ struct gprs_rlcmac_bts {
 	uint8_t cs3;
 	uint8_t cs4;
 	uint8_t initial_cs_dl, initial_cs_ul;
+	uint8_t initial_mcs_dl, initial_mcs_ul;
 	uint8_t max_cs_dl, max_cs_ul;
+	uint8_t max_mcs_dl, max_mcs_ul;
 	uint8_t force_cs;	/* 0=use from BTS 1=use from VTY */
 	uint16_t force_llc_lifetime; /* overrides lifetime from SGSN */
 	uint32_t llc_discard_csec;
@@ -184,12 +188,8 @@ struct gprs_rlcmac_bts {
 	uint8_t cs_adj_lower_limit;
 	struct {int16_t low; int16_t high;} cs_lqual_ranges[4];
 	uint16_t cs_downgrade_threshold; /* downgrade if less packets left (DL) */
-
-	/* TBF handling, make private or move into TBFController */
-	/* list of uplink TBFs */
-	struct llist_head ul_tbfs;
-	/* list of downlink TBFs */
-	struct llist_head dl_tbfs;
+	uint16_t ws_base;
+	uint16_t ws_pdch; /* increase WS by this value per PDCH */
 
 	/* State for dynamic algorithm selection */
 	int multislot_disabled;
@@ -212,11 +212,14 @@ public:
 	enum {
 		CTR_TBF_DL_ALLOCATED,
 		CTR_TBF_DL_FREED,
+		CTR_TBF_DL_ABORTED,
 		CTR_TBF_UL_ALLOCATED,
 		CTR_TBF_UL_FREED,
+		CTR_TBF_UL_ABORTED,
 		CTR_TBF_REUSED,
 		CTR_TBF_ALLOC_ALGO_A,
 		CTR_TBF_ALLOC_ALGO_B,
+		CTR_TBF_FAILED_EGPRS_ONLY,
 		CTR_RLC_SENT,
 		CTR_RLC_RESENT,
 		CTR_RLC_RESTARTED,
@@ -285,11 +288,14 @@ public:
 	 */
 	void tbf_dl_created();
 	void tbf_dl_freed();
+	void tbf_dl_aborted();
 	void tbf_ul_created();
 	void tbf_ul_freed();
+	void tbf_ul_aborted();
 	void tbf_reused();
 	void tbf_alloc_algo_a();
 	void tbf_alloc_algo_b();
+	void tbf_failed_egprs_only();
 	void rlc_sent();
 	void rlc_resent();
 	void rlc_restarted();
@@ -319,6 +325,8 @@ public:
 	struct rate_ctr_group *rate_counters() const;
 	struct osmo_stat_item_group *stat_items() const;
 
+	LListHead<gprs_rlcmac_tbf>& ul_tbfs();
+	LListHead<gprs_rlcmac_tbf>& dl_tbfs();
 private:
 	int m_cur_fn;
 	int m_cur_blk_fn;
@@ -330,6 +338,11 @@ private:
 
 	GprsMsStorage m_ms_store;
 
+	/* list of uplink TBFs */
+	LListHead<gprs_rlcmac_tbf> m_ul_tbfs;
+	/* list of downlink TBFs */
+	LListHead<gprs_rlcmac_tbf> m_dl_tbfs;
+
 private:
 	/* disable copying to avoid slicing */
 	BTS(const BTS&);
@@ -361,6 +374,16 @@ inline GprsMs *BTS::ms_by_imsi(const char *imsi)
 	return ms_store().get_ms(0, 0, imsi);
 }
 
+inline LListHead<gprs_rlcmac_tbf>& BTS::ul_tbfs()
+{
+	return m_ul_tbfs;
+}
+
+inline LListHead<gprs_rlcmac_tbf>& BTS::dl_tbfs()
+{
+	return m_dl_tbfs;
+}
+
 inline BTS *gprs_rlcmac_pdch::bts() const
 {
 	return trx->bts;
@@ -405,11 +428,14 @@ inline struct osmo_stat_item_group *BTS::stat_items() const
 
 CREATE_COUNT_INLINE(tbf_dl_created, CTR_TBF_DL_ALLOCATED)
 CREATE_COUNT_INLINE(tbf_dl_freed, CTR_TBF_DL_FREED)
+CREATE_COUNT_INLINE(tbf_dl_aborted, CTR_TBF_DL_ABORTED)
 CREATE_COUNT_INLINE(tbf_ul_created, CTR_TBF_UL_ALLOCATED)
 CREATE_COUNT_INLINE(tbf_ul_freed, CTR_TBF_UL_FREED)
+CREATE_COUNT_INLINE(tbf_ul_aborted, CTR_TBF_UL_ABORTED)
 CREATE_COUNT_INLINE(tbf_reused, CTR_TBF_REUSED)
 CREATE_COUNT_INLINE(tbf_alloc_algo_a, CTR_TBF_ALLOC_ALGO_A)
 CREATE_COUNT_INLINE(tbf_alloc_algo_b, CTR_TBF_ALLOC_ALGO_B)
+CREATE_COUNT_INLINE(tbf_failed_egprs_only, CTR_TBF_FAILED_EGPRS_ONLY)
 CREATE_COUNT_INLINE(rlc_sent, CTR_RLC_SENT)
 CREATE_COUNT_INLINE(rlc_resent, CTR_RLC_RESENT)
 CREATE_COUNT_INLINE(rlc_restarted, CTR_RLC_RESTARTED)
diff --git a/src/csn1.cpp b/src/csn1.cpp
index 377c50f4..54cc411c 100644
--- a/src/csn1.cpp
+++ b/src/csn1.cpp
@@ -549,6 +549,7 @@ csnStreamDecoder(csnStream_t* ar, const CSN_DESCR* pDescr, bitvec *vector, unsig
 
         csnStreamInit(&arT, bit_offset, length);
         arT.direction = 1;
+        LOGPC(DCSN1, LOGL_NOTICE, "ptr = %p | offset = %d | ", (void *)data, (int)pDescr->offset);
         Status = serialize(&arT, vector, readIndex, pvDATA(data, pDescr->offset));
 
         if (Status >= 0)
diff --git a/src/decoding.cpp b/src/decoding.cpp
index abaa97f4..f2b548c1 100644
--- a/src/decoding.cpp
+++ b/src/decoding.cpp
@@ -23,6 +23,7 @@
 
 extern "C" {
 #include <osmocom/core/utils.h>
+#include <osmocom/core/bitcomp.h>
 }
 
 #include <arpa/inet.h>
@@ -180,7 +181,7 @@ static int parse_extensions_gprs(const uint8_t *data, unsigned int data_len,
 }
 
 int Decoding::rlc_data_from_ul_data(
-	const struct gprs_rlc_ul_data_block_info *rdbi, GprsCodingScheme cs,
+	const struct gprs_rlc_data_block_info *rdbi, GprsCodingScheme cs,
 	const uint8_t *data, RlcData *chunks, unsigned int chunks_size,
 	uint32_t *tlli)
 {
@@ -281,47 +282,6 @@ int Decoding::rlc_data_from_ul_data(
 	return num_chunks;
 }
 
-int Decoding::tlli_from_ul_data(const uint8_t *data, uint8_t len,
-					uint32_t *tlli)
-{
-	struct rlc_ul_header *rh = (struct rlc_ul_header *)data;
-	struct rlc_li_field *li;
-	uint8_t e;
-	uint32_t _tlli;
-
-	if (!rh->ti)
-		return -EINVAL;
-	
-	data += 3;
-	len -= 3;
-	e = rh->e;
-	/* if E is not set (LI follows) */
-	while (!e) {
-		if (!len) {
-			LOGP(DRLCMACUL, LOGL_NOTICE, "UL DATA LI extended, "
-				"but no more data\n");
-			return -EINVAL;
-		}
-		/* get new E */
-		li = (struct rlc_li_field *)data;
-		if (li->e == 0) /* if LI==0, E is interpreted as '1' */
-			e = 1;
-		else
-			e = li->e;
-		data++;
-		len--;
-	}
-	if (len < 4) {
-		LOGP(DRLCMACUL, LOGL_NOTICE, "UL DATA TLLI out of frame "
-			"border\n");
-		return -EINVAL;
-	}
-	memcpy(&_tlli, data, 4);
-	*tlli = ntohl(_tlli);
-
-	return 0;
-}
-
 uint8_t Decoding::get_ms_class_by_capability(MS_Radio_Access_capability_t *cap)
 {
 	int i;
@@ -369,23 +329,35 @@ void Decoding::extract_rbb(const uint8_t *rbb, char *show_rbb)
 	show_rbb[64] = '\0';
 }
 
-int Decoding::rlc_parse_ul_data_header(struct gprs_rlc_ul_header_egprs *rlc,
+void Decoding::extract_rbb(const struct bitvec *rbb, char *show_rbb)
+{
+	unsigned int i;
+	for (i = 0; i < rbb->cur_bit; i++) {
+		uint8_t bit;
+		bit = bitvec_get_bit_pos(rbb, i);
+		show_rbb[i] = bit == 1 ? 'R' : 'I';
+	}
+
+	show_rbb[i] = '\0';
+}
+
+int Decoding::rlc_parse_ul_data_header(struct gprs_rlc_data_info *rlc,
 	const uint8_t *data, GprsCodingScheme cs)
 {
 	const struct gprs_rlc_ul_header_egprs_3 *egprs3;
 	const struct rlc_ul_header *gprs;
 	unsigned int e_ti_header;
 	unsigned int cur_bit = 0;
-	unsigned int data_len = 0;
-
-	rlc->cs = cs;
-
-	data_len = cs.maxDataBlockBytes();
+	int punct, punct2, with_padding, cps;
+	unsigned int offs;
 
 	switch(cs.headerTypeData()) {
 	case GprsCodingScheme::HEADER_GPRS_DATA:
 		gprs = static_cast<struct rlc_ul_header *>
 			((void *)data);
+
+		gprs_rlc_data_info_init_ul(rlc, cs, false);
+
 		rlc->r      = gprs->r;
 		rlc->si     = gprs->si;
 		rlc->tfi    = gprs->tfi;
@@ -400,20 +372,23 @@ int Decoding::rlc_parse_ul_data_header(struct gprs_rlc_ul_header_egprs *rlc,
 		rlc->block_info[0].ti  = gprs->ti;
 		rlc->block_info[0].spb = 0;
 
-		cur_bit += 3 * 8;
-
-		rlc->data_offs_bits[0] = cur_bit;
-		rlc->block_info[0].data_len = data_len;
-		cur_bit += data_len * 8;
+		cur_bit += rlc->data_offs_bits[0];
 
+		/* skip data area */
+		cur_bit += cs.maxDataBlockBytes() * 8;
 		break;
 	case GprsCodingScheme::HEADER_EGPRS_DATA_TYPE_3:
 		egprs3 = static_cast<struct gprs_rlc_ul_header_egprs_3 *>
 			((void *)data);
+
+		cps    = (egprs3->cps_a << 0)  | (egprs3->cps_b << 2);
+		gprs_rlc_mcs_cps_decode(cps, cs, &punct, &punct2, &with_padding);
+		gprs_rlc_data_info_init_ul(rlc, cs, with_padding);
+
 		rlc->r      = egprs3->r;
 		rlc->si     = egprs3->si;
 		rlc->tfi    = (egprs3->tfi_a << 0)  | (egprs3->tfi_b << 2);
-		rlc->cps    = (egprs3->cps_a << 0)  | (egprs3->cps_b << 2);
+		rlc->cps    = cps;
 		rlc->rsb    = egprs3->rsb;
 
 		rlc->num_data_blocks = 1;
@@ -423,17 +398,18 @@ int Decoding::rlc_parse_ul_data_header(struct gprs_rlc_ul_header_egprs *rlc,
 		rlc->block_info[0].bsn =
 			(egprs3->bsn1_a << 0) | (egprs3->bsn1_b << 5);
 
-		cur_bit += 3 * 8 + 7;
+		cur_bit += rlc->data_offs_bits[0] - 2;
 
-		e_ti_header = (data[3] + (data[4] << 8)) >> 7;
+		offs = rlc->data_offs_bits[0] / 8;
+		OSMO_ASSERT(rlc->data_offs_bits[0] % 8 == 1);
+
+		e_ti_header = (data[offs-1] + (data[offs] << 8)) >> 7;
 		rlc->block_info[0].e   = !!(e_ti_header & 0x01);
 		rlc->block_info[0].ti  = !!(e_ti_header & 0x02);
 		cur_bit += 2;
 
-		rlc->data_offs_bits[0] = cur_bit;
-		rlc->block_info[0].data_len = data_len;
-		cur_bit += data_len * 8;
-
+		/* skip data area */
+		cur_bit += cs.maxDataBlockBytes() * 8;
 		break;
 
 	case GprsCodingScheme::HEADER_EGPRS_DATA_TYPE_1:
@@ -464,7 +440,7 @@ int Decoding::rlc_parse_ul_data_header(struct gprs_rlc_ul_header_egprs *rlc,
  * \returns  the number of bytes copied
  */
 unsigned int Decoding::rlc_copy_to_aligned_buffer(
-	const struct gprs_rlc_ul_header_egprs *rlc,
+	const struct gprs_rlc_data_info *rlc,
 	unsigned int data_block_idx,
 	const uint8_t *src, uint8_t *buffer)
 {
@@ -474,7 +450,7 @@ unsigned int Decoding::rlc_copy_to_aligned_buffer(
 
 	uint8_t c, last_c;
 	uint8_t *dst;
-	const struct gprs_rlc_ul_data_block_info *rdbi;
+	const struct gprs_rlc_data_block_info *rdbi;
 
 	OSMO_ASSERT(data_block_idx < rlc->num_data_blocks);
 	rdbi = &rlc->block_info[data_block_idx];
@@ -514,7 +490,7 @@ unsigned int Decoding::rlc_copy_to_aligned_buffer(
  *          buffer otherwise.
  */
 const uint8_t *Decoding::rlc_get_data_aligned(
-	const struct gprs_rlc_ul_header_egprs *rlc,
+	const struct gprs_rlc_data_info *rlc,
 	unsigned int data_block_idx,
 	const uint8_t *src, uint8_t *buffer)
 {
@@ -534,3 +510,168 @@ const uint8_t *Decoding::rlc_get_data_aligned(
 	Decoding::rlc_copy_to_aligned_buffer(rlc, data_block_idx, src, buffer);
 	return buffer;
 }
+
+static int handle_final_ack(bitvec *bits, int *bsn_begin, int *bsn_end,
+	gprs_rlc_dl_window *window)
+{
+	int num_blocks, i;
+
+	num_blocks = window->mod_sns(window->v_s() - window->v_a());
+	for (i = 0; i < num_blocks; i++)
+		bitvec_set_bit(bits, ONE);
+
+	*bsn_begin = window->v_a();
+	*bsn_end   = window->mod_sns(*bsn_begin + num_blocks);
+	return num_blocks;
+}
+
+int Decoding::decode_egprs_acknack_bits(const EGPRS_AckNack_Desc_t *desc,
+	bitvec *bits, int *bsn_begin, int *bsn_end, gprs_rlc_dl_window *window)
+{
+	int urbb_len = desc->URBB_LENGTH;
+	int crbb_len = 0;
+	int num_blocks = 0;
+	struct bitvec urbb;
+	int i;
+	bool have_bitmap;
+	int implicitly_acked_blocks;
+	int ssn = desc->STARTING_SEQUENCE_NUMBER;
+	int rc;
+
+	if (desc->FINAL_ACK_INDICATION)
+		return handle_final_ack(bits, bsn_begin, bsn_end, window);
+
+	if (desc->Exist_CRBB)
+		crbb_len = desc->CRBB_LENGTH;
+
+	have_bitmap = (urbb_len + crbb_len) > 0;
+
+	/*
+	 * bow & bitmap present:
+	 *   V(A)-> [ 11111...11111 0 SSN-> BBBBB...BBBBB ] (SSN+Nbits) .... V(S)
+	 * bow & not bitmap present:
+	 *   V(A)-> [ 11111...11111 ] . SSN .... V(S)
+	 * not bow & bitmap present:
+	 *   V(A)-> ... [ 0 SSN-> BBBBB...BBBBB ](SSN+N) .... V(S)
+	 * not bow & not bitmap present:
+	 *   V(A)-> ... [] . SSN .... V(S)
+	 */
+
+	if (desc->BEGINNING_OF_WINDOW) {
+		implicitly_acked_blocks = window->mod_sns(ssn - 1 - window->v_a());
+
+		for (i = 0; i < implicitly_acked_blocks; i++)
+			bitvec_set_bit(bits, ONE);
+
+		num_blocks += implicitly_acked_blocks;
+	}
+
+	if (!have_bitmap)
+		goto aborted;
+
+	/* next bit refers to V(Q) and thus is always zero (and not
+	 * transmitted) */
+	bitvec_set_bit(bits, ZERO);
+	num_blocks += 1;
+
+	if (crbb_len > 0) {
+		int old_len = bits->cur_bit;
+		struct bitvec crbb;
+
+		crbb.data = (uint8_t *)desc->CRBB;
+		crbb.data_len = sizeof(desc->CRBB);
+		crbb.cur_bit = desc->CRBB_LENGTH;
+
+		rc = osmo_t4_decode(&crbb, desc->CRBB_STARTING_COLOR_CODE,
+			bits);
+
+		if (rc < 0) {
+			LOGP(DRLCMACUL, LOGL_NOTICE,
+				"Failed to decode CRBB: "
+				"length %d, data '%s'\n",
+				desc->CRBB_LENGTH,
+				osmo_hexdump(crbb.data, crbb.data_len));
+			/* We don't know the SSN offset for the URBB,
+			 * return what we have so far and assume the
+			 * bitmap has stopped here */
+			goto aborted;
+		}
+
+		LOGP(DRLCMACDL, LOGL_DEBUG,
+			"CRBB len: %d, decoded len: %d, cc: %d, crbb: '%s'\n",
+			desc->CRBB_LENGTH, bits->cur_bit - old_len,
+			desc->CRBB_STARTING_COLOR_CODE,
+			osmo_hexdump(
+				desc->CRBB, (desc->CRBB_LENGTH + 7)/8)
+		    );
+
+		num_blocks += (bits->cur_bit - old_len);
+	}
+
+	urbb.cur_bit = 0;
+	urbb.data = (uint8_t *)desc->URBB;
+	urbb.data_len = sizeof(desc->URBB);
+
+	for (i = urbb_len; i > 0; i--) {
+		/*
+		 * Set bit at the appropriate position (see 3GPP TS
+		 * 44.060 12.3.1)
+		 */
+		int is_ack = bitvec_get_bit_pos(&urbb, i-1);
+		bitvec_set_bit(bits, is_ack == 1 ? ONE : ZERO);
+	}
+	num_blocks += urbb_len;
+
+aborted:
+	*bsn_begin = window->v_a();
+	*bsn_end   = window->mod_sns(*bsn_begin + num_blocks);
+
+	return num_blocks;
+}
+
+int Decoding::decode_gprs_acknack_bits(const Ack_Nack_Description_t *desc,
+	bitvec *bits, int *bsn_begin, int *bsn_end, gprs_rlc_dl_window *window)
+{
+	int urbb_len = RLC_GPRS_WS;
+	int num_blocks;
+	struct bitvec urbb;
+
+	if (desc->FINAL_ACK_INDICATION)
+		return handle_final_ack(bits, bsn_begin, bsn_end, window);
+
+	*bsn_begin = window->v_a();
+	*bsn_end   = desc->STARTING_SEQUENCE_NUMBER;
+
+	num_blocks = window->mod_sns(*bsn_end - *bsn_begin);
+
+	if (num_blocks < 0 || num_blocks > urbb_len) {
+		*bsn_end  = *bsn_begin;
+		LOGP(DRLCMACUL, LOGL_NOTICE,
+			"Invalid GPRS Ack/Nack window %d:%d (length %d)\n",
+			*bsn_begin, *bsn_end, num_blocks);
+		return -EINVAL;
+	}
+
+	urbb.cur_bit = 0;
+	urbb.data = (uint8_t *)desc->RECEIVED_BLOCK_BITMAP;
+	urbb.data_len = sizeof(desc->RECEIVED_BLOCK_BITMAP);
+
+	/*
+	 * TS 44.060, 12.3:
+	 * BSN = (SSN - bit_number) modulo 128, for bit_number = 1 to 64.
+	 * The BSN values represented range from (SSN - 1) mod 128 to (SSN - 64) mod 128.
+	 *
+	 * We are only interested in the range from V(A) to SSN-1 which is
+	 * num_blocks large. The RBB is laid out as
+	 *   [SSN-1] [SSN-2] ... [V(A)] ... [SSN-64]
+	 * so we want to start with [V(A)] and go backwards until we reach
+	 * [SSN-1] to get the needed BSNs in an increasing order. Note that
+	 * the bit numbers are counted from the end of the buffer.
+	 */
+	for (int i = num_blocks; i > 0; i--) {
+		int is_ack = bitvec_get_bit_pos(&urbb, urbb_len - i);
+		bitvec_set_bit(bits, is_ack == 1 ? ONE : ZERO);
+	}
+
+	return num_blocks;
+}
diff --git a/src/decoding.h b/src/decoding.h
index 1cda7b42..58ecd18e 100644
--- a/src/decoding.h
+++ b/src/decoding.h
@@ -24,6 +24,8 @@
 
 #include <stdint.h>
 
+struct bitvec;
+
 class Decoding {
 public:
 	struct RlcData {
@@ -32,25 +34,32 @@ public:
 		bool	is_complete;
 	};
 
-	static int tlli_from_ul_data(const uint8_t *data, uint8_t len,
-					uint32_t *tlli);
 	static int rlc_data_from_ul_data(
-		const struct gprs_rlc_ul_data_block_info *rdbi,
+		const struct gprs_rlc_data_block_info *rdbi,
 		GprsCodingScheme cs, const uint8_t *data, RlcData *chunks,
 		unsigned int chunks_size, uint32_t *tlli);
 	static uint8_t get_ms_class_by_capability(MS_Radio_Access_capability_t *cap);
 	static uint8_t get_egprs_ms_class_by_capability(MS_Radio_Access_capability_t *cap);
 
 	static void extract_rbb(const uint8_t *rbb, char *extracted_rbb);
+	static void extract_rbb(const struct bitvec *rbb, char *show_rbb);
 
-	static int rlc_parse_ul_data_header(struct gprs_rlc_ul_header_egprs *rlc,
+	static int rlc_parse_ul_data_header(struct gprs_rlc_data_info *rlc,
 		const uint8_t *data, GprsCodingScheme cs);
 	static unsigned int rlc_copy_to_aligned_buffer(
-		const struct gprs_rlc_ul_header_egprs *rlc,
+		const struct gprs_rlc_data_info *rlc,
 		unsigned int data_block_idx,
 		const uint8_t *src, uint8_t *buffer);
 	static const uint8_t *rlc_get_data_aligned(
-		const struct gprs_rlc_ul_header_egprs *rlc,
+		const struct gprs_rlc_data_info *rlc,
 		unsigned int data_block_idx,
 		const uint8_t *src, uint8_t *buffer);
+	static int decode_egprs_acknack_bits(
+		const EGPRS_AckNack_Desc_t *desc,
+		struct bitvec *bits, int *bsn_begin, int *bsn_end,
+		struct gprs_rlc_dl_window *window);
+	static int decode_gprs_acknack_bits(
+		const Ack_Nack_Description_t *desc,
+		bitvec *bits, int *bsn_begin, int *bsn_end,
+		gprs_rlc_dl_window *window);
 };
diff --git a/src/encoding.cpp b/src/encoding.cpp
index 7f4bf797..6c50abe3 100644
--- a/src/encoding.cpp
+++ b/src/encoding.cpp
@@ -25,17 +25,142 @@
 #include <tbf.h>
 #include <gprs_debug.h>
 
-// GSM 04.08 9.1.18 Immediate assignment
+#include <errno.h>
+#include <string.h>
+
+static int write_ia_rest_downlink(
+	gprs_rlcmac_dl_tbf *tbf,
+	bitvec * dest, unsigned& wp,
+	uint8_t polling, uint32_t fn,
+	uint8_t alpha, uint8_t gamma, int8_t ta_idx)
+{
+	if (!tbf) {
+		LOGP(DRLCMACDL, LOGL_ERROR,
+			"Cannot encode DL IMMEDIATE ASSIGNMENT without TBF\n");
+		return -EINVAL;
+	}
+	// GSM 04.08 10.5.2.16 IA Rest Octets
+	bitvec_write_field_lh(dest, wp, 3, 2);   // "HH"
+	bitvec_write_field(dest, wp, 1, 2);   // "01" Packet Downlink Assignment
+	bitvec_write_field(dest, wp,tbf->tlli(),32); // TLLI
+	bitvec_write_field(dest, wp,0x1,1);   // switch TFI   : on
+	bitvec_write_field(dest, wp,tbf->tfi(),5);   // TFI
+	bitvec_write_field(dest, wp,0x0,1);   // RLC acknowledged mode
+	if (alpha) {
+		bitvec_write_field(dest, wp,0x1,1);   // ALPHA = present
+		bitvec_write_field(dest, wp,alpha,4);   // ALPHA
+	} else {
+		bitvec_write_field(dest, wp,0x0,1);   // ALPHA = not present
+	}
+	bitvec_write_field(dest, wp,gamma,5);   // GAMMA power control parameter
+	bitvec_write_field(dest, wp,polling,1);   // Polling Bit
+	bitvec_write_field(dest, wp,!polling,1);   // TA_VALID ???
+	if (ta_idx < 0) {
+		bitvec_write_field(dest, wp,0x0,1);   // switch TIMING_ADVANCE_INDEX = off
+	} else {
+		bitvec_write_field(dest, wp,0x1,1);   // switch TIMING_ADVANCE_INDEX = on
+		bitvec_write_field(dest, wp,ta_idx,4);   // TIMING_ADVANCE_INDEX
+	}
+	if (polling) {
+		bitvec_write_field(dest, wp,0x1,1);   // TBF Starting TIME present
+		bitvec_write_field(dest, wp,(fn / (26 * 51)) % 32,5); // T1'
+		bitvec_write_field(dest, wp,fn % 51,6);               // T3
+		bitvec_write_field(dest, wp,fn % 26,5);               // T2
+	} else {
+		bitvec_write_field(dest, wp,0x0,1);   // TBF Starting TIME present
+	}
+	bitvec_write_field(dest, wp,0x0,1);   // P0 not present
+	//		bitvec_write_field(dest, wp,0x1,1);   // P0 not present
+	//		bitvec_write_field(dest, wp,0xb,4);
+	if (tbf->is_egprs_enabled()) {
+		/* see GMS 44.018, 10.5.2.16 */
+		unsigned int ws_enc = (tbf->m_window.ws() - 64) / 32;
+		bitvec_write_field_lh(dest, wp, 1, 1);  // "H"
+		bitvec_write_field(dest, wp, ws_enc,5); // EGPRS Window Size
+		bitvec_write_field(dest, wp, 0x0,2);    // LINK_QUALITY_MEASUREMENT_MODE
+		bitvec_write_field(dest, wp, 0,1);      // BEP_PERIOD2 not present
+	}
+
+	return 0;
+}
+
+static int write_ia_rest_uplink(
+	gprs_rlcmac_ul_tbf *tbf,
+	bitvec * dest, unsigned& wp,
+	uint8_t usf, uint32_t fn,
+	uint8_t alpha, uint8_t gamma, int8_t ta_idx)
+{
+	OSMO_ASSERT(!tbf || !tbf->is_egprs_enabled());
+
+	// GMS 04.08 10.5.2.37b 10.5.2.16
+	bitvec_write_field_lh(dest, wp, 3, 2);    // "HH"
+	bitvec_write_field(dest, wp, 0, 2);    // "0" Packet Uplink Assignment
+	if (tbf == NULL) {
+		bitvec_write_field(dest, wp, 0, 1);    // Block Allocation : Single Block Allocation
+		if (alpha) {
+			bitvec_write_field(dest, wp,0x1,1);   // ALPHA = present
+			bitvec_write_field(dest, wp,alpha,4);   // ALPHA = present
+		} else
+			bitvec_write_field(dest, wp,0x0,1);   // ALPHA = not present
+		bitvec_write_field(dest, wp,gamma,5);   // GAMMA power control parameter
+		if (ta_idx < 0) {
+			bitvec_write_field(dest, wp,0x0,1);   // switch TIMING_ADVANCE_INDEX = off
+		} else {
+			bitvec_write_field(dest, wp,0x1,1);   // switch TIMING_ADVANCE_INDEX = on
+			bitvec_write_field(dest, wp,ta_idx,4);   // TIMING_ADVANCE_INDEX
+		}
+		bitvec_write_field(dest, wp, 1, 1);    // TBF_STARTING_TIME_FLAG
+		bitvec_write_field(dest, wp,(fn / (26 * 51)) % 32,5); // T1'
+		bitvec_write_field(dest, wp,fn % 51,6);               // T3
+		bitvec_write_field(dest, wp,fn % 26,5);               // T2
+	} else {
+		bitvec_write_field(dest, wp, 1, 1);    // Block Allocation : Not Single Block Allocation
+		bitvec_write_field(dest, wp, tbf->tfi(), 5);  // TFI_ASSIGNMENT Temporary Flow Identity
+		bitvec_write_field(dest, wp, 0, 1);    // POLLING
+		bitvec_write_field(dest, wp, 0, 1);    // ALLOCATION_TYPE: dynamic
+		bitvec_write_field(dest, wp, usf, 3);    // USF
+		bitvec_write_field(dest, wp, 0, 1);    // USF_GRANULARITY
+		bitvec_write_field(dest, wp, 0, 1);   // "0" power control: Not Present
+		bitvec_write_field(dest, wp, tbf->current_cs().to_num()-1, 2);    // CHANNEL_CODING_COMMAND 
+		bitvec_write_field(dest, wp, 1, 1);    // TLLI_BLOCK_CHANNEL_CODING
+		if (alpha) {
+			bitvec_write_field(dest, wp,0x1,1);   // ALPHA = present
+			bitvec_write_field(dest, wp,alpha,4);   // ALPHA
+		} else
+			bitvec_write_field(dest, wp,0x0,1);   // ALPHA = not present
+		bitvec_write_field(dest, wp,gamma,5);   // GAMMA power control parameter
+		/* note: there is no choise for TAI and no starting time */
+		bitvec_write_field(dest, wp, 0, 1);   // switch TIMING_ADVANCE_INDEX = off
+		bitvec_write_field(dest, wp, 0, 1);    // TBF_STARTING_TIME_FLAG
+	}
+	return 0;
+}
+
+static int write_ia_rest_egprs_uplink(
+	gprs_rlcmac_ul_tbf *tbf,
+	bitvec * dest, unsigned& wp,
+	uint8_t usf, uint32_t fn,
+	uint8_t alpha, uint8_t gamma, int8_t ta_idx)
+{
+	LOGP(DRLCMACUL, LOGL_ERROR,
+		"EGPRS Packet Uplink Assignment is not yet implemented\n");
+	return -EINVAL;
+}
+
+/*
+ * Immediate assignment, sent on the CCCH/AGCH
+ * see GSM 04.08, 9.1.18 and GSM 44.018, 9.1.18 + 10.5.2.16
+ */
 int Encoding::write_immediate_assignment(
-	struct gprs_rlcmac_bts *bts,
+	struct gprs_rlcmac_tbf *tbf,
 	bitvec * dest, uint8_t downlink, uint8_t ra,
 	uint32_t ref_fn, uint8_t ta, uint16_t arfcn, uint8_t ts, uint8_t tsc,
-	uint8_t tfi, uint8_t usf, uint32_t tlli,
-	uint8_t polling, uint32_t fn, uint8_t single_block, uint8_t alpha,
+	uint8_t usf, uint8_t polling, uint32_t fn, uint8_t alpha,
 	uint8_t gamma, int8_t ta_idx)
 {
 	unsigned wp = 0;
-	uint8_t plen;
+	int plen;
+	int rc;
 
 	bitvec_write_field(dest, wp,0x0,4);  // Skip Indicator
 	bitvec_write_field(dest, wp,0x6,4);  // Protocol Discriminator
@@ -78,84 +203,24 @@ int Encoding::write_immediate_assignment(
 	plen = wp / 8;
 
 	if (downlink)
-	{
-		// GSM 04.08 10.5.2.16 IA Rest Octets
-		bitvec_write_field(dest, wp, 3, 2);   // "HH"
-		bitvec_write_field(dest, wp, 1, 2);   // "01" Packet Downlink Assignment
-		bitvec_write_field(dest, wp,tlli,32); // TLLI
-		bitvec_write_field(dest, wp,0x1,1);   // switch TFI   : on
-		bitvec_write_field(dest, wp,tfi,5);   // TFI
-		bitvec_write_field(dest, wp,0x0,1);   // RLC acknowledged mode
-		if (alpha) {
-			bitvec_write_field(dest, wp,0x1,1);   // ALPHA = present
-			bitvec_write_field(dest, wp,alpha,4);   // ALPHA
-		} else {
-			bitvec_write_field(dest, wp,0x0,1);   // ALPHA = not present
-		}
-		bitvec_write_field(dest, wp,gamma,5);   // GAMMA power control parameter
-		bitvec_write_field(dest, wp,polling,1);   // Polling Bit
-		bitvec_write_field(dest, wp,!polling,1);   // TA_VALID ???
-		if (ta_idx < 0) {
-			bitvec_write_field(dest, wp,0x0,1);   // switch TIMING_ADVANCE_INDEX = off
-		} else {
-			bitvec_write_field(dest, wp,0x1,1);   // switch TIMING_ADVANCE_INDEX = on
-			bitvec_write_field(dest, wp,ta_idx,4);   // TIMING_ADVANCE_INDEX
-		}
-		if (polling) {
-			bitvec_write_field(dest, wp,0x1,1);   // TBF Starting TIME present
-			bitvec_write_field(dest, wp,(fn / (26 * 51)) % 32,5); // T1'
-			bitvec_write_field(dest, wp,fn % 51,6);               // T3
-			bitvec_write_field(dest, wp,fn % 26,5);               // T2
-		} else {
-			bitvec_write_field(dest, wp,0x0,1);   // TBF Starting TIME present
-		}
-		bitvec_write_field(dest, wp,0x0,1);   // P0 not present
-//		bitvec_write_field(dest, wp,0x1,1);   // P0 not present
-//		bitvec_write_field(dest, wp,0xb,4);
-	}
+		rc = write_ia_rest_downlink(as_dl_tbf(tbf), dest, wp,
+			polling, fn,
+			alpha, gamma, ta_idx);
+	else if (as_ul_tbf(tbf) && as_ul_tbf(tbf)->is_egprs_enabled())
+		rc = write_ia_rest_egprs_uplink(as_ul_tbf(tbf), dest, wp,
+			usf, fn,
+			alpha, gamma, ta_idx);
 	else
-	{
-		// GMS 04.08 10.5.2.37b 10.5.2.16
-		bitvec_write_field(dest, wp, 3, 2);    // "HH"
-		bitvec_write_field(dest, wp, 0, 2);    // "0" Packet Uplink Assignment
-		if (single_block) {
-			bitvec_write_field(dest, wp, 0, 1);    // Block Allocation : Single Block Allocation
-			if (alpha) {
-				bitvec_write_field(dest, wp,0x1,1);   // ALPHA = present
-				bitvec_write_field(dest, wp,alpha,4);   // ALPHA = present
-			} else
-				bitvec_write_field(dest, wp,0x0,1);   // ALPHA = not present
-			bitvec_write_field(dest, wp,gamma,5);   // GAMMA power control parameter
-			if (ta_idx < 0) {
-				bitvec_write_field(dest, wp,0x0,1);   // switch TIMING_ADVANCE_INDEX = off
-			} else {
-				bitvec_write_field(dest, wp,0x1,1);   // switch TIMING_ADVANCE_INDEX = on
-				bitvec_write_field(dest, wp,ta_idx,4);   // TIMING_ADVANCE_INDEX
-			}
-			bitvec_write_field(dest, wp, 1, 1);    // TBF_STARTING_TIME_FLAG
-			bitvec_write_field(dest, wp,(fn / (26 * 51)) % 32,5); // T1'
-			bitvec_write_field(dest, wp,fn % 51,6);               // T3
-			bitvec_write_field(dest, wp,fn % 26,5);               // T2
-		} else {
-			bitvec_write_field(dest, wp, 1, 1);    // Block Allocation : Not Single Block Allocation
-			bitvec_write_field(dest, wp, tfi, 5);  // TFI_ASSIGNMENT Temporary Flow Identity
-			bitvec_write_field(dest, wp, 0, 1);    // POLLING
-			bitvec_write_field(dest, wp, 0, 1);    // ALLOCATION_TYPE: dynamic
-			bitvec_write_field(dest, wp, usf, 3);    // USF
-			bitvec_write_field(dest, wp, 0, 1);    // USF_GRANULARITY
-			bitvec_write_field(dest, wp, 0, 1);   // "0" power control: Not Present
-			bitvec_write_field(dest, wp, bts->initial_cs_ul-1, 2);    // CHANNEL_CODING_COMMAND 
-			bitvec_write_field(dest, wp, 1, 1);    // TLLI_BLOCK_CHANNEL_CODING
-			if (alpha) {
-				bitvec_write_field(dest, wp,0x1,1);   // ALPHA = present
-				bitvec_write_field(dest, wp,alpha,4);   // ALPHA
-			} else
-				bitvec_write_field(dest, wp,0x0,1);   // ALPHA = not present
-			bitvec_write_field(dest, wp,gamma,5);   // GAMMA power control parameter
-			/* note: there is no choise for TAI and no starting time */
-			bitvec_write_field(dest, wp, 0, 1);   // switch TIMING_ADVANCE_INDEX = off
-			bitvec_write_field(dest, wp, 0, 1);    // TBF_STARTING_TIME_FLAG
-		}
+		rc = write_ia_rest_uplink(as_ul_tbf(tbf), dest, wp,
+			usf, fn,
+			alpha, gamma, ta_idx);
+
+	if (rc < 0) {
+		LOGP(DRLCMAC, LOGL_ERROR,
+			"Failed to create IMMEDIATE ASSIGMENT (%s) for %s\n",
+			downlink ? "downlink" : "uplink",
+			tbf ? tbf->name() : "single block allocation");
+		return rc;
 	}
 
 	return plen;
@@ -166,7 +231,7 @@ void Encoding::write_packet_uplink_assignment(
 	struct gprs_rlcmac_bts *bts,
 	bitvec * dest, uint8_t old_tfi,
 	uint8_t old_downlink, uint32_t tlli, uint8_t use_tlli,
-	struct gprs_rlcmac_ul_tbf *tbf, uint8_t poll, uint8_t alpha,
+	struct gprs_rlcmac_ul_tbf *tbf, uint8_t poll, uint8_t rrbp, uint8_t alpha,
 	uint8_t gamma, int8_t ta_idx, int8_t use_egprs)
 {
 	// TODO We should use our implementation of encode RLC/MAC Control messages.
@@ -193,7 +258,7 @@ void Encoding::write_packet_uplink_assignment(
 
 	if (!use_egprs) {
 		bitvec_write_field(dest, wp,0x0,1); // Message escape
-		bitvec_write_field(dest, wp,tbf->current_cs()-1, 2); // CHANNEL_CODING_COMMAND 
+		bitvec_write_field(dest, wp,tbf->current_cs().to_num()-1, 2); // CHANNEL_CODING_COMMAND 
 		bitvec_write_field(dest, wp,0x1,1); // TLLI_BLOCK_CHANNEL_CODING 
 		bitvec_write_field(dest, wp,0x1,1); // switch TIMING_ADVANCE_VALUE = on
 		bitvec_write_field(dest, wp,tbf->ta(),6); // TIMING_ADVANCE_VALUE
@@ -205,13 +270,14 @@ void Encoding::write_packet_uplink_assignment(
 		}
 
 	} else { /* EPGRS */
+		unsigned int ws_enc = (tbf->m_window.ws() - 64) / 32;
 		bitvec_write_field(dest, wp,0x1,1); // Message escape
 		bitvec_write_field(dest, wp,0x0,2); // EGPRS message contents
 		bitvec_write_field(dest, wp,0x0,1); // No CONTENTION_RESOLUTION_TLLI
 		bitvec_write_field(dest, wp,0x0,1); // No COMPACT reduced MA
-		bitvec_write_field(dest, wp,tbf->current_cs()-1, 4); // EGPRS Modulation and Coding IE
+		bitvec_write_field(dest, wp,tbf->current_cs().to_num()-1, 4); // EGPRS Modulation and Coding IE
 		bitvec_write_field(dest, wp,0x0,1); // No RESEGMENT
-		bitvec_write_field(dest, wp,0x0,5); // EGPRS Window Size = 64
+		bitvec_write_field(dest, wp,ws_enc,5); // EGPRS Window Size
 		bitvec_write_field(dest, wp,0x0,1); // No Access Technologies Request
 		bitvec_write_field(dest, wp,0x0,1); // No ARAC RETRANSMISSION REQUEST
 		bitvec_write_field(dest, wp,0x1,1); // TLLI_BLOCK_CHANNEL_CODING 
@@ -269,16 +335,21 @@ void Encoding::write_packet_uplink_assignment(
 
 
 /* generate downlink assignment */
-void Encoding::write_packet_downlink_assignment(RlcMacDownlink_t * block, uint8_t old_tfi,
-	uint8_t old_downlink, struct gprs_rlcmac_tbf *tbf, uint8_t poll,
-	uint8_t alpha, uint8_t gamma, int8_t ta_idx, uint8_t ta_ts)
+void Encoding::write_packet_downlink_assignment(RlcMacDownlink_t * block,
+	bool old_tfi_is_valid, uint8_t old_tfi, uint8_t old_downlink,
+	struct gprs_rlcmac_tbf *tbf, uint8_t poll, uint8_t rrbp,
+	uint8_t alpha, uint8_t gamma, int8_t ta_idx,
+	uint8_t ta_ts, bool use_egprs)
 {
 	// Packet downlink assignment TS 44.060 11.2.7
 
+	PDA_AdditionsR99_t *pda_r99;
+
 	uint8_t tn;
+	unsigned int ws_enc;
 
 	block->PAYLOAD_TYPE = 0x1;  // RLC/MAC control block that does not include the optional octets of the RLC/MAC control header
-	block->RRBP         = 0x0;  // N+13
+	block->RRBP         = rrbp;  // 0: N+13
 	block->SP           = poll; // RRBP field is valid
 	block->USF          = 0x0;  // Uplink state flag
 
@@ -287,9 +358,14 @@ void Encoding::write_packet_downlink_assignment(RlcMacDownlink_t * block, uint8_
 
 	block->u.Packet_Downlink_Assignment.Exist_PERSISTENCE_LEVEL      = 0x0;          // PERSISTENCE_LEVEL: off
 
-	block->u.Packet_Downlink_Assignment.ID.UnionType                 = 0x0;          // TFI = on
-	block->u.Packet_Downlink_Assignment.ID.u.Global_TFI.UnionType    = old_downlink; // 0=UPLINK TFI, 1=DL TFI
-	block->u.Packet_Downlink_Assignment.ID.u.Global_TFI.u.UPLINK_TFI = old_tfi;      // TFI
+	if (old_tfi_is_valid) {
+		block->u.Packet_Downlink_Assignment.ID.UnionType                 = 0x0;          // TFI = on
+		block->u.Packet_Downlink_Assignment.ID.u.Global_TFI.UnionType    = old_downlink; // 0=UPLINK TFI, 1=DL TFI
+		block->u.Packet_Downlink_Assignment.ID.u.Global_TFI.u.UPLINK_TFI = old_tfi;      // TFI
+	} else {
+		block->u.Packet_Downlink_Assignment.ID.UnionType                 = 0x1;          // TLLI
+		block->u.Packet_Downlink_Assignment.ID.u.TLLI                    = tbf->tlli();
+	}
 
 	block->u.Packet_Downlink_Assignment.MAC_MODE            = 0x0;          // Dynamic Allocation
 	block->u.Packet_Downlink_Assignment.RLC_MODE            = 0x0;          // RLC acknowledged mode
@@ -338,7 +414,21 @@ void Encoding::write_packet_downlink_assignment(RlcMacDownlink_t * block, uint8_
 
 	block->u.Packet_Downlink_Assignment.Exist_TBF_Starting_Time   = 0x0; // TBF Starting TIME = off
 	block->u.Packet_Downlink_Assignment.Exist_Measurement_Mapping = 0x0; // Measurement_Mapping = off
-	block->u.Packet_Downlink_Assignment.Exist_AdditionsR99        = 0x0; // AdditionsR99 = off
+	if (!use_egprs) {
+		block->u.Packet_Downlink_Assignment.Exist_AdditionsR99        = 0x0; // AdditionsR99 = off
+		return;
+	}
+
+	ws_enc = (tbf->window()->ws() - 64) / 32;
+
+	block->u.Packet_Downlink_Assignment.Exist_AdditionsR99        = 0x1; // AdditionsR99 = on
+	pda_r99 = &block->u.Packet_Downlink_Assignment.AdditionsR99;
+	pda_r99->Exist_EGPRS_Params = 1;
+	pda_r99->EGPRS_WindowSize = ws_enc; /* see TS 44.060, table 12.5.2.1 */
+	pda_r99->LINK_QUALITY_MEASUREMENT_MODE = 0x0; /* no meas, see TS 44.060, table 11.2.7.2 */
+	pda_r99->Exist_BEP_PERIOD2 = 0; /* No extra EGPRS BEP PERIOD */
+	pda_r99->Exist_Packet_Extended_Timing_Advance = 0;
+	pda_r99->Exist_COMPACT_ReducedMA = 0;
 }
 
 /* generate paging request */
@@ -399,50 +489,181 @@ void Encoding::encode_rbb(const char *show_rbb, uint8_t *rbb)
 	}
 }
 
-/* generate uplink ack */
-void Encoding::write_packet_uplink_ack(struct gprs_rlcmac_bts *bts,
-	RlcMacDownlink_t * block, struct gprs_rlcmac_ul_tbf *tbf,
-	uint8_t final)
+static void write_packet_ack_nack_desc_gprs(
+	struct gprs_rlcmac_bts *bts, bitvec * dest, unsigned& wp,
+	gprs_rlc_ul_window *window, bool is_final)
 {
-	// Packet Uplink Ack/Nack  TS 44.060 11.2.28
-
 	char rbb[65];
 
-	tbf->m_window.update_rbb(rbb);
+	window->update_rbb(rbb);
 
-	LOGP(DRLCMACUL, LOGL_DEBUG, "Encoding Ack/Nack for %s "
-		"(final=%d)\n", tbf_name(tbf), final);
+	rbb[64] = 0;
+	LOGP(DRLCMACUL, LOGL_DEBUG, "- V(N): \"%s\" R=Received "
+		"I=Invalid\n", rbb);
 
-	block->PAYLOAD_TYPE = 0x1;   // RLC/MAC control block that does not include the optional octets of the RLC/MAC control header
-	block->RRBP         = 0x0;   // N+13
-	block->SP           = final; // RRBP field is valid, if it is final ack
-	block->USF          = 0x0;   // Uplink state flag
+	bitvec_write_field(dest, wp, is_final, 1); // FINAL_ACK_INDICATION
+	bitvec_write_field(dest, wp, window->ssn(), 7); // STARTING_SEQUENCE_NUMBER
 
-	block->u.Packet_Uplink_Ack_Nack.MESSAGE_TYPE = 0x9;      // Packet Downlink Assignment
-	block->u.Packet_Uplink_Ack_Nack.PAGE_MODE    = 0x0;      // Normal Paging
-	block->u.Packet_Uplink_Ack_Nack.UPLINK_TFI   = tbf->tfi(); // Uplink TFI
+	for (int i = 0; i < 64; i++) {
+		/* Set bit at the appropriate position (see 3GPP TS 04.60 9.1.8.1) */
+		bool is_ack = (rbb[i] == 'R');
+		bitvec_write_field(dest, wp, is_ack, 1);
+	}
+}
 
-	block->u.Packet_Uplink_Ack_Nack.UnionType    = 0x0;      // PU_AckNack_GPRS = on
-	block->u.Packet_Uplink_Ack_Nack.u.PU_AckNack_GPRS_Struct.CHANNEL_CODING_COMMAND                        = tbf->current_cs() - 1;             // CS1
-	block->u.Packet_Uplink_Ack_Nack.u.PU_AckNack_GPRS_Struct.Ack_Nack_Description.FINAL_ACK_INDICATION     = final;           // FINAL ACK INDICATION
-	block->u.Packet_Uplink_Ack_Nack.u.PU_AckNack_GPRS_Struct.Ack_Nack_Description.STARTING_SEQUENCE_NUMBER = tbf->m_window.ssn(); // STARTING_SEQUENCE_NUMBER
+static void write_packet_uplink_ack_gprs(
+	struct gprs_rlcmac_bts *bts, bitvec * dest, unsigned& wp,
+	struct gprs_rlcmac_ul_tbf *tbf, bool is_final)
+{
 
-	encode_rbb(rbb, block->u.Packet_Uplink_Ack_Nack.u.PU_AckNack_GPRS_Struct.Ack_Nack_Description.RECEIVED_BLOCK_BITMAP);
+	bitvec_write_field(dest, wp, tbf->current_cs().to_num() - 1, 2); // CHANNEL_CODING_COMMAND
+	write_packet_ack_nack_desc_gprs(bts, dest, wp, &tbf->m_window, is_final);
+
+	bitvec_write_field(dest, wp, 1, 1); // 1: have CONTENTION_RESOLUTION_TLLI
+	bitvec_write_field(dest, wp, tbf->tlli(), 32); // CONTENTION_RESOLUTION_TLLI
+
+	bitvec_write_field(dest, wp, 0, 1); // 0: don't have Packet Timing Advance
+	bitvec_write_field(dest, wp, 0, 1); // 0: don't have Power Control Parameters
+	bitvec_write_field(dest, wp, 0, 1); // 0: don't have Extension Bits
+	bitvec_write_field(dest, wp, 0, 1); // fixed 0
+	bitvec_write_field(dest, wp, 1, 1); // 1: have Additions R99
+	bitvec_write_field(dest, wp, 0, 1); // 0: don't have Packet Extended Timing Advance
+	bitvec_write_field(dest, wp, 1, 1); // TBF_EST (enabled)
+	bitvec_write_field(dest, wp, 0, 1); // 0: don't have REL 5
+};
+
+static void write_packet_ack_nack_desc_egprs(
+	struct gprs_rlcmac_bts *bts, bitvec * dest, unsigned& wp,
+	gprs_rlc_ul_window *window, bool is_final)
+{
+	int urbb_len = 0;
+	int crbb_len = 0;
+	int len;
+	bool bow = true;
+	bool eow = true;
+	int ssn = window->mod_sns(window->v_q() + 1);
+	int num_blocks = window->mod_sns(window->v_r() - window->v_q());
+	int esn_crbb = window->mod_sns(ssn - 1);
+	int rest_bits = dest->data_len * 8 - wp;
+
+	if (num_blocks > 0)
+		/* V(Q) is NACK and omitted -> SSN = V(Q) + 1 */
+		num_blocks -= 1;
+
+	if (num_blocks > window->ws())
+		num_blocks = window->ws();
+
+	if (num_blocks > rest_bits) {
+		eow = false;
+		urbb_len = rest_bits;
+		/* TODO: use compression, start encoding bits and stop when the
+		 * space is exhausted. Use the first combination that encodes
+		 * all bits. If there is none, use the combination that encodes
+		 * the largest number of bits (e.g. by setting num_blocks to the
+		 * max and repeating the construction).
+		 */
+	} else if (num_blocks > rest_bits - 9) {
+		/* union bit and length field take 9 bits */
+		eow = false;
+		urbb_len = rest_bits - 9;
+		/* TODO: use compression (see above) */
+	}
 
-	/* rbb is not NULL terminated */
-	rbb[64] = 0;
-	LOGP(DRLCMACUL, LOGL_DEBUG, "- V(N): \"%s\" R=Received "
-		"I=Invalid\n", rbb);
+	if (urbb_len + crbb_len == rest_bits)
+		len = -1;
+	else if (crbb_len == 0)
+		len = urbb_len + 15;
+	else
+		len = urbb_len + crbb_len + 23;
 
-	block->u.Packet_Uplink_Ack_Nack.u.PU_AckNack_GPRS_Struct.UnionType              = 0x0; // Fixed Allocation Dummy = on
-	block->u.Packet_Uplink_Ack_Nack.u.PU_AckNack_GPRS_Struct.u.FixedAllocationDummy = 0x0; // Fixed Allocation Dummy
-	block->u.Packet_Uplink_Ack_Nack.u.PU_AckNack_GPRS_Struct.Exist_AdditionsR99     = 0x0; // AdditionsR99 = off
+	/* EGPRS Ack/Nack Description IE */
+	if (len < 0) {
+		bitvec_write_field(dest, wp, 0, 1); // 0: don't have length
+	} else {
+		bitvec_write_field(dest, wp, 1, 1); // 1: have length
+		bitvec_write_field(dest, wp, len, 8); // length
+	}
 
-	block->u.Packet_Uplink_Ack_Nack.u.PU_AckNack_GPRS_Struct.Common_Uplink_Ack_Nack_Data.Exist_CONTENTION_RESOLUTION_TLLI = 0x1;
-	block->u.Packet_Uplink_Ack_Nack.u.PU_AckNack_GPRS_Struct.Common_Uplink_Ack_Nack_Data.CONTENTION_RESOLUTION_TLLI       = tbf->tlli();
-	block->u.Packet_Uplink_Ack_Nack.u.PU_AckNack_GPRS_Struct.Common_Uplink_Ack_Nack_Data.Exist_Packet_Timing_Advance      = 0x0;
-	block->u.Packet_Uplink_Ack_Nack.u.PU_AckNack_GPRS_Struct.Common_Uplink_Ack_Nack_Data.Exist_Extension_Bits             = 0x0;
-	block->u.Packet_Uplink_Ack_Nack.u.PU_AckNack_GPRS_Struct.Common_Uplink_Ack_Nack_Data.Exist_Power_Control_Parameters   = 0x0;
+	bitvec_write_field(dest, wp, is_final, 1); // FINAL_ACK_INDICATION
+	bitvec_write_field(dest, wp, bow, 1); // BEGINNING_OF_WINDOW
+	bitvec_write_field(dest, wp, eow, 1); // END_OF_WINDOW
+	bitvec_write_field(dest, wp, ssn, 11); // STARTING_SEQUENCE_NUMBER
+	bitvec_write_field(dest, wp, 0, 1); // 0: don't have CRBB
+
+	/* TODO: Add CRBB support */
+
+	LOGP(DRLCMACUL, LOGL_DEBUG,
+		" - EGPRS URBB, len = %d, SSN = %d, ESN_CRBB = %d, "
+		"SNS = %d, WS = %d, V(Q) = %d, V(R) = %d%s%s\n",
+		urbb_len, ssn, esn_crbb,
+		window->sns(), window->ws(), window->v_q(), window->v_r(),
+		bow ? ", BOW" : "", eow ? ", EOW" : "");
+	for (int i = urbb_len; i > 0; i--) {
+		/* Set bit at the appropriate position (see 3GPP TS 04.60 12.3.1) */
+		bool is_ack = window->m_v_n.is_received(esn_crbb + i);
+		bitvec_write_field(dest, wp, is_ack, 1);
+	}
+}
+
+static void write_packet_uplink_ack_egprs(
+	struct gprs_rlcmac_bts *bts, bitvec * dest, unsigned& wp,
+	struct gprs_rlcmac_ul_tbf *tbf, bool is_final)
+{
+	bitvec_write_field(dest, wp, 0, 2); // fixed 00
+	bitvec_write_field(dest, wp, 2, 4); // CHANNEL_CODING_COMMAND: MCS-3
+	// bitvec_write_field(dest, wp, tbf->current_cs() - 1, 4); // CHANNEL_CODING_COMMAND
+	bitvec_write_field(dest, wp, 0, 1); // 0: no RESEGMENT (nyi)
+	bitvec_write_field(dest, wp, 1, 1); // PRE_EMPTIVE_TRANSMISSION, TODO: This resembles GPRS, change it?
+	bitvec_write_field(dest, wp, 0, 1); // 0: no PRR_RETRANSMISSION_REQUEST, TODO: clarify
+	bitvec_write_field(dest, wp, 0, 1); // 0: no ARAC_RETRANSMISSION_REQUEST, TODO: clarify
+	bitvec_write_field(dest, wp, 1, 1); // 1: have CONTENTION_RESOLUTION_TLLI
+	bitvec_write_field(dest, wp, tbf->tlli(), 32); // CONTENTION_RESOLUTION_TLLI
+	bitvec_write_field(dest, wp, 1, 1); // TBF_EST (enabled)
+	bitvec_write_field(dest, wp, 0, 1); // 0: don't have Packet Timing Advance
+	bitvec_write_field(dest, wp, 0, 1); // 0: don't have Packet Extended Timing Advance
+	bitvec_write_field(dest, wp, 0, 1); // 0: don't have Power Control Parameters
+	bitvec_write_field(dest, wp, 0, 1); // 0: don't have Extension Bits
+
+	write_packet_ack_nack_desc_egprs(bts, dest, wp, &tbf->m_window, is_final);
+
+	bitvec_write_field(dest, wp, 0, 1); // fixed 0
+	bitvec_write_field(dest, wp, 0, 1); // 0: don't have REL 5
+};
+
+void Encoding::write_packet_uplink_ack(
+	struct gprs_rlcmac_bts *bts, bitvec * dest,
+	struct gprs_rlcmac_ul_tbf *tbf, bool is_final,
+	uint8_t rrbp)
+{
+	unsigned wp = 0;
+
+	LOGP(DRLCMACUL, LOGL_DEBUG, "Encoding Ack/Nack for %s "
+		"(final=%d)\n", tbf_name(tbf), is_final);
+
+	bitvec_write_field(dest, wp, 0x1, 2);  // Payload Type
+	bitvec_write_field(dest, wp, rrbp, 2);  // Uplink block with TDMA framenumber
+	bitvec_write_field(dest, wp, is_final, 1);  // Suppl/Polling Bit
+	bitvec_write_field(dest, wp, 0x0, 3);  // Uplink state flag
+	bitvec_write_field(dest, wp, 0x9, 6);  // MESSAGE TYPE Uplink Ack/Nack
+	bitvec_write_field(dest, wp, 0x0, 2);  // Page Mode
+
+	bitvec_write_field(dest, wp, 0x0, 2);  // fixed 00
+	bitvec_write_field(dest, wp, tbf->tfi(), 5);  // Uplink TFI
+
+	if (tbf->is_egprs_enabled()) {
+		/* PU_AckNack_EGPRS = on */
+		bitvec_write_field(dest, wp, 1, 1);  // 1: EGPRS
+		write_packet_uplink_ack_egprs(bts, dest, wp, tbf, is_final);
+	} else {
+		/* PU_AckNack_GPRS = on */
+		bitvec_write_field(dest, wp, 0, 1);  // 0: GPRS
+		write_packet_uplink_ack_gprs(bts, dest, wp, tbf, is_final);
+	}
+
+	LOGP(DRLCMACUL, LOGL_DEBUG,
+		"Uplink Ack/Nack bit count %d, max %d, message = %s\n",
+		wp, dest->data_len * 8,
+		osmo_hexdump(dest->data, dest->data_len));
 }
 
 unsigned Encoding::write_packet_paging_request(bitvec * dest)
@@ -487,3 +708,468 @@ unsigned Encoding::write_repeated_page_info(bitvec * dest, unsigned& wp, uint8_t
 	return wp;
 }
 
+int Encoding::rlc_write_dl_data_header(const struct gprs_rlc_data_info *rlc,
+	uint8_t *data)
+{
+	struct gprs_rlc_dl_header_egprs_1 *egprs1;
+	struct gprs_rlc_dl_header_egprs_2 *egprs2;
+	struct gprs_rlc_dl_header_egprs_3 *egprs3;
+	struct rlc_dl_header *gprs;
+	unsigned int e_fbi_header;
+	GprsCodingScheme cs = rlc->cs;
+	unsigned int offs;
+	unsigned int bsn_delta;
+
+	switch(cs.headerTypeData()) {
+	case GprsCodingScheme::HEADER_GPRS_DATA:
+		gprs = static_cast<struct rlc_dl_header *>
+			((void *)data);
+
+		gprs->usf   = rlc->usf;
+		gprs->s_p   = rlc->es_p != 0 ? 1 : 0;
+		gprs->rrbp  = rlc->rrbp;
+		gprs->pt    = 0;
+		gprs->tfi   = rlc->tfi;
+		gprs->pr    = rlc->pr;
+
+		gprs->fbi   = rlc->block_info[0].cv == 0;
+		gprs->e     = rlc->block_info[0].e;
+		gprs->bsn   = rlc->block_info[0].bsn;
+		break;
+
+	case GprsCodingScheme::HEADER_EGPRS_DATA_TYPE_1:
+		egprs1 = static_cast<struct gprs_rlc_dl_header_egprs_1 *>
+			((void *)data);
+
+		egprs1->usf    = rlc->usf;
+		egprs1->es_p   = rlc->es_p;
+		egprs1->rrbp   = rlc->rrbp;
+		egprs1->tfi_a  = rlc->tfi >> 0; /* 1 bit LSB */
+		egprs1->tfi_b  = rlc->tfi >> 1; /* 4 bits */
+		egprs1->pr     = rlc->pr;
+		egprs1->cps    = rlc->cps;
+
+		egprs1->bsn1_a = rlc->block_info[0].bsn >> 0; /* 2 bits LSB */
+		egprs1->bsn1_b = rlc->block_info[0].bsn >> 2; /* 8 bits */
+		egprs1->bsn1_c = rlc->block_info[0].bsn >> 10; /* 1 bit */
+
+		bsn_delta = (rlc->block_info[1].bsn - rlc->block_info[0].bsn) &
+			(RLC_EGPRS_SNS - 1);
+
+		egprs1->bsn2_a = bsn_delta >> 0; /* 7 bits LSB */
+		egprs1->bsn2_b = bsn_delta >> 7; /* 3 bits */
+
+		/* first FBI/E header */
+		e_fbi_header   = rlc->block_info[0].e       ? 0x01 : 0;
+		e_fbi_header  |= rlc->block_info[0].cv == 0 ? 0x02 : 0; /* FBI */
+		offs = rlc->data_offs_bits[0] / 8;
+		OSMO_ASSERT(rlc->data_offs_bits[0] % 8 == 2);
+		e_fbi_header <<= 0;
+		data[offs] = (data[offs] & 0b11111100) | e_fbi_header;
+
+		/* second FBI/E header */
+		e_fbi_header   = rlc->block_info[1].e       ? 0x01 : 0;
+		e_fbi_header  |= rlc->block_info[1].cv == 0 ? 0x02 : 0; /* FBI */
+		offs = rlc->data_offs_bits[1] / 8;
+		OSMO_ASSERT(rlc->data_offs_bits[1] % 8 == 4);
+		e_fbi_header <<= 2;
+		data[offs] = (data[offs] & 0b11110011) | e_fbi_header;
+		break;
+
+	case GprsCodingScheme::HEADER_EGPRS_DATA_TYPE_2:
+		egprs2 = static_cast<struct gprs_rlc_dl_header_egprs_2 *>
+			((void *)data);
+
+		egprs2->usf    = rlc->usf;
+		egprs2->es_p   = rlc->es_p;
+		egprs2->rrbp   = rlc->rrbp;
+		egprs2->tfi_a  = rlc->tfi >> 0; /* 1 bit LSB */
+		egprs2->tfi_b  = rlc->tfi >> 1; /* 4 bits */
+		egprs2->pr     = rlc->pr;
+		egprs2->cps    = rlc->cps;
+
+		egprs2->bsn1_a = rlc->block_info[0].bsn >> 0; /* 2 bits LSB */
+		egprs2->bsn1_b = rlc->block_info[0].bsn >> 2; /* 8 bits */
+		egprs2->bsn1_c = rlc->block_info[0].bsn >> 10; /* 1 bit */
+
+		e_fbi_header   = rlc->block_info[0].e       ? 0x01 : 0;
+		e_fbi_header  |= rlc->block_info[0].cv == 0 ? 0x02 : 0; /* FBI */
+		offs = rlc->data_offs_bits[0] / 8;
+		OSMO_ASSERT(rlc->data_offs_bits[0] % 8 == 6);
+		e_fbi_header <<= 4;
+		data[offs] = (data[offs] & 0b11001111) | e_fbi_header;
+		break;
+
+	case GprsCodingScheme::HEADER_EGPRS_DATA_TYPE_3:
+		egprs3 = static_cast<struct gprs_rlc_dl_header_egprs_3 *>
+			((void *)data);
+
+		egprs3->usf    = rlc->usf;
+		egprs3->es_p   = rlc->es_p;
+		egprs3->rrbp   = rlc->rrbp;
+		egprs3->tfi_a  = rlc->tfi >> 0; /* 1 bit LSB */
+		egprs3->tfi_b  = rlc->tfi >> 1; /* 4 bits */
+		egprs3->pr     = rlc->pr;
+		egprs3->cps    = rlc->cps;
+
+		egprs3->bsn1_a = rlc->block_info[0].bsn >> 0; /* 2 bits LSB */
+		egprs3->bsn1_b = rlc->block_info[0].bsn >> 2; /* 8 bits */
+		egprs3->bsn1_c = rlc->block_info[0].bsn >> 10; /* 1 bit */
+
+		egprs3->spb    = rlc->block_info[0].spb;
+
+		e_fbi_header   = rlc->block_info[0].e       ? 0x01 : 0;
+		e_fbi_header  |= rlc->block_info[0].cv == 0 ? 0x02 : 0; /* FBI */
+		offs = rlc->data_offs_bits[0] / 8;
+		OSMO_ASSERT(rlc->data_offs_bits[0] % 8 == 1);
+		e_fbi_header <<= 7;
+		data[offs-1] = (data[offs-1] & 0b01111111) | (e_fbi_header >> 0);
+		data[offs]   = (data[offs]   & 0b11111110) | (e_fbi_header >> 8);
+		break;
+
+	default:
+		LOGP(DRLCMACDL, LOGL_ERROR,
+			"Encoding of uplink %s data blocks not yet supported.\n",
+			cs.name());
+		return -ENOTSUP;
+	};
+
+	return 0;
+}
+
+/**
+ * \brief Copy LSB bitstream RLC data block from byte aligned buffer.
+ *
+ * Note that the bitstream is encoded in LSB first order, so the two octets
+ * 654321xx xxxxxx87 contain the octet 87654321 starting at bit position 3
+ * (LSB has bit position 1). This is a different order than the one used by
+ * CSN.1.
+ *
+ * \param data_block_idx  The block index, 0..1 for header type 1, 0 otherwise
+ * \param src     A pointer to the start of the RLC block (incl. the header)
+ * \param buffer  A data area of a least the size of the RLC block
+ * \returns  the number of bytes copied
+ */
+unsigned int Encoding::rlc_copy_from_aligned_buffer(
+	const struct gprs_rlc_data_info *rlc,
+	unsigned int data_block_idx,
+	uint8_t *dst, const uint8_t *buffer)
+{
+	unsigned int hdr_bytes;
+	unsigned int extra_bits;
+	unsigned int i;
+
+	uint8_t c, last_c;
+	const uint8_t *src;
+	const struct gprs_rlc_data_block_info *rdbi;
+
+	OSMO_ASSERT(data_block_idx < rlc->num_data_blocks);
+	rdbi = &rlc->block_info[data_block_idx];
+
+	hdr_bytes = rlc->data_offs_bits[data_block_idx] / 8;
+	extra_bits = (rlc->data_offs_bits[data_block_idx] % 8);
+
+	if (extra_bits == 0) {
+		/* It is aligned already */
+		memmove(dst + hdr_bytes, buffer, rdbi->data_len);
+		return rdbi->data_len;
+	}
+
+	src = buffer;
+	dst = dst + hdr_bytes;
+	last_c = *dst << (8 - extra_bits);
+
+	for (i = 0; i < rdbi->data_len; i++) {
+		c = src[i];
+		*(dst++) = (last_c >> (8 - extra_bits)) | (c << extra_bits);
+		last_c = c;
+	}
+
+	/* overwrite the lower extra_bits */
+	*dst = (*dst & (0xff << extra_bits)) | (last_c >> (8 - extra_bits));
+
+	return rdbi->data_len;
+}
+
+static Encoding::AppendResult rlc_data_to_dl_append_gprs(
+	struct gprs_rlc_data_block_info *rdbi,
+	gprs_llc *llc, int *offset, int *num_chunks,
+	uint8_t *data_block,
+	bool is_final)
+{
+	int chunk;
+	int space;
+	struct rlc_li_field *li;
+	uint8_t *delimiter, *data, *e_pointer;
+
+	data = data_block + *offset;
+	delimiter = data_block + *num_chunks;
+	e_pointer = (*num_chunks ? delimiter - 1 : NULL);
+
+	chunk = llc->chunk_size();
+	space = rdbi->data_len - *offset;
+
+	/* if chunk will exceed block limit */
+	if (chunk > space) {
+		LOGP(DRLCMACDL, LOGL_DEBUG, "-- Chunk with length %d "
+			"larger than space (%d) left in block: copy "
+			"only remaining space, and we are done\n",
+			chunk, space);
+		/* block is filled, so there is no extension */
+		if (e_pointer)
+			*e_pointer |= 0x01;
+		/* fill only space */
+		llc->consume(data, space);
+		/* return data block as message */
+		*offset = rdbi->data_len;
+		(*num_chunks)++;
+		return Encoding::AR_NEED_MORE_BLOCKS;
+	}
+	/* if FINAL chunk would fit precisely in space left */
+	if (chunk == space && is_final)
+	{
+		LOGP(DRLCMACDL, LOGL_DEBUG, "-- Chunk with length %d "
+			"would exactly fit into space (%d): because "
+			"this is a final block, we don't add length "
+			"header, and we are done\n", chunk, space);
+		/* block is filled, so there is no extension */
+		if (e_pointer)
+			*e_pointer |= 0x01;
+		/* fill space */
+		llc->consume(data, space);
+		*offset = rdbi->data_len;
+		(*num_chunks)++;
+		rdbi->cv = 0;
+		return Encoding::AR_COMPLETED_BLOCK_FILLED;
+	}
+	/* if chunk would fit exactly in space left */
+	if (chunk == space) {
+		LOGP(DRLCMACDL, LOGL_DEBUG, "-- Chunk with length %d "
+			"would exactly fit into space (%d): add length "
+			"header with LI=0, to make frame extend to "
+			"next block, and we are done\n", chunk, space);
+		/* make space for delimiter */
+		if (delimiter != data)
+			memmove(delimiter + 1, delimiter,
+				data - delimiter);
+		data++;
+		(*offset)++;
+		space--;
+		/* add LI with 0 length */
+		li = (struct rlc_li_field *)delimiter;
+		li->e = 1; /* not more extension */
+		li->m = 0; /* shall be set to 0, in case of li = 0 */
+		li->li = 0; /* chunk fills the complete space */
+		rdbi->e = 0; /* 0: extensions present */
+		// no need to set e_pointer nor increase delimiter
+		/* fill only space, which is 1 octet less than chunk */
+		llc->consume(data, space);
+		/* return data block as message */
+		*offset = rdbi->data_len;
+		(*num_chunks)++;
+		return Encoding::AR_NEED_MORE_BLOCKS;
+	}
+
+	LOGP(DRLCMACDL, LOGL_DEBUG, "-- Chunk with length %d is less "
+		"than remaining space (%d): add length header to "
+		"to delimit LLC frame\n", chunk, space);
+	/* the LLC frame chunk ends in this block */
+	/* make space for delimiter */
+	if (delimiter != data)
+		memmove(delimiter + 1, delimiter, data - delimiter);
+	data++;
+	(*offset)++;
+	space--;
+	/* add LI to delimit frame */
+	li = (struct rlc_li_field *)delimiter;
+	li->e = 0; /* Extension bit, maybe set later */
+	li->m = 0; /* will be set later, if there is more LLC data */
+	li->li = chunk; /* length of chunk */
+	rdbi->e = 0; /* 0: extensions present */
+	(*num_chunks)++;
+	/* copy (rest of) LLC frame to space and reset later */
+	llc->consume(data, chunk);
+	data += chunk;
+	space -= chunk;
+	(*offset) += chunk;
+	/* if we have more data and we have space left */
+	if (space > 0 && !is_final) {
+		li->m = 1; /* we indicate more frames to follow */
+		return Encoding::AR_COMPLETED_SPACE_LEFT;
+	}
+	/* if we don't have more LLC frames */
+	if (is_final) {
+		LOGP(DRLCMACDL, LOGL_DEBUG, "-- Final block, so we "
+			"done.\n");
+		li->e = 1; /* we cannot extend */
+		rdbi->cv = 0;
+		return Encoding::AR_COMPLETED_BLOCK_FILLED;
+	}
+	/* we have no space left */
+	LOGP(DRLCMACDL, LOGL_DEBUG, "-- No space left, so we are "
+		"done.\n");
+	li->e = 1; /* we cannot extend */
+	return Encoding::AR_COMPLETED_BLOCK_FILLED;
+}
+
+static Encoding::AppendResult rlc_data_to_dl_append_egprs(
+	struct gprs_rlc_data_block_info *rdbi,
+	gprs_llc *llc, int *offset, int *num_chunks,
+	uint8_t *data_block,
+	bool is_final)
+{
+	int chunk;
+	int space;
+	struct rlc_li_field_egprs *li;
+	struct rlc_li_field_egprs *prev_li;
+	uint8_t *delimiter, *data;
+
+	data = data_block + *offset;
+	delimiter = data_block + *num_chunks;
+	prev_li = (struct rlc_li_field_egprs *)
+		(*num_chunks ? delimiter - 1 : NULL);
+
+	chunk = llc->chunk_size();
+	space = rdbi->data_len - *offset;
+
+	/* if chunk will exceed block limit */
+	if (chunk > space) {
+		LOGP(DRLCMACDL, LOGL_DEBUG, "-- Chunk with length %d "
+			"larger than space (%d) left in block: copy "
+			"only remaining space, and we are done\n",
+			chunk, space);
+		/* fill only space */
+		llc->consume(data, space);
+		/* return data block as message */
+		*offset = rdbi->data_len;
+		(*num_chunks)++;
+		return Encoding::AR_NEED_MORE_BLOCKS;
+	}
+	/* if FINAL chunk would fit precisely in space left */
+	if (chunk == space && is_final)
+	{
+		LOGP(DRLCMACDL, LOGL_DEBUG, "-- Chunk with length %d "
+			"would exactly fit into space (%d): because "
+			"this is a final block, we don't add length "
+			"header, and we are done\n", chunk, space);
+		/* fill space */
+		llc->consume(data, space);
+		*offset = rdbi->data_len;
+		(*num_chunks)++;
+		rdbi->cv = 0;
+		return Encoding::AR_COMPLETED_BLOCK_FILLED;
+	}
+	/* if chunk would fit exactly in space left */
+	if (chunk == space) {
+		LOGP(DRLCMACDL, LOGL_DEBUG, "-- Chunk with length %d "
+			"would exactly fit into space (%d): just copy "
+			"it, and we are done. The next block will have "
+			"to start with an empty chunk\n",
+			chunk, space);
+		/* fill space */
+		llc->consume(data, space);
+		*offset = rdbi->data_len;
+		(*num_chunks)++;
+		return Encoding::AR_NEED_MORE_BLOCKS;
+	}
+
+	LOGP(DRLCMACDL, LOGL_DEBUG, "-- Chunk with length %d is less "
+		"than remaining space (%d): add length header to "
+		"to delimit LLC frame\n", chunk, space);
+	/* the LLC frame chunk ends in this block */
+	/* make space for delimiter */
+
+	if (delimiter != data)
+		memmove(delimiter + 1, delimiter, data - delimiter);
+
+	data      += 1;
+	(*offset) += 1;
+	space     -= 1;
+	/* add LI to delimit frame */
+	li = (struct rlc_li_field_egprs *)delimiter;
+	li->e = 1; /* Extension bit, maybe set later */
+	li->li = chunk; /* length of chunk */
+	/* tell previous extension header about the new one */
+	if (prev_li)
+		prev_li->e = 0;
+	rdbi->e = 0; /* 0: extensions present */
+	delimiter++;
+	prev_li = li;
+	(*num_chunks)++;
+	/* copy (rest of) LLC frame to space and reset later */
+	llc->consume(data, chunk);
+	data += chunk;
+	space -= chunk;
+	(*offset) += chunk;
+	/* if we have more data and we have space left */
+	if (space > 0) {
+		if (!is_final)
+			return Encoding::AR_COMPLETED_SPACE_LEFT;
+
+		/* we don't have more LLC frames */
+		/* We will have to add another chunk with filling octets */
+		LOGP(DRLCMACDL, LOGL_DEBUG,
+			"-- There is remaining space (%d): add filling byte chunk\n",
+			space);
+
+		if (delimiter != data)
+			memmove(delimiter + 1, delimiter, data - delimiter);
+
+		data      += 1;
+		(*offset) += 1;
+		space     -= 1;
+
+		/* set filling bytes extension */
+		li = (struct rlc_li_field_egprs *)delimiter;
+		li->e = 1;
+		li->li = 127;
+
+		/* tell previous extension header about the new one */
+		if (prev_li)
+			prev_li->e = 0;
+
+		delimiter++;
+		(*num_chunks)++;
+
+		rdbi->cv = 0;
+
+		LOGP(DRLCMACDL, LOGL_DEBUG, "-- Final block, so we "
+			"are done.\n");
+
+		*offset = rdbi->data_len;
+		return Encoding::AR_COMPLETED_BLOCK_FILLED;
+	}
+
+	if (is_final) {
+		/* we don't have more LLC frames */
+		LOGP(DRLCMACDL, LOGL_DEBUG, "-- Final block, so we "
+			"are done.\n");
+		rdbi->cv = 0;
+		return Encoding::AR_COMPLETED_BLOCK_FILLED;
+	}
+
+	/* we have no space left */
+	LOGP(DRLCMACDL, LOGL_DEBUG, "-- No space left, so we are "
+		"done.\n");
+	return Encoding::AR_COMPLETED_BLOCK_FILLED;
+}
+
+Encoding::AppendResult Encoding::rlc_data_to_dl_append(
+	struct gprs_rlc_data_block_info *rdbi, GprsCodingScheme cs,
+	gprs_llc *llc, int *offset, int *num_chunks,
+	uint8_t *data_block,
+	bool is_final)
+{
+	if (cs.isGprs())
+		return rlc_data_to_dl_append_gprs(rdbi,
+			llc, offset, num_chunks, data_block, is_final);
+
+	if (cs.isEgprs())
+		return rlc_data_to_dl_append_egprs(rdbi,
+			llc, offset, num_chunks, data_block, is_final);
+
+	LOGP(DRLCMACDL, LOGL_ERROR, "%s data block encoding not implemented\n",
+		cs.name());
+
+	return AR_NEED_MORE_BLOCKS;
+}
diff --git a/src/encoding.h b/src/encoding.h
index ac66838d..94e9a02f 100644
--- a/src/encoding.h
+++ b/src/encoding.h
@@ -22,10 +22,13 @@
 
 #include <stdint.h>
 #include <gsm_rlcmac.h>
+#include <gprs_coding_scheme.h>
 
 struct gprs_rlcmac_bts;
 struct gprs_rlcmac_tbf;
 struct bitvec;
+struct gprs_llc;
+struct gprs_rlc_data_block_info;
 
 /**
  * I help with encoding data into CSN1 messages.
@@ -36,28 +39,33 @@ struct bitvec;
 class Encoding {
 public:
 	static int write_immediate_assignment(
-			struct gprs_rlcmac_bts *bts,
-			bitvec * dest, uint8_t downlink, uint8_t ra, 
-		        uint32_t ref_fn, uint8_t ta, uint16_t arfcn, uint8_t ts, uint8_t tsc, 
-		        uint8_t tfi, uint8_t usf, uint32_t tlli, uint8_t polling,
-			uint32_t fn, uint8_t single_block, uint8_t alpha, uint8_t gamma,
+			struct gprs_rlcmac_tbf *tbf,
+			bitvec * dest, uint8_t downlink, uint8_t ra,
+			uint32_t ref_fn, uint8_t ta, uint16_t arfcn, uint8_t ts,
+			uint8_t tsc, uint8_t usf, uint8_t polling,
+			uint32_t fn, uint8_t alpha, uint8_t gamma,
 			int8_t ta_idx);
 
 	static void write_packet_uplink_assignment(
 			struct gprs_rlcmac_bts *bts,
 			bitvec * dest, uint8_t old_tfi,
-			uint8_t old_downlink, uint32_t tlli, uint8_t use_tlli, 
-			struct gprs_rlcmac_ul_tbf *tbf, uint8_t poll, uint8_t alpha,
-			uint8_t gamma, int8_t ta_idx, int8_t use_egprs);
+			uint8_t old_downlink, uint32_t tlli, uint8_t use_tlli,
+			struct gprs_rlcmac_ul_tbf *tbf, uint8_t poll, uint8_t rrbp,
+			uint8_t alpha, uint8_t gamma, int8_t ta_idx,
+			int8_t use_egprs);
 
-	static void write_packet_downlink_assignment(RlcMacDownlink_t * block, uint8_t old_tfi,
-			uint8_t old_downlink, struct gprs_rlcmac_tbf *tbf, uint8_t poll,
-			uint8_t alpha, uint8_t gamma, int8_t ta_idx, uint8_t ta_ts);
+	static void write_packet_downlink_assignment(RlcMacDownlink_t * block,
+			bool old_tfi_is_valid, uint8_t old_tfi, uint8_t old_downlink,
+			struct gprs_rlcmac_tbf *tbf, uint8_t poll, uint8_t rrbp,
+			uint8_t alpha, uint8_t gamma,
+			int8_t ta_idx, uint8_t ta_ts, bool use_egprs);
 
 	static void encode_rbb(const char *show_rbb, uint8_t *rbb);
 
-	static void write_packet_uplink_ack(struct gprs_rlcmac_bts *bts, RlcMacDownlink_t * block, struct gprs_rlcmac_ul_tbf *tbf,
-		        uint8_t final);
+	static void write_packet_uplink_ack(
+			struct gprs_rlcmac_bts *bts, bitvec * dest,
+			struct gprs_rlcmac_ul_tbf *tbf, bool is_final,
+			uint8_t rrbp);
 
 	static int write_paging_request(bitvec * dest, uint8_t *ptmsi, uint16_t ptmsi_len);
 
@@ -65,4 +73,24 @@ public:
 			uint8_t *identity, uint8_t chan_needed);
 
 	static unsigned write_packet_paging_request(bitvec * dest);
+
+	static int rlc_write_dl_data_header(
+			const struct gprs_rlc_data_info *rlc,
+			uint8_t *data);
+	static unsigned int rlc_copy_from_aligned_buffer(
+			const struct gprs_rlc_data_info *rlc,
+			unsigned int data_block_idx,
+			uint8_t *dst, const uint8_t *buffer);
+
+	enum AppendResult {
+		AR_NEED_MORE_BLOCKS,
+		AR_COMPLETED_SPACE_LEFT,
+		AR_COMPLETED_BLOCK_FILLED,
+	};
+
+	static AppendResult rlc_data_to_dl_append(
+		struct gprs_rlc_data_block_info *rdbi, GprsCodingScheme cs,
+		gprs_llc *llc, int *offset, int *num_chunks,
+		uint8_t *data,
+		bool is_final);
 };
diff --git a/src/gprs_bssgp_pcu.cpp b/src/gprs_bssgp_pcu.cpp
index 838c667d..da1d26c1 100644
--- a/src/gprs_bssgp_pcu.cpp
+++ b/src/gprs_bssgp_pcu.cpp
@@ -23,6 +23,7 @@
 #include <pcu_l1_if.h>
 #include <bts.h>
 #include <tbf.h>
+#include <decoding.h>
 
 #define BSSGP_TIMER_T1	30	/* Guards the (un)blocking procedures */
 #define BSSGP_TIMER_T2	30	/* Guards the reset procedure */
@@ -73,44 +74,30 @@ static int parse_imsi(struct tlv_parsed *tp, char *imsi)
 	return 0;
 }
 
-static int parse_ra_cap_ms_class(struct tlv_parsed *tp)
+static int parse_ra_cap(struct tlv_parsed *tp, MS_Radio_Access_capability_t *rac)
 {
 	bitvec *block;
-	unsigned rp = 0;
-	uint8_t ms_class = 0;
 	uint8_t cap_len;
 	uint8_t *cap;
 
+	memset(rac, 0, sizeof(*rac));
+
 	if (!TLVP_PRESENT(tp, BSSGP_IE_MS_RADIO_ACCESS_CAP))
-		return ms_class;
+		return -EINVAL;
 
 	cap_len = TLVP_LEN(tp, BSSGP_IE_MS_RADIO_ACCESS_CAP);
 	cap = (uint8_t *) TLVP_VAL(tp, BSSGP_IE_MS_RADIO_ACCESS_CAP);
 
+	LOGP(DBSSGP, LOGL_DEBUG, "Got BSSGP RA Capability of size %d\n", cap_len);
+
 	block = bitvec_alloc(cap_len);
 	bitvec_unpack(block, cap);
-	bitvec_read_field(block, rp, 4); // Access Technology Type
-	bitvec_read_field(block, rp, 7); // Length of Access Capabilities
-	bitvec_read_field(block, rp, 3); // RF Power Capability
-	if (bitvec_read_field(block, rp, 1)) // A5 Bits Present
-		bitvec_read_field(block, rp, 7); // A5 Bits
-	bitvec_read_field(block, rp, 1); // ES IND
-	bitvec_read_field(block, rp, 1); // PS
-	bitvec_read_field(block, rp, 1); // VGCS
-	bitvec_read_field(block, rp, 1); // VBS
-	if (bitvec_read_field(block, rp, 1)) { // Multislot Cap Present
-		if (bitvec_read_field(block, rp, 1)) // HSCSD Present
-			bitvec_read_field(block, rp, 5); // Class
-		if (bitvec_read_field(block, rp, 1)) { // GPRS Present
-			ms_class = bitvec_read_field(block, rp, 5); // Class
-			bitvec_read_field(block, rp, 1); // Ext.
-		}
-		if (bitvec_read_field(block, rp, 1)) // SMS Present
-			bitvec_read_field(block, rp, 4); // SMS Value
-	}
+
+	/* TS 24.008, 10.5.5.12a */
+	decode_gsm_ra_cap(block, rac);
 
 	bitvec_free(block);
-	return ms_class;
+	return 0;
 }
 
 static int gprs_bssgp_pcu_rx_dl_ud(struct msgb *msg, struct tlv_parsed *tp)
@@ -122,6 +109,11 @@ static int gprs_bssgp_pcu_rx_dl_ud(struct msgb *msg, struct tlv_parsed *tp)
 	uint8_t *data;
 	uint16_t len;
 	char imsi[16] = "000";
+	uint8_t ms_class = 0;
+	uint8_t egprs_ms_class = 0;
+#if 0
+	MS_Radio_Access_capability_t rac;
+#endif
 
 	budh = (struct bssgp_ud_hdr *)msgb_bssgph(msg);
 	tlli = ntohl(budh->tlli);
@@ -146,10 +138,17 @@ static int gprs_bssgp_pcu_rx_dl_ud(struct msgb *msg, struct tlv_parsed *tp)
 	 * will listen to all paging blocks. */
 	parse_imsi(tp, imsi);
 
+#if 0 /* Do not rely on this IE. TODO: make this configurable */
 	/* parse ms radio access capability */
-	uint8_t ms_class = parse_ra_cap_ms_class(tp);
-	/* TODO: Get the EGPRS class from the CSN.1 RA capability */
-	uint8_t egprs_ms_class = 0;
+	if (parse_ra_cap(tp, &rac) >= 0) {
+		/* Get the EGPRS class from the RA capability */
+		ms_class = Decoding::get_ms_class_by_capability(&rac);
+		egprs_ms_class =
+			Decoding::get_egprs_ms_class_by_capability(&rac);
+		LOGP(DBSSGP, LOGL_DEBUG, "Got downlink MS class %d/%d\n",
+			ms_class, egprs_ms_class);
+	}
+#endif
 
 	/* get lifetime */
 	uint16_t delay_csec = 0xffff;
@@ -534,19 +533,12 @@ static unsigned count_pdch(const struct gprs_rlcmac_bts *bts)
 	return num_pdch;
 }
 
-static uint32_t gprs_bssgp_max_leak_rate(unsigned cs, int num_pdch)
+static uint32_t gprs_bssgp_max_leak_rate(GprsCodingScheme cs, int num_pdch)
 {
-	static const uint32_t max_lr_per_ts[4] = {
-		20 * (1000 / 20), /* CS-1: 20 byte payload per 20ms */
-		30 * (1000 / 20), /* CS-2: 30 byte payload per 20ms */
-		36 * (1000 / 20), /* CS-3: 36 byte payload per 20ms */
-		50 * (1000 / 20), /* CS-4: 50 byte payload per 20ms */
-	};
-
-	if (cs > ARRAY_SIZE(max_lr_per_ts))
-		cs = 1;
+	int bytes_per_rlc_block = cs.maxDataBlockBytes() * cs.numDataBlocks();
 
-	return max_lr_per_ts[cs-1] * num_pdch;
+	/* n byte payload per 20ms */
+	return bytes_per_rlc_block * (1000 / 20) * num_pdch;
 }
 
 static uint32_t compute_bucket_size(struct gprs_rlcmac_bts *bts,
@@ -619,6 +611,45 @@ static int get_and_reset_measured_leak_rate(int *usage_by_1000, unsigned num_pdc
 	return rate;
 }
 
+static GprsCodingScheme max_coding_scheme_dl(struct gprs_rlcmac_bts *bts)
+{
+	int num;
+
+	if (bts->egprs_enabled) {
+		if (!bts->cs_adj_enabled) {
+			if (bts->initial_mcs_dl)
+				num = bts->initial_mcs_dl;
+			else
+				num = 1;
+		} else if (bts->max_mcs_dl) {
+			num = bts->max_mcs_dl;
+		} else {
+			num = 9;
+		}
+
+		return GprsCodingScheme::getEgprsByNum(num);
+	}
+
+	if (!bts->cs_adj_enabled) {
+		if (bts->initial_cs_dl)
+			num = bts->initial_cs_dl;
+		else if (bts->cs4)
+			num = 4;
+		else if (bts->cs3)
+			num = 3;
+		else if (bts->cs2)
+			num = 2;
+		else
+			num = 1;
+	} else if (bts->max_cs_dl) {
+		num = bts->max_cs_dl;
+	} else {
+		num = 4;
+	}
+
+	return GprsCodingScheme::getGprsByNum(num);
+}
+
 int gprs_bssgp_tx_fc_bvc(void)
 {
 	struct gprs_rlcmac_bts *bts;
@@ -628,7 +659,7 @@ int gprs_bssgp_tx_fc_bvc(void)
 	uint32_t ms_leak_rate; /* oct/s */
 	uint32_t avg_delay_ms;
 	int num_pdch = -1;
-	int max_cs_dl;
+	GprsCodingScheme max_cs_dl;
 
 	if (!the_pcu.bctx) {
 		LOGP(DBSSGP, LOGL_ERROR, "No bctx\n");
@@ -636,21 +667,7 @@ int gprs_bssgp_tx_fc_bvc(void)
 	}
 	bts = bts_main_data();
 
-	if (bts->cs_adj_enabled) {
-		max_cs_dl = bts->max_cs_dl;
-		if (!max_cs_dl) {
-			if (bts->cs4)
-				max_cs_dl = 4;
-			else if (bts->cs3)
-				max_cs_dl = 3;
-			else if (bts->cs2)
-				max_cs_dl = 2;
-			else
-				max_cs_dl = 1;
-		}
-	} else {
-		max_cs_dl = bts->initial_cs_dl;
-	}
+	max_cs_dl = max_coding_scheme_dl(bts);
 
 	bucket_size = bts->fc_bvc_bucket_size;
 	leak_rate = bts->fc_bvc_leak_rate;
@@ -684,8 +701,8 @@ int gprs_bssgp_tx_fc_bvc(void)
 		leak_rate = gprs_bssgp_max_leak_rate(max_cs_dl, num_pdch);
 
 		LOGP(DBSSGP, LOGL_DEBUG,
-			"Computed BVC leak rate = %d, num_pdch = %d, cs = %d\n",
-			leak_rate, num_pdch, max_cs_dl);
+			"Computed BVC leak rate = %d, num_pdch = %d, cs = %s\n",
+			leak_rate, num_pdch, max_cs_dl.name());
 	};
 
 	if (ms_leak_rate == 0) {
@@ -707,8 +724,9 @@ int gprs_bssgp_tx_fc_bvc(void)
 		 * should be derived from the max number of PDCH TS per TRX.
 		 */
 		LOGP(DBSSGP, LOGL_DEBUG,
-			"Computed MS default leak rate = %d, ms_num_pdch = %d, cs = %d\n",
-			ms_leak_rate, ms_num_pdch, max_cs_dl);
+			"Computed MS default leak rate = %d, ms_num_pdch = %d, "
+			"cs = %s\n",
+			ms_leak_rate, ms_num_pdch, max_cs_dl.name());
 	};
 
 	/* TODO: Force leak_rate to 0 on buffer bloat */
diff --git a/src/gprs_coding_scheme.cpp b/src/gprs_coding_scheme.cpp
index 04eb3b6f..8601d4f5 100644
--- a/src/gprs_coding_scheme.cpp
+++ b/src/gprs_coding_scheme.cpp
@@ -25,30 +25,61 @@ static struct {
 	struct {
 		unsigned int bytes;
 		unsigned int ext_bits;
+		unsigned int data_header_bits;
 	} uplink, downlink;
 	unsigned int data_bytes;
-	unsigned int num_blocks;
+	unsigned int optional_padding_bits;
 	const char *name;
 	GprsCodingScheme::HeaderType data_hdr;
+	GprsCodingScheme::Family family;
 } mcs_info[GprsCodingScheme::NUM_SCHEMES] = {
-	{{0, 0},    {0, 0},    0, 0, "UNKNOWN", GprsCodingScheme::HEADER_INVALID},
-	{{23, 0},   {23, 0},  20, 1, "CS-1", GprsCodingScheme::HEADER_GPRS_DATA},
-	{{33, 7},   {33, 7},  30, 1, "CS-2", GprsCodingScheme::HEADER_GPRS_DATA},
-	{{39, 3},   {39, 3},  36, 1, "CS-3", GprsCodingScheme::HEADER_GPRS_DATA},
-	{{53, 7},   {53, 7},  50, 1, "CS-4", GprsCodingScheme::HEADER_GPRS_DATA},
-
-	{{26, 1},   {26, 1},  22, 1, "MCS-1", GprsCodingScheme::HEADER_EGPRS_DATA_TYPE_3},
-	{{32, 1},   {32, 1},  28, 1, "MCS-2", GprsCodingScheme::HEADER_EGPRS_DATA_TYPE_3},
-	{{41, 1},   {41, 1},  37, 1, "MCS-3", GprsCodingScheme::HEADER_EGPRS_DATA_TYPE_3},
-	{{48, 1},   {48, 1},  44, 1, "MCS-4", GprsCodingScheme::HEADER_EGPRS_DATA_TYPE_3},
-
-	{{60, 7},   {59, 6},  56, 1, "MCS-5", GprsCodingScheme::HEADER_EGPRS_DATA_TYPE_2},
-	{{78, 7},   {77, 6},  74, 1, "MCS-6", GprsCodingScheme::HEADER_EGPRS_DATA_TYPE_2},
-	{{118, 2},  {117, 4}, 56, 2, "MCS-7", GprsCodingScheme::HEADER_EGPRS_DATA_TYPE_1},
-	{{142, 2},  {141, 4}, 68, 2, "MCS-8", GprsCodingScheme::HEADER_EGPRS_DATA_TYPE_1},
-	{{154, 2},  {153, 4}, 74, 2, "MCS-9", GprsCodingScheme::HEADER_EGPRS_DATA_TYPE_1},
+	{{0, 0},   {0, 0},    0,  0, "UNKNOWN",
+		GprsCodingScheme::HEADER_INVALID, GprsCodingScheme::FAMILY_INVALID},
+	{{23, 0},  {23, 0},  20,  0, "CS-1",
+		GprsCodingScheme::HEADER_GPRS_DATA, GprsCodingScheme::FAMILY_INVALID},
+	{{33, 7},  {33, 7},  30,  0, "CS-2",
+		GprsCodingScheme::HEADER_GPRS_DATA, GprsCodingScheme::FAMILY_INVALID},
+	{{39, 3},  {39, 3},  36,  0, "CS-3",
+		GprsCodingScheme::HEADER_GPRS_DATA, GprsCodingScheme::FAMILY_INVALID},
+	{{53, 7},  {53, 7},  50,  0, "CS-4",
+		GprsCodingScheme::HEADER_GPRS_DATA, GprsCodingScheme::FAMILY_INVALID},
+
+	{{26, 1},  {26, 1},  22,  0, "MCS-1",
+		GprsCodingScheme::HEADER_EGPRS_DATA_TYPE_3, GprsCodingScheme::FAMILY_C},
+	{{32, 1},  {32, 1},  28,  0, "MCS-2",
+		GprsCodingScheme::HEADER_EGPRS_DATA_TYPE_3, GprsCodingScheme::FAMILY_B},
+	{{41, 1},  {41, 1},  37, 48, "MCS-3",
+		GprsCodingScheme::HEADER_EGPRS_DATA_TYPE_3, GprsCodingScheme::FAMILY_A},
+	{{48, 1},  {48, 1},  44,  0, "MCS-4",
+		GprsCodingScheme::HEADER_EGPRS_DATA_TYPE_3, GprsCodingScheme::FAMILY_C},
+
+	{{60, 7},  {59, 6},  56,  0, "MCS-5",
+		GprsCodingScheme::HEADER_EGPRS_DATA_TYPE_2, GprsCodingScheme::FAMILY_B},
+	{{78, 7},  {77, 6},  74, 48, "MCS-6",
+		GprsCodingScheme::HEADER_EGPRS_DATA_TYPE_2, GprsCodingScheme::FAMILY_A},
+	{{118, 2}, {117, 4}, 56,  0, "MCS-7",
+		GprsCodingScheme::HEADER_EGPRS_DATA_TYPE_1, GprsCodingScheme::FAMILY_B},
+	{{142, 2}, {141, 4}, 68,  0, "MCS-8",
+		GprsCodingScheme::HEADER_EGPRS_DATA_TYPE_1, GprsCodingScheme::FAMILY_A},
+	{{154, 2}, {153, 4}, 74,  0, "MCS-9",
+		GprsCodingScheme::HEADER_EGPRS_DATA_TYPE_1, GprsCodingScheme::FAMILY_A},
 };
 
+static struct {
+	struct {
+		int data_header_bits;
+	} uplink, downlink;
+	unsigned int data_block_header_bits;
+	unsigned int num_blocks;
+	const char *name;
+} hdr_type_info[GprsCodingScheme::NUM_HEADER_TYPES] = {
+	{{0},       {0},       0, 0, "INVALID"},
+	{{1*8 + 0}, {1*8 + 0}, 0, 0, "CONTROL"},
+	{{3*8 + 0}, {3*8 + 0}, 0, 1, "GPRS_DATA"},
+	{{5*8 + 6}, {5*8 + 0}, 2, 2, "EGPRS_DATA_TYPE1"},
+	{{4*8 + 5}, {3*8 + 4}, 2, 1, "EGPRS_DATA_TYPE2"},
+	{{3*8 + 7}, {3*8 + 7}, 2, 1, "EGPRS_DATA_TYPE3"},
+};
 
 GprsCodingScheme GprsCodingScheme::getBySizeUL(unsigned size)
 {
@@ -73,7 +104,15 @@ GprsCodingScheme GprsCodingScheme::getBySizeUL(unsigned size)
 
 unsigned int GprsCodingScheme::sizeUL() const
 {
-	return maxBytesUL() + (spareBitsUL() ? 1 : 0);
+	return mcs_info[m_scheme].uplink.bytes + (spareBitsUL() ? 1 : 0);
+}
+
+unsigned int GprsCodingScheme::usedSizeUL() const
+{
+	if (mcs_info[m_scheme].data_hdr == HEADER_GPRS_DATA)
+		return mcs_info[m_scheme].uplink.bytes;
+	else
+		return sizeUL();
 }
 
 unsigned int GprsCodingScheme::maxBytesUL() const
@@ -88,7 +127,15 @@ unsigned int GprsCodingScheme::spareBitsUL() const
 
 unsigned int GprsCodingScheme::sizeDL() const
 {
-	return maxBytesDL() + (spareBitsDL() ? 1 : 0);
+	return mcs_info[m_scheme].downlink.bytes + (spareBitsDL() ? 1 : 0);
+}
+
+unsigned int GprsCodingScheme::usedSizeDL() const
+{
+	if (mcs_info[m_scheme].data_hdr == HEADER_GPRS_DATA)
+		return mcs_info[m_scheme].downlink.bytes;
+	else
+		return sizeDL();
 }
 
 unsigned int GprsCodingScheme::maxBytesDL() const
@@ -106,9 +153,29 @@ unsigned int GprsCodingScheme::maxDataBlockBytes() const
 	return mcs_info[m_scheme].data_bytes;
 }
 
+unsigned int GprsCodingScheme::optionalPaddingBits() const
+{
+	return mcs_info[m_scheme].optional_padding_bits;
+}
+
 unsigned int GprsCodingScheme::numDataBlocks() const
 {
-	return mcs_info[m_scheme].num_blocks;
+	return hdr_type_info[headerTypeData()].num_blocks;
+}
+
+unsigned int GprsCodingScheme::numDataHeaderBitsUL() const
+{
+	return hdr_type_info[headerTypeData()].uplink.data_header_bits;
+}
+
+unsigned int GprsCodingScheme::numDataHeaderBitsDL() const
+{
+	return hdr_type_info[headerTypeData()].downlink.data_header_bits;
+}
+
+unsigned int GprsCodingScheme::numDataBlockHeaderBits() const
+{
+	return hdr_type_info[headerTypeData()].data_block_header_bits;
 }
 
 const char *GprsCodingScheme::name() const
@@ -120,3 +187,100 @@ GprsCodingScheme::HeaderType GprsCodingScheme::headerTypeData() const
 {
 	return mcs_info[m_scheme].data_hdr;
 }
+
+GprsCodingScheme::Family GprsCodingScheme::family() const
+{
+	return mcs_info[m_scheme].family;
+}
+
+void GprsCodingScheme::inc(Mode mode)
+{
+	if (!isCompatible(mode))
+		/* This should not happen. TODO: Use assert? */
+		return;
+
+	Scheme new_cs(Scheme(m_scheme + 1));
+	if (!GprsCodingScheme(new_cs).isCompatible(mode))
+		/* Clipping, do not change the value */
+		return;
+
+	m_scheme = new_cs;
+}
+
+void GprsCodingScheme::dec(Mode mode)
+{
+	if (!isCompatible(mode))
+		/* This should not happen. TODO: Use assert? */
+		return;
+
+	Scheme new_cs(Scheme(m_scheme - 1));
+	if (!GprsCodingScheme(new_cs).isCompatible(mode))
+		/* Clipping, do not change the value */
+		return;
+
+	m_scheme = new_cs;
+}
+
+void GprsCodingScheme::inc()
+{
+	if (isGprs() && m_scheme == CS4)
+		return;
+
+	if (isEgprs() && m_scheme == MCS9)
+		return;
+
+	if (!isValid())
+		return;
+
+	m_scheme = Scheme(m_scheme + 1);
+}
+
+void GprsCodingScheme::dec()
+{
+	if (isGprs() && m_scheme == CS1)
+		return;
+
+	if (isEgprs() && m_scheme == MCS1)
+		return;
+
+	if (!isValid())
+		return;
+
+	m_scheme = Scheme(m_scheme - 1);
+}
+
+const char *GprsCodingScheme::modeName(Mode mode)
+{
+	switch (mode) {
+	case GPRS:       return "GPRS";
+	case EGPRS_GMSK: return "EGPRS_GMSK-only";
+	case EGPRS:      return "EGPRS";
+	default:         return "???";
+	}
+}
+
+bool GprsCodingScheme::isFamilyCompatible(GprsCodingScheme o) const
+{
+	if (*this == o)
+		return true;
+
+	if (family() == FAMILY_INVALID)
+		return false;
+
+	return family() == o.family();
+}
+
+bool GprsCodingScheme::isCombinable(GprsCodingScheme o) const
+{
+	return numDataBlocks() == o.numDataBlocks();
+}
+
+void GprsCodingScheme::decToSingleBlock(bool *needStuffing)
+{
+	switch (m_scheme) {
+	case MCS7: *needStuffing = false; m_scheme = MCS5; break;
+	case MCS8: *needStuffing =  true; m_scheme = MCS6; break;
+	case MCS9: *needStuffing = false; m_scheme = MCS6; break;
+	default:   *needStuffing = false; break;
+	}
+}
diff --git a/src/gprs_coding_scheme.h b/src/gprs_coding_scheme.h
index a91d1bde..aec37623 100644
--- a/src/gprs_coding_scheme.h
+++ b/src/gprs_coding_scheme.h
@@ -47,41 +47,81 @@ public:
 		HEADER_EGPRS_DATA_TYPE_1,
 		HEADER_EGPRS_DATA_TYPE_2,
 		HEADER_EGPRS_DATA_TYPE_3,
+		NUM_HEADER_TYPES
+	};
+
+	enum Family {
+		FAMILY_INVALID,
+		FAMILY_A,
+		FAMILY_B,
+		FAMILY_C,
 	};
 
 	GprsCodingScheme(Scheme s = UNKNOWN);
 
 	operator bool() const {return m_scheme != UNKNOWN;}
-	operator int()  const {return (int)m_scheme;}
-	void operator =(Scheme s);
-	void operator =(GprsCodingScheme o);
+	operator Scheme() const {return m_scheme;}
+	unsigned int to_num() const;
+
+	GprsCodingScheme& operator =(Scheme s);
+	GprsCodingScheme& operator =(GprsCodingScheme o);
+
 	bool isValid()   const {return UNKNOWN <= m_scheme && m_scheme <= MCS9;}
 	bool isGprs()   const {return CS1 <= m_scheme && m_scheme <= CS4;}
 	bool isEgprs()  const {return m_scheme >= MCS1;}
 	bool isEgprsGmsk()  const {return isEgprs() && m_scheme <= MCS4;}
 	bool isCompatible(Mode mode) const;
+	bool isCompatible(GprsCodingScheme o) const;
+	bool isFamilyCompatible(GprsCodingScheme o) const;
+	bool isCombinable(GprsCodingScheme o) const;
 
 	void inc(Mode mode);
 	void dec(Mode mode);
+	void inc();
+	void dec();
+	void decToSingleBlock(bool *needStuffing);
 
 	unsigned int sizeUL() const;
 	unsigned int sizeDL() const;
+	unsigned int usedSizeUL() const;
+	unsigned int usedSizeDL() const;
 	unsigned int maxBytesUL() const;
 	unsigned int maxBytesDL() const;
 	unsigned int spareBitsUL() const;
 	unsigned int spareBitsDL() const;
 	unsigned int maxDataBlockBytes() const;
 	unsigned int numDataBlocks() const;
+	unsigned int numDataHeaderBitsUL() const;
+	unsigned int numDataHeaderBitsDL() const;
+	unsigned int numDataBlockHeaderBits() const;
+	unsigned int optionalPaddingBits() const;
 	const char *name() const;
 	HeaderType headerTypeData() const;
 	HeaderType headerTypeControl() const;
+	Family family() const;
 
 	static GprsCodingScheme getBySizeUL(unsigned size);
+	static GprsCodingScheme getGprsByNum(unsigned num);
+	static GprsCodingScheme getEgprsByNum(unsigned num);
 
+	static const char *modeName(Mode mode);
 private:
+	GprsCodingScheme(int s); /* fail on use */
+	GprsCodingScheme& operator =(int s); /* fail on use */
 	enum Scheme m_scheme;
 };
 
+inline unsigned int GprsCodingScheme::to_num() const
+{
+	if (isGprs())
+		return (m_scheme - CS1) + 1;
+
+	if (isEgprs())
+		return (m_scheme - MCS1) + 1;
+
+	return 0;
+}
+
 inline bool GprsCodingScheme::isCompatible(Mode mode) const
 {
 	switch (mode) {
@@ -93,32 +133,9 @@ inline bool GprsCodingScheme::isCompatible(Mode mode) const
 	return false;
 }
 
-inline void GprsCodingScheme::inc(Mode mode)
-{
-	if (!isCompatible(mode))
-		/* This should not happen. TODO: Use assert? */
-		return;
-
-	Scheme new_cs(Scheme(m_scheme + 1));
-	if (!GprsCodingScheme(new_cs).isCompatible(mode))
-		/* Clipping, do not change the value */
-		return;
-
-	m_scheme = new_cs;
-}
-
-inline void GprsCodingScheme::dec(Mode mode)
+inline bool GprsCodingScheme::isCompatible(GprsCodingScheme o) const
 {
-	if (!isCompatible(mode))
-		/* This should not happen. TODO: Use assert? */
-		return;
-
-	Scheme new_cs(Scheme(m_scheme - 1));
-	if (!GprsCodingScheme(new_cs).isCompatible(mode))
-		/* Clipping, do not change the value */
-		return;
-
-	m_scheme = new_cs;
+	return (isGprs() && o.isGprs()) || (isEgprs() && o.isEgprs());
 }
 
 inline GprsCodingScheme::HeaderType GprsCodingScheme::headerTypeControl() const
@@ -133,15 +150,67 @@ inline GprsCodingScheme::GprsCodingScheme(Scheme s)
 		m_scheme = UNKNOWN;
 }
 
-inline void GprsCodingScheme::operator =(Scheme s)
+inline GprsCodingScheme& GprsCodingScheme::operator =(Scheme s)
 {
 	m_scheme = s;
 
 	if (!isValid())
 		m_scheme = UNKNOWN;
+
+	return *this;
 }
 
-inline void GprsCodingScheme::operator =(GprsCodingScheme o)
+inline GprsCodingScheme& GprsCodingScheme::operator =(GprsCodingScheme o)
 {
 	m_scheme = o.m_scheme;
+	return *this;
+}
+
+inline GprsCodingScheme GprsCodingScheme::getGprsByNum(unsigned num)
+{
+	if (num < 1 || num > 4)
+		return GprsCodingScheme();
+
+	return GprsCodingScheme(Scheme(CS1 + (num - 1)));
+}
+
+inline GprsCodingScheme GprsCodingScheme::getEgprsByNum(unsigned num)
+{
+	if (num < 1 || num > 9)
+		return GprsCodingScheme();
+
+	return GprsCodingScheme(Scheme(MCS1 + (num - 1)));
+}
+
+/* The coding schemes form a partial ordering */
+inline bool operator ==(GprsCodingScheme a, GprsCodingScheme b)
+{
+	return GprsCodingScheme::Scheme(a) == GprsCodingScheme::Scheme(b);
+}
+
+inline bool operator !=(GprsCodingScheme a, GprsCodingScheme b)
+{
+	return !(a == b);
+}
+
+inline bool operator <(GprsCodingScheme a, GprsCodingScheme b)
+{
+	return a.isCompatible(b) &&
+		GprsCodingScheme::Scheme(a) < GprsCodingScheme::Scheme(b);
+}
+
+inline bool operator >(GprsCodingScheme a, GprsCodingScheme b)
+{
+	return b < a;
 }
+
+inline bool operator <=(GprsCodingScheme a, GprsCodingScheme b)
+{
+	return a == b || a < b;
+}
+
+inline bool operator >=(GprsCodingScheme a, GprsCodingScheme b)
+{
+	return a == b || a > b;
+}
+
diff --git a/src/gprs_ms.cpp b/src/gprs_ms.cpp
index 76fe47c2..78f03f84 100644
--- a/src/gprs_ms.cpp
+++ b/src/gprs_ms.cpp
@@ -25,6 +25,7 @@
 #include "tbf.h"
 #include "gprs_debug.h"
 #include "gprs_codel.h"
+#include "pcu_utils.h"
 
 #include <time.h>
 
@@ -97,8 +98,6 @@ GprsMs::GprsMs(BTS *bts, uint32_t tlli) :
 	m_ta(0),
 	m_ms_class(0),
 	m_egprs_ms_class(0),
-	m_current_cs_ul(1),
-	m_current_cs_dl(1),
 	m_is_idle(true),
 	m_ref(0),
 	m_list(this),
@@ -107,7 +106,8 @@ GprsMs::GprsMs(BTS *bts, uint32_t tlli) :
 	m_reserved_dl_slots(0),
 	m_reserved_ul_slots(0),
 	m_current_trx(NULL),
-	m_codel_state(NULL)
+	m_codel_state(NULL),
+	m_mode(GprsCodingScheme::GPRS)
 {
 	int codel_interval = LLC_CODEL_USE_DEFAULT;
 
@@ -117,17 +117,11 @@ GprsMs::GprsMs(BTS *bts, uint32_t tlli) :
 	memset(&m_timer, 0, sizeof(m_timer));
 	m_timer.cb = GprsMs::timeout;
 	m_llc_queue.init();
-	if (m_bts) {
-		m_current_cs_ul = m_bts->bts_data()->initial_cs_ul;
-		if (m_current_cs_ul < 1)
-			m_current_cs_ul = 1;
 
-		m_current_cs_dl = m_bts->bts_data()->initial_cs_dl;
-		if (m_current_cs_dl < 1)
-			m_current_cs_dl = 1;
+	set_mode(m_mode);
 
+	if (m_bts)
 		codel_interval = m_bts->bts_data()->llc_codel_interval_msec;
-	}
 
 	if (codel_interval) {
 		if (codel_interval == LLC_CODEL_USE_DEFAULT)
@@ -215,12 +209,53 @@ void GprsMs::stop_timer()
 	unref();
 }
 
+void GprsMs::set_mode(GprsCodingScheme::Mode mode)
+{
+	m_mode = mode;
+
+	if (!m_bts)
+		return;
+
+	switch (m_mode) {
+	case GprsCodingScheme::GPRS:
+		if (!m_current_cs_ul.isGprs()) {
+			m_current_cs_ul = GprsCodingScheme::getGprsByNum(
+				m_bts->bts_data()->initial_cs_ul);
+			if (!m_current_cs_ul.isValid())
+				m_current_cs_ul = GprsCodingScheme::CS1;
+		}
+		if (!m_current_cs_dl.isGprs()) {
+			m_current_cs_dl = GprsCodingScheme::getGprsByNum(
+				m_bts->bts_data()->initial_cs_dl);
+			if (!m_current_cs_dl.isValid())
+				m_current_cs_dl = GprsCodingScheme::CS1;
+		}
+		break;
+
+	case GprsCodingScheme::EGPRS_GMSK:
+	case GprsCodingScheme::EGPRS:
+		if (!m_current_cs_ul.isEgprs()) {
+			m_current_cs_ul = GprsCodingScheme::getEgprsByNum(
+				m_bts->bts_data()->initial_mcs_ul);
+			if (!m_current_cs_dl.isValid())
+				m_current_cs_ul = GprsCodingScheme::MCS1;
+		}
+		if (!m_current_cs_dl.isEgprs()) {
+			m_current_cs_dl = GprsCodingScheme::getEgprsByNum(
+				m_bts->bts_data()->initial_mcs_dl);
+			if (!m_current_cs_dl.isValid())
+				m_current_cs_dl = GprsCodingScheme::MCS1;
+		}
+		break;
+	}
+}
+
 void GprsMs::attach_tbf(struct gprs_rlcmac_tbf *tbf)
 {
 	if (tbf->direction == GPRS_RLCMAC_DL_TBF)
-		attach_dl_tbf(static_cast<gprs_rlcmac_dl_tbf *>(tbf));
+		attach_dl_tbf(as_dl_tbf(tbf));
 	else
-		attach_ul_tbf(static_cast<gprs_rlcmac_ul_tbf *>(tbf));
+		attach_ul_tbf(as_ul_tbf(tbf));
 }
 
 void GprsMs::attach_ul_tbf(struct gprs_rlcmac_ul_tbf *tbf)
@@ -464,9 +499,9 @@ void GprsMs::update_error_rate(gprs_rlcmac_tbf *tbf, int error_rate)
 {
 	struct gprs_rlcmac_bts *bts_data;
 	int64_t now;
-	uint8_t max_cs_dl = 4;
+	GprsCodingScheme max_cs_dl = this->max_cs_dl();
 
-	OSMO_ASSERT(m_bts != NULL);
+	OSMO_ASSERT(max_cs_dl);
 	bts_data = m_bts->bts_data();
 
 	if (error_rate < 0)
@@ -474,32 +509,30 @@ void GprsMs::update_error_rate(gprs_rlcmac_tbf *tbf, int error_rate)
 
 	now = now_msec();
 
-	if (bts_data->max_cs_dl)
-		max_cs_dl = bts_data->max_cs_dl;
-
 	/* TODO: Check for TBF direction */
 	/* TODO: Support different CS values for UL and DL */
 
 	m_nack_rate_dl = error_rate;
 
 	if (error_rate > bts_data->cs_adj_upper_limit) {
-		if (m_current_cs_dl > 1) {
-			m_current_cs_dl -= 1;
+		if (m_current_cs_dl.to_num() > 1) {
+			m_current_cs_dl.dec(mode());
 			LOGP(DRLCMACDL, LOGL_INFO,
 				"MS (IMSI %s): High error rate %d%%, "
-				"reducing CS level to %d\n",
-				imsi(), error_rate, m_current_cs_dl);
+				"reducing CS level to %s\n",
+				imsi(), error_rate, m_current_cs_dl.name());
 			m_last_cs_not_low = now;
 		}
 	} else if (error_rate < bts_data->cs_adj_lower_limit) {
 		if (m_current_cs_dl < max_cs_dl) {
 		       if (now - m_last_cs_not_low > 1000) {
-			       m_current_cs_dl += 1;
+			       m_current_cs_dl.inc(mode());
 
 			       LOGP(DRLCMACDL, LOGL_INFO,
 				       "MS (IMSI %s): Low error rate %d%%, "
-				       "increasing DL CS level to %d\n",
-				       imsi(), error_rate, m_current_cs_dl);
+				       "increasing DL CS level to %s\n",
+				       imsi(), error_rate,
+				       m_current_cs_dl.name());
 			       m_last_cs_not_low = now;
 		       } else {
 			       LOGP(DRLCMACDL, LOGL_DEBUG,
@@ -516,46 +549,125 @@ void GprsMs::update_error_rate(gprs_rlcmac_tbf *tbf, int error_rate)
 	}
 }
 
-void GprsMs::update_l1_meas(const pcu_l1_meas *meas)
+GprsCodingScheme GprsMs::max_cs_ul() const
 {
 	struct gprs_rlcmac_bts *bts_data;
-	uint8_t max_cs_ul = 4;
-	unsigned i;
 
 	OSMO_ASSERT(m_bts != NULL);
 	bts_data = m_bts->bts_data();
 
-	if (bts_data->max_cs_ul)
-		max_cs_ul = bts_data->max_cs_ul;
+	if (m_current_cs_ul.isGprs()) {
+		if (!bts_data->max_cs_ul)
+			return GprsCodingScheme(GprsCodingScheme::CS4);
 
-	if (meas->have_link_qual) {
-		int old_link_qual = meas->link_qual;
-		int low  = bts_data->cs_lqual_ranges[current_cs_ul()-1].low;
-		int high  = bts_data->cs_lqual_ranges[current_cs_ul()-1].high;
-		uint8_t new_cs_ul = m_current_cs_ul;
+		return GprsCodingScheme::getGprsByNum(bts_data->max_cs_ul);
+	}
 
-		if (m_l1_meas.have_link_qual)
-		       old_link_qual = m_l1_meas.link_qual;
+	if (!m_current_cs_ul.isEgprs())
+		return GprsCodingScheme(); /* UNKNOWN */
 
-		if (meas->link_qual < low &&  old_link_qual < low)
-			new_cs_ul = m_current_cs_ul - 1;
-		else if (meas->link_qual > high &&  old_link_qual > high &&
-			m_current_cs_ul < max_cs_ul)
-			new_cs_ul = m_current_cs_ul + 1;
+	if (bts_data->max_mcs_ul)
+		return GprsCodingScheme::getEgprsByNum(bts_data->max_mcs_ul);
+	else if (bts_data->max_cs_ul)
+		return GprsCodingScheme::getEgprsByNum(bts_data->max_cs_ul);
 
-		if (m_current_cs_ul != new_cs_ul) {
-			LOGP(DRLCMACDL, LOGL_INFO,
-				"MS (IMSI %s): "
-				"Link quality %ddB (%ddB) left window [%d, %d], "
-				"modifying uplink CS level: %d -> %d\n",
-				imsi(), meas->link_qual, old_link_qual,
-				low, high,
-				m_current_cs_ul, new_cs_ul);
-
-			m_current_cs_ul = new_cs_ul;
-		}
+	return GprsCodingScheme(GprsCodingScheme::MCS4);
+}
+
+GprsCodingScheme GprsMs::max_cs_dl() const
+{
+	struct gprs_rlcmac_bts *bts_data;
+
+	OSMO_ASSERT(m_bts != NULL);
+	bts_data = m_bts->bts_data();
+
+	if (m_current_cs_dl.isGprs()) {
+		if (!bts_data->max_cs_dl)
+			return GprsCodingScheme(GprsCodingScheme::CS4);
+
+		return GprsCodingScheme::getGprsByNum(bts_data->max_cs_dl);
 	}
 
+	if (!m_current_cs_dl.isEgprs())
+		return GprsCodingScheme(); /* UNKNOWN */
+
+	if (bts_data->max_mcs_dl)
+		return GprsCodingScheme::getEgprsByNum(bts_data->max_mcs_dl);
+	else if (bts_data->max_cs_dl)
+		return GprsCodingScheme::getEgprsByNum(bts_data->max_cs_dl);
+
+	return GprsCodingScheme(GprsCodingScheme::MCS4);
+}
+
+void GprsMs::update_cs_ul(const pcu_l1_meas *meas)
+{
+	struct gprs_rlcmac_bts *bts_data;
+	GprsCodingScheme max_cs_ul = this->max_cs_ul();
+
+	int old_link_qual;
+	int low;
+	int high;
+	GprsCodingScheme new_cs_ul = m_current_cs_ul;
+	unsigned current_cs_num = m_current_cs_ul.to_num();
+
+	bts_data = m_bts->bts_data();
+
+	if (!max_cs_ul) {
+		LOGP(DRLCMACDL, LOGL_ERROR,
+			"max_cs_ul cannot be derived (current UL CS: %s)\n",
+			m_current_cs_ul.name());
+		return;
+	}
+
+	if (!m_current_cs_ul)
+		return;
+
+	if (!meas->have_link_qual)
+		return;
+
+	old_link_qual = meas->link_qual;
+
+	if (m_current_cs_ul.isGprs()) {
+		low  = bts_data->cs_lqual_ranges[current_cs_num-1].low;
+		high = bts_data->cs_lqual_ranges[current_cs_num-1].high;
+	} else if (m_current_cs_ul.isEgprs()) {
+		/* TODO, use separate table */
+		if (current_cs_num > 4)
+			current_cs_num = 4;
+		low  = bts_data->cs_lqual_ranges[current_cs_num-1].low;
+		high = bts_data->cs_lqual_ranges[current_cs_num-1].high;
+	} else {
+		return;
+	}
+
+	if (m_l1_meas.have_link_qual)
+		old_link_qual = m_l1_meas.link_qual;
+
+	if (meas->link_qual < low &&  old_link_qual < low)
+		new_cs_ul.dec(mode());
+	else if (meas->link_qual > high &&  old_link_qual > high &&
+		m_current_cs_ul < max_cs_ul)
+		new_cs_ul.inc(mode());
+
+	if (m_current_cs_ul != new_cs_ul) {
+		LOGP(DRLCMACDL, LOGL_INFO,
+			"MS (IMSI %s): "
+			"Link quality %ddB (%ddB) left window [%d, %d], "
+			"modifying uplink CS level: %s -> %s\n",
+			imsi(), meas->link_qual, old_link_qual,
+			low, high,
+			m_current_cs_ul.name(), new_cs_ul.name());
+
+		m_current_cs_ul = new_cs_ul;
+	}
+}
+
+void GprsMs::update_l1_meas(const pcu_l1_meas *meas)
+{
+	unsigned i;
+
+	update_cs_ul(meas);
+
 	if (meas->have_rssi)
 		m_l1_meas.set_rssi(meas->rssi);
 	if (meas->have_bto)
@@ -582,9 +694,9 @@ void GprsMs::update_l1_meas(const pcu_l1_meas *meas)
 	}
 }
 
-uint8_t GprsMs::current_cs_dl() const
+GprsCodingScheme GprsMs::current_cs_dl() const
 {
-	uint8_t cs = m_current_cs_dl;
+	GprsCodingScheme cs = m_current_cs_dl;
 	size_t unencoded_octets;
 
 	if (!m_bts)
@@ -594,7 +706,7 @@ uint8_t GprsMs::current_cs_dl() const
 
 	/* If the DL TBF is active, add number of unencoded chunk octets */
 	if (m_dl_tbf)
-		unencoded_octets = m_dl_tbf->m_llc.chunk_size();
+		unencoded_octets += m_dl_tbf->m_llc.chunk_size();
 
 	/* There are many unencoded octets, don't reduce */
 	if (unencoded_octets >= m_bts->bts_data()->cs_downgrade_threshold)
@@ -605,11 +717,11 @@ uint8_t GprsMs::current_cs_dl() const
 		return cs;
 
 	/* The throughput would probably be better if the CS level was reduced */
-	cs -= 1;
+	cs.dec(mode());
 
 	/* CS-2 doesn't gain throughput with small packets, further reduce to CS-1 */
-	if (cs == 2)
-		cs -= 1;
+	if (cs == GprsCodingScheme(GprsCodingScheme::CS2))
+		cs.dec(mode());
 
 	return cs;
 }
@@ -651,6 +763,31 @@ uint8_t GprsMs::ul_slots() const
 	return slots;
 }
 
+uint8_t GprsMs::current_pacch_slots() const
+{
+	uint8_t slots = 0;
+
+	bool is_dl_active = m_dl_tbf && m_dl_tbf->is_tfi_assigned();
+	bool is_ul_active = m_ul_tbf && m_ul_tbf->is_tfi_assigned();
+
+	if (!is_dl_active && !is_ul_active)
+		return 0;
+
+	/* see TS 44.060, 8.1.1.2.2 */
+	if (is_dl_active && !is_ul_active)
+		slots =  m_dl_tbf->dl_slots();
+	else if (!is_dl_active && is_ul_active)
+		slots =  m_ul_tbf->ul_slots();
+	else
+		slots =  m_ul_tbf->ul_slots() & m_dl_tbf->dl_slots();
+
+	/* Assume a multislot class 1 device */
+	/* TODO: For class 2 devices, this could be removed */
+	slots = pcu_lsb(slots);
+
+	return slots;
+}
+
 void GprsMs::set_reserved_slots(gprs_rlcmac_trx *trx,
 	uint8_t ul_slots, uint8_t dl_slots)
 {
diff --git a/src/gprs_ms.h b/src/gprs_ms.h
index f9b63f26..b07f1757 100644
--- a/src/gprs_ms.h
+++ b/src/gprs_ms.h
@@ -74,6 +74,8 @@ public:
 	bool check_tlli(uint32_t tlli);
 
 	void reset();
+	GprsCodingScheme::Mode mode() const;
+	void set_mode(GprsCodingScheme::Mode mode);
 
 	const char *imsi() const;
 	void set_imsi(const char *imsi);
@@ -85,14 +87,17 @@ public:
 	void set_ms_class(uint8_t ms_class);
 	void set_egprs_ms_class(uint8_t ms_class);
 
-	uint8_t current_cs_ul() const;
-	uint8_t current_cs_dl() const;
+	GprsCodingScheme current_cs_ul() const;
+	GprsCodingScheme current_cs_dl() const;
+	GprsCodingScheme max_cs_ul() const;
+	GprsCodingScheme max_cs_dl() const;
 
 	int first_common_ts() const;
 	uint8_t dl_slots() const;
 	uint8_t ul_slots() const;
 	uint8_t reserved_dl_slots() const;
 	uint8_t reserved_ul_slots() const;
+	uint8_t current_pacch_slots() const;
 	gprs_rlcmac_trx *current_trx() const;
 	void set_reserved_slots(gprs_rlcmac_trx *trx,
 		uint8_t ul_slots, uint8_t dl_slots);
@@ -134,6 +139,7 @@ protected:
 	void unref();
 	void start_timer();
 	void stop_timer();
+	void update_cs_ul(const pcu_l1_meas*);
 
 private:
 	BTS *m_bts;
@@ -152,8 +158,8 @@ private:
 	uint8_t m_ms_class;
 	uint8_t m_egprs_ms_class;
 	/* current coding scheme */
-	uint8_t m_current_cs_ul;
-	uint8_t m_current_cs_dl;
+	GprsCodingScheme m_current_cs_ul;
+	GprsCodingScheme m_current_cs_dl;
 
 	gprs_llc_queue m_llc_queue;
 
@@ -172,6 +178,7 @@ private:
 	gprs_rlcmac_trx *m_current_trx;
 
 	struct gprs_codel *m_codel_state;
+	GprsCodingScheme::Mode m_mode;
 };
 
 inline bool GprsMs::is_idle() const
@@ -220,11 +227,16 @@ inline uint8_t GprsMs::egprs_ms_class() const
 	return m_egprs_ms_class;
 }
 
-inline uint8_t GprsMs::current_cs_ul() const
+inline GprsCodingScheme GprsMs::current_cs_ul() const
 {
 	return m_current_cs_ul;
 }
 
+inline GprsCodingScheme::Mode GprsMs::mode() const
+{
+	return m_mode;
+}
+
 inline void GprsMs::set_timeout(unsigned secs)
 {
 	m_delay = secs;
diff --git a/src/gprs_rlcmac_sched.cpp b/src/gprs_rlcmac_sched.cpp
index b4db88ce..313e23f4 100644
--- a/src/gprs_rlcmac_sched.cpp
+++ b/src/gprs_rlcmac_sched.cpp
@@ -25,7 +25,7 @@
 
 #include "pcu_utils.h"
 
-static uint32_t sched_poll(struct gprs_rlcmac_bts *bts,
+static uint32_t sched_poll(BTS *bts,
 		    uint8_t trx, uint8_t ts, uint32_t fn, uint8_t block_nr,
 		    struct gprs_rlcmac_tbf **poll_tbf,
 		    struct gprs_rlcmac_tbf **ul_ass_tbf,
@@ -34,7 +34,7 @@ static uint32_t sched_poll(struct gprs_rlcmac_bts *bts,
 {
 	struct gprs_rlcmac_ul_tbf *ul_tbf;
 	struct gprs_rlcmac_dl_tbf *dl_tbf;
-	struct llist_pods *lpods;
+	LListHead<gprs_rlcmac_tbf> *pos;
 	uint32_t poll_fn;
 
 	/* check special TBF for events */
@@ -42,9 +42,11 @@ static uint32_t sched_poll(struct gprs_rlcmac_bts *bts,
 	if ((block_nr % 3) == 2)
 		poll_fn ++;
 	poll_fn = poll_fn % 2715648;
-	llist_pods_for_each_entry(ul_tbf, &bts->ul_tbfs, list, lpods) {
+	llist_for_each(pos, &bts->ul_tbfs()) {
+		ul_tbf = as_ul_tbf(pos->entry());
+		OSMO_ASSERT(ul_tbf);
 		/* this trx, this ts */
-		if (ul_tbf->trx->trx_no != trx || ul_tbf->control_ts != ts)
+		if (ul_tbf->trx->trx_no != trx || !ul_tbf->is_control_ts(ts))
 			continue;
 		/* polling for next uplink block */
 		if (ul_tbf->poll_state == GPRS_RLCMAC_POLL_SCHED
@@ -58,9 +60,11 @@ static uint32_t sched_poll(struct gprs_rlcmac_bts *bts,
 			*ul_ass_tbf = ul_tbf;
 #warning "Is this supposed to be fair? The last TBF for each wins? Maybe use llist_add_tail and skip once we have all states?"
 	}
-	llist_pods_for_each_entry(dl_tbf, &bts->dl_tbfs, list, lpods) {
+	llist_for_each(pos, &bts->dl_tbfs()) {
+		dl_tbf = as_dl_tbf(pos->entry());
+		OSMO_ASSERT(dl_tbf);
 		/* this trx, this ts */
-		if (dl_tbf->trx->trx_no != trx || dl_tbf->control_ts != ts)
+		if (dl_tbf->trx->trx_no != trx || !dl_tbf->is_control_ts(ts))
 			continue;
 		/* polling for next uplink block */
 		if (dl_tbf->poll_state == GPRS_RLCMAC_POLL_SCHED
@@ -126,14 +130,18 @@ static struct msgb *sched_select_ctrl_msg(
 		if (!tbf)
 			continue;
 
-		if (tbf == ul_ass_tbf)
-			msg = ul_ass_tbf->create_ul_ass(fn);
-		else if (tbf == dl_ass_tbf)
-			msg = dl_ass_tbf->create_dl_ass(fn);
+		/*
+		 * Assignments for the same direction have lower precedence,
+		 * because they may kill the TBF when the CONTOL ACK is
+		 * received, thus preventing the others from being processed.
+		 */
+
+		if (tbf == ul_ass_tbf && tbf->direction == GPRS_RLCMAC_DL_TBF)
+			msg = ul_ass_tbf->create_ul_ass(fn, ts);
+		else if (tbf == dl_ass_tbf && tbf->direction == GPRS_RLCMAC_UL_TBF)
+			msg = dl_ass_tbf->create_dl_ass(fn, ts);
 		else if (tbf == ul_ack_tbf)
-			msg = ul_ack_tbf->create_ul_ack(fn);
-		else
-			abort();
+			msg = ul_ack_tbf->create_ul_ack(fn, ts);
 
 		if (!msg) {
 			tbf = NULL;
@@ -145,6 +153,21 @@ static struct msgb *sched_select_ctrl_msg(
 		break;
 	}
 
+	if (!msg) {
+		/*
+		 * If one of these is left, the response (CONTROL ACK) from the
+		 * MS will kill the current TBF, only one of them can be
+		 * non-NULL
+		 */
+		if (dl_ass_tbf) {
+			tbf = dl_ass_tbf;
+			msg = dl_ass_tbf->create_dl_ass(fn, ts);
+		} else if (ul_ass_tbf) {
+			tbf = ul_ass_tbf;
+			msg = ul_ass_tbf->create_ul_ass(fn, ts);
+		}
+	}
+
 	/* any message */
 	if (msg) {
 		tbf->rotate_in_list();
@@ -292,7 +315,7 @@ int gprs_rlcmac_rcv_rts_block(struct gprs_rlcmac_bts *bts,
 	/* store last frame number of RTS */
 	pdch->last_rts_fn = fn;
 
-	poll_fn = sched_poll(bts, trx, ts, fn, block_nr, &poll_tbf, &ul_ass_tbf,
+	poll_fn = sched_poll(bts->bts, trx, ts, fn, block_nr, &poll_tbf, &ul_ass_tbf,
 		&dl_ass_tbf, &ul_ack_tbf);
 	/* check uplink resource for polling */
 	if (poll_tbf)
@@ -330,6 +353,7 @@ int gprs_rlcmac_rcv_rts_block(struct gprs_rlcmac_bts *bts,
 	/* msg is now available */
 
 	/* set USF */
+	OSMO_ASSERT(msgb_length(msg) > 0);
 	msg->data[0] = (msg->data[0] & 0xf8) | usf;
 
 	/* Used to measure the leak rate, count all blocks */
diff --git a/src/gprs_rlcmac_ts_alloc.cpp b/src/gprs_rlcmac_ts_alloc.cpp
index 0daeaf5c..57197b22 100644
--- a/src/gprs_rlcmac_ts_alloc.cpp
+++ b/src/gprs_rlcmac_ts_alloc.cpp
@@ -24,6 +24,7 @@
 #include <bts.h>
 #include <tbf.h>
 #include <gprs_ms.h>
+#include <pcu_utils.h>
 
 #include <errno.h>
 #include <values.h>
@@ -80,20 +81,6 @@ static const struct gprs_ms_multislot_class gprs_ms_multislot_class[32] = {
 /* N/A */	{ MS_NA,MS_NA,	MS_NA,	MS_NA,	MS_NA,	MS_NA,	MS_NA,	MS_NA },
 };
 
-static unsigned lsb(unsigned x)
-{
-	return x & -x;
-}
-
-static unsigned bitcount(unsigned x)
-{
-	unsigned count = 0;
-	for (count = 0; x; count += 1)
-		x &= x - 1;
-
-	return count;
-}
-
 static char *set_flag_chars(char *buf, uint8_t val, char set_char, char unset_char = 0)
 {
 	int i;
@@ -480,14 +467,12 @@ int alloc_algorithm_a(struct gprs_rlcmac_bts *bts,
 	/* The allocation will be successful, so the system state and tbf_/ms_
 	 * may be modified from now on. */
 	if (tbf->direction == GPRS_RLCMAC_UL_TBF) {
-		struct gprs_rlcmac_ul_tbf *ul_tbf =
-			static_cast<gprs_rlcmac_ul_tbf *>(tbf_);
+		struct gprs_rlcmac_ul_tbf *ul_tbf = as_ul_tbf(tbf_);
 		LOGP(DRLCMAC, LOGL_DEBUG, "- Assign uplink TS=%d TFI=%d USF=%d\n",
 			ts, tfi, usf);
 		assign_uplink_tbf_usf(pdch, ul_tbf, tfi, usf);
 	} else {
-		struct gprs_rlcmac_dl_tbf *dl_tbf =
-			static_cast<gprs_rlcmac_dl_tbf *>(tbf_);
+		struct gprs_rlcmac_dl_tbf *dl_tbf = as_dl_tbf(tbf_);
 		LOGP(DRLCMAC, LOGL_DEBUG, "- Assign downlink TS=%d TFI=%d\n",
 			ts, tfi);
 		assign_dlink_tbf(pdch, dl_tbf, tfi);
@@ -642,7 +627,7 @@ static int find_multi_slots(struct gprs_rlcmac_bts *bts,
 		if ((tx_window & (1 << ((ul_ts+num_tx-1) % 8))) == 0)
 			continue;
 
-		tx_slot_count = bitcount(tx_window);
+		tx_slot_count = pcu_bitcount(tx_window);
 
 		max_rx = OSMO_MIN(ms_class->rx, ms_class->sum - num_tx);
 		rx_valid_win = (1 << max_rx) - 1;
@@ -671,7 +656,7 @@ static int find_multi_slots(struct gprs_rlcmac_bts *bts,
 		 * testing */
 
 		rx_window = rx_good & rx_valid_win;
-		rx_slot_count = bitcount(rx_window);
+		rx_slot_count = pcu_bitcount(rx_window);
 
 #if 0
 		LOGP(DRLCMAC, LOGL_DEBUG, "n_tx=%d, n_rx=%d, mask_sel=%d, "
@@ -736,7 +721,7 @@ static int find_multi_slots(struct gprs_rlcmac_bts *bts,
 			continue;
 
 		/* Check number of common slots according to TS 54.002, 6.4.2.2 */
-		common_slot_count = bitcount(tx_window & rx_window);
+		common_slot_count = pcu_bitcount(tx_window & rx_window);
 		req_common_slots = OSMO_MIN(tx_slot_count, rx_slot_count);
 		if (ms_class->type == 1)
 			req_common_slots = OSMO_MIN(req_common_slots, 2);
@@ -893,7 +878,7 @@ int alloc_algorithm_b(struct gprs_rlcmac_bts *bts,
 				dl_slots & ul_slots, compute_usage_by_num_tbfs,
 				NULL, NULL);
 		if (ts < 0)
-			ul_slots = dl_slots = lsb(dl_slots & ul_slots);
+			ul_slots = dl_slots = pcu_lsb(dl_slots & ul_slots);
 		else
 			ul_slots = dl_slots = (dl_slots & ul_slots) & (1<<ts);
 	}
@@ -922,9 +907,9 @@ int alloc_algorithm_b(struct gprs_rlcmac_bts *bts,
 				"available\n");
 			return -EINVAL;
 		}
-		slotcount = bitcount(dl_slots);
+		slotcount = pcu_bitcount(dl_slots);
 		first_ts = ffs(dl_slots) - 1;
-		avail_count = bitcount(reserved_dl_slots);
+		avail_count = pcu_bitcount(reserved_dl_slots);
 
 	} else {
 		int free_usf = -1;
@@ -960,7 +945,7 @@ int alloc_algorithm_b(struct gprs_rlcmac_bts *bts,
 		/* We will stick to that single UL slot, unreserve the others */
 		reserved_ul_slots = ul_slots;
 
-		avail_count = bitcount(reserved_ul_slots);
+		avail_count = pcu_bitcount(reserved_ul_slots);
 	}
 
 	first_common_ts = ffs(dl_slots & ul_slots) - 1;
@@ -1010,8 +995,7 @@ int alloc_algorithm_b(struct gprs_rlcmac_bts *bts,
 	tbf_->first_ts = first_ts;
 
 	if (tbf->direction == GPRS_RLCMAC_DL_TBF) {
-		struct gprs_rlcmac_dl_tbf *dl_tbf =
-			static_cast<gprs_rlcmac_dl_tbf *>(tbf_);
+		struct gprs_rlcmac_dl_tbf *dl_tbf = as_dl_tbf(tbf_);
 		for (ts = 0; ts < 8; ts++) {
 			if (!(dl_slots & (1 << ts)))
 				continue;
@@ -1021,8 +1005,7 @@ int alloc_algorithm_b(struct gprs_rlcmac_bts *bts,
 			assign_dlink_tbf(&trx->pdch[ts], dl_tbf, tfi);
 		}
 	} else {
-		struct gprs_rlcmac_ul_tbf *ul_tbf =
-			static_cast<gprs_rlcmac_ul_tbf *>(tbf_);
+		struct gprs_rlcmac_ul_tbf *ul_tbf = as_ul_tbf(tbf_);
 
 		for (ts = 0; ts < 8; ts++) {
 			if (!(ul_slots & (1 << ts)))
diff --git a/src/gsm_rlcmac.cpp b/src/gsm_rlcmac.cpp
index 44bc5e13..6b43aa6d 100644
--- a/src/gsm_rlcmac.cpp
+++ b/src/gsm_rlcmac.cpp
@@ -5509,3 +5509,11 @@ void encode_gsm_rlcmac_downlink_data(bitvec * vector, RlcMacDownlinkDataBlock_t
     LOGPC(DRLCMACDATA, LOGL_NOTICE, "\n");
   }
 }
+
+void decode_gsm_ra_cap(bitvec * vector, MS_Radio_Access_capability_t *data)
+{
+  csnStream_t      ar;
+  unsigned readIndex = 0;
+  csnStreamInit(&ar, 0, 8 * vector->data_len);
+  /*ret =*/ csnStreamDecoder(&ar, CSNDESCR(MS_Radio_Access_capability_t), vector, readIndex, data);
+}
diff --git a/src/gsm_rlcmac.h b/src/gsm_rlcmac.h
index 017b3112..8f4039c5 100644
--- a/src/gsm_rlcmac.h
+++ b/src/gsm_rlcmac.h
@@ -476,7 +476,7 @@ typedef struct
 
 typedef struct
 {
-  guint8   LENGTH;
+  /* guint8   LENGTH; */
   EGPRS_AckNack_Desc_t Desc;
 } EGPRS_AckNack_w_len_t;
 
@@ -5136,4 +5136,5 @@ typedef struct
  void encode_gsm_rlcmac_uplink(bitvec * vector, RlcMacUplink_t * data);
  void decode_gsm_rlcmac_uplink_data(bitvec * vector, RlcMacUplinkDataBlock_t * data);
  void encode_gsm_rlcmac_downlink_data(bitvec * vector, RlcMacDownlinkDataBlock_t * data);
+ void decode_gsm_ra_cap(bitvec * vector, MS_Radio_Access_capability_t * data);
 #endif /* __PACKET_GSM_RLCMAC_H__ */
diff --git a/src/pcu_main.cpp b/src/pcu_main.cpp
index 96dc6d7b..8eb7441a 100644
--- a/src/pcu_main.cpp
+++ b/src/pcu_main.cpp
@@ -164,6 +164,7 @@ int main(int argc, char *argv[])
 	bts = bts_main_data();
 	bts->fc_interval = 1;
 	bts->initial_cs_dl = bts->initial_cs_ul = 1;
+	bts->initial_mcs_dl = bts->initial_mcs_ul = 1;
 	bts->cs1 = 1;
 	bts->t3142 = 20;
 	bts->t3169 = 5;
@@ -180,6 +181,8 @@ int main(int argc, char *argv[])
 	bts->cs_adj_lower_limit = 10; /* Increase CS if the error rate is below */
 	bts->max_cs_ul = 4;
 	bts->max_cs_dl = 4;
+	bts->max_mcs_ul = 4;
+	bts->max_mcs_dl = 4;
 	/* CS-1 to CS-4 */
 	bts->cs_lqual_ranges[0].low = -256;
 	bts->cs_lqual_ranges[0].high = 6;
@@ -191,6 +194,10 @@ int main(int argc, char *argv[])
 	bts->cs_lqual_ranges[3].high = 256;
 	bts->cs_downgrade_threshold = 200;
 
+	/* TODO: increase them when CRBB decoding is implemented */
+	bts->ws_base = 64;
+	bts->ws_pdch = 0;
+
 	bts->llc_codel_interval_msec = LLC_CODEL_USE_DEFAULT;
 	bts->dl_tbf_idle_msec = 2000;
 	bts->llc_idle_ack_csec = 10;
diff --git a/src/pcu_utils.h b/src/pcu_utils.h
index 5ca234a4..d6644462 100644
--- a/src/pcu_utils.h
+++ b/src/pcu_utils.h
@@ -24,3 +24,19 @@ inline void csecs_to_timeval(unsigned csecs, struct timeval *tv) {
 	tv->tv_sec  = csecs / 100;
 	tv->tv_usec = (csecs % 100) * 10000;
 }
+
+template <typename T>
+inline unsigned int pcu_bitcount(T x)
+{
+	unsigned int count = 0;
+	for (count = 0; x; count += 1)
+		x &= x - 1;
+
+	return count;
+}
+
+template <typename T>
+inline T pcu_lsb(T x)
+{
+	return x & -x;
+}
diff --git a/src/pcu_vty.c b/src/pcu_vty.c
index aa29b4c6..58fea993 100644
--- a/src/pcu_vty.c
+++ b/src/pcu_vty.c
@@ -55,9 +55,7 @@ static int config_write_pcu(struct vty *vty)
 
 	vty_out(vty, "pcu%s", VTY_NEWLINE);
 	if (bts->egprs_enabled)
-		vty_out(vty, " egprs%s", VTY_NEWLINE);
-	else
-		vty_out(vty, " no egprs%s", VTY_NEWLINE);
+		vty_out(vty, " egprs only%s", VTY_NEWLINE);
 
 	vty_out(vty, " flow-control-interval %d%s", bts->fc_interval,
 		VTY_NEWLINE);
@@ -111,6 +109,26 @@ static int config_write_pcu(struct vty *vty)
 		bts->cs_lqual_ranges[3].low,
 		VTY_NEWLINE);
 
+	if (bts->initial_mcs_dl != 1 && bts->initial_mcs_ul != 1) {
+		if (bts->initial_mcs_ul == bts->initial_mcs_dl)
+			vty_out(vty, " mcs %d%s", bts->initial_mcs_dl,
+				VTY_NEWLINE);
+		else
+			vty_out(vty, " mcs %d %d%s", bts->initial_mcs_dl,
+				bts->initial_mcs_ul, VTY_NEWLINE);
+	}
+	if (bts->max_mcs_dl && bts->max_mcs_ul) {
+		if (bts->max_mcs_ul == bts->max_mcs_dl)
+			vty_out(vty, " mcs max %d%s", bts->max_mcs_dl,
+				VTY_NEWLINE);
+		else
+			vty_out(vty, " mcs max %d %d%s", bts->max_mcs_dl,
+				bts->max_mcs_ul, VTY_NEWLINE);
+	}
+
+	vty_out(vty, " window-size %d %d%s", bts->ws_base, bts->ws_pdch,
+		VTY_NEWLINE);
+
 	if (bts->force_llc_lifetime == 0xffff)
 		vty_out(vty, " queue lifetime infinite%s", VTY_NEWLINE);
 	else if (bts->force_llc_lifetime)
@@ -162,8 +180,8 @@ DEFUN(cfg_pcu,
 
 DEFUN(cfg_pcu_egprs,
       cfg_pcu_egprs_cmd,
-      "egprs",
-      EGPRS_STR)
+      "egprs only",
+      EGPRS_STR "Use EGPRS and disable plain GPRS\n")
 {
 	struct gprs_rlcmac_bts *bts = bts_main_data();
 
@@ -361,11 +379,12 @@ DEFUN(cfg_pcu_no_cs,
 	return CMD_SUCCESS;
 }
 
+#define CS_MAX_STR "Set maximum values for adaptive CS selection (overrides BTS config)\n"
 DEFUN(cfg_pcu_cs_max,
       cfg_pcu_cs_max_cmd,
       "cs max <1-4> [<1-4>]",
       CS_STR
-      "Set maximum values for adaptive CS selection (overrides BTS config)\n"
+      CS_MAX_STR
       "Maximum CS value to be used\n"
       "Use a different maximum CS value for the uplink")
 {
@@ -384,8 +403,7 @@ DEFUN(cfg_pcu_cs_max,
 DEFUN(cfg_pcu_no_cs_max,
       cfg_pcu_no_cs_max_cmd,
       "no cs max",
-      NO_STR CS_STR
-      "Set maximum values for adaptive CS selection (overrides BTS config)\n")
+      NO_STR CS_STR CS_MAX_STR)
 {
 	struct gprs_rlcmac_bts *bts = bts_main_data();
 
@@ -395,6 +413,93 @@ DEFUN(cfg_pcu_no_cs_max,
 	return CMD_SUCCESS;
 }
 
+#define MCS_STR "Modulation and Coding Scheme configuration (EGPRS)\n"
+
+DEFUN(cfg_pcu_mcs,
+      cfg_pcu_mcs_cmd,
+      "mcs <1-9> [<1-9>]",
+      MCS_STR
+      "Initial MCS value to be used (default 1)\n"
+      "Use a different initial MCS value for the uplink")
+{
+	struct gprs_rlcmac_bts *bts = bts_main_data();
+	uint8_t cs = atoi(argv[0]);
+
+	bts->initial_mcs_dl = cs;
+	if (argc > 1)
+		bts->initial_mcs_ul = atoi(argv[1]);
+	else
+		bts->initial_mcs_ul = cs;
+
+	return CMD_SUCCESS;
+}
+
+DEFUN(cfg_pcu_no_mcs,
+      cfg_pcu_no_mcs_cmd,
+      "no mcs",
+      NO_STR MCS_STR)
+{
+	struct gprs_rlcmac_bts *bts = bts_main_data();
+
+	bts->initial_mcs_dl = 1;
+	bts->initial_mcs_ul = 1;
+
+	return CMD_SUCCESS;
+}
+
+DEFUN(cfg_pcu_mcs_max,
+      cfg_pcu_mcs_max_cmd,
+      "mcs max <1-9> [<1-9>]",
+      MCS_STR
+      CS_MAX_STR
+      "Maximum MCS value to be used\n"
+      "Use a different maximum MCS value for the uplink")
+{
+	struct gprs_rlcmac_bts *bts = bts_main_data();
+	uint8_t mcs = atoi(argv[0]);
+
+	bts->max_mcs_dl = mcs;
+	if (argc > 1)
+		bts->max_mcs_ul = atoi(argv[1]);
+	else
+		bts->max_mcs_ul = mcs;
+
+	return CMD_SUCCESS;
+}
+
+DEFUN(cfg_pcu_no_mcs_max,
+      cfg_pcu_no_mcs_max_cmd,
+      "no mcs max",
+      NO_STR MCS_STR CS_MAX_STR)
+{
+	struct gprs_rlcmac_bts *bts = bts_main_data();
+
+	bts->max_mcs_dl = 0;
+	bts->max_mcs_ul = 0;
+
+	return CMD_SUCCESS;
+}
+
+DEFUN(cfg_pcu_window_size,
+      cfg_pcu_window_size_cmd,
+      "window-size <0-1024> [<0-256>]",
+      "Window size configuration (b + N_PDCH * f)\n"
+      "Base value (b)\n"
+      "Factor for number of PDCH (f)")
+{
+	struct gprs_rlcmac_bts *bts = bts_main_data();
+	uint16_t b = atoi(argv[0]);
+
+	bts->ws_base = b;
+	if (argc > 1) {
+		uint16_t f = atoi(argv[1]);
+		bts->ws_pdch = f;
+	}
+
+	return CMD_SUCCESS;
+}
+
+
 #define QUEUE_STR "Packet queue options\n"
 #define LIFETIME_STR "Set lifetime limit of LLC frame in centi-seconds " \
 	"(overrides the value given by SGSN)\n"
@@ -775,19 +880,7 @@ DEFUN(show_tbf,
       SHOW_STR "information about TBFs\n" "All TBFs\n")
 {
 	struct gprs_rlcmac_bts *bts = bts_main_data();
-	struct llist_head *tbf;
-
-	vty_out(vty, "UL TBFs%s", VTY_NEWLINE);
-	llist_for_each(tbf, &bts->ul_tbfs) {
-		tbf_print_vty_info(vty, tbf);
-	}
-
-	vty_out(vty, "%sDL TBFs%s", VTY_NEWLINE, VTY_NEWLINE);
-	llist_for_each(tbf, &bts->dl_tbfs) {
-		tbf_print_vty_info(vty, tbf);
-	}
-
-	return CMD_SUCCESS;
+	return pcu_vty_show_tbf_all(vty, bts);
 }
 
 DEFUN(show_ms_all,
@@ -860,6 +953,11 @@ int pcu_vty_init(const struct log_info *cat)
 	install_element(PCU_NODE, &cfg_pcu_cs_downgrade_thrsh_cmd);
 	install_element(PCU_NODE, &cfg_pcu_no_cs_downgrade_thrsh_cmd);
 	install_element(PCU_NODE, &cfg_pcu_cs_lqual_ranges_cmd);
+	install_element(PCU_NODE, &cfg_pcu_mcs_cmd);
+	install_element(PCU_NODE, &cfg_pcu_no_mcs_cmd);
+	install_element(PCU_NODE, &cfg_pcu_mcs_max_cmd);
+	install_element(PCU_NODE, &cfg_pcu_no_mcs_max_cmd);
+	install_element(PCU_NODE, &cfg_pcu_window_size_cmd);
 	install_element(PCU_NODE, &cfg_pcu_queue_lifetime_cmd);
 	install_element(PCU_NODE, &cfg_pcu_queue_lifetime_inf_cmd);
 	install_element(PCU_NODE, &cfg_pcu_no_queue_lifetime_cmd);
diff --git a/src/pcu_vty_functions.cpp b/src/pcu_vty_functions.cpp
index ce2a006a..166b15eb 100644
--- a/src/pcu_vty_functions.cpp
+++ b/src/pcu_vty_functions.cpp
@@ -39,6 +39,61 @@ int pcu_vty_config_write_pcu_ext(struct vty *vty)
 	return CMD_SUCCESS;
 }
 
+static void tbf_print_vty_info(struct vty *vty, gprs_rlcmac_tbf *tbf)
+{
+	gprs_rlcmac_ul_tbf *ul_tbf = as_ul_tbf(tbf);
+	gprs_rlcmac_dl_tbf *dl_tbf = as_dl_tbf(tbf);
+
+	vty_out(vty, "TBF: TFI=%d TLLI=0x%08x (%s) DIR=%s IMSI=%s%s", tbf->tfi(),
+			tbf->tlli(), tbf->is_tlli_valid() ? "valid" : "invalid",
+			tbf->direction == GPRS_RLCMAC_UL_TBF ? "UL" : "DL",
+			tbf->imsi(), VTY_NEWLINE);
+	vty_out(vty, " created=%lu state=%08x 1st_TS=%d 1st_cTS=%d ctrl_TS=%d "
+			"MS_CLASS=%d/%d%s",
+			tbf->created_ts(), tbf->state_flags, tbf->first_ts,
+			tbf->first_common_ts, tbf->control_ts,
+			tbf->ms_class(),
+			tbf->ms() ? tbf->ms()->egprs_ms_class() : -1,
+			VTY_NEWLINE);
+	vty_out(vty, " TS_alloc=");
+	for (int i = 0; i < 8; i++) {
+		bool is_ctrl = tbf->is_control_ts(i);
+		if (tbf->pdch[i])
+			vty_out(vty, "%d%s ", i, is_ctrl ? "!" : "");
+	}
+	vty_out(vty, " CS=%s WS=%d",
+		tbf->current_cs().name(), tbf->window()->ws());
+
+	if (ul_tbf) {
+		gprs_rlc_ul_window *win = &ul_tbf->m_window;
+		vty_out(vty, " V(Q)=%d V(R)=%d",
+			win->v_q(), win->v_r());
+	}
+	if (dl_tbf) {
+		gprs_rlc_dl_window *win = &dl_tbf->m_window;
+		vty_out(vty, " V(A)=%d V(S)=%d nBSN=%d%s",
+			win->v_a(), win->v_s(), win->resend_needed(),
+			win->window_stalled() ? " STALLED" : "");
+	}
+	vty_out(vty, "%s%s", VTY_NEWLINE, VTY_NEWLINE);
+}
+
+int pcu_vty_show_tbf_all(struct vty *vty, struct gprs_rlcmac_bts *bts_data)
+{
+	BTS *bts = bts_data->bts;
+	LListHead<gprs_rlcmac_tbf> *ms_iter;
+
+	vty_out(vty, "UL TBFs%s", VTY_NEWLINE);
+	llist_for_each(ms_iter, &bts->ul_tbfs())
+		tbf_print_vty_info(vty, ms_iter->entry());
+
+	vty_out(vty, "%sDL TBFs%s", VTY_NEWLINE, VTY_NEWLINE);
+	llist_for_each(ms_iter, &bts->dl_tbfs())
+		tbf_print_vty_info(vty, ms_iter->entry());
+
+	return CMD_SUCCESS;
+}
+
 int pcu_vty_show_ms_all(struct vty *vty, struct gprs_rlcmac_bts *bts_data)
 {
 	BTS *bts = bts_data->bts;
@@ -47,10 +102,11 @@ int pcu_vty_show_ms_all(struct vty *vty, struct gprs_rlcmac_bts *bts_data)
 	llist_for_each(ms_iter, &bts->ms_store().ms_list()) {
 		GprsMs *ms = ms_iter->entry();
 
-		vty_out(vty, "MS TLLI=%08x, TA=%d, CS-UL=%d, CS-DL=%d, LLC=%d, "
+		vty_out(vty, "MS TLLI=%08x, TA=%d, CS-UL=%s, CS-DL=%s, LLC=%d, "
 			"IMSI=%s%s",
 			ms->tlli(),
-			ms->ta(), ms->current_cs_ul(), ms->current_cs_dl(),
+			ms->ta(), ms->current_cs_ul().name(),
+			ms->current_cs_dl().name(),
 			ms->llc_queue()->size(),
 			ms->imsi(),
 			VTY_NEWLINE);
@@ -62,15 +118,24 @@ static int show_ms(struct vty *vty, GprsMs *ms)
 {
 	unsigned i;
 	LListHead<gprs_rlcmac_tbf> *i_tbf;
+	uint8_t slots;
 
 	vty_out(vty, "MS TLLI=%08x, IMSI=%s%s", ms->tlli(), ms->imsi(), VTY_NEWLINE);
 	vty_out(vty, "  Timing advance (TA):    %d%s", ms->ta(), VTY_NEWLINE);
-	vty_out(vty, "  Coding scheme uplink:   CS-%d%s", ms->current_cs_ul(),
+	vty_out(vty, "  Coding scheme uplink:   %s%s", ms->current_cs_ul().name(),
 		VTY_NEWLINE);
-	vty_out(vty, "  Coding scheme downlink: CS-%d%s", ms->current_cs_dl(),
+	vty_out(vty, "  Coding scheme downlink: %s%s", ms->current_cs_dl().name(),
 		VTY_NEWLINE);
+	vty_out(vty, "  Mode:                   %s%s",
+		GprsCodingScheme::modeName(ms->mode()), VTY_NEWLINE);
 	vty_out(vty, "  MS class:               %d%s", ms->ms_class(), VTY_NEWLINE);
 	vty_out(vty, "  EGPRS MS class:         %d%s", ms->egprs_ms_class(), VTY_NEWLINE);
+	vty_out(vty, "  PACCH:                  ");
+	slots = ms->current_pacch_slots();
+	for (int i = 0; i < 8; i++)
+		if (slots & (1 << i))
+			vty_out(vty, "%d ", i);
+	vty_out(vty, "%s", VTY_NEWLINE);
 	vty_out(vty, "  LLC queue length:       %d%s", ms->llc_queue()->size(),
 		VTY_NEWLINE);
 	vty_out(vty, "  LLC queue octets:       %d%s", ms->llc_queue()->octets(),
diff --git a/src/pcu_vty_functions.h b/src/pcu_vty_functions.h
index 1f4ad916..35acf64c 100644
--- a/src/pcu_vty_functions.h
+++ b/src/pcu_vty_functions.h
@@ -28,6 +28,7 @@ struct vty;
 struct gprs_rlcmac_bts;
 
 int pcu_vty_config_write_pcu_ext(struct vty *vty);
+int pcu_vty_show_tbf_all(struct vty *vty, struct gprs_rlcmac_bts *bts_data);
 int pcu_vty_show_ms_all(struct vty *vty, struct gprs_rlcmac_bts *bts_data);
 int pcu_vty_show_ms_by_tlli(struct vty *vty, struct gprs_rlcmac_bts *bts_data,
 	uint32_t tlli);
diff --git a/src/poll_controller.cpp b/src/poll_controller.cpp
index 8108f742..54e3bc76 100644
--- a/src/poll_controller.cpp
+++ b/src/poll_controller.cpp
@@ -30,14 +30,14 @@ PollController::PollController(BTS& bts)
 
 void PollController::expireTimedout(int frame_number, unsigned max_delay)
 {
-	struct gprs_rlcmac_bts *bts = m_bts.bts_data();
 	struct gprs_rlcmac_dl_tbf *dl_tbf;
 	struct gprs_rlcmac_ul_tbf *ul_tbf;
 	struct gprs_rlcmac_sba *sba, *sba2;
-	struct llist_pods *lpods;
+	LListHead<gprs_rlcmac_tbf> *pos;
 	uint32_t elapsed;
 
-	llist_pods_for_each_entry(ul_tbf, &bts->ul_tbfs, list, lpods) {
+	llist_for_each(pos, &m_bts.ul_tbfs()) {
+		ul_tbf = as_ul_tbf(pos->entry());
 		if (ul_tbf->poll_state == GPRS_RLCMAC_POLL_SCHED) {
 			elapsed = (frame_number + 2715648 - ul_tbf->poll_fn)
 								% 2715648;
@@ -45,7 +45,8 @@ void PollController::expireTimedout(int frame_number, unsigned max_delay)
 				ul_tbf->poll_timeout();
 		}
 	}
-	llist_pods_for_each_entry(dl_tbf, &bts->dl_tbfs, list, lpods) {
+	llist_for_each(pos, &m_bts.dl_tbfs()) {
+		dl_tbf = as_dl_tbf(pos->entry());
 		if (dl_tbf->poll_state == GPRS_RLCMAC_POLL_SCHED) {
 			elapsed = (frame_number + 2715648 - dl_tbf->poll_fn)
 								% 2715648;
diff --git a/src/rlc.cpp b/src/rlc.cpp
index 227fa362..79d8f48a 100644
--- a/src/rlc.cpp
+++ b/src/rlc.cpp
@@ -57,7 +57,7 @@ void gprs_rlc_dl_window::reset()
 
 int gprs_rlc_dl_window::resend_needed()
 {
-	for (uint16_t bsn = v_a(); bsn != v_s(); bsn = (bsn + 1) & mod_sns()) {
+	for (uint16_t bsn = v_a(); bsn != v_s(); bsn = mod_sns(bsn + 1)) {
 		if (m_v_b.is_nacked(bsn) || m_v_b.is_resend(bsn))
 			return bsn;
 	}
@@ -69,7 +69,7 @@ int gprs_rlc_dl_window::mark_for_resend()
 {
 	int resend = 0;
 
-	for (uint16_t bsn = v_a(); bsn != v_s(); bsn = (bsn + 1) & mod_sns()) {
+	for (uint16_t bsn = v_a(); bsn != v_s(); bsn = mod_sns(bsn + 1)) {
 		if (m_v_b.is_unacked(bsn)) {
 			/* mark to be re-send */
 			m_v_b.mark_resend(bsn);
@@ -85,7 +85,7 @@ int gprs_rlc_dl_window::count_unacked()
 	uint16_t unacked = 0;
 	uint16_t bsn;
 
-	for (bsn = v_a(); bsn != v_s(); bsn = (bsn + 1) & mod_sns()) {
+	for (bsn = v_a(); bsn != v_s(); bsn = mod_sns(bsn + 1)) {
 		if (!m_v_b.is_acked(bsn))
 			unacked += 1;
 	}
@@ -93,19 +93,50 @@ int gprs_rlc_dl_window::count_unacked()
 	return unacked;
 }
 
-static uint16_t bitnum_to_bsn(int bitnum, uint16_t ssn, uint16_t mod_sns)
+static uint16_t bitnum_to_bsn(int bitnum, uint16_t ssn)
 {
-	return (ssn - 1 - bitnum) & mod_sns;
+	return (ssn - 1 - bitnum);
 }
 
-void gprs_rlc_dl_window::update(BTS *bts, char *show_rbb, uint8_t ssn,
+void gprs_rlc_dl_window::update(BTS *bts, const struct bitvec *rbb,
+			uint16_t first_bsn, uint16_t *lost,
+			uint16_t *received)
+{
+	unsigned num_blocks = rbb->cur_bit;
+	unsigned bsn;
+
+	/* first_bsn is in range V(A)..V(S) */
+
+	for (unsigned int bitpos = 0; bitpos < num_blocks; bitpos++) {
+		bool is_ack;
+		bsn = mod_sns(first_bsn + bitpos);
+		if (bsn == mod_sns(v_a() - 1))
+			break;
+
+		is_ack = bitvec_get_bit_pos(rbb, bitpos) == 1;
+
+		if (is_ack) {
+			LOGP(DRLCMACDL, LOGL_DEBUG, "- got ack for BSN=%d\n", bsn);
+			if (!m_v_b.is_acked(bsn))
+				*received += 1;
+			m_v_b.mark_acked(bsn);
+		} else {
+			LOGP(DRLCMACDL, LOGL_DEBUG, "- got NACK for BSN=%d\n", bsn);
+			m_v_b.mark_nacked(bsn);
+			bts->rlc_nacked();
+			*lost += 1;
+		}
+	}
+}
+
+void gprs_rlc_dl_window::update(BTS *bts, char *show_rbb, uint16_t ssn,
 			uint16_t *lost, uint16_t *received)
 {
 	/* SSN - 1 is in range V(A)..V(S)-1 */
 	for (int bitpos = 0; bitpos < ws(); bitpos++) {
-		uint16_t bsn = bitnum_to_bsn(bitpos, ssn, mod_sns());
+		uint16_t bsn = mod_sns(bitnum_to_bsn(bitpos, ssn));
 
-		if (bsn == ((v_a() - 1) & mod_sns()))
+		if (bsn == mod_sns(v_a() - 1))
 			break;
 
 		if (show_rbb[ws() - 1 - bitpos] == 'R') {
@@ -128,7 +159,7 @@ int gprs_rlc_dl_window::move_window()
 	uint16_t bsn;
 	int moved = 0;
 
-	for (i = 0, bsn = v_a(); bsn != v_s(); i++, bsn = (bsn + 1) & mod_sns()) {
+	for (i = 0, bsn = v_a(); bsn != v_s(); i++, bsn = mod_sns(bsn + 1)) {
 		if (m_v_b.is_acked(bsn)) {
 			m_v_b.mark_invalid(bsn);
 			moved += 1;
@@ -144,7 +175,7 @@ void gprs_rlc_dl_window::show_state(char *show_v_b)
 	int i;
 	uint16_t bsn;
 
-	for (i = 0, bsn = v_a(); bsn != v_s(); i++, bsn = (bsn + 1) & mod_sns()) {
+	for (i = 0, bsn = v_a(); bsn != v_s(); i++, bsn = mod_sns(bsn + 1)) {
 		uint16_t index = bsn & mod_sns_half();
 		switch(m_v_b.get_state(index)) {
 		case GPRS_RLC_DL_BSN_INVALID:
@@ -172,6 +203,22 @@ void gprs_rlc_v_n::reset()
 		m_v_n[i] = GPRS_RLC_UL_BSN_INVALID;
 }
 
+void gprs_rlc_window::set_sns(uint16_t sns)
+{
+	OSMO_ASSERT(sns >= RLC_GPRS_SNS);
+	OSMO_ASSERT(sns <= RLC_MAX_SNS);
+	/* check for 2^n */
+	OSMO_ASSERT((sns & (-sns)) == sns);
+	m_sns = sns;
+}
+
+void gprs_rlc_window::set_ws(uint16_t ws)
+{
+	OSMO_ASSERT(ws >= RLC_GPRS_SNS/2);
+	OSMO_ASSERT(ws <= RLC_MAX_SNS/2);
+	m_ws = ws;
+}
+
 /* Update the receive block bitmap */
 void gprs_rlc_ul_window::update_rbb(char *rbb)
 {
@@ -188,7 +235,7 @@ void gprs_rlc_ul_window::update_rbb(char *rbb)
 void gprs_rlc_ul_window::raise_v_r(const uint16_t bsn)
 {
 	uint16_t offset_v_r;
-	offset_v_r = (bsn + 1 - v_r()) & mod_sns();
+	offset_v_r = mod_sns(bsn + 1 - v_r());
 	/* Positive offset, so raise. */
 	if (offset_v_r < (sns() >> 1)) {
 		while (offset_v_r--) {
@@ -212,7 +259,7 @@ uint16_t gprs_rlc_ul_window::raise_v_q()
 		if (!m_v_n.is_received(v_q()))
 			break;
 		LOGP(DRLCMACUL, LOGL_DEBUG, "- Taking block %d out, raising "
-			"V(Q) to %d\n", v_q(), (v_q() + 1) & mod_sns());
+			"V(Q) to %d\n", v_q(), mod_sns(v_q() + 1));
 		raise_v_q(1);
 		count += 1;
 	}
@@ -233,3 +280,108 @@ bool gprs_rlc_ul_window::invalidate_bsn(const uint16_t bsn)
 
 	return was_valid;
 }
+
+static void gprs_rlc_data_header_init(struct gprs_rlc_data_info *rlc,
+	GprsCodingScheme cs, bool with_padding, unsigned int header_bits)
+{
+	unsigned int i;
+	unsigned int padding_bits = with_padding ? cs.optionalPaddingBits() : 0;
+
+	memset(rlc, 0, sizeof(*rlc));
+
+	rlc->cs = cs;
+	rlc->with_padding = with_padding;
+	rlc->num_data_blocks = cs.numDataBlocks();
+
+	OSMO_ASSERT(rlc->num_data_blocks <= ARRAY_SIZE(rlc->block_info));
+
+	for (i = 0; i < rlc->num_data_blocks; i++) {
+		gprs_rlc_data_block_info_init(&rlc->block_info[i], cs,
+			with_padding);
+
+		rlc->data_offs_bits[i] =
+			header_bits + padding_bits +
+			(i+1) * cs.numDataBlockHeaderBits() +
+			i * 8 * rlc->block_info[0].data_len;
+	}
+}
+
+void gprs_rlc_data_info_init_dl(struct gprs_rlc_data_info *rlc,
+	GprsCodingScheme cs, bool with_padding)
+{
+	return gprs_rlc_data_header_init(rlc, cs, with_padding,
+		cs.numDataHeaderBitsDL());
+}
+
+void gprs_rlc_data_info_init_ul(struct gprs_rlc_data_info *rlc,
+	GprsCodingScheme cs, bool with_padding)
+{
+	return gprs_rlc_data_header_init(rlc, cs, with_padding,
+		cs.numDataHeaderBitsUL());
+}
+
+void gprs_rlc_data_block_info_init(struct gprs_rlc_data_block_info *rdbi,
+	GprsCodingScheme cs, bool with_padding)
+{
+	unsigned int data_len = cs.maxDataBlockBytes();
+	if (with_padding)
+		data_len -= cs.optionalPaddingBits() / 8;
+
+	rdbi->data_len = data_len;
+	rdbi->bsn = 0;
+	rdbi->ti  = 0;
+	rdbi->e   = 1;
+	rdbi->cv  = 15;
+	rdbi->pi  = 0;
+	rdbi->spb = 0;
+}
+
+unsigned int gprs_rlc_mcs_cps(GprsCodingScheme cs, int punct, int punct2,
+	int with_padding)
+{
+	switch (GprsCodingScheme::Scheme(cs)) {
+	case GprsCodingScheme::MCS1: return 0b1011 + punct % 2;
+	case GprsCodingScheme::MCS2: return 0b1001 + punct % 2;
+	case GprsCodingScheme::MCS3: return (with_padding ? 0b0110 : 0b0011) +
+					    punct % 3;
+	case GprsCodingScheme::MCS4: return 0b0000 + punct % 3;
+	case GprsCodingScheme::MCS5: return  0b100 + punct % 2;
+	case GprsCodingScheme::MCS6: return (with_padding ? 0b010 : 0b000) +
+					    punct % 2;
+	case GprsCodingScheme::MCS7: return 0b10100 + 3 * (punct % 3) + punct2 % 3;
+	case GprsCodingScheme::MCS8: return 0b01011 + 3 * (punct % 3) + punct2 % 3;
+	case GprsCodingScheme::MCS9: return 0b00000 + 4 * (punct % 3) + punct2 % 3;
+	default: ;
+	}
+
+	return -1;
+}
+
+void gprs_rlc_mcs_cps_decode(unsigned int cps,
+	GprsCodingScheme cs, int *punct, int *punct2, int *with_padding)
+{
+	*punct2 = -1;
+	*with_padding = 0;
+
+	switch (GprsCodingScheme::Scheme(cs)) {
+	case GprsCodingScheme::MCS1:
+		cps -= 0b1011; *punct = cps % 2; break;
+	case GprsCodingScheme::MCS2:
+		cps -= 0b1001; *punct = cps % 2; break;
+	case GprsCodingScheme::MCS3:
+		cps -= 0b0011; *punct = cps % 3; *with_padding = cps >= 3; break;
+	case GprsCodingScheme::MCS4:
+		cps -= 0b0000; *punct = cps % 3; break;
+	case GprsCodingScheme::MCS5:
+		cps -= 0b100; *punct = cps % 2; break;
+	case GprsCodingScheme::MCS6:
+		cps -= 0b000; *punct = cps % 2; *with_padding = cps >= 2; break;
+	case GprsCodingScheme::MCS7:
+		cps -= 0b10100; *punct = cps / 3; *punct2 = cps % 3; break;
+	case GprsCodingScheme::MCS8:
+		cps -= 0b01011; *punct = cps / 3; *punct2 = cps % 3; break;
+	case GprsCodingScheme::MCS9:
+		cps -= 0b00000; *punct = cps / 4; *punct2 = cps % 3; break;
+	default: ;
+	}
+}
diff --git a/src/rlc.h b/src/rlc.h
index f2acb98a..54f28dfd 100644
--- a/src/rlc.h
+++ b/src/rlc.h
@@ -25,9 +25,16 @@
 
 #include <stdint.h>
 
-#define RLC_MAX_SNS 128 /* GPRS, must be power of 2 */
-#define RLC_MAX_WS  64 /* max window size */
-#define RLC_MAX_LEN 54 /* CS-4 including spare bits */
+#define RLC_GPRS_SNS 128 /* GPRS, must be power of 2 */
+#define RLC_GPRS_WS  64 /* max window size */
+#define RLC_EGPRS_SNS 2048 /* EGPRS, must be power of 2 */
+#define RLC_EGPRS_MIN_WS 64 /* min window size */
+#define RLC_EGPRS_MAX_WS 1024 /* min window size */
+#define RLC_EGPRS_SNS 2048 /* EGPRS, must be power of 2 */
+#define RLC_EGPRS_MAX_BSN_DELTA 512
+#define RLC_MAX_SNS  RLC_EGPRS_SNS
+#define RLC_MAX_WS   RLC_EGPRS_MAX_WS
+#define RLC_MAX_LEN 74 /* MCS-9 data unit */
 
 struct BTS;
 struct gprs_rlc_v_n;
@@ -55,26 +62,31 @@ static inline uint16_t mod_sns_half()
 	return (RLC_MAX_SNS / 2) - 1;
 }
 
-struct gprs_rlc_ul_data_block_info {
+struct gprs_rlc_data_block_info {
 	unsigned int data_len; /* EGPRS: N2, GPRS: N2-2, N-2 */
 	unsigned int bsn;
 	unsigned int ti;
 	unsigned int e;
-	unsigned int cv;
+	unsigned int cv; /* FBI == 1 <=> CV == 0 */
 	unsigned int pi;
 	unsigned int spb;
 };
 
-struct gprs_rlc_ul_header_egprs {
+struct gprs_rlc_data_info {
 	GprsCodingScheme cs;
 	unsigned int r;
 	unsigned int si;
 	unsigned int tfi;
 	unsigned int cps;
 	unsigned int rsb;
+	unsigned int usf;
+	unsigned int es_p;
+	unsigned int rrbp;
+	unsigned int pr;
 	unsigned int num_data_blocks;
+	unsigned int with_padding;
 	unsigned int data_offs_bits[2];
-	struct gprs_rlc_ul_data_block_info block_info[2];
+	struct gprs_rlc_data_block_info block_info[2];
 };
 
 struct gprs_rlc_data {
@@ -86,10 +98,21 @@ struct gprs_rlc_data {
 	/* block len of history */
 	uint8_t len;
 
-	struct gprs_rlc_ul_data_block_info block_info;
+	struct gprs_rlc_data_block_info block_info;
 	GprsCodingScheme cs;
 };
 
+void gprs_rlc_data_info_init_dl(struct gprs_rlc_data_info *rlc,
+	GprsCodingScheme cs, bool with_padding);
+void gprs_rlc_data_info_init_ul(struct gprs_rlc_data_info *rlc,
+	GprsCodingScheme cs, bool with_padding);
+void gprs_rlc_data_block_info_init(struct gprs_rlc_data_block_info *rdbi,
+	GprsCodingScheme cs, bool with_padding);
+unsigned int gprs_rlc_mcs_cps(GprsCodingScheme cs, int punct, int punct2,
+	int with_padding);
+void gprs_rlc_mcs_cps_decode(unsigned int cps, GprsCodingScheme cs,
+	int *punct, int *punct2, int *with_padding);
+
 /*
  * I hold the currently transferred blocks and will provide
  * the routines to manipulate these arrays.
@@ -131,18 +154,28 @@ private:
 
 
 /**
- * TODO: The UL/DL code could/should share a baseclass but
- * we are using llist_for_each_entry for the TBF which
- * requires everything which creates a requirement for a POD
- * type and in < C++11 something that is using even if the
- * most simple form of inheritance is not a POD anymore.
+ * TODO: The UL/DL code could/should share a base class.
  */
-struct gprs_rlc_dl_window {
-	void reset();
+class gprs_rlc_window {
+public:
+	gprs_rlc_window();
+
 	const uint16_t mod_sns() const;
+	const uint16_t mod_sns(uint16_t bsn) const;
 	const uint16_t sns() const;
 	const uint16_t ws() const;
 
+	void set_sns(uint16_t sns);
+	void set_ws(uint16_t ws);
+
+protected:
+	uint16_t m_sns;
+	uint16_t m_ws;
+};
+
+struct gprs_rlc_dl_window: public gprs_rlc_window {
+	void reset();
+
 	bool window_stalled() const;
 	bool window_empty() const;
 
@@ -157,8 +190,11 @@ struct gprs_rlc_dl_window {
 	/* Methods to manage reception */
 	int resend_needed();
 	int mark_for_resend();
-	void update(BTS *bts, char *show_rbb, uint8_t ssn,
+	void update(BTS *bts, char *show_rbb, uint16_t ssn,
 			uint16_t *lost, uint16_t *received);
+	void update(BTS *bts, const struct bitvec *rbb,
+			uint16_t first_bsn, uint16_t *lost,
+			uint16_t *received);
 	int move_window();
 	void show_state(char *show_rbb);
 	int count_unacked();
@@ -167,6 +203,8 @@ struct gprs_rlc_dl_window {
 	uint16_t m_v_a;	/* ack state */
 
 	gprs_rlc_v_b m_v_b;
+
+	gprs_rlc_dl_window();
 };
 
 struct gprs_rlc_v_n {
@@ -184,18 +222,14 @@ private:
 	gprs_rlc_ul_bsn_state m_v_n[RLC_MAX_SNS/2]; /* receive state array */
 };
 
-struct gprs_rlc_ul_window {
-	const uint16_t mod_sns() const;
-	const uint16_t sns() const;
-	const uint16_t ws() const;
-
+struct gprs_rlc_ul_window: public gprs_rlc_window {
 	const uint16_t v_r() const;
 	const uint16_t v_q() const;
 
 	const uint16_t ssn() const;
 
-	bool is_in_window(uint8_t bsn) const;
-	bool is_received(uint8_t bsn) const;
+	bool is_in_window(uint16_t bsn) const;
+	bool is_received(uint16_t bsn) const;
 
 	void update_rbb(char *rbb);
 	void raise_v_r_to(int moves);
@@ -211,6 +245,8 @@ struct gprs_rlc_ul_window {
 	uint16_t m_v_q;	/* receive window state */
 
 	gprs_rlc_v_n m_v_n;
+
+	gprs_rlc_ul_window();
 };
 
 extern "C" {
@@ -268,6 +304,50 @@ struct gprs_rlc_ul_header_egprs_3 {
 		spare:1,
 		dummy:1;
 } __attribute__ ((packed));
+
+struct gprs_rlc_dl_header_egprs_1 {
+	uint8_t usf:3,
+		es_p:2,
+		rrbp:2,
+		tfi_a:1;
+	uint8_t tfi_b:4,
+		pr:2,
+		bsn1_a:2;
+	uint8_t bsn1_b:8;
+	uint8_t bsn1_c:1,
+		bsn2_a:7;
+	uint8_t bsn2_b:3,
+		cps:5;
+} __attribute__ ((packed));
+
+struct gprs_rlc_dl_header_egprs_2 {
+	uint8_t usf:3,
+		es_p:2,
+		rrbp:2,
+		tfi_a:1;
+	uint8_t tfi_b:4,
+		pr:2,
+		bsn1_a:2;
+	uint8_t bsn1_b:8;
+	uint8_t bsn1_c:1,
+		cps:3,
+		dummy:4;
+} __attribute__ ((packed));
+
+struct gprs_rlc_dl_header_egprs_3 {
+	uint8_t usf:3,
+		es_p:2,
+		rrbp:2,
+		tfi_a:1;
+	uint8_t tfi_b:4,
+		pr:2,
+		bsn1_a:2;
+	uint8_t bsn1_b:8;
+	uint8_t bsn1_c:1,
+		cps:4,
+		spb:2,
+		dummy:1;
+} __attribute__ ((packed));
 #else
 #  error "Only little endian headers are supported yet. TODO: add missing structs"
 #endif
@@ -338,21 +418,38 @@ inline void gprs_rlc_v_b::mark_invalid(int bsn)
 	return mark(bsn, GPRS_RLC_DL_BSN_INVALID);
 }
 
-inline const uint16_t gprs_rlc_dl_window::sns() const
+inline gprs_rlc_window::gprs_rlc_window()
+	: m_sns(RLC_GPRS_SNS)
+	, m_ws(RLC_GPRS_WS)
 {
-	return RLC_MAX_SNS;
 }
 
-inline const uint16_t gprs_rlc_dl_window::ws() const
+inline const uint16_t gprs_rlc_window::sns() const
 {
-	return RLC_MAX_WS;
+	return m_sns;
 }
 
-inline const uint16_t gprs_rlc_dl_window::mod_sns() const
+inline const uint16_t gprs_rlc_window::ws() const
+{
+	return m_ws;
+}
+
+inline const uint16_t gprs_rlc_window::mod_sns() const
 {
 	return sns() - 1;
 }
 
+inline const uint16_t gprs_rlc_window::mod_sns(uint16_t bsn) const
+{
+	return bsn & mod_sns();
+}
+
+inline gprs_rlc_dl_window::gprs_rlc_dl_window()
+	: m_v_s(0)
+	, m_v_a(0)
+{
+}
+
 inline const uint16_t gprs_rlc_dl_window::v_s() const
 {
 	return m_v_s;
@@ -360,7 +457,7 @@ inline const uint16_t gprs_rlc_dl_window::v_s() const
 
 inline const uint16_t gprs_rlc_dl_window::v_s_mod(int offset) const
 {
-	return (m_v_s + offset) & mod_sns();
+	return mod_sns(m_v_s + offset);
 }
 
 inline const uint16_t gprs_rlc_dl_window::v_a() const
@@ -370,7 +467,7 @@ inline const uint16_t gprs_rlc_dl_window::v_a() const
 
 inline bool gprs_rlc_dl_window::window_stalled() const
 {
-	return ((m_v_s - m_v_a) & mod_sns()) == ws();
+	return (mod_sns(m_v_s - m_v_a)) == ws();
 }
 
 inline bool gprs_rlc_dl_window::window_empty() const
@@ -393,7 +490,13 @@ inline const int16_t gprs_rlc_dl_window::distance() const
 	return (m_v_s - m_v_a) & mod_sns();
 }
 
-inline bool gprs_rlc_ul_window::is_in_window(uint8_t bsn) const
+inline gprs_rlc_ul_window::gprs_rlc_ul_window()
+	: m_v_r(0)
+	, m_v_q(0)
+{
+}
+
+inline bool gprs_rlc_ul_window::is_in_window(uint16_t bsn) const
 {
 	uint16_t offset_v_q;
 
@@ -404,7 +507,7 @@ inline bool gprs_rlc_ul_window::is_in_window(uint8_t bsn) const
 	return offset_v_q < ws();
 }
 
-inline bool gprs_rlc_ul_window::is_received(uint8_t bsn) const
+inline bool gprs_rlc_ul_window::is_received(uint16_t bsn) const
 {
 	uint16_t offset_v_r;
 
@@ -413,21 +516,6 @@ inline bool gprs_rlc_ul_window::is_received(uint8_t bsn) const
 	return is_in_window(bsn) && m_v_n.is_received(bsn) && offset_v_r < ws();
 }
 
-inline const uint16_t gprs_rlc_ul_window::sns() const
-{
-	return RLC_MAX_SNS;
-}
-
-inline const uint16_t gprs_rlc_ul_window::ws() const
-{
-	return RLC_MAX_WS;
-}
-
-inline const uint16_t gprs_rlc_ul_window::mod_sns() const
-{
-	return sns() - 1;
-}
-
 inline const uint16_t gprs_rlc_ul_window::v_r() const
 {
 	return m_v_r;
@@ -445,12 +533,12 @@ inline const uint16_t gprs_rlc_ul_window::ssn() const
 
 inline void gprs_rlc_ul_window::raise_v_r_to(int moves)
 {
-	m_v_r = (m_v_r + moves) & mod_sns();
+	m_v_r = mod_sns(m_v_r + moves);
 }
 
 inline void gprs_rlc_ul_window::raise_v_q(int incr)
 {
-	m_v_q = (m_v_q + incr) & mod_sns();
+	m_v_q = mod_sns(m_v_q + incr);
 }
 
 inline void gprs_rlc_v_n::mark_received(int bsn)
diff --git a/src/tbf.cpp b/src/tbf.cpp
index 556f6e83..69b9e3a0 100644
--- a/src/tbf.cpp
+++ b/src/tbf.cpp
@@ -28,6 +28,7 @@
 #include <gprs_bssgp_pcu.h>
 #include <gprs_ms.h>
 #include <decoding.h>
+#include <pcu_utils.h>
 
 extern "C" {
 #include <osmocom/core/msgb.h>
@@ -60,6 +61,7 @@ gprs_rlcmac_tbf::gprs_rlcmac_tbf(BTS *bts_, gprs_rlcmac_tbf_direction dir) :
 	ul_ack_state(GPRS_RLCMAC_UL_ACK_NONE),
 	poll_state(GPRS_RLCMAC_POLL_NONE),
 	poll_fn(0),
+	poll_ts(0),
 	n3105(0),
 	T(0),
 	num_T_exp(0),
@@ -74,12 +76,12 @@ gprs_rlcmac_tbf::gprs_rlcmac_tbf(BTS *bts_, gprs_rlcmac_tbf_direction dir) :
 	m_ms(NULL),
 	m_ta(0),
 	m_ms_class(0),
+	m_list(this),
 	m_ms_list(this),
 	m_egprs_enabled(false)
 {
 	/* The classes of these members do not have proper constructors yet.
 	 * Just set them to 0 like talloc_zero did */
-	memset(&list, 0, sizeof(list));
 	memset(&pdch, 0, sizeof(pdch));
 	memset(&timer, 0, sizeof(timer));
 	memset(&m_rlc, 0, sizeof(m_rlc));
@@ -88,9 +90,6 @@ gprs_rlcmac_tbf::gprs_rlcmac_tbf(BTS *bts_, gprs_rlcmac_tbf_direction dir) :
 	m_llc.init();
 
 	m_name_buf[0] = '\0';
-
-	/* Back pointer for PODS llist compatibility */
-	list.back = this;
 }
 
 gprs_rlcmac_bts *gprs_rlcmac_tbf::bts_data() const
@@ -168,15 +167,15 @@ void gprs_rlcmac_tbf::set_ms_class(uint8_t ms_class_)
 	m_ms_class = ms_class_;
 }
 
-uint8_t gprs_rlcmac_tbf::current_cs() const
+GprsCodingScheme gprs_rlcmac_tbf::current_cs() const
 {
-	uint8_t cs;
+	GprsCodingScheme cs;
 	if (direction == GPRS_RLCMAC_UL_TBF)
-		cs = m_ms ? m_ms->current_cs_ul() : bts->bts_data()->initial_cs_ul;
+		cs = m_ms ? m_ms->current_cs_ul() : GprsCodingScheme();
 	else
-		cs = m_ms ? m_ms->current_cs_dl() : bts->bts_data()->initial_cs_dl;
+		cs = m_ms ? m_ms->current_cs_dl() : GprsCodingScheme();
 
-	return cs < 1 ? 1 : cs;
+	return cs;
 }
 
 gprs_llc_queue *gprs_rlcmac_tbf::llc_queue()
@@ -312,11 +311,23 @@ static void tbf_unlink_pdch(struct gprs_rlcmac_tbf *tbf)
 
 void tbf_free(struct gprs_rlcmac_tbf *tbf)
 {
+	/* update counters */
+	if (tbf->direction == GPRS_RLCMAC_UL_TBF) {
+		tbf->bts->tbf_ul_freed();
+		if (tbf->state_is(GPRS_RLCMAC_FLOW))
+			tbf->bts->tbf_ul_aborted();
+	} else {
+		tbf->bts->tbf_dl_freed();
+		if (tbf->state_is(GPRS_RLCMAC_FLOW))
+			tbf->bts->tbf_dl_aborted();
+	}
+
 	/* Give final measurement report */
 	gprs_rlcmac_rssi_rep(tbf);
 	if (tbf->direction == GPRS_RLCMAC_DL_TBF) {
-		gprs_rlcmac_dl_tbf *dl_tbf = static_cast<gprs_rlcmac_dl_tbf *>(tbf);
-		gprs_rlcmac_lost_rep(dl_tbf);
+		gprs_rlcmac_dl_tbf *dl_tbf = as_dl_tbf(tbf);
+
+		dl_tbf->abort();
 		dl_tbf->cleanup();
 	}
 
@@ -336,12 +347,7 @@ void tbf_free(struct gprs_rlcmac_tbf *tbf)
 	tbf->stop_timer();
 	#warning "TODO: Could/Should generate  bssgp_tx_llc_discarded"
 	tbf_unlink_pdch(tbf);
-	llist_del(&tbf->list.list);
-
-	if (tbf->direction == GPRS_RLCMAC_UL_TBF)
-		tbf->bts->tbf_ul_freed();
-	else
-		tbf->bts->tbf_dl_freed();
+	llist_del(&tbf->list());
 
 	if (tbf->ms())
 		tbf->set_ms(NULL);
@@ -388,6 +394,7 @@ int tbf_assign_control_ts(struct gprs_rlcmac_tbf *tbf)
 const char *gprs_rlcmac_tbf::tbf_state_name[] = {
 	"NULL",
 	"ASSIGN",
+	"WAIT ASSIGN",
 	"FLOW",
 	"FINISHED",
 	"WAIT RELEASE",
@@ -429,10 +436,53 @@ void gprs_rlcmac_tbf::stop_timer()
 	}
 }
 
+int gprs_rlcmac_tbf::check_polling(uint32_t fn, uint8_t ts,
+	uint32_t *poll_fn_, unsigned int *rrbp_)
+{
+	uint32_t fn_offs = 13;
+	uint32_t new_poll_fn = (fn + fn_offs) % 2715648;
+
+	if (!is_control_ts(ts)) {
+		LOGP(DRLCMAC, LOGL_DEBUG, "Polling cannot be "
+			"scheduled in this TS %d (first control TS %d)\n",
+			ts, control_ts);
+		return -EINVAL;
+	}
+	if (poll_state != GPRS_RLCMAC_POLL_NONE) {
+		LOGP(DRLCMAC, LOGL_DEBUG,
+			"Polling is already scheduled for %s\n",
+			name());
+		return -EBUSY;
+	}
+	if (bts->sba()->find(trx->trx_no, ts, (fn + 13) % 2715648)) {
+		LOGP(DRLCMAC, LOGL_DEBUG, "%s: Polling is already scheduled "
+			"for single block allocation at FN %d TS %d ...\n",
+			name(), new_poll_fn, ts);
+		return -EBUSY;
+	}
+
+	*poll_fn_ = new_poll_fn;
+	*rrbp_ = 0;
+
+	return 0;
+}
+
+void gprs_rlcmac_tbf::set_polling(uint32_t new_poll_fn, uint8_t ts)
+{
+	LOGP(DRLCMAC, LOGL_DEBUG,
+		"%s: Scheduling polling at FN %d TS %d\n",
+		name(), new_poll_fn, ts);
+
+	/* schedule polling */
+	poll_state = GPRS_RLCMAC_POLL_SCHED;
+	poll_fn = new_poll_fn;
+	poll_ts = ts;
+}
+
 void gprs_rlcmac_tbf::poll_timeout()
 {
-	LOGP(DRLCMAC, LOGL_NOTICE, "%s poll timeout for FN=%d (curr FN %d)\n",
-		tbf_name(this), poll_fn, bts->current_frame_number());
+	LOGP(DRLCMAC, LOGL_NOTICE, "%s poll timeout for FN=%d, TS=%d (curr FN %d)\n",
+		tbf_name(this), poll_fn, poll_ts, bts->current_frame_number());
 
 	poll_state = GPRS_RLCMAC_POLL_NONE;
 
@@ -446,7 +496,7 @@ void gprs_rlcmac_tbf::poll_timeout()
 		ul_ack_state = GPRS_RLCMAC_UL_ACK_NONE;
 		bts->rlc_ack_timedout();
 		if (state_is(GPRS_RLCMAC_FINISHED)) {
-			gprs_rlcmac_ul_tbf *ul_tbf = static_cast<gprs_rlcmac_ul_tbf *>(this);
+			gprs_rlcmac_ul_tbf *ul_tbf = as_ul_tbf(this);
 			ul_tbf->m_n3103++;
 			if (ul_tbf->m_n3103 == ul_tbf->bts->bts_data()->n3103) {
 				LOGP(DRLCMAC, LOGL_NOTICE,
@@ -500,7 +550,7 @@ void gprs_rlcmac_tbf::poll_timeout()
 		/* reschedule DL assignment */
 		dl_ass_state = GPRS_RLCMAC_DL_ASS_SEND_ASS;
 	} else if (direction == GPRS_RLCMAC_DL_TBF) {
-		gprs_rlcmac_dl_tbf *dl_tbf = static_cast<gprs_rlcmac_dl_tbf *>(this);
+		gprs_rlcmac_dl_tbf *dl_tbf = as_dl_tbf(this);
 
 		if (!(dl_tbf->state_flags & (1 << GPRS_RLCMAC_FLAG_TO_DL_ACK))) {
 			LOGP(DRLCMAC, LOGL_NOTICE, "- Timeout for polling "
@@ -546,11 +596,8 @@ static int setup_tbf(struct gprs_rlcmac_tbf *tbf,
 
 	bts = tbf->bts->bts_data();
 
-	if (egprs_ms_class > 0 && bts->egprs_enabled) {
-		/* TODO: only for 8PSK, otherwise the GPRS MS class has to be used */
+	if (ms->mode() == GprsCodingScheme::EGPRS)
 		ms_class = egprs_ms_class;
-		tbf->enable_egprs();
-	}
 
 	tbf->m_created_ts = time(NULL);
 	tbf->set_ms_class(ms_class);
@@ -587,7 +634,6 @@ gprs_rlcmac_ul_tbf::gprs_rlcmac_ul_tbf(BTS *bts_) :
 	m_contention_resolution_done(0),
 	m_final_ack_sent(0)
 {
-	memset(&m_window, 0, sizeof(m_window));
 	memset(&m_usf, 0, sizeof(m_usf));
 }
 
@@ -597,6 +643,17 @@ static int ul_tbf_dtor(struct gprs_rlcmac_ul_tbf *tbf)
 	return 0;
 }
 
+static void setup_egprs_mode(gprs_rlcmac_bts *bts, GprsMs *ms)
+{
+	if (GprsCodingScheme::getEgprsByNum(bts->max_mcs_ul).isEgprsGmsk() &&
+		GprsCodingScheme::getEgprsByNum(bts->max_mcs_dl).isEgprsGmsk() &&
+		ms->mode() != GprsCodingScheme::EGPRS)
+	{
+		ms->set_mode(GprsCodingScheme::EGPRS_GMSK);
+	} else {
+		ms->set_mode(GprsCodingScheme::EGPRS);
+	}
+}
 
 struct gprs_rlcmac_ul_tbf *tbf_alloc_ul_tbf(struct gprs_rlcmac_bts *bts,
 	GprsMs *ms, int8_t use_trx,
@@ -605,6 +662,13 @@ struct gprs_rlcmac_ul_tbf *tbf_alloc_ul_tbf(struct gprs_rlcmac_bts *bts,
 	struct gprs_rlcmac_ul_tbf *tbf;
 	int rc;
 
+	if (egprs_ms_class == 0 && bts->egprs_enabled) {
+		LOGP(DRLCMAC, LOGL_NOTICE,
+			"Not accepting non-EGPRS phone in EGPRS-only mode\n");
+		bts->bts->tbf_failed_egprs_only();
+		return NULL;
+	}
+
 	LOGP(DRLCMAC, LOGL_DEBUG, "********** TBF starts here **********\n");
 	LOGP(DRLCMAC, LOGL_INFO, "Allocating %s TBF: MS_CLASS=%d/%d\n",
 		"UL", ms_class, egprs_ms_class);
@@ -620,6 +684,16 @@ struct gprs_rlcmac_ul_tbf *tbf_alloc_ul_tbf(struct gprs_rlcmac_bts *bts,
 	if (!ms)
 		ms = bts->bts->ms_alloc(ms_class, egprs_ms_class);
 
+	if (egprs_ms_class > 0 && bts->egprs_enabled) {
+		tbf->enable_egprs();
+		tbf->m_window.set_sns(RLC_EGPRS_SNS);
+		/* TODO: Allow bigger UL windows when CRBB encoding is supported */
+		tbf->m_window.set_ws(RLC_EGPRS_MIN_WS);
+		setup_egprs_mode(bts, ms);
+		LOGP(DRLCMAC, LOGL_INFO, "Enabled EGPRS for %s, mode %s\n",
+			tbf->name(), GprsCodingScheme::modeName(ms->mode()));
+	}
+
 	rc = setup_tbf(tbf, ms, use_trx, ms_class, egprs_ms_class, single_slot);
 	/* if no resource */
 	if (rc < 0) {
@@ -627,7 +701,7 @@ struct gprs_rlcmac_ul_tbf *tbf_alloc_ul_tbf(struct gprs_rlcmac_bts *bts,
 		return NULL;
 	}
 
-	llist_add(&tbf->list.list, &bts->ul_tbfs);
+	llist_add(&tbf->list(), &bts->bts->ul_tbfs());
 	tbf->bts->tbf_ul_created();
 
 	return tbf;
@@ -650,7 +724,6 @@ gprs_rlcmac_dl_tbf::gprs_rlcmac_dl_tbf(BTS *bts_) :
 	m_last_dl_poll_fn(0),
 	m_last_dl_drained_fn(0)
 {
-	memset(&m_window, 0, sizeof(m_window));
 	memset(&m_llc_timer, 0, sizeof(m_llc_timer));
 }
 
@@ -667,9 +740,19 @@ struct gprs_rlcmac_dl_tbf *tbf_alloc_dl_tbf(struct gprs_rlcmac_bts *bts,
 	struct gprs_rlcmac_dl_tbf *tbf;
 	int rc;
 
+	if (egprs_ms_class == 0 && bts->egprs_enabled) {
+		if (ms_class > 0) {
+			LOGP(DRLCMAC, LOGL_NOTICE,
+				"Not accepting non-EGPRS phone in EGPRS-only mode\n");
+			bts->bts->tbf_failed_egprs_only();
+			return NULL;
+		}
+		egprs_ms_class = 1;
+	}
+
 	LOGP(DRLCMAC, LOGL_DEBUG, "********** TBF starts here **********\n");
-	LOGP(DRLCMAC, LOGL_INFO, "Allocating %s TBF: MS_CLASS=%d\n",
-		"DL", ms_class);
+	LOGP(DRLCMAC, LOGL_INFO, "Allocating %s TBF: MS_CLASS=%d/%d\n",
+		"DL", ms_class, egprs_ms_class);
 
 	tbf = talloc(tall_pcu_ctx, struct gprs_rlcmac_dl_tbf);
 
@@ -682,6 +765,14 @@ struct gprs_rlcmac_dl_tbf *tbf_alloc_dl_tbf(struct gprs_rlcmac_bts *bts,
 	if (!ms)
 		ms = bts->bts->ms_alloc(ms_class, egprs_ms_class);
 
+	if (egprs_ms_class > 0 && bts->egprs_enabled) {
+		tbf->enable_egprs();
+		tbf->m_window.set_sns(RLC_EGPRS_SNS);
+		setup_egprs_mode(bts, ms);
+		LOGP(DRLCMAC, LOGL_INFO, "Enabled EGPRS for %s, mode %s\n",
+			tbf->name(), GprsCodingScheme::modeName(ms->mode()));
+	}
+
 	rc = setup_tbf(tbf, ms, use_trx, ms_class, 0, single_slot);
 	/* if no resource */
 	if (rc < 0) {
@@ -689,7 +780,22 @@ struct gprs_rlcmac_dl_tbf *tbf_alloc_dl_tbf(struct gprs_rlcmac_bts *bts,
 		return NULL;
 	}
 
-	llist_add(&tbf->list.list, &bts->dl_tbfs);
+	if (tbf->is_egprs_enabled()) {
+		unsigned int num_pdch = pcu_bitcount(tbf->dl_slots());
+		unsigned int ws = bts->ws_base + num_pdch * bts->ws_pdch;
+		ws = (ws / 32) * 32;
+		ws = OSMO_MAX(64, ws);
+		if (num_pdch == 1)
+			ws = OSMO_MIN(192, ws);
+		else
+			ws = OSMO_MIN(128 * num_pdch, ws);
+
+		LOGP(DRLCMAC, LOGL_INFO, "%s: Setting EGPRS window size to %d\n",
+			tbf->name(), ws);
+		tbf->m_window.set_ws(ws);
+	}
+
+	llist_add(&tbf->list(), &bts->bts->dl_tbfs());
 	tbf->bts->tbf_dl_created();
 
 	tbf->m_last_dl_poll_fn = -1;
@@ -719,6 +825,12 @@ void gprs_rlcmac_tbf::handle_timeout()
 		if ((state_flags & (1 << GPRS_RLCMAC_FLAG_PACCH))) {
 			if (state_is(GPRS_RLCMAC_ASSIGN)) {
 				LOGP(DRLCMAC, LOGL_NOTICE, "%s releasing due to "
+					"PACCH assignment timeout (not yet sent).\n",
+					tbf_name(this));
+				tbf_free(this);
+				return;
+			} else if (state_is(GPRS_RLCMAC_WAIT_ASSIGN)) {
+				LOGP(DRLCMAC, LOGL_NOTICE, "%s releasing due to "
 					"PACCH assignment timeout.\n", tbf_name(this));
 				tbf_free(this);
 				return;
@@ -727,9 +839,9 @@ void gprs_rlcmac_tbf::handle_timeout()
 					"in assign state\n", tbf_name(this));
 		}
 		if ((state_flags & (1 << GPRS_RLCMAC_FLAG_CCCH))) {
-			gprs_rlcmac_dl_tbf *dl_tbf = static_cast<gprs_rlcmac_dl_tbf *>(this);
+			gprs_rlcmac_dl_tbf *dl_tbf = as_dl_tbf(this);
 			dl_tbf->m_wait_confirm = 0;
-			if (dl_tbf->state_is(GPRS_RLCMAC_ASSIGN)) {
+			if (dl_tbf->state_is(GPRS_RLCMAC_WAIT_ASSIGN)) {
 				tbf_assign_control_ts(dl_tbf);
 
 				if (!dl_tbf->upgrade_to_multislot) {
@@ -746,7 +858,6 @@ void gprs_rlcmac_tbf::handle_timeout()
 
 				/* keep to flags */
 				dl_tbf->state_flags &= GPRS_RLCMAC_FLAG_TO_MASK;
-				dl_tbf->state_flags &= ~(1 << GPRS_RLCMAC_FLAG_CCCH);
 
 				dl_tbf->update();
 
@@ -794,36 +905,40 @@ int gprs_rlcmac_tbf::rlcmac_diag()
 	return 0;
 }
 
-struct msgb *gprs_rlcmac_tbf::create_dl_ass(uint32_t fn)
+struct msgb *gprs_rlcmac_tbf::create_dl_ass(uint32_t fn, uint8_t ts)
 {
 	struct msgb *msg;
 	struct gprs_rlcmac_dl_tbf *new_dl_tbf = NULL;
 	int poll_ass_dl = 1;
+	unsigned int rrbp = 0;
+	uint32_t new_poll_fn = 0;
+	int rc;
+	bool old_tfi_is_valid = is_tfi_assigned();
 
-	if (direction == GPRS_RLCMAC_DL_TBF && control_ts != first_common_ts) {
+	if (direction == GPRS_RLCMAC_DL_TBF && !is_control_ts(ts)) {
 		LOGP(DRLCMAC, LOGL_NOTICE, "Cannot poll for downlink "
-			"assigment, because MS cannot reply. (control TS=%d, "
-			"first common TS=%d)\n", control_ts,
+			"assigment, because MS cannot reply. (TS=%d, "
+			"first common TS=%d)\n", ts,
 			first_common_ts);
 		poll_ass_dl = 0;
 	}
 	if (poll_ass_dl) {
-		if (poll_state != GPRS_RLCMAC_POLL_NONE) {
-			LOGP(DRLCMAC, LOGL_DEBUG, "Polling is already sheduled "
-				"for %s, so we must wait for downlink "
-				"assignment...\n", tbf_name(this));
-				return NULL;
-		}
-		if (bts->sba()->find(trx->trx_no, control_ts, (fn + 13) % 2715648)) {
+		if (poll_state == GPRS_RLCMAC_POLL_SCHED &&
+			ul_ass_state == GPRS_RLCMAC_UL_ASS_WAIT_ACK)
+		{
 			LOGP(DRLCMACUL, LOGL_DEBUG, "Polling is already "
-				"scheduled for single block allocation...\n");
+				"scheduled for %s, so we must wait for the uplink "
+				"assignment...\n", tbf_name(this));
 			return NULL;
 		}
+		rc = check_polling(fn, ts, &new_poll_fn, &rrbp);
+		if (rc < 0)
+			return NULL;
 	}
 
 	/* on uplink TBF we get the downlink TBF to be assigned. */
 	if (direction == GPRS_RLCMAC_UL_TBF) {
-		gprs_rlcmac_ul_tbf *ul_tbf = static_cast<gprs_rlcmac_ul_tbf *>(this);
+		gprs_rlcmac_ul_tbf *ul_tbf = as_ul_tbf(this);
 
 		/* be sure to check first, if contention resolution is done,
 		 * otherwise we cannot send the assignment yet */
@@ -846,6 +961,19 @@ struct msgb *gprs_rlcmac_tbf::create_dl_ass(uint32_t fn)
 		return NULL;
 	}
 
+	if (new_dl_tbf == as_dl_tbf(this))
+		LOGP(DRLCMAC, LOGL_DEBUG,
+			"New and old TBF are the same %s\n", name());
+
+	if (old_tfi_is_valid && !new_dl_tbf->is_tlli_valid()) {
+		LOGP(DRLCMACDL, LOGL_ERROR,
+			"The old TFI is not assigned and there is no "
+			"TLLI. Old TBF %s, new TBF %s\n",
+			name(), new_dl_tbf->name());
+		dl_ass_state = GPRS_RLCMAC_DL_ASS_NONE;
+		return NULL;
+	}
+
 	new_dl_tbf->was_releasing = was_releasing;
 	msg = msgb_alloc(23, "rlcmac_dl_ass");
 	if (!msg)
@@ -859,9 +987,11 @@ struct msgb *gprs_rlcmac_tbf::create_dl_ass(uint32_t fn)
 		"2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b");
 	LOGP(DRLCMAC, LOGL_INFO, "%s  start Packet Downlink Assignment (PACCH)\n", tbf_name(new_dl_tbf));
 	RlcMacDownlink_t * mac_control_block = (RlcMacDownlink_t *)talloc_zero(tall_pcu_ctx, RlcMacDownlink_t);
-	Encoding::write_packet_downlink_assignment(mac_control_block, m_tfi,
-		(direction == GPRS_RLCMAC_DL_TBF), new_dl_tbf,
-		poll_ass_dl, bts_data()->alpha, bts_data()->gamma, -1, 0);
+	Encoding::write_packet_downlink_assignment(mac_control_block,
+		old_tfi_is_valid, m_tfi, (direction == GPRS_RLCMAC_DL_TBF),
+		new_dl_tbf, poll_ass_dl, rrbp,
+		bts_data()->alpha, bts_data()->gamma, -1, 0,
+		is_egprs_enabled());
 	LOGP(DRLCMAC, LOGL_DEBUG, "+++++++++++++++++++++++++ TX : Packet Downlink Assignment +++++++++++++++++++++++++\n");
 	encode_gsm_rlcmac_downlink(ass_vec, mac_control_block);
 	LOGPC(DCSN1, LOGL_NOTICE, "\n");
@@ -871,12 +1001,13 @@ struct msgb *gprs_rlcmac_tbf::create_dl_ass(uint32_t fn)
 	talloc_free(mac_control_block);
 
 	if (poll_ass_dl) {
-		poll_state = GPRS_RLCMAC_POLL_SCHED;
-		poll_fn = (fn + 13) % 2715648;
+		set_polling(new_poll_fn, ts);
+		if (new_dl_tbf->state_is(GPRS_RLCMAC_ASSIGN))
+			new_dl_tbf->set_state(GPRS_RLCMAC_WAIT_ASSIGN);
 		dl_ass_state = GPRS_RLCMAC_DL_ASS_WAIT_ACK;
 		LOGP(DRLCMACDL, LOGL_INFO,
-			"%s Scheduled DL Assignment polling on FN=%d\n",
-			name(), poll_fn);
+			"%s Scheduled DL Assignment polling on FN=%d, TS=%d\n",
+			name(), poll_fn, poll_ts);
 	} else {
 		dl_ass_state = GPRS_RLCMAC_DL_ASS_NONE;
 		new_dl_tbf->set_state(GPRS_RLCMAC_FLOW);
@@ -889,22 +1020,25 @@ struct msgb *gprs_rlcmac_tbf::create_dl_ass(uint32_t fn)
 	return msg;
 }
 
-struct msgb *gprs_rlcmac_tbf::create_ul_ass(uint32_t fn)
+struct msgb *gprs_rlcmac_tbf::create_ul_ass(uint32_t fn, uint8_t ts)
 {
 	struct msgb *msg;
 	struct gprs_rlcmac_ul_tbf *new_tbf = NULL;
+	int rc;
+	unsigned int rrbp;
+	uint32_t new_poll_fn;
 
-	if (poll_state != GPRS_RLCMAC_POLL_NONE) {
+	if (poll_state == GPRS_RLCMAC_POLL_SCHED &&
+		ul_ass_state == GPRS_RLCMAC_UL_ASS_WAIT_ACK) {
 		LOGP(DRLCMACUL, LOGL_DEBUG, "Polling is already "
-			"sheduled for %s, so we must wait for uplink "
+			"scheduled for %s, so we must wait for the uplink "
 			"assignment...\n", tbf_name(this));
 			return NULL;
 	}
-	if (bts->sba()->find(trx->trx_no, control_ts, (fn + 13) % 2715648)) {
-		LOGP(DRLCMACUL, LOGL_DEBUG, "Polling is already scheduled for "
-			"single block allocation...\n");
-			return NULL;
-	}
+
+	rc = check_polling(fn, ts, &new_poll_fn, &rrbp);
+	if (rc < 0)
+		return NULL;
 
 	if (ms())
 		new_tbf = ms()->ul_tbf();
@@ -929,7 +1063,7 @@ struct msgb *gprs_rlcmac_tbf::create_ul_ass(uint32_t fn)
 		"2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b");
 	Encoding::write_packet_uplink_assignment(bts_data(), ass_vec, m_tfi,
 		(direction == GPRS_RLCMAC_DL_TBF), tlli(),
-		is_tlli_valid(), new_tbf, 1, bts_data()->alpha,
+		is_tlli_valid(), new_tbf, 1, rrbp, bts_data()->alpha,
 		bts_data()->gamma, -1, is_egprs_enabled());
 	bitvec_pack(ass_vec, msgb_put(msg, 23));
 	RlcMacDownlink_t * mac_control_block = (RlcMacDownlink_t *)talloc_zero(tall_pcu_ctx, RlcMacDownlink_t);
@@ -940,12 +1074,13 @@ struct msgb *gprs_rlcmac_tbf::create_ul_ass(uint32_t fn)
 	bitvec_free(ass_vec);
 	talloc_free(mac_control_block);
 
-	poll_state = GPRS_RLCMAC_POLL_SCHED;
-	poll_fn = (fn + 13) % 2715648;
+	set_polling(new_poll_fn, ts);
 	ul_ass_state = GPRS_RLCMAC_UL_ASS_WAIT_ACK;
+	if (new_tbf->state_is(GPRS_RLCMAC_ASSIGN))
+		new_tbf->set_state(GPRS_RLCMAC_WAIT_ASSIGN);
 	LOGP(DRLCMACDL, LOGL_INFO,
-		"%s Scheduled UL Assignment polling on FN=%d\n",
-		name(), poll_fn);
+		"%s Scheduled UL Assignment polling on FN=%d, TS=%d\n",
+		name(), poll_fn, poll_ts);
 
 	return msg;
 }
@@ -1010,15 +1145,8 @@ int gprs_rlcmac_tbf::set_tlli_from_ul(uint32_t new_tlli)
 
 		if (!ms())
 			set_ms(old_ms);
-
-		/* there might be an active and valid downlink TBF */
-		if (!ms()->dl_tbf() && dl_tbf)
-			/* Move it to the current MS (see the guard above) */
-			dl_tbf->set_ms(ms());
 	}
 
-	/* The TLLI has been taken from an UL message */
-	update_ms(new_tlli, GPRS_RLCMAC_UL_TBF);
 	if (dl_tbf && dl_tbf->ms() != ms()) {
 		LOGP(DRLCMACUL, LOGL_NOTICE, "Got RACH from "
 			"TLLI=0x%08x while %s still exists. "
@@ -1035,34 +1163,15 @@ int gprs_rlcmac_tbf::set_tlli_from_ul(uint32_t new_tlli)
 		tbf_free(ul_tbf);
 		ul_tbf = NULL;
 	}
-	return 1;
-}
-
-int gprs_rlcmac_tbf::extract_tlli(const uint8_t *data, const size_t len)
-{
-	struct rlc_ul_header *rh = (struct rlc_ul_header *)data;
-	uint32_t new_tlli;
-	int rc;
-
-	OSMO_ASSERT(direction == GPRS_RLCMAC_UL_TBF);
 
-	/* no TLLI yet */
-	if (!rh->ti) {
-		LOGP(DRLCMACUL, LOGL_NOTICE, "UL DATA TFI=%d without "
-			"TLLI, but no TLLI received yet\n", rh->tfi);
-		return 0;
-	}
-	rc = Decoding::tlli_from_ul_data(data, len, &new_tlli);
-	if (rc) {
-		bts->decode_error();
-		LOGP(DRLCMACUL, LOGL_NOTICE, "Failed to decode TLLI "
-		"of UL DATA TFI=%d.\n", rh->tfi);
-		return 0;
-	}
-	LOGP(DRLCMACUL, LOGL_INFO, "Decoded premier TLLI=0x%08x of "
-		"UL DATA TFI=%d.\n", new_tlli, rh->tfi);
+	/* The TLLI has been taken from an UL message */
+	update_ms(new_tlli, GPRS_RLCMAC_UL_TBF);
 
-	return set_tlli_from_ul(new_tlli);
+#if 0 /* REMOVEME ??? */
+	if (ms()->need_dl_tbf())
+		establish_dl_tbf_on_pacch();
+#endif
+	return 1;
 }
 
 const char *tbf_name(gprs_rlcmac_tbf *tbf)
@@ -1088,11 +1197,11 @@ const char *gprs_rlcmac_tbf::name() const
 
 void gprs_rlcmac_tbf::rotate_in_list()
 {
-	llist_del(&list.list);
+	llist_del(&list());
 	if (direction == GPRS_RLCMAC_UL_TBF)
-		llist_add(&list.list, &bts->bts_data()->ul_tbfs);
+		llist_add(&list(), &bts->ul_tbfs());
 	else
-		llist_add(&list.list, &bts->bts_data()->dl_tbfs);
+		llist_add(&list(), &bts->dl_tbfs());
 }
 
 uint8_t gprs_rlcmac_tbf::tsc() const
@@ -1136,24 +1245,7 @@ uint8_t gprs_rlcmac_tbf::ul_slots() const
 	return slots;
 }
 
-void tbf_print_vty_info(struct vty *vty, struct llist_head *ltbf)
+bool gprs_rlcmac_tbf::is_control_ts(uint8_t ts) const
 {
-	gprs_rlcmac_tbf *tbf = llist_pods_entry(ltbf, gprs_rlcmac_tbf);
-
-	vty_out(vty, "TBF: TFI=%d TLLI=0x%08x (%s) DIR=%s IMSI=%s%s", tbf->tfi(),
-			tbf->tlli(), tbf->is_tlli_valid() ? "valid" : "invalid",
-			tbf->direction == GPRS_RLCMAC_UL_TBF ? "UL" : "DL",
-			tbf->imsi(), VTY_NEWLINE);
-	vty_out(vty, " created=%lu state=%08x 1st_TS=%d 1st_cTS=%d ctrl_TS=%d "
-			"MS_CLASS=%d%s",
-			tbf->created_ts(), tbf->state_flags, tbf->first_ts,
-			tbf->first_common_ts, tbf->control_ts, tbf->ms_class(),
-			VTY_NEWLINE);
-	vty_out(vty, " TS_alloc=");
-	for (int i = 0; i < 8; i++) {
-		if (tbf->pdch[i])
-			vty_out(vty, "%d ", i);
-	}
-	vty_out(vty, " CS=%d%s%s", tbf->ms() ? tbf->ms()->current_cs_dl() : 1,
-		VTY_NEWLINE, VTY_NEWLINE);
+	return ts == control_ts;
 }
diff --git a/src/tbf.h b/src/tbf.h
index a3a2eeba..ad8ad4c2 100644
--- a/src/tbf.h
+++ b/src/tbf.h
@@ -43,7 +43,8 @@ class GprsMs;
 
 enum gprs_rlcmac_tbf_state {
 	GPRS_RLCMAC_NULL = 0,	/* new created TBF */
-	GPRS_RLCMAC_ASSIGN,	/* wait for downlink assignment */
+	GPRS_RLCMAC_ASSIGN,     /* wait for DL transmission */
+	GPRS_RLCMAC_WAIT_ASSIGN,/* wait for confirmation */
 	GPRS_RLCMAC_FLOW,	/* RLC/MAC flow, resource needed */
 	GPRS_RLCMAC_FINISHED,	/* flow finished, wait for release */
 	GPRS_RLCMAC_WAIT_RELEASE,/* wait for release or restart of DL TBF */
@@ -88,28 +89,6 @@ enum gprs_rlcmac_tbf_direction {
 #define GPRS_RLCMAC_FLAG_TO_DL_ASS	7
 #define GPRS_RLCMAC_FLAG_TO_MASK	0xf0 /* timeout bits */
 
-struct llist_pods {
-	struct llist_head list;
-	void *back;
-};
-
-#define llist_pods_entry(ptr, type) \
-	((type *)(container_of(ptr, struct llist_pods, list)->back))
-
-/**
- * llist_pods_for_each_entry - like llist_for_each_entry, but uses
- * struct llist_pods ->back to access the entry.
- * This is necessary for non-PODS classes because container_of is
- * not guaranteed to work anymore. */
-#define llist_pods_for_each_entry(pos, head, member, lpods)			\
-	for (lpods = llist_entry((head)->next, typeof(struct llist_pods), list), \
-		     pos = ((typeof(pos))lpods->back),	\
-		     prefetch(pos->member.list.next);				\
-	     &lpods->list != (head);					\
-	     lpods = llist_entry(lpods->list.next, typeof(struct llist_pods), list), \
-		     pos = ((typeof(pos))lpods->back),\
-		     prefetch(pos->member.list.next))
-
 struct gprs_rlcmac_tbf {
 	gprs_rlcmac_tbf(BTS *bts_, gprs_rlcmac_tbf_direction dir);
 
@@ -123,12 +102,14 @@ struct gprs_rlcmac_tbf {
 
 	const char *name() const;
 
-	struct msgb *create_dl_ass(uint32_t fn);
-	struct msgb *create_ul_ass(uint32_t fn);
+	struct msgb *create_dl_ass(uint32_t fn, uint8_t ts);
+	struct msgb *create_ul_ass(uint32_t fn, uint8_t ts);
 
 	GprsMs *ms() const;
 	void set_ms(GprsMs *ms);
 
+	gprs_rlc_window *window();
+
 	uint8_t tsc() const;
 
 	int rlcmac_diag();
@@ -139,6 +120,9 @@ struct gprs_rlcmac_tbf {
 	void stop_t3191();
 	int establish_dl_tbf_on_pacch();
 
+	int check_polling(uint32_t fn, uint8_t ts,
+		uint32_t *poll_fn, unsigned int *rrbp);
+	void set_polling(uint32_t poll_fn, uint8_t ts);
 	void poll_timeout();
 
 	/** tlli handling */
@@ -149,6 +133,7 @@ struct gprs_rlcmac_tbf {
 	void update_ms(uint32_t tlli, enum gprs_rlcmac_tbf_direction);
 
 	uint8_t tfi() const;
+	bool is_tfi_assigned() const;
 
 	const char *imsi() const;
 	void assign_imsi(const char *imsi);
@@ -156,7 +141,7 @@ struct gprs_rlcmac_tbf {
 	void set_ta(uint8_t);
 	uint8_t ms_class() const;
 	void set_ms_class(uint8_t);
-	uint8_t current_cs() const;
+	GprsCodingScheme current_cs() const;
 	gprs_llc_queue *llc_queue();
 	const gprs_llc_queue *llc_queue() const;
 
@@ -164,6 +149,8 @@ struct gprs_rlcmac_tbf {
 	uint8_t dl_slots() const;
 	uint8_t ul_slots() const;
 
+	bool is_control_ts(uint8_t ts) const;
+
 	/* EGPRS */
 	bool is_egprs_enabled() const;
 	void enable_egprs();
@@ -175,7 +162,9 @@ struct gprs_rlcmac_tbf {
 	LListHead<gprs_rlcmac_tbf>& ms_list() {return this->m_ms_list;}
 	const LListHead<gprs_rlcmac_tbf>& ms_list() const {return this->m_ms_list;}
 
-	struct llist_pods list;
+	LListHead<gprs_rlcmac_tbf>& list();
+	const LListHead<gprs_rlcmac_tbf>& list() const;
+
 	uint32_t state_flags;
 	enum gprs_rlcmac_tbf_direction direction;
 	struct gprs_rlcmac_trx *trx;
@@ -193,6 +182,7 @@ struct gprs_rlcmac_tbf {
 
 	enum gprs_rlcmac_tbf_poll_state poll_state;
 	uint32_t poll_fn; /* frame number to poll */
+	uint8_t poll_ts; /* TS to poll */
 
 	gprs_rlc m_rlc;
 	
@@ -238,11 +228,10 @@ struct gprs_rlcmac_tbf {
 protected:
 	gprs_rlcmac_bts *bts_data() const;
 
-	int extract_tlli(const uint8_t *data, const size_t len);
 	int set_tlli_from_ul(uint32_t new_tlli);
 	void merge_and_clear_ms(GprsMs *old_ms);
 
-	static const char *tbf_state_name[6];
+	static const char *tbf_state_name[7];
 
 	class GprsMs *m_ms;
 
@@ -251,6 +240,7 @@ protected:
 	uint8_t m_ms_class;
 
 private:
+	LListHead<gprs_rlcmac_tbf> m_list;
 	LListHead<gprs_rlcmac_tbf> m_ms_list;
 	bool m_egprs_enabled;
 
@@ -302,6 +292,16 @@ inline void gprs_rlcmac_tbf::set_state(enum gprs_rlcmac_tbf_state new_state)
 	state = new_state;
 }
 
+inline LListHead<gprs_rlcmac_tbf>& gprs_rlcmac_tbf::list()
+{
+	return this->m_list;
+}
+
+inline const LListHead<gprs_rlcmac_tbf>& gprs_rlcmac_tbf::list() const
+{
+	return this->m_list;
+}
+
 inline GprsMs *gprs_rlcmac_tbf::ms() const
 {
 	return m_ms;
@@ -312,6 +312,13 @@ inline bool gprs_rlcmac_tbf::is_tlli_valid() const
 	return tlli() != 0;
 }
 
+inline bool gprs_rlcmac_tbf::is_tfi_assigned() const
+{
+	/* The TBF is established or has been assigned by a IMM.ASS for
+	 * download */
+	return state > GPRS_RLCMAC_ASSIGN;
+}
+
 inline uint8_t gprs_rlcmac_tbf::tfi() const
 {
 	return m_tfi;
@@ -354,6 +361,7 @@ struct gprs_rlcmac_dl_tbf : public gprs_rlcmac_tbf {
 			const uint8_t *data, const uint16_t len);
 
 	int rcvd_dl_ack(uint8_t final, uint8_t ssn, uint8_t *rbb);
+	int rcvd_dl_ack(uint8_t final_ack, unsigned first_bsn, struct bitvec *rbb);
 	struct msgb *create_dl_acked_block(uint32_t fn, uint8_t ts);
 	void request_dl_ack();
 	bool need_control_ts() const;
@@ -362,11 +370,7 @@ struct gprs_rlcmac_dl_tbf : public gprs_rlcmac_tbf {
 	int frames_since_last_drain(unsigned fn) const;
 	bool keep_open(unsigned fn) const;
 	int release();
-	void enable_egprs();
-
-	bool is_control_ts(uint8_t ts) const {
-		return ts == control_ts;
-	}
+	int abort();
 
 	/* TODO: add the gettimeofday as parameter */
 	struct msgb *llc_dequeue(bssgp_bvc_ctx *bctx);
@@ -402,10 +406,14 @@ protected:
 		unsigned lost_bytes;
 	};
 
-	struct msgb *create_new_bsn(const uint32_t fn, const uint8_t ts);
+	int take_next_bsn(uint32_t fn, int previous_bsn,
+		bool *may_combine);
+	bool restart_bsn_cycle();
+	int create_new_bsn(const uint32_t fn, GprsCodingScheme cs);
 	struct msgb *create_dl_acked_block(const uint32_t fn, const uint8_t ts,
-					const int index);
+					int index, int index2 = -1);
 	int update_window(const uint8_t ssn, const uint8_t *rbb);
+	int update_window(unsigned first_bsn, const struct bitvec *rbb);
 	int maybe_start_new_window();
 	bool dl_window_stalled() const;
 	void reuse_tbf();
@@ -419,12 +427,12 @@ protected:
 struct gprs_rlcmac_ul_tbf : public gprs_rlcmac_tbf {
 	gprs_rlcmac_ul_tbf(BTS *bts);
 
-	struct msgb *create_ul_ack(uint32_t fn);
+	struct msgb *create_ul_ack(uint32_t fn, uint8_t ts);
 
 	/* blocks were acked */
 	int rcv_data_block_acknowledged(
-		const struct gprs_rlc_ul_header_egprs *rlc,
-		uint8_t *data, uint8_t len, struct pcu_l1_meas *meas);
+		const struct gprs_rlc_data_info *rlc,
+		uint8_t *data, struct pcu_l1_meas *meas);
 
 
 	/* TODO: extract LLC class? */
@@ -444,7 +452,7 @@ struct gprs_rlcmac_ul_tbf : public gprs_rlcmac_tbf {
 	uint8_t m_final_ack_sent; /* set if we sent final ack */
 
 protected:
-	void maybe_schedule_uplink_acknack(const gprs_rlc_ul_header_egprs *rlc);
+	void maybe_schedule_uplink_acknack(const gprs_rlc_data_info *rlc);
 };
 
 inline enum gprs_rlcmac_tbf_direction reverse(enum gprs_rlcmac_tbf_direction dir)
@@ -452,15 +460,31 @@ inline enum gprs_rlcmac_tbf_direction reverse(enum gprs_rlcmac_tbf_direction dir
 	return (enum gprs_rlcmac_tbf_direction)
 		((int)GPRS_RLCMAC_UL_TBF - (int)dir + (int)GPRS_RLCMAC_DL_TBF);
 }
-#endif
 
-#ifdef __cplusplus
-extern "C" {
-#endif
-#include <osmocom/vty/command.h>
-#include <osmocom/vty/vty.h>
+inline gprs_rlcmac_ul_tbf *as_ul_tbf(gprs_rlcmac_tbf *tbf)
+{
+	if (tbf && tbf->direction == GPRS_RLCMAC_UL_TBF)
+		return static_cast<gprs_rlcmac_ul_tbf *>(tbf);
+	else
+		return NULL;
+}
 
-	void tbf_print_vty_info(struct vty *vty, struct llist_head *tbf);
-#ifdef __cplusplus
+inline gprs_rlcmac_dl_tbf *as_dl_tbf(gprs_rlcmac_tbf *tbf)
+{
+	if (tbf && tbf->direction == GPRS_RLCMAC_DL_TBF)
+		return static_cast<gprs_rlcmac_dl_tbf *>(tbf);
+	else
+		return NULL;
 }
+
+inline gprs_rlc_window *gprs_rlcmac_tbf::window()
+{
+	switch (direction)
+	{
+	case GPRS_RLCMAC_UL_TBF: return &as_ul_tbf(this)->m_window;
+	case GPRS_RLCMAC_DL_TBF: return &as_dl_tbf(this)->m_window;
+	}
+	return NULL;
+}
+
 #endif
diff --git a/src/tbf_dl.cpp b/src/tbf_dl.cpp
index 70194f7c..7540d1b3 100644
--- a/src/tbf_dl.cpp
+++ b/src/tbf_dl.cpp
@@ -27,6 +27,7 @@
 #include <gprs_bssgp_pcu.h>
 #include <gprs_codel.h>
 #include <decoding.h>
+#include <encoding.h>
 
 #include "pcu_utils.h"
 
@@ -180,7 +181,7 @@ static int tbf_new_dl_assignment(struct gprs_rlcmac_bts *bts,
  */
 int gprs_rlcmac_dl_tbf::handle(struct gprs_rlcmac_bts *bts,
 		const uint32_t tlli, const uint32_t tlli_old, const char *imsi,
-		const uint8_t ms_class, const uint8_t egprs_ms_class,
+		uint8_t ms_class, uint8_t egprs_ms_class,
 		const uint16_t delay_csec,
 		const uint8_t *data, const uint16_t len)
 {
@@ -190,9 +191,16 @@ int gprs_rlcmac_dl_tbf::handle(struct gprs_rlcmac_bts *bts,
 
 	/* check for existing TBF */
 	ms = bts->bts->ms_store().get_ms(tlli, tlli_old, imsi);
-	if (ms)
+	if (ms) {
 		dl_tbf = ms->dl_tbf();
 
+		/* If we known the GPRS/EGPRS MS class, use it */
+		if (ms->ms_class() || ms->egprs_ms_class()) {
+			ms_class = ms->ms_class();
+			egprs_ms_class = ms->egprs_ms_class();
+		}
+	}
+
 	if (ms && strlen(ms->imsi()) == 0) {
 		ms_old = bts->bts->ms_store().get_ms(0, 0, imsi);
 		if (ms_old && ms_old != ms) {
@@ -318,81 +326,139 @@ drop_frame:
 	return msg;
 }
 
-/*
- * Create DL data block
- * The messages are fragmented and forwarded as data blocks.
- */
-struct msgb *gprs_rlcmac_dl_tbf::create_dl_acked_block(uint32_t fn, uint8_t ts)
+bool gprs_rlcmac_dl_tbf::restart_bsn_cycle()
 {
-	LOGP(DRLCMACDL, LOGL_DEBUG, "%s downlink (V(A)==%d .. "
-		"V(S)==%d)\n", tbf_name(this),
-		m_window.v_a(), m_window.v_s());
+	/* If V(S) == V(A) and finished state, we would have received
+	 * acknowledgement of all transmitted block.  In this case we would
+	 * have transmitted the final block, and received ack from MS. But in
+	 * this case we did not receive the final ack indication from MS.  This
+	 * should never happen if MS works correctly.
+	 */
+	if (m_window.window_empty()) {
+		LOGP(DRLCMACDL, LOGL_DEBUG, "- MS acked all blocks\n");
+		return false;
+	}
 
-do_resend:
-	/* check if there is a block with negative acknowledgement */
-	int resend_bsn = m_window.resend_needed();
-	if (resend_bsn >= 0) {
-		LOGP(DRLCMACDL, LOGL_DEBUG, "- Resending BSN %d\n", resend_bsn);
-		/* re-send block with negative aknowlegement */
-		m_window.m_v_b.mark_unacked(resend_bsn);
-		bts->rlc_resent();
-		return create_dl_acked_block(fn, ts, resend_bsn);
+	/* cycle through all unacked blocks */
+	int resend = m_window.mark_for_resend();
+
+	/* At this point there should be at least one unacked block
+	 * to be resent. If not, this is an software error. */
+	if (resend == 0) {
+		LOGP(DRLCMACDL, LOGL_ERROR, "Software error: "
+			"There are no unacknowledged blocks, but V(A) "
+			" != V(S). PLEASE FIX!\n");
+		return false;
 	}
 
-	/* if the window has stalled, or transfer is complete,
-	 * send an unacknowledged block */
-	if (state_is(GPRS_RLCMAC_FINISHED)) {
+	return true;
+}
+
+int gprs_rlcmac_dl_tbf::take_next_bsn(uint32_t fn,
+	int previous_bsn, bool *may_combine)
+{
+	int bsn;
+	int data_len2, force_data_len = -1;
+	GprsCodingScheme cs2;
+	GprsCodingScheme force_cs;
+
+	bsn = m_window.resend_needed();
+
+	if (previous_bsn >= 0) {
+		force_cs = m_rlc.block(previous_bsn)->cs;
+		if (!force_cs.isEgprs())
+			return -1;
+		force_data_len = m_rlc.block(previous_bsn)->len;
+	}
+
+	if (bsn >= 0) {
+		if (previous_bsn == bsn)
+			return -1;
+
+		if (previous_bsn >= 0 &&
+			m_window.mod_sns(bsn - previous_bsn) > RLC_EGPRS_MAX_BSN_DELTA)
+			return -1;
+
+		cs2 = m_rlc.block(bsn)->cs;
+		data_len2 = m_rlc.block(bsn)->len;
+		if (force_data_len > 0 && force_data_len != data_len2)
+			return -1;
+		LOGP(DRLCMACDL, LOGL_DEBUG, "- Resending BSN %d\n", bsn);
+		/* re-send block with negative aknowlegement */
+		m_window.m_v_b.mark_unacked(bsn);
+		bts->rlc_resent();
+	} else if (state_is(GPRS_RLCMAC_FINISHED)) {
 		LOGP(DRLCMACDL, LOGL_DEBUG, "- Restarting at BSN %d, "
 			"because all blocks have been transmitted.\n",
 			m_window.v_a());
 		bts->rlc_restarted();
+		if (restart_bsn_cycle())
+			return take_next_bsn(fn, previous_bsn, may_combine);
 	} else if (dl_window_stalled()) {
 		LOGP(DRLCMACDL, LOGL_NOTICE, "- Restarting at BSN %d, "
-			"because all window is stalled.\n",
+			"because the window is stalled.\n",
 			m_window.v_a());
 		bts->rlc_stalled();
+		if (restart_bsn_cycle())
+			return take_next_bsn(fn, previous_bsn, may_combine);
 	} else if (have_data()) {
 		/* New blocks may be send */
-		return create_new_bsn(fn, ts);
+		cs2 = force_cs ? force_cs : current_cs();
+		LOGP(DRLCMACDL, LOGL_DEBUG,
+			"- Sending new block at BSN %d, CS=%s\n",
+			m_window.v_s(), cs2.name());
+
+		bsn = create_new_bsn(fn, cs2);
 	} else if (!m_window.window_empty()) {
 		LOGP(DRLCMACDL, LOGL_DEBUG, "- Restarting at BSN %d, "
 			"because all blocks have been transmitted (FLOW).\n",
 			m_window.v_a());
 		bts->rlc_restarted();
+		if (restart_bsn_cycle())
+			return take_next_bsn(fn, previous_bsn, may_combine);
 	} else {
 		/* Nothing left to send, create dummy LLC commands */
-		return create_new_bsn(fn, ts);
+		LOGP(DRLCMACDL, LOGL_DEBUG,
+			"- Sending new dummy block at BSN %d, CS=%s\n",
+			m_window.v_s(), current_cs().name());
+		bsn = create_new_bsn(fn, current_cs());
+		/* Don't send a second block, so don't set cs2 */
 	}
 
-	/* If V(S) == V(A) and finished state, we would have received
-	 * acknowledgement of all transmitted block. In this case we
-	 * would have transmitted the final block, and received ack
-	 * from MS. But in this case we did not receive the final ack
-	 * indication from MS. This should never happen if MS works
-	 * correctly. */
-	if (m_window.window_empty()) {
-		LOGP(DRLCMACDL, LOGL_DEBUG, "- MS acked all blocks, "
-			"so we re-transmit final block!\n");
+	if (bsn < 0) {
 		/* we just send final block again */
-		int16_t index = m_window.v_s_mod(-1);
+		LOGP(DRLCMACDL, LOGL_DEBUG,
+			"- Nothing else to send, Re-transmit final block!\n");
+		bsn = m_window.v_s_mod(-1);
 		bts->rlc_resent();
-		return create_dl_acked_block(fn, ts, index);
 	}
 
-	/* cycle through all unacked blocks */
-	int resend = m_window.mark_for_resend();
+	*may_combine = cs2.numDataBlocks() > 1;
 
-	/* At this point there should be at least one unacked block
-	 * to be resent. If not, this is an software error. */
-	if (resend == 0) {
-		LOGP(DRLCMACDL, LOGL_ERROR, "Software error: "
-			"There are no unacknowledged blocks, but V(A) "
-			" != V(S). PLEASE FIX!\n");
-		/* we just send final block again */
-		int16_t index = m_window.v_s_mod(-1);
-		return create_dl_acked_block(fn, ts, index);
-	}
-	goto do_resend;
+	return bsn;
+}
+
+/*
+ * Create DL data block
+ * The messages are fragmented and forwarded as data blocks.
+ */
+struct msgb *gprs_rlcmac_dl_tbf::create_dl_acked_block(uint32_t fn, uint8_t ts)
+{
+	int bsn, bsn2 = -1;
+	bool may_combine;
+
+	LOGP(DRLCMACDL, LOGL_DEBUG, "%s downlink (V(A)==%d .. "
+		"V(S)==%d)\n", tbf_name(this),
+		m_window.v_a(), m_window.v_s());
+
+	bsn = take_next_bsn(fn, -1, &may_combine);
+	if (bsn < 0)
+		return NULL;
+
+	if (may_combine)
+		bsn2 = take_next_bsn(fn, bsn, &may_combine);
+
+	return create_dl_acked_block(fn, ts, bsn, bsn2);
 }
 
 void gprs_rlcmac_dl_tbf::schedule_next_frame()
@@ -417,55 +483,43 @@ void gprs_rlcmac_dl_tbf::schedule_next_frame()
 	m_last_dl_drained_fn = -1;
 }
 
-struct msgb *gprs_rlcmac_dl_tbf::create_new_bsn(const uint32_t fn, const uint8_t ts)
+int gprs_rlcmac_dl_tbf::create_new_bsn(const uint32_t fn, GprsCodingScheme cs)
 {
-	struct rlc_dl_header *rh;
-	struct rlc_li_field *li;
-	uint8_t *delimiter, *data, *e_pointer;
-	uint16_t space, chunk;
+	uint8_t *data;
 	gprs_rlc_data *rlc_data;
 	const uint16_t bsn = m_window.v_s();
-	uint8_t cs_n = 1;
+	gprs_rlc_data_block_info *rdbi;
+	int num_chunks = 0;
+	int write_offset = 0;
+	Encoding::AppendResult ar;
 
 	if (m_llc.frame_length() == 0)
 		schedule_next_frame();
 
-	cs_n = current_cs();
-
-	LOGP(DRLCMACDL, LOGL_DEBUG, "- Sending new block at BSN %d, CS=%d\n",
-		m_window.v_s(), cs_n);
+	OSMO_ASSERT(cs.isValid());
 
-	OSMO_ASSERT(cs_n >= 1);
-	OSMO_ASSERT(cs_n <= 4);
-
-	/* TODO: Use GprsCodingScheme everywhere and remove cast */
-	GprsCodingScheme cs((GprsCodingScheme::Scheme)cs_n);
-
-	/* total length of block, including spare bits */
-	const uint8_t block_length = cs.sizeDL();
-	/* length of usable data of block, w/o spare bits, inc. MAC */
-	const uint8_t block_data_len = cs.maxBytesDL();
+	/* length of usable data block (single data unit w/o header) */
+	const uint8_t block_data_len = cs.maxDataBlockBytes();
 
 	/* now we still have untransmitted LLC data, so we fill mac block */
 	rlc_data = m_rlc.block(bsn);
 	data = rlc_data->prepare(block_data_len);
 	rlc_data->cs = cs;
+	rlc_data->len = block_data_len;
+
+	rdbi = &(rlc_data->block_info);
+	memset(rdbi, 0, sizeof(*rdbi));
+	rdbi->data_len = block_data_len;
+
+	rdbi->cv = 15; /* Final Block Indicator, set late, if true */
+	rdbi->bsn = bsn; /* Block Sequence Number */
+	rdbi->e = 1; /* Extension bit, maybe set later (1: no extension) */
+
+	do {
+		bool is_final;
 
-	rh = (struct rlc_dl_header *)data;
-	rh->pt = 0; /* Data Block */
-	rh->rrbp = rh->s_p = 0; /* Polling, set later, if required */
-	rh->usf = 7; /* will be set at scheduler */
-	rh->pr = 0; /* FIXME: power reduction */
-	rh->tfi = m_tfi; /* TFI */
-	rh->fbi = 0; /* Final Block Indicator, set late, if true */
-	rh->bsn = bsn; /* Block Sequence Number */
-	rh->e = 0; /* Extension bit, maybe set later */
-	e_pointer = data + 2; /* points to E of current chunk */
-	data += sizeof(*rh);
-	delimiter = data; /* where next length header would be stored */
-	space = block_data_len - sizeof(*rh);
-	while (1) {
 		if (m_llc.frame_length() == 0) {
+			int space = block_data_len - write_offset;
 			/* A header will need to by added, so we just need
 			 * space-1 octets */
 			m_llc.put_dummy_frame(space - 1);
@@ -485,141 +539,145 @@ struct msgb *gprs_rlcmac_dl_tbf::create_new_bsn(const uint32_t fn, const uint8_t
 				m_llc.frame_length(), frames_since_last_drain(fn));
 		}
 
-		chunk = m_llc.chunk_size();
-
-		/* if chunk will exceed block limit */
-		if (chunk > space) {
-			LOGP(DRLCMACDL, LOGL_DEBUG, "-- Chunk with length %d "
-				"larger than space (%d) left in block: copy "
-				"only remaining space, and we are done\n",
-				chunk, space);
-			/* block is filled, so there is no extension */
-			*e_pointer |= 0x01;
-			/* fill only space */
-			m_llc.consume(data, space);
-			/* return data block as message */
-			break;
-		}
-		/* if FINAL chunk would fit precisely in space left */
-		if (chunk == space && llc_queue()->size() == 0 && !keep_open(fn))
-		{
-			LOGP(DRLCMACDL, LOGL_DEBUG, "-- Chunk with length %d "
-				"would exactly fit into space (%d): because "
-				"this is a final block, we don't add length "
-				"header, and we are done\n", chunk, space);
-			LOGP(DRLCMACDL, LOGL_INFO, "Complete DL frame for "
-				"%s that fits precisely in last block: "
-				"len=%d\n", tbf_name(this), m_llc.frame_length());
-			gprs_rlcmac_dl_bw(this, m_llc.frame_length());
-			/* block is filled, so there is no extension */
-			*e_pointer |= 0x01;
-			/* fill space */
-			m_llc.consume(data, space);
-			m_llc.reset();
-			/* final block */
-			rh->fbi = 1; /* we indicate final block */
-			request_dl_ack();
-			set_state(GPRS_RLCMAC_FINISHED);
-			/* return data block as message */
-			break;
-		}
-		/* if chunk would fit exactly in space left */
-		if (chunk == space) {
-			LOGP(DRLCMACDL, LOGL_DEBUG, "-- Chunk with length %d "
-				"would exactly fit into space (%d): add length "
-				"header with LI=0, to make frame extend to "
-				"next block, and we are done\n", chunk, space);
-			/* make space for delimiter */
-			if (delimiter != data)
-				memmove(delimiter + 1, delimiter,
-					data - delimiter);
-			data++;
-			space--;
-			/* add LI with 0 length */
-			li = (struct rlc_li_field *)delimiter;
-			li->e = 1; /* not more extension */
-			li->m = 0; /* shall be set to 0, in case of li = 0 */
-			li->li = 0; /* chunk fills the complete space */
-			// no need to set e_pointer nor increase delimiter
-			/* fill only space, which is 1 octet less than chunk */
-			m_llc.consume(data, space);
-			/* return data block as message */
+		is_final = llc_queue()->size() == 0 && !keep_open(fn);
+
+		ar = Encoding::rlc_data_to_dl_append(rdbi, cs,
+			&m_llc, &write_offset, &num_chunks, data, is_final);
+
+		if (ar == Encoding::AR_NEED_MORE_BLOCKS)
 			break;
-		}
 
-		LOGP(DRLCMACDL, LOGL_DEBUG, "-- Chunk with length %d is less "
-			"than remaining space (%d): add length header to "
-			"to delimit LLC frame\n", chunk, space);
-		/* the LLC frame chunk ends in this block */
-		/* make space for delimiter */
-		if (delimiter != data)
-			memmove(delimiter + 1, delimiter, data - delimiter);
-		data++;
-		space--;
-		/* add LI to delimit frame */
-		li = (struct rlc_li_field *)delimiter;
-		li->e = 0; /* Extension bit, maybe set later */
-		li->m = 0; /* will be set later, if there is more LLC data */
-		li->li = chunk; /* length of chunk */
-		e_pointer = delimiter; /* points to E of current delimiter */
-		delimiter++;
-		/* copy (rest of) LLC frame to space and reset later */
-		m_llc.consume(data, chunk);
-		data += chunk;
-		space -= chunk;
 		LOGP(DRLCMACDL, LOGL_INFO, "Complete DL frame for %s"
 			"len=%d\n", tbf_name(this), m_llc.frame_length());
 		gprs_rlcmac_dl_bw(this, m_llc.frame_length());
 		m_llc.reset();
-		/* dequeue next LLC frame, if any */
-		schedule_next_frame();
-		/* if we have more data and we have space left */
-		if (space > 0 && (m_llc.frame_length() || keep_open(fn))) {
-			li->m = 1; /* we indicate more frames to follow */
-			continue;
-		}
-		/* if we don't have more LLC frames */
-		if (!m_llc.frame_length() && !keep_open(fn)) {
-			LOGP(DRLCMACDL, LOGL_DEBUG, "-- Final block, so we "
-				"done.\n");
-			li->e = 1; /* we cannot extend */
 
-			rh->fbi = 1; /* we indicate final block */
+		if (is_final) {
 			request_dl_ack();
 			set_state(GPRS_RLCMAC_FINISHED);
-			break;
 		}
-		/* we have no space left */
-		LOGP(DRLCMACDL, LOGL_DEBUG, "-- No space left, so we are "
-			"done.\n");
-		li->e = 1; /* we cannot extend */
-		break;
-	}
-	LOGP(DRLCMACDL, LOGL_DEBUG, "data block: %s\n",
-		osmo_hexdump(rlc_data->block, block_length));
-#warning "move this up?"
-	rlc_data->len = block_length;
+
+		/* dequeue next LLC frame, if any */
+		schedule_next_frame();
+	} while (ar == Encoding::AR_COMPLETED_SPACE_LEFT);
+
+	LOGP(DRLCMACDL, LOGL_DEBUG, "data block (BSN %d, %s): %s\n",
+		bsn, rlc_data->cs.name(),
+		osmo_hexdump(rlc_data->block, block_data_len));
 	/* raise send state and set ack state array */
 	m_window.m_v_b.mark_unacked(bsn);
 	m_window.increment_send();
 
-	return create_dl_acked_block(fn, ts, bsn);
+	return bsn;
 }
 
 struct msgb *gprs_rlcmac_dl_tbf::create_dl_acked_block(
 				const uint32_t fn, const uint8_t ts,
-				const int index)
+				int index, int index2)
 {
-	uint8_t *data;
-	struct rlc_dl_header *rh;
+	uint8_t *msg_data;
 	struct msgb *dl_msg;
-	uint8_t len;
+	unsigned msg_len;
 	bool need_poll;
+	/* TODO: support MCS-7 - MCS-9, where data_block_idx can be 1 */
+	unsigned int data_block_idx = 0;
+	unsigned int rrbp;
+	uint32_t new_poll_fn;
+	int rc;
+	bool is_final = false;
+	gprs_rlc_data_info rlc;
+	GprsCodingScheme cs;
+	int bsns[ARRAY_SIZE(rlc.block_info)];
+	unsigned num_bsns;
+	int punct[ARRAY_SIZE(rlc.block_info)];
+	bool need_padding = false;
+
+	/*
+	 * TODO: This is an experimental work-around to put 2 BSN into
+	 * MSC-7 to MCS-9 encoded messages. It just sends the same BSN
+	 * twice in the block. The cs should be derived from the TBF's
+	 * current CS such that both BSNs (that must be compatible) can
+	 * be put into the data area, even if the resulting CS is higher than
+	 * the current limit.
+	 */
+	cs = m_rlc.block(index)->cs;
+	bsns[0] = index;
+	num_bsns = 1;
+
+	if (index2 >= 0) {
+		bsns[num_bsns] = index2;
+		num_bsns += 1;
+	}
+
+	if (num_bsns == 1) {
+		/* TODO: remove the conditional when MCS-6 padding isn't
+		 * failing to be decoded by MEs anymore */
+		if (cs != GprsCodingScheme(GprsCodingScheme::MCS8))
+			cs.decToSingleBlock(&need_padding);
+	}
 
-	/* get data and header from current block */
-	data = m_rlc.block(index)->block;
-	len = m_rlc.block(index)->len;
-	rh = (struct rlc_dl_header *)data;
+	gprs_rlc_data_info_init_dl(&rlc, cs, need_padding);
+
+	rlc.usf = 7; /* will be set at scheduler */
+	rlc.pr = 0; /* FIXME: power reduction */
+	rlc.tfi = m_tfi; /* TFI */
+
+	/* return data block(s) as message */
+	msg_len = cs.sizeDL();
+	dl_msg = msgb_alloc(msg_len, "rlcmac_dl_data");
+	if (!dl_msg)
+		return NULL;
+
+	msg_data = msgb_put(dl_msg, msg_len);
+
+	/* Copy block(s) to RLC message */
+	for (data_block_idx = 0; data_block_idx < rlc.num_data_blocks;
+		data_block_idx++)
+	{
+		int bsn;
+		GprsCodingScheme cs_enc;
+		uint8_t *block_data;
+		gprs_rlc_data_block_info *rdbi, *block_info;
+
+		/* Check if there are more blocks than BSNs */
+		if (data_block_idx < num_bsns)
+			bsn = bsns[data_block_idx];
+		else
+			bsn = bsns[0];
+
+		cs_enc = m_rlc.block(bsn)->cs;
+
+		/* get data and header from current block */
+		block_data = m_rlc.block(bsn)->block;
+
+		/* TODO: Use real puncturing values */
+		punct[data_block_idx] = data_block_idx;
+
+		rdbi = &rlc.block_info[data_block_idx];
+		block_info = &m_rlc.block(bsn)->block_info;
+
+		if(rdbi->data_len != m_rlc.block(bsn)->len) {
+			LOGP(DRLCMACDL, LOGL_ERROR,
+				"ERROR: Expected len = %d for %s instead of "
+				"%d in data unit %d (BSN %d, %s)\n",
+				rdbi->data_len, cs.name(), m_rlc.block(bsn)->len,
+				data_block_idx, bsn, cs_enc.name());
+			OSMO_ASSERT(rdbi->data_len == m_rlc.block(bsn)->len);
+		}
+		rdbi->e   = block_info->e;
+		rdbi->cv  = block_info->cv;
+		rdbi->bsn = bsn;
+		is_final = is_final || rdbi->cv == 0;
+
+		LOGP(DRLCMACDL, LOGL_DEBUG, "- Copying data unit %d (BSN %d)\n",
+			data_block_idx, bsn);
+
+		Encoding::rlc_copy_from_aligned_buffer(&rlc, data_block_idx,
+			msg_data, block_data);
+	}
+
+	OSMO_ASSERT(ARRAY_SIZE(punct) >= 2);
+	rlc.cps = gprs_rlc_mcs_cps(cs, punct[0], punct[1], need_padding);
 
 	/* If the TBF has just started, relate frames_since_last_poll to the
 	 * current fn */
@@ -627,9 +685,7 @@ struct msgb *gprs_rlcmac_dl_tbf::create_dl_acked_block(
 		m_last_dl_poll_fn = fn;
 
 	need_poll = state_flags & (1 << GPRS_RLCMAC_FLAG_TO_DL_ACK);
-	/* Clear Polling, if still set in history buffer */
-	rh->s_p = 0;
-		
+
 	/* poll after POLL_ACK_AFTER_FRAMES frames, or when final block is tx.
 	 */
 	if (m_tx_counter >= POLL_ACK_AFTER_FRAMES || m_dl_ack_requested ||
@@ -646,31 +702,18 @@ struct msgb *gprs_rlcmac_dl_tbf::create_dl_acked_block(
 				"polling, because %d blocks sent.\n",
 				POLL_ACK_AFTER_FRAMES);
 		}
-		/* scheduling not possible, because: */
-		if (poll_state != GPRS_RLCMAC_POLL_NONE)
-			LOGP(DRLCMACDL, LOGL_DEBUG, "Polling is already "
-				"sheduled for %s, so we must wait for "
-				"requesting downlink ack\n", tbf_name(this));
-		else if (control_ts != ts)
-			LOGP(DRLCMACDL, LOGL_DEBUG, "Polling cannot be "
-				"sheduled in this TS %d, waiting for "
-				"TS %d\n", ts, control_ts);
-		else if (bts->sba()->find(trx->trx_no, ts, (fn + 13) % 2715648))
-			LOGP(DRLCMACDL, LOGL_DEBUG, "Polling cannot be "
-				"sheduled, because single block alllocation "
-				"already exists\n");
-		else  {
-			LOGP(DRLCMACDL, LOGL_DEBUG, "Polling sheduled in this "
+
+		rc = check_polling(fn, ts, &new_poll_fn, &rrbp);
+		if (rc >= 0) {
+			set_polling(new_poll_fn, ts);
+
+			LOGP(DRLCMACDL, LOGL_DEBUG, "Polling scheduled in this "
 				"TS %d\n", ts);
 			m_tx_counter = 0;
 			/* start timer whenever we send the final block */
-			if (rh->fbi == 1)
+			if (is_final)
 				tbf_timer_start(this, 3191, bts_data()->t3191, 0);
 
-			/* schedule polling */
-			poll_state = GPRS_RLCMAC_POLL_SCHED;
-			poll_fn = (fn + 13) % 2715648;
-
 			/* Clear poll timeout flag */
 			state_flags &= ~(1 << GPRS_RLCMAC_FLAG_TO_DL_ACK);
 
@@ -678,34 +721,35 @@ struct msgb *gprs_rlcmac_dl_tbf::create_dl_acked_block(
 			m_dl_ack_requested = false;
 
 			/* set polling in header */
-			rh->rrbp = 0; /* N+13 */
-			rh->s_p = 1; /* Polling */
+			rlc.rrbp = rrbp;
+			rlc.es_p = 1; /* Polling */
 
 			m_last_dl_poll_fn = poll_fn;
 
 			LOGP(DRLCMACDL, LOGL_INFO,
 				"%s Scheduled Ack/Nack polling on FN=%d, TS=%d\n",
-				name(), poll_fn, ts);
+				name(), poll_fn, poll_ts);
 		}
 	}
 
-	/* return data block as message */
-	dl_msg = msgb_alloc(len, "rlcmac_dl_data");
-	if (!dl_msg)
-		return NULL;
+	Encoding::rlc_write_dl_data_header(&rlc, msg_data);
+
+	LOGP(DRLCMACDL, LOGL_DEBUG, "msg block (BSN %d, %s%s): %s\n",
+		index, cs.name(),
+		need_padding ? ", padded" : "",
+		msgb_hexdump(dl_msg));
 
 	/* Increment TX-counter */
 	m_tx_counter++;
 
-	memcpy(msgb_put(dl_msg, len), data, len);
 	bts->rlc_sent();
 
 	return dl_msg;
 }
 
-static uint16_t bitnum_to_bsn(int bitnum, uint16_t ssn, uint16_t mod_sns)
+static uint16_t bitnum_to_bsn(int bitnum, uint16_t ssn)
 {
-	return (ssn - 1 - bitnum) & mod_sns;
+	return ssn - 1 - bitnum;
 }
 
 int gprs_rlcmac_dl_tbf::analyse_errors(char *show_rbb, uint8_t ssn,
@@ -713,20 +757,24 @@ int gprs_rlcmac_dl_tbf::analyse_errors(char *show_rbb, uint8_t ssn,
 {
 	gprs_rlc_data *rlc_data;
 	uint16_t lost = 0, received = 0, skipped = 0;
-	char info[65];
-	memset(info, '.', sizeof(info));
-	info[64] = 0;
+	char info[RLC_MAX_WS + 1];
+	memset(info, '.', m_window.ws());
+	info[m_window.ws()] = 0;
 	uint16_t bsn = 0;
 	unsigned received_bytes = 0, lost_bytes = 0;
 	unsigned received_packets = 0, lost_packets = 0;
+	unsigned num_blocks = strlen(show_rbb);
 
 	/* SSN - 1 is in range V(A)..V(S)-1 */
-	for (int bitpos = 0; bitpos < m_window.ws(); bitpos++) {
-		bool is_received = show_rbb[m_window.ws() - 1 - bitpos] == 'R';
+	for (unsigned int bitpos = 0; bitpos < num_blocks; bitpos++) {
+		bool is_received;
+		int index = num_blocks - 1 - bitpos;
 
-		bsn = bitnum_to_bsn(bitpos, ssn, m_window.mod_sns());
+		is_received = (index >= 0 && show_rbb[index] == 'R');
 
-		if (bsn == ((m_window.v_a() - 1) & m_window.mod_sns())) {
+		bsn = m_window.mod_sns(bitnum_to_bsn(bitpos, ssn));
+
+		if (bsn == m_window.mod_sns(m_window.v_a() - 1)) {
 			info[bitpos] = '$';
 			break;
 		}
@@ -748,8 +796,7 @@ int gprs_rlcmac_dl_tbf::analyse_errors(char *show_rbb, uint8_t ssn,
 
 		/* Get statistics for current CS */
 
-		/* TODO: Use GprsCodingScheme everywhere and remove cast */
-		if (rlc_data->cs != (GprsCodingScheme::Scheme)current_cs()) {
+		if (rlc_data->cs != current_cs()) {
 			/* This block has already been encoded with a different
 			 * CS, so it doesn't help us to decide, whether the
 			 * current CS is ok. Ignore it. */
@@ -787,6 +834,69 @@ int gprs_rlcmac_dl_tbf::analyse_errors(char *show_rbb, uint8_t ssn,
 	return lost * 100 / (lost + received);
 }
 
+int gprs_rlcmac_dl_tbf::update_window(unsigned first_bsn,
+	const struct bitvec *rbb)
+{
+	int16_t dist; /* must be signed */
+	uint16_t lost = 0, received = 0;
+	char show_v_b[RLC_MAX_SNS + 1];
+	char show_rbb[RLC_MAX_SNS + 1];
+	int error_rate;
+	struct ana_result ana_res;
+	unsigned num_blocks = rbb->cur_bit;
+	unsigned behind_last_bsn = m_window.mod_sns(first_bsn + num_blocks);
+
+	Decoding::extract_rbb(rbb, show_rbb);
+	/* show received array in debug */
+	LOGP(DRLCMACDL, LOGL_DEBUG, "- ack:  (BSN=%d)\"%s\""
+		"(BSN=%d)  R=ACK I=NACK\n", first_bsn,
+		show_rbb, m_window.mod_sns(behind_last_bsn - 1));
+
+	/* apply received array to receive state (first_bsn..behind_last_bsn-1) */
+	if (num_blocks > 0) {
+		/* calculate distance of ssn from V(S) */
+		dist = m_window.mod_sns(m_window.v_s() - behind_last_bsn);
+		/* check if distance is less than distance V(A)..V(S) */
+		if (dist >= m_window.distance()) {
+			/* this might happpen, if the downlink assignment
+			 * was not received by ms and the ack refers
+			 * to previous TBF
+			 * FIXME: we should implement polling for
+			 * control ack!
+			 * TODO: check whether this FIXME still makes sense
+			 */
+			LOGP(DRLCMACDL, LOGL_NOTICE, "- ack range is out of "
+				"V(A)..V(S) range %s Free TBF!\n", tbf_name(this));
+			return 1; /* indicate to free TBF */
+		}
+	}
+
+	error_rate = analyse_errors(show_rbb, behind_last_bsn, &ana_res);
+
+	if (bts_data()->cs_adj_enabled && ms())
+		ms()->update_error_rate(this, error_rate);
+
+	m_window.update(bts, rbb, first_bsn, &lost, &received);
+
+	/* report lost and received packets */
+	gprs_rlcmac_received_lost(this, received, lost);
+
+	/* Used to measure the leak rate */
+	gprs_bssgp_update_bytes_received(ana_res.received_bytes,
+		ana_res.received_packets + ana_res.lost_packets);
+
+	/* raise V(A), if possible */
+	m_window.raise(m_window.move_window());
+
+	/* show receive state array in debug (V(A)..V(S)-1) */
+	m_window.show_state(show_v_b);
+	LOGP(DRLCMACDL, LOGL_DEBUG, "- V(B): (V(A)=%d)\"%s\""
+		"(V(S)-1=%d)  A=Acked N=Nacked U=Unacked "
+		"X=Resend-Unacked I=Invalid\n",
+		m_window.v_a(), show_v_b,
+		m_window.v_s_mod(-1));
+	return 0;
+}
 
 int gprs_rlcmac_dl_tbf::update_window(const uint8_t ssn, const uint8_t *rbb)
 {
@@ -794,19 +904,18 @@ int gprs_rlcmac_dl_tbf::update_window(const uint8_t ssn, const uint8_t *rbb)
 	uint16_t lost = 0, received = 0;
 	char show_rbb[65];
 	char show_v_b[RLC_MAX_SNS + 1];
-	const uint16_t mod_sns = m_window.mod_sns();
 	int error_rate;
 	struct ana_result ana_res;
 
 	Decoding::extract_rbb(rbb, show_rbb);
 	/* show received array in debug (bit 64..1) */
 	LOGP(DRLCMACDL, LOGL_DEBUG, "- ack:  (BSN=%d)\"%s\""
-		"(BSN=%d)  R=ACK I=NACK\n", (ssn - 64) & mod_sns,
-		show_rbb, (ssn - 1) & mod_sns);
+		"(BSN=%d)  R=ACK I=NACK\n", m_window.mod_sns(ssn - 64),
+		show_rbb, m_window.mod_sns(ssn - 1));
 
 	/* apply received array to receive state (SSN-64..SSN-1) */
 	/* calculate distance of ssn from V(S) */
-	dist = (m_window.v_s() - ssn) & mod_sns;
+	dist = m_window.mod_sns(m_window.v_s() - ssn);
 	/* check if distance is less than distance V(A)..V(S) */
 	if (dist >= m_window.distance()) {
 		/* this might happpen, if the downlink assignment
@@ -895,6 +1004,53 @@ int gprs_rlcmac_dl_tbf::release()
 	return 0;
 }
 
+int gprs_rlcmac_dl_tbf::abort()
+{
+	uint16_t lost;
+
+	if (state_is(GPRS_RLCMAC_FLOW)) {
+		/* range V(A)..V(S)-1 */
+		lost = m_window.count_unacked();
+
+		/* report all outstanding packets as lost */
+		gprs_rlcmac_received_lost(this, 0, lost);
+		gprs_rlcmac_lost_rep(this);
+
+		/* TODO: Reschedule all LLC frames starting with the one that is
+		 * (partly) encoded in chunk 1 of block V(A). (optional) */
+	}
+
+	set_state(GPRS_RLCMAC_RELEASING);
+
+	/* reset rlc states */
+	m_window.reset();
+
+	/* keep to flags */
+	state_flags &= GPRS_RLCMAC_FLAG_TO_MASK;
+	state_flags &= ~(1 << GPRS_RLCMAC_FLAG_CCCH);
+
+	return 0;
+}
+
+int gprs_rlcmac_dl_tbf::rcvd_dl_ack(uint8_t final_ack, unsigned first_bsn,
+	struct bitvec *rbb)
+{
+	int rc;
+	LOGP(DRLCMACDL, LOGL_DEBUG, "%s downlink acknowledge\n", tbf_name(this));
+
+	rc = update_window(first_bsn, rbb);
+
+	if (final_ack) {
+		LOGP(DRLCMACDL, LOGL_DEBUG, "- Final ACK received.\n");
+		rc = maybe_start_new_window();
+	} else if (state_is(GPRS_RLCMAC_FINISHED) && m_window.window_empty()) {
+		LOGP(DRLCMACDL, LOGL_NOTICE, "Received acknowledge of "
+			"all blocks, but without final ack "
+			"indication (don't worry)\n");
+	}
+
+	return rc;
+}
 
 int gprs_rlcmac_dl_tbf::rcvd_dl_ack(uint8_t final_ack, uint8_t ssn, uint8_t *rbb)
 {
diff --git a/src/tbf_ul.cpp b/src/tbf_ul.cpp
index a133d3e7..9e763f4c 100644
--- a/src/tbf_ul.cpp
+++ b/src/tbf_ul.cpp
@@ -50,7 +50,7 @@ int gprs_rlcmac_ul_tbf::assemble_forward_llc(const gprs_rlc_data *_data)
 {
 	const uint8_t *data = _data->block;
 	uint8_t len = _data->len;
-	const struct gprs_rlc_ul_data_block_info *rdbi = &_data->block_info;
+	const struct gprs_rlc_data_block_info *rdbi = &_data->block_info;
 	GprsCodingScheme cs = _data->cs;
 
 	Decoding::RlcData frames[16], *frame;
@@ -87,23 +87,26 @@ int gprs_rlcmac_ul_tbf::assemble_forward_llc(const gprs_rlc_data *_data)
 }
 
 
-struct msgb *gprs_rlcmac_ul_tbf::create_ul_ack(uint32_t fn)
+struct msgb *gprs_rlcmac_ul_tbf::create_ul_ack(uint32_t fn, uint8_t ts)
 {
 	int final = (state_is(GPRS_RLCMAC_FINISHED));
 	struct msgb *msg;
+	int rc;
+	unsigned int rrbp = 0;
+	uint32_t new_poll_fn = 0;
 
 	if (final) {
-		if (poll_state != GPRS_RLCMAC_POLL_NONE) {
+		if (poll_state == GPRS_RLCMAC_POLL_SCHED &&
+			ul_ack_state == GPRS_RLCMAC_UL_ACK_WAIT_ACK) {
 			LOGP(DRLCMACUL, LOGL_DEBUG, "Polling is already "
-				"sheduled for %s, so we must wait for "
-				"final uplink ack...\n", tbf_name(this));
+				"scheduled for %s, so we must wait for "
+				"the final uplink ack...\n", tbf_name(this));
 			return NULL;
 		}
-		if (bts->sba()->find(trx->trx_no, control_ts, (fn + 13) % 2715648)) {
-			LOGP(DRLCMACUL, LOGL_DEBUG, "Polling is already "
-				"scheduled for single block allocation...\n");
+
+		rc = check_polling(fn, ts, &new_poll_fn, &rrbp);
+		if (rc < 0)
 			return NULL;
-		}
 	}
 
 	msg = msgb_alloc(23, "rlcmac_ul_ack");
@@ -116,20 +119,16 @@ struct msgb *gprs_rlcmac_ul_tbf::create_ul_ack(uint32_t fn)
 	}
 	bitvec_unhex(ack_vec,
 		"2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b");
-	RlcMacDownlink_t * mac_control_block = (RlcMacDownlink_t *)talloc_zero(tall_pcu_ctx, RlcMacDownlink_t);
-	Encoding::write_packet_uplink_ack(bts_data(), mac_control_block, this, final);
-	encode_gsm_rlcmac_downlink(ack_vec, mac_control_block);
+	Encoding::write_packet_uplink_ack(bts_data(), ack_vec, this, final, rrbp);
 	bitvec_pack(ack_vec, msgb_put(msg, 23));
 	bitvec_free(ack_vec);
-	talloc_free(mac_control_block);
 
 	/* now we must set this flag, so we are allowed to assign downlink
 	 * TBF on PACCH. it is only allowed when TLLI is acknowledged. */
 	m_contention_resolution_done = 1;
 
 	if (final) {
-		poll_state = GPRS_RLCMAC_POLL_SCHED;
-		poll_fn = (fn + 13) % 2715648;
+		set_polling(new_poll_fn, ts);
 		/* waiting for final acknowledge */
 		ul_ack_state = GPRS_RLCMAC_UL_ACK_WAIT_ACK;
 		m_final_ack_sent = 1;
@@ -140,12 +139,11 @@ struct msgb *gprs_rlcmac_ul_tbf::create_ul_ack(uint32_t fn)
 }
 
 int gprs_rlcmac_ul_tbf::rcv_data_block_acknowledged(
-	const struct gprs_rlc_ul_header_egprs *rlc,
-	uint8_t *data, uint8_t len, struct pcu_l1_meas *meas)
+	const struct gprs_rlc_data_info *rlc,
+	uint8_t *data, struct pcu_l1_meas *meas)
 {
 	int8_t rssi = meas->have_rssi ? meas->rssi : 0;
 
-	const uint16_t mod_sns = m_window.mod_sns();
 	const uint16_t ws = m_window.ws();
 
 	this->state_flags |= (1 << GPRS_RLCMAC_FLAG_UL_DATA);
@@ -174,7 +172,7 @@ int gprs_rlcmac_ul_tbf::rcv_data_block_acknowledged(
 	for (block_idx = 0; block_idx < rlc->num_data_blocks; block_idx++) {
 		int num_chunks;
 		uint8_t *rlc_data;
-		const struct gprs_rlc_ul_data_block_info *rdbi =
+		const struct gprs_rlc_data_block_info *rdbi =
 			&rlc->block_info[block_idx];
 		bool need_rlc_data = false;
 		struct gprs_rlc_data *block;
@@ -194,7 +192,7 @@ int gprs_rlcmac_ul_tbf::rcv_data_block_acknowledged(
 			LOGP(DRLCMACUL, LOGL_DEBUG, "- BSN %d out of window "
 				"%d..%d (it's normal)\n", rdbi->bsn,
 				m_window.v_q(),
-				(m_window.v_q() + ws - 1) & mod_sns);
+				m_window.mod_sns(m_window.v_q() + ws - 1));
 		} else if (m_window.is_received(rdbi->bsn)) {
 			LOGP(DRLCMACUL, LOGL_DEBUG,
 				"- BSN %d already received\n", rdbi->bsn);
@@ -219,7 +217,7 @@ int gprs_rlcmac_ul_tbf::rcv_data_block_acknowledged(
 
 		LOGP(DRLCMACUL, LOGL_DEBUG, "- BSN %d storing in window (%d..%d)\n",
 			rdbi->bsn, m_window.v_q(),
-			(m_window.v_q() + ws - 1) & mod_sns);
+			m_window.mod_sns(m_window.v_q() + ws - 1));
 		block = m_rlc.block(rdbi->bsn);
 		block->block_info = *rdbi;
 		block->cs = rlc->cs;
@@ -242,6 +240,9 @@ int gprs_rlcmac_ul_tbf::rcv_data_block_acknowledged(
 			Decoding::rlc_copy_to_aligned_buffer(rlc, block_idx, data,
 				rlc_data);
 
+		LOGP(DRLCMACUL, LOGL_DEBUG,
+			"%s: data_length=%d, data=%s\n",
+			name(), block->len, osmo_hexdump(rlc_data, block->len));
 
 		/* TODO: Handle SPB != 0 -> set state to partly received
 		 * (upper/lower) and continue with the loop, unless the other
@@ -293,7 +294,7 @@ int gprs_rlcmac_ul_tbf::rcv_data_block_acknowledged(
 
 	/* Retrieve LLC frames from blocks that are ready */
 	for (uint16_t i = 0; i < count; ++i) {
-		uint16_t index = (v_q_beg + i) & mod_sns;
+		uint16_t index = m_window.mod_sns(v_q_beg + i);
 		assemble_forward_llc(m_rlc.block(index));
 	}
 
@@ -301,8 +302,8 @@ int gprs_rlcmac_ul_tbf::rcv_data_block_acknowledged(
 	if (this->state_is(GPRS_RLCMAC_FLOW) /* still in flow state */
 	 && this->m_window.v_q() == this->m_window.v_r()) { /* if complete */
 		struct gprs_rlc_data *block =
-			m_rlc.block((m_window.v_r() - 1) & mod_sns);
-		const struct gprs_rlc_ul_data_block_info *rdbi =
+			m_rlc.block(m_window.mod_sns(m_window.v_r() - 1));
+		const struct gprs_rlc_data_block_info *rdbi =
 			&block->block_info;
 
 		LOGP(DRLCMACUL, LOGL_DEBUG, "- No gaps in received block, "
@@ -325,7 +326,7 @@ int gprs_rlcmac_ul_tbf::rcv_data_block_acknowledged(
 }
 
 void gprs_rlcmac_ul_tbf::maybe_schedule_uplink_acknack(
-	const gprs_rlc_ul_header_egprs *rlc)
+	const gprs_rlc_data_info *rlc)
 {
 	bool have_ti = rlc->block_info[0].ti ||
 		(rlc->num_data_blocks > 1 && rlc->block_info[1].ti);