diff --git a/pcu/GPRS_Components.ttcn b/pcu/GPRS_Components.ttcn
index 3eb3f9f..6018b7b 100644
--- a/pcu/GPRS_Components.ttcn
+++ b/pcu/GPRS_Components.ttcn
@@ -333,14 +333,32 @@
 	f_pcuif_tx_data_ind(data, ms.lqual_cb, fn);
 }
 
-function f_ms_tx_ul_block(inout GprsMS ms, template (value) RlcmacUlBlock ul_data, uint32_t fn := 0)
+function f_ms_tx_ul_block(inout GprsMS ms, template (value) RlcmacUlBlock ul_data, uint32_t fn := 0, template (omit) CodingScheme force_cs_mcs := omit)
 runs on MS_BTS_IFACE_CT return integer {
 	var octetstring data;
 	var integer padding_len;
+	var CodingScheme cs_mcs;
+	var uint32_t cs_mcs_len;
+
 	/* Encode the payload of DATA.ind */
 	data := enc_RlcmacUlBlock(valueof(ul_data));
-	padding_len := 23 - lengthof(data);
-	data := f_pad_oct(data, 23, '00'O); /* CS-1 */
+
+	if (ispresent(force_cs_mcs)) {
+		cs_mcs := valueof(force_cs_mcs);
+	} else if (ischosen(ul_data.ctrl)) {
+		cs_mcs := CS_1; /* CTRL is always CS1 */
+	} else {
+		/* Add padding to encode payload to minimum required CS/MCS: */
+		cs_mcs := f_rlcmac_block_len_required_cs_mcs(lengthof(data), ischosen(ul_data.data_egprs));
+	}
+
+	cs_mcs_len := f_rlcmac_cs_mcs2block_len(cs_mcs);
+	padding_len := cs_mcs_len - lengthof(data);
+	if (padding_len < 0) {
+		setverdict(fail, "Unable to encode UL block of size ", lengthof(data), " with ", cs_mcs);
+		f_shutdown(__BFILE__, __LINE__);
+	}
+	data := f_pad_oct(data, cs_mcs_len, '00'O);
 
 	/* Enqueue DATA.ind (both TDMA frame and block numbers to be patched) */
 	f_ms_tx_data_ind(ms, data, fn);