--- a/gsm-fw/L1/cfile/tch_feature.c	Sun Mar 20 23:07:46 2016 +0000
+++ b/gsm-fw/L1/cfile/tch_feature.c	Mon Mar 21 02:27:51 2016 +0000
@@ -7,6 +7,10 @@
 #include "l1_confg.h"
 #include "l1_types.h"
 #include "sys_types.h"
+#include <string.h>
+#include "l1_const.h"
+#include "l1_defty.h"
+#include "l1_varex.h"
 #include "l1_trace.h"
 #include "tch_feature.h"
 
@@ -18,7 +22,7 @@
 	T_RVT_BUFFER buf;
 	T_RVT_RET rc;
 	UINT8 *dp;
-	API apiword;
+	UWORD16 apiword;
 	int i;
 
 	rc = rvt_mem_alloc(tch_reroute_rvt_id, 41, &buf);
@@ -34,7 +38,66 @@
 	rvt_send_trace_no_cpy(buf, tch_reroute_rvt_id, 41, RVT_BINARY_FORMAT);
 }
 
-static void handle_tch_config_reg(T_RVT_BUFFER pkt)
+static void send_tch_ulbits_conf()
+{
+	T_RVT_BUFFER buf;
+	T_RVT_RET rc;
+
+	rc = rvt_mem_alloc(tch_reroute_rvt_id, 1, &buf);
+	if (rc == RVT_OK) {
+		buf[0] = TCH_ULBITS_CONF;
+		rvt_send_trace_no_cpy(buf, tch_reroute_rvt_id, 1,
+					RVT_BINARY_FORMAT);
+	}
+}
+
+#define	UPLINK_QUEUE_SIZE	5
+#define	WORDS_PER_ENTRY		17
+
+static UWORD16 uplink_data[UPLINK_QUEUE_SIZE][WORDS_PER_ENTRY];
+static volatile int ul_read_ptr, ul_write_ptr;
+
+void tch_substitute_uplink(API *dsp_buffer)
+{
+	int read_ptr;
+	int i;
+
+	read_ptr = ul_read_ptr;
+	if (read_ptr == ul_write_ptr) {
+		/* no uplink substitution */
+		l1s_dsp_com.dsp_ndb_ptr->d_tch_mode &= ~(1 << B_PLAY_UL);
+		return;
+	}
+	for (i = 0; i < WORDS_PER_ENTRY; i++)
+		dsp_buffer[i+3] = uplink_data[read_ptr][i];
+	// Fill data block Header...
+	dsp_buffer[0] = (1 << B_BLUD);		// 1st word: Set B_BLU bit.
+	dsp_buffer[1] = 0;			// 2nd word: cleared.
+	dsp_buffer[2] = 0;			// 3rd word: cleared.
+	l1s_dsp_com.dsp_ndb_ptr->d_tch_mode |= (1 << B_PLAY_UL);
+	/* advance the read pointer and send TCH_ULBITS_CONF */
+	read_ptr++;
+	if (read_ptr >= UPLINK_QUEUE_SIZE)
+		read_ptr = 0;
+	ul_read_ptr = read_ptr;
+	send_tch_ulbits_conf();
+}
+
+static void handle_tch_ulbits_req(T_RVT_BUFFER pkt)
+{
+	int write_ptr, write_next;
+
+	write_ptr = ul_write_ptr;
+	write_next = write_ptr + 1;
+	if (write_next >= UPLINK_QUEUE_SIZE)
+		write_next = 0;
+	if (write_next == ul_read_ptr)	/* queue full */
+		return;
+	memcpy(uplink_data[write_ptr], pkt + 1, 34);
+	ul_write_ptr = write_next;
+}
+
+static void handle_tch_config_req(T_RVT_BUFFER pkt)
 {
 	UWORD8 config;
 	T_RVT_BUFFER buf;
@@ -65,10 +128,12 @@
 	case TCH_CONFIG_REQ:
 		if (pktlen != 2)
 			return;
-		handle_tch_config_reg(pkt);
+		handle_tch_config_req(pkt);
 		break;
 	case TCH_ULBITS_REQ:
-		/* to be filled */
+		if (pktlen < 34)
+			return;
+		handle_tch_ulbits_req(pkt);
 		break;
 	}
 }
@@ -77,4 +142,6 @@
 {
 	rvt_register_id("TCH", &tch_reroute_rvt_id, tch_rvt_input_callback);
 	tch_reroute_downlink = FALSE;
+	ul_read_ptr = 0;
+	ul_write_ptr = 0;
 }