diff --git a/src/gprs_rlcmac_ts_alloc.cpp b/src/gprs_rlcmac_ts_alloc.cpp
index 8ab3b93..0955411 100644
--- a/src/gprs_rlcmac_ts_alloc.cpp
+++ b/src/gprs_rlcmac_ts_alloc.cpp
@@ -790,6 +790,44 @@
 	return sl;
 }
 
+/*! Allocate USF according to a given UL TS mapping
+ *
+ * N. B: this is legacy implementation which ignores given selected_ul_slots
+ *  \param[in] trx Pointer to TRX object
+ *  \param[in] tbf Pointer to TBF object
+ *  \param[in] first_common_ts First TS which is common to both UL and DL
+ *  \param[in] selected_ul_slots set of UL timeslots selected for allocation
+ *  \param[in] dl_slots set of DL timeslots
+ *  \param[out] usf array for allocated USF
+ *  \returns updated UL TS or negative on error
+ */
+static int allocate_usf(const gprs_rlcmac_trx *trx, int8_t first_common_ts, uint8_t selected_ul_slots, uint8_t dl_slots,
+			int *usf)
+{
+	int free_usf = -1, ts;
+	uint8_t ul_slots = selected_ul_slots;
+
+	if (first_common_ts >= 0)
+		ul_slots = 1 << first_common_ts;
+	else
+		ul_slots = ul_slots & dl_slots;
+
+	ts = find_least_busy_pdch(trx, GPRS_RLCMAC_UL_TBF, ul_slots, compute_usage_by_num_tbfs, NULL, &free_usf);
+
+	if (free_usf < 0 || ts < 0) {
+		LOGP(DRLCMAC, LOGL_NOTICE, "No USF available\n");
+		return -EBUSY;
+	}
+
+	OSMO_ASSERT(ts >= 0 && ts <= 8);
+
+	/* We will stick to that single UL slot, unreserve the others */
+	ul_slots = 1 << ts;
+	usf[ts] = free_usf;
+
+	return ul_slots;
+}
+
 /*! Slot Allocation: Algorithm B
  *
  * Assign as many downlink slots as possible.
@@ -867,27 +905,12 @@
 		dl_slots = rc;
 		update_slot_counters(dl_slots, reserved_dl_slots, &slotcount, &avail_count);
 	} else {
-		int free_usf = -1;
-
-		ul_slots = rc;
-
-		if (first_common_ts >= 0)
-			ul_slots = 1 << first_common_ts;
-		else
-			ul_slots = ul_slots & dl_slots;
-
-		ts = find_least_busy_pdch(trx, GPRS_RLCMAC_UL_TBF, ul_slots, compute_usage_by_num_tbfs, NULL, &free_usf);
-
-		if (free_usf < 0) {
-			LOGP(DRLCMAC, LOGL_NOTICE, "No USF available\n");
-			return -EBUSY;
-		}
-
-		OSMO_ASSERT(ts >= 0 && ts <= 8);
+		rc = allocate_usf(trx, first_common_ts, rc, dl_slots, usf);
+		if (rc < 0)
+			return rc;
 
 		/* We will stick to that single UL slot, unreserve the others */
-		ul_slots = 1 << ts;
-		usf[ts] = free_usf;
+		ul_slots = rc;
 		reserved_ul_slots = ul_slots;
 
 		update_slot_counters(ul_slots, reserved_ul_slots, &slotcount, &avail_count);