FreeCalypso > hg > freecalypso-citrine
diff L1/cfile/tch_feature.c @ 0:75a11d740a02
initial import of gsm-fw from freecalypso-sw rev 1033:5ab737ac3ad7
author | Mychaela Falconia <falcon@freecalypso.org> |
---|---|
date | Thu, 09 Jun 2016 00:02:41 +0000 |
parents | |
children |
line wrap: on
line diff
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/L1/cfile/tch_feature.c Thu Jun 09 00:02:41 2016 +0000 @@ -0,0 +1,152 @@ +/* + * This module is a FreeCalypso addition; it contains code implementing + * our custom voice TCH rerouting feature. + */ + +#include "config.h" +#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" + +T_RVT_USER_ID tch_reroute_rvt_id; +BOOL tch_reroute_downlink; + +void tch_send_downlink_bits(API *dsp_buffer) +{ + T_RVT_BUFFER buf; + T_RVT_RET rc; + UINT8 *dp; + UWORD16 apiword; + int i; + + rc = rvt_mem_alloc(tch_reroute_rvt_id, 41, &buf); + if (rc != RVT_OK) + return; + dp = buf; + *dp++ = TCH_DLBITS_IND; + for (i = 0; i < 20; i++) { + apiword = dsp_buffer[i]; + *dp++ = apiword >> 8; + *dp++ = apiword; + } + rvt_send_trace_no_cpy(buf, tch_reroute_rvt_id, 41, RVT_BINARY_FORMAT); +} + +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 &= ~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 |= 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, i; + UINT8 *sp; + + 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; + sp = pkt + 1; + for (i = 0; i < WORDS_PER_ENTRY; i++) { + uplink_data[write_ptr][i] = (sp[0] << 8) | sp[1]; + sp += 2; + } + ul_write_ptr = write_next; +} + +static void handle_tch_config_req(T_RVT_BUFFER pkt) +{ + UWORD8 config; + T_RVT_BUFFER buf; + T_RVT_RET rc; + + config = pkt[1] & 0x01; + tch_reroute_downlink = config; + + /* send TCH_CONFIG_CONF response */ + rc = rvt_mem_alloc(tch_reroute_rvt_id, 2, &buf); + if (rc == RVT_OK) { + buf[0] = TCH_CONFIG_CONF; + buf[1] = config; + rvt_send_trace_no_cpy(buf, tch_reroute_rvt_id, 2, + RVT_BINARY_FORMAT); + } +} + +/* + * The following function is the callback registered with RVT; it gets + * called in RVT HISR context. + */ +static void tch_rvt_input_callback(T_RVT_BUFFER pkt, UINT16 pktlen) +{ + if (pktlen < 1) + return; + switch (pkt[0]) { + case TCH_CONFIG_REQ: + if (pktlen != 2) + return; + handle_tch_config_req(pkt); + break; + case TCH_ULBITS_REQ: + if (pktlen < 34) + return; + handle_tch_ulbits_req(pkt); + break; + } +} + +void feature_tch_reroute_init() +{ + rvt_register_id("TCH", &tch_reroute_rvt_id, tch_rvt_input_callback); + tch_reroute_downlink = FALSE; + ul_read_ptr = 0; + ul_write_ptr = 0; +}