/* CP detection algorithm taken from:
* "SSS Detection Method for Initial Cell Search in 3GPP LTE FDD/TDD Dual Mode Receiver"
* by Jung-In Kim et al.
*/
srslte_cp_t srslte_sync_detect_cp(srslte_sync_t *q, const cf_t *input, uint32_t peak_pos)
{
float R_norm=0, R_ext=0, C_norm=0, C_ext=0;
float M_norm=0, M_ext=0;
uint32_t cp_norm_len = SRSLTE_CP_LEN_NORM(7, q->fft_size);
uint32_t cp_ext_len = SRSLTE_CP_LEN_EXT(q->fft_size);
uint32_t nof_symbols = peak_pos/(q->fft_size+cp_ext_len);
if (nof_symbols > 3) {
nof_symbols = 3;
}
if (nof_symbols > 0) {
const cf_t *input_cp_norm = &input[peak_pos-nof_symbols*(q->fft_size+cp_norm_len)];
const cf_t *input_cp_ext = &input[peak_pos-nof_symbols*(q->fft_size+cp_ext_len)];
for (int i=0;i<nof_symbols;i++) {
R_norm += crealf(srslte_vec_dot_prod_conj_ccc(&input_cp_norm[q->fft_size], input_cp_norm, cp_norm_len));
C_norm += cp_norm_len * srslte_vec_avg_power_cf(input_cp_norm, cp_norm_len);
input_cp_norm += q->fft_size+cp_norm_len;
}
if (C_norm > 0) {
M_norm = R_norm/C_norm;
}
q->M_norm_avg = SRSLTE_VEC_EMA(M_norm/nof_symbols, q->M_norm_avg, CP_EMA_ALPHA);
for (int i=0;i<nof_symbols;i++) {
R_ext += crealf(srslte_vec_dot_prod_conj_ccc(&input_cp_ext[q->fft_size], input_cp_ext, cp_ext_len));
C_ext += cp_ext_len * srslte_vec_avg_power_cf(input_cp_ext, cp_ext_len);
input_cp_ext += q->fft_size+cp_ext_len;
}
if (C_ext > 0) {
M_ext = R_ext/C_ext;
}
q->M_ext_avg = SRSLTE_VEC_EMA(M_ext/nof_symbols, q->M_ext_avg, CP_EMA_ALPHA);
if (q->M_norm_avg > q->M_ext_avg) {
return SRSLTE_CP_NORM;
} else if (q->M_norm_avg < q->M_ext_avg) {
return SRSLTE_CP_EXT;
} else {
if (R_norm > R_ext) {
return SRSLTE_CP_NORM;
} else {
return SRSLTE_CP_EXT;
}
}
} else {
return SRSLTE_CP_NORM;
}
}
/**
* Transforms input samples into output OFDM symbols.
* Performs FFT on a each symbol and removes CP.
*/
void srslte_ofdm_rx_slot(srslte_ofdm_t *q, cf_t *input, cf_t *output) {
uint32_t i;
for (i=0;i<q->nof_symbols;i++) {
input += SRSLTE_CP_ISNORM(q->cp)?SRSLTE_CP_LEN_NORM(i, q->symbol_sz):SRSLTE_CP_LEN_EXT(q->symbol_sz);
srslte_dft_run_c(&q->fft_plan, input, q->tmp);
memcpy(output, &q->tmp[q->nof_guards], q->nof_re * sizeof(cf_t));
input += q->symbol_sz;
output += q->nof_re;
}
}
void srslte_sync_set_cp(srslte_sync_t *q, srslte_cp_t cp)
{
q->cp = cp;
q->cp_len = SRSLTE_CP_ISNORM(q->cp)?SRSLTE_CP_LEN_NORM(1,q->fft_size):SRSLTE_CP_LEN_EXT(q->fft_size);
if (q->frame_size < q->fft_size) {
q->nof_symbols = 1;
} else {
q->nof_symbols = q->frame_size/(q->fft_size+q->cp_len)-1;
}
}
/**
* Transforms input OFDM symbols into output samples.
* Performs FFT on a each symbol and adds CP.
*/
void srslte_ofdm_tx_slot(srslte_ofdm_t *q, cf_t *input, cf_t *output) {
uint32_t i, cp_len;
for (i=0;i<q->nof_symbols;i++) {
cp_len = SRSLTE_CP_ISNORM(q->cp)?SRSLTE_CP_LEN_NORM(i, q->symbol_sz):SRSLTE_CP_LEN_EXT(q->symbol_sz);
memcpy(&q->tmp[q->nof_guards], input, q->nof_re * sizeof(cf_t));
srslte_dft_run_c(&q->fft_plan, q->tmp, &output[cp_len]);
input += q->nof_re;
/* add CP */
memcpy(output, &output[q->symbol_sz], cp_len * sizeof(cf_t));
output += q->symbol_sz + cp_len;
}
}
/* Shifts the signal after the iFFT or before the FFT.
* Freq_shift is relative to inter-carrier spacing.
* Caution: This function shall not be called during run-time
*/
int srslte_ofdm_set_freq_shift(srslte_ofdm_t *q, float freq_shift) {
q->shift_buffer = srslte_vec_malloc(sizeof(cf_t) * SRSLTE_SF_LEN(q->symbol_sz));
if (!q->shift_buffer) {
perror("malloc");
return -1;
}
cf_t *ptr = q->shift_buffer;
for (uint32_t n=0;n<2;n++) {
for (uint32_t i=0;i<q->nof_symbols;i++) {
uint32_t cplen = SRSLTE_CP_ISNORM(q->cp)?SRSLTE_CP_LEN_NORM(i, q->symbol_sz):SRSLTE_CP_LEN_EXT(q->symbol_sz);
for (uint32_t t=0;t<q->symbol_sz+cplen;t++) {
ptr[t] = cexpf(I*2*M_PI*((float) t-(float)cplen)*freq_shift/q->symbol_sz);
}
ptr += q->symbol_sz+cplen;
}
}
/* Disable DC carrier addition */
srslte_dft_plan_set_dc(&q->fft_plan, false);
q->freq_shift = true;
return SRSLTE_SUCCESS;
}
/** Finds the PSS sequence previously defined by a call to srslte_sync_set_N_id_2()
* around the position find_offset in the buffer input.
* Returns 1 if the correlation peak exceeds the threshold set by srslte_sync_set_threshold()
* or 0 otherwise. Returns a negative number on error (if N_id_2 has not been set)
*
* The maximum of the correlation peak is always stored in *peak_position
*/
int srslte_sync_find(srslte_sync_t *q, cf_t *input, uint32_t find_offset, uint32_t *peak_position)
{
int ret = SRSLTE_ERROR_INVALID_INPUTS;
if (q != NULL &&
input != NULL &&
srslte_N_id_2_isvalid(q->N_id_2) &&
fft_size_isvalid(q->fft_size))
{
int peak_pos;
ret = SRSLTE_SUCCESS;
if (peak_position) {
*peak_position = 0;
}
/* Estimate CFO using CP */
if (q->enable_cfo_corr) {
uint32_t cp_offset = srslte_cp_synch(&q->cp_synch, input, q->nof_symbols, q->nof_symbols, SRSLTE_CP_LEN_NORM(1,q->fft_size));
cf_t cp_corr_max = srslte_cp_synch_corr_output(&q->cp_synch, cp_offset);
float cfo = -carg(cp_corr_max) / M_PI / 2;
/* compute cumulative moving average CFO */
INFO("cp_offset_pos=%d, abs=%f, cfo=%f, mean_cfo=%f, nof_symb=%d\n",
cp_offset, cabs(cp_corr_max), cfo, q->mean_cfo, q->nof_symbols);
if (q->mean_cfo) {
q->mean_cfo = SRSLTE_VEC_EMA(cfo, q->mean_cfo, q->cfo_ema_alpha);
} else {
q->mean_cfo = cfo;
}
/* Correct CFO with the averaged CFO estimation */
srslte_cfo_correct(&q->cfocorr, input, input, -q->mean_cfo / q->fft_size);
}
if (q->find_cfo_i && q->enable_cfo_corr) {
float peak_value;
float max_peak_value = -99;
peak_pos = 0;
srslte_pss_synch_t *pss_obj[3] = {&q->pss_i[0], &q->pss, &q->pss_i[1]};
for (int cfo_i=0;cfo_i<3;cfo_i++) {
srslte_pss_synch_set_N_id_2(pss_obj[cfo_i], q->N_id_2);
int p = srslte_pss_synch_find_pss(pss_obj[cfo_i], &input[find_offset], &peak_value);
if (peak_value > max_peak_value) {
max_peak_value = peak_value;
peak_pos = p;
q->peak_value = peak_value;
q->cfo_i = cfo_i-1;
}
}
if (q->cfo_i != 0) {
srslte_vec_prod_ccc(input, q->cfo_i_corr[q->cfo_i<0?0:1], input, q->frame_size);
INFO("Compensating cfo_i=%d\n", q->cfo_i);
}
} else {
srslte_pss_synch_set_N_id_2(&q->pss, q->N_id_2);
peak_pos = srslte_pss_synch_find_pss(&q->pss, &input[find_offset], &q->peak_value);
if (peak_pos < 0) {
fprintf(stderr, "Error calling finding PSS sequence\n");
return SRSLTE_ERROR;
}
}
q->mean_peak_value = SRSLTE_VEC_EMA(q->peak_value, q->mean_peak_value, MEANPEAK_EMA_ALPHA);
if (peak_position) {
*peak_position = (uint32_t) peak_pos;
}
/* If peak is over threshold, compute CFO and SSS */
if (q->peak_value >= q->threshold) {
// Try to detect SSS
if (q->sss_en) {
// Set an invalid N_id_1 indicating SSS is yet to be detected
q->N_id_1 = 1000;
if (sync_sss(q, input, find_offset + peak_pos, q->cp) < 0) {
DEBUG("No space for SSS processing. Frame starts at %d\n", peak_pos);
}
}
if (q->detect_cp) {
if (peak_pos + find_offset >= 2*(q->fft_size + SRSLTE_CP_LEN_EXT(q->fft_size))) {
srslte_sync_set_cp(q, srslte_sync_detect_cp(q, input, peak_pos + find_offset));
} else {
DEBUG("Not enough room to detect CP length. Peak position: %d\n", peak_pos);
}
}
// Return 1 (peak detected) even if we couldn't estimate CFO and SSS
ret = 1;
} else {
ret = 0;
}
DEBUG("SYNC ret=%d N_id_2=%d find_offset=%d pos=%d peak=%.2f threshold=%.2f sf_idx=%d, CFO=%.3f KHz\n",
ret, q->N_id_2, find_offset, peak_pos, q->peak_value, q->threshold, q->sf_idx, 15*(q->cfo_i+q->mean_cfo));
} else if (srslte_N_id_2_isvalid(q->N_id_2)) {
fprintf(stderr, "Must call srslte_sync_set_N_id_2() first!\n");
}
return ret;
}
int srslte_ue_sync_init(srslte_ue_sync_t *q,
srslte_cell_t cell,
int (recv_callback)(void*, void*, uint32_t,srslte_timestamp_t*),
void *stream_handler)
{
int ret = SRSLTE_ERROR_INVALID_INPUTS;
if (q != NULL &&
stream_handler != NULL &&
srslte_nofprb_isvalid(cell.nof_prb) &&
recv_callback != NULL)
{
ret = SRSLTE_ERROR;
bzero(q, sizeof(srslte_ue_sync_t));
q->stream = stream_handler;
q->recv_callback = recv_callback;
q->cell = cell;
q->fft_size = srslte_symbol_sz(q->cell.nof_prb);
q->sf_len = SRSLTE_SF_LEN(q->fft_size);
q->file_mode = false;
q->correct_cfo = true;
q->agc_period = 0;
q->sample_offset_correct_period = DEFAULT_SAMPLE_OFFSET_CORRECT_PERIOD;
q->sfo_ema = DEFAULT_SFO_EMA_COEFF;
if (cell.id == 1000) {
/* If the cell is unkown, we search PSS/SSS in 5 ms */
q->nof_recv_sf = 5;
q->decode_sss_on_track = true;
} else {
/* If the cell is known, we work on a 1ms basis */
q->nof_recv_sf = 1;
q->decode_sss_on_track = true;
}
q->frame_len = q->nof_recv_sf*q->sf_len;
if(srslte_sync_init(&q->sfind, q->frame_len, q->frame_len, q->fft_size)) {
fprintf(stderr, "Error initiating sync find\n");
goto clean_exit;
}
if (cell.id == 1000) {
if(srslte_sync_init(&q->strack, q->frame_len, TRACK_FRAME_SIZE, q->fft_size)) {
fprintf(stderr, "Error initiating sync track\n");
goto clean_exit;
}
} else {
if(srslte_sync_init(&q->strack, q->frame_len, SRSLTE_CP_LEN_NORM(1,q->fft_size), q->fft_size)) {
fprintf(stderr, "Error initiating sync track\n");
goto clean_exit;
}
}
if (cell.id == 1000) {
/* If the cell id is unknown, enable CP detection on find */
// FIXME: CP detection not working very well. Not supporting Extended CP right now
srslte_sync_cp_en(&q->sfind, false);
srslte_sync_cp_en(&q->strack, false);
srslte_sync_set_cfo_ema_alpha(&q->sfind, 0.8);
srslte_sync_set_cfo_ema_alpha(&q->strack, 0.1);
srslte_sync_cfo_i_detec_en(&q->sfind, false);
q->nof_avg_find_frames = FIND_NOF_AVG_FRAMES;
srslte_sync_set_threshold(&q->sfind, 2.0);
srslte_sync_set_threshold(&q->strack, 1.2);
} else {
srslte_sync_set_N_id_2(&q->sfind, cell.id%3);
srslte_sync_set_N_id_2(&q->strack, cell.id%3);
q->sfind.cp = cell.cp;
q->strack.cp = cell.cp;
srslte_sync_cp_en(&q->sfind, false);
srslte_sync_cp_en(&q->strack, false);
srslte_sync_cfo_i_detec_en(&q->sfind, false);
srslte_sync_set_cfo_ema_alpha(&q->sfind, 0.1);
srslte_sync_set_cfo_ema_alpha(&q->strack, 0.1);
/* In find phase and if the cell is known, do not average pss correlation
* because we only capture 1 subframe and do not know where the peak is.
*/
q->nof_avg_find_frames = 1;
srslte_sync_set_em_alpha(&q->sfind, 1);
srslte_sync_set_threshold(&q->sfind, 3.0);
srslte_sync_set_em_alpha(&q->strack, 0.2);
srslte_sync_set_threshold(&q->strack, 1.2);
}
/* FIXME: Go for zerocopy only and eliminate this allocation */
q->input_buffer = srslte_vec_malloc(2*q->frame_len * sizeof(cf_t));
if (!q->input_buffer) {
perror("malloc");
goto clean_exit;
}
srslte_ue_sync_reset(q);
ret = SRSLTE_SUCCESS;
}
clean_exit:
if (ret == SRSLTE_ERROR) {
srslte_ue_sync_free(q);
}
return ret;
}
static float cfo_cp_estimate(srslte_sync_t *q, const cf_t *input)
{
uint32_t cp_offset = 0;
cp_offset = srslte_cp_synch(&q->cp_synch, input, q->max_offset, q->cfo_cp_nsymbols, SRSLTE_CP_LEN_NORM(1,q->fft_size));
cf_t cp_corr_max = srslte_cp_synch_corr_output(&q->cp_synch, cp_offset);
float cfo = -carg(cp_corr_max) / M_PI / 2;
return cfo;
}
