int main(int argc, char **argv) {
int i;
cf_t *input, *output;
srslte_cfo_t cfocorr;
float mse;
if (argc < 5) {
usage(argv[0]);
exit(-1);
}
parse_args(argc, argv);
input = malloc(sizeof(cf_t) * num_samples);
if (!input) {
perror("malloc");
exit(-1);
}
output = malloc(sizeof(cf_t) * num_samples);
if (!output) {
perror("malloc");
exit(-1);
}
for (i=0;i<num_samples;i++) {
input[i] = 100 * (rand()/RAND_MAX + I*rand()/RAND_MAX);
output[i] = input[i];
}
if (srslte_cfo_init(&cfocorr, num_samples)) {
fprintf(stderr, "Error initiating CFO\n");
return -1;
}
srslte_cfo_correct(&cfocorr, output, output, freq);
srslte_cfo_correct(&cfocorr, output, output, -freq);
mse = 0;
for (i=0;i<num_samples;i++) {
mse += cabsf(input[i] - output[i]) / num_samples;
}
srslte_cfo_free(&cfocorr);
free(input);
free(output);
printf("MSE: %f\n", mse);
if (mse > MAX_MSE) {
printf("MSE too large\n");
exit(-1);
} else {
printf("Ok\n");
exit(0);
}
}
int srslte_ue_dl_decode_fft_estimate(srslte_ue_dl_t *q, cf_t *input, uint32_t sf_idx, uint32_t *cfi) {
if (input && q && cfi && sf_idx < SRSLTE_NSUBFRAMES_X_FRAME) {
/* Run FFT for all subframe data */
srslte_ofdm_rx_sf(&q->fft, input, q->sf_symbols);
/* Correct SFO multiplying by complex exponential in the time domain */
if (q->sample_offset) {
struct timeval t[3];
gettimeofday(&t[1], NULL);
for (int i=0;i<2*SRSLTE_CP_NSYMB(q->cell.cp);i++) {
srslte_cfo_correct(&q->sfo_correct,
&q->sf_symbols[i*q->cell.nof_prb*SRSLTE_NRE],
&q->sf_symbols[i*q->cell.nof_prb*SRSLTE_NRE],
q->sample_offset / q->fft.symbol_sz);
}
gettimeofday(&t[2], NULL);
get_time_interval(t);
}
return srslte_ue_dl_decode_estimate(q, sf_idx, cfi);
} else {
return SRSLTE_ERROR_INVALID_INPUTS;
}
}
int srslte_ue_dl_decode_fft_estimate_noguru(srslte_ue_dl_t *q, cf_t *input[SRSLTE_MAX_PORTS], uint32_t sf_idx, uint32_t *cfi)
{
if (input && q && sf_idx < SRSLTE_NSUBFRAMES_X_FRAME) {
/* Run FFT for all subframe data */
for (int j=0;j<q->nof_rx_antennas;j++) {
srslte_ofdm_rx_sf_ng(&q->fft[j], input[j], q->sf_symbols_m[j]);
/* Correct SFO multiplying by complex exponential in the time domain */
if (q->sample_offset) {
int nsym = SRSLTE_CP_NSYMB(q->cell.cp);
for (int i=0;i<2*nsym;i++) {
srslte_cfo_correct(&q->sfo_correct,
&q->sf_symbols_m[j][i*q->cell.nof_prb*SRSLTE_NRE],
&q->sf_symbols_m[j][i*q->cell.nof_prb*SRSLTE_NRE],
q->sample_offset / q->fft[j].symbol_sz);
}
}
}
return srslte_ue_dl_decode_estimate_mbsfn(q, sf_idx, cfi, SRSLTE_SF_NORM);
} else {
return SRSLTE_ERROR_INVALID_INPUTS;
}
}
/** 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 main(int argc, char **argv) {
cf_t *buffer;
int frame_cnt, n;
void *uhd;
srslte_pss_synch_t pss;
srslte_cfo_t cfocorr, cfocorr64;
srslte_sss_synch_t sss;
int32_t flen;
int peak_idx, last_peak;
float peak_value;
float mean_peak;
uint32_t nof_det, nof_nodet, nof_nopeak, nof_nopeakdet;
cf_t ce[SRSLTE_PSS_LEN];
parse_args(argc, argv);
if (N_id_2_sync == -1) {
N_id_2_sync = cell_id%3;
}
uint32_t N_id_2 = cell_id%3;
uint32_t N_id_1 = cell_id/3;
#ifndef DISABLE_GRAPHICS
if (!disable_plots)
init_plots();
#endif
float srate = 15000.0*fft_size;
flen = srate*5/1000;
printf("Opening UHD device...\n");
if (cuhd_open(uhd_args, &uhd)) {
fprintf(stderr, "Error opening uhd\n");
exit(-1);
}
if (srate < 10e6) {
cuhd_set_master_clock_rate(uhd, 4*srate);
} else {
cuhd_set_master_clock_rate(uhd, srate);
}
printf("Set RX rate: %.2f MHz\n", cuhd_set_rx_srate(uhd, srate) / 1000000);
printf("Set RX gain: %.1f dB\n", cuhd_set_rx_gain(uhd, uhd_gain));
printf("Set RX freq: %.2f MHz\n", cuhd_set_rx_freq(uhd, uhd_freq) / 1000000);
cuhd_rx_wait_lo_locked(uhd);
buffer = malloc(sizeof(cf_t) * flen * 2);
if (!buffer) {
perror("malloc");
exit(-1);
}
if (srslte_pss_synch_init_fft(&pss, flen, fft_size)) {
fprintf(stderr, "Error initiating PSS\n");
exit(-1);
}
if (srslte_pss_synch_set_N_id_2(&pss, N_id_2_sync)) {
fprintf(stderr, "Error setting N_id_2=%d\n",N_id_2_sync);
exit(-1);
}
srslte_cfo_init(&cfocorr, flen);
srslte_cfo_init(&cfocorr64, flen);
if (srslte_sss_synch_init(&sss, fft_size)) {
fprintf(stderr, "Error initializing SSS object\n");
return SRSLTE_ERROR;
}
srslte_sss_synch_set_N_id_2(&sss, N_id_2);
printf("N_id_2: %d\n", N_id_2);
cuhd_start_rx_stream(uhd);
printf("Frame length %d samples\n", flen);
printf("PSS detection threshold: %.2f\n", threshold);
nof_det = nof_nodet = nof_nopeak = nof_nopeakdet = 0;
frame_cnt = 0;
last_peak = 0;
mean_peak = 0;
int peak_offset = 0;
float cfo;
float mean_cfo = 0;
uint32_t m0, m1;
uint32_t sss_error1 = 0, sss_error2 = 0, sss_error3 = 0;
uint32_t cp_is_norm = 0;
srslte_sync_t ssync;
bzero(&ssync, sizeof(srslte_sync_t));
ssync.fft_size = fft_size;
while(frame_cnt < nof_frames || nof_frames == -1) {
n = cuhd_recv(uhd, buffer, flen - peak_offset, 1);
if (n < 0) {
fprintf(stderr, "Error receiving samples\n");
exit(-1);
}
peak_idx = srslte_pss_synch_find_pss(&pss, buffer, &peak_value);
if (peak_idx < 0) {
fprintf(stderr, "Error finding PSS peak\n");
exit(-1);
}
mean_peak = SRSLTE_VEC_CMA(peak_value, mean_peak, frame_cnt);
if (peak_value >= threshold) {
nof_det++;
if (peak_idx >= fft_size) {
// Estimate CFO
cfo = srslte_pss_synch_cfo_compute(&pss, &buffer[peak_idx-fft_size]);
mean_cfo = SRSLTE_VEC_CMA(cfo, mean_cfo, frame_cnt);
// Correct CFO
srslte_cfo_correct(&cfocorr, buffer, buffer, -mean_cfo / fft_size);
// Estimate channel
if (srslte_pss_synch_chest(&pss, &buffer[peak_idx-fft_size], ce)) {
fprintf(stderr, "Error computing channel estimation\n");
exit(-1);
}
// Find SSS
int sss_idx = peak_idx-2*fft_size-(SRSLTE_CP_ISNORM(cp)?SRSLTE_CP_LEN(fft_size, SRSLTE_CP_NORM_LEN):SRSLTE_CP_LEN(fft_size, SRSLTE_CP_EXT_LEN));
if (sss_idx >= 0 && sss_idx < flen-fft_size) {
srslte_sss_synch_m0m1_partial(&sss, &buffer[sss_idx], 3, NULL, &m0, &m0_value, &m1, &m1_value);
if (srslte_sss_synch_N_id_1(&sss, m0, m1) != N_id_1) {
sss_error2++;
}
INFO("Partial N_id_1: %d\n", srslte_sss_synch_N_id_1(&sss, m0, m1));
srslte_sss_synch_m0m1_diff(&sss, &buffer[sss_idx], &m0, &m0_value, &m1, &m1_value);
if (srslte_sss_synch_N_id_1(&sss, m0, m1) != N_id_1) {
sss_error3++;
}
INFO("Diff N_id_1: %d\n", srslte_sss_synch_N_id_1(&sss, m0, m1));
srslte_sss_synch_m0m1_partial(&sss, &buffer[sss_idx], 1, NULL, &m0, &m0_value, &m1, &m1_value);
if (srslte_sss_synch_N_id_1(&sss, m0, m1) != N_id_1) {
sss_error1++;
}
INFO("Full N_id_1: %d\n", srslte_sss_synch_N_id_1(&sss, m0, m1));
}
// Estimate CP
if (peak_idx > 2*(fft_size + SRSLTE_CP_LEN_EXT(fft_size))) {
srslte_cp_t cp = srslte_sync_detect_cp(&ssync, buffer, peak_idx);
if (SRSLTE_CP_ISNORM(cp)) {
cp_is_norm++;
}
}
} else {
INFO("No space for CFO computation. Frame starts at \n",peak_idx);
}
if(srslte_sss_synch_subframe(m0,m1) == 0)
{
#ifndef DISABLE_GRAPHICS
if (!disable_plots)
do_plots_sss(sss.corr_output_m0, sss.corr_output_m1);
#endif
}
} else {
nof_nodet++;
}
if (frame_cnt > 100) {
if (abs(last_peak-peak_idx) > 4) {
if (peak_value >= threshold) {
nof_nopeakdet++;
}
nof_nopeak++;
}
}
frame_cnt++;
printf("[%5d]: Pos: %5d, PSR: %4.1f (~%4.1f) Pdet: %4.2f, "
"FA: %4.2f, CFO: %+4.1f KHz SSSmiss: %4.2f/%4.2f/%4.2f CPNorm: %.0f%%\r",
frame_cnt,
peak_idx,
peak_value, mean_peak,
(float) nof_det/frame_cnt,
(float) nof_nopeakdet/frame_cnt, mean_cfo*15,
(float) sss_error1/nof_det,(float) sss_error2/nof_det,(float) sss_error3/nof_det,
(float) cp_is_norm/nof_det * 100);
if (SRSLTE_VERBOSE_ISINFO()) {
printf("\n");
}
#ifndef DISABLE_GRAPHICS
if (!disable_plots)
do_plots(pss.conv_output_avg, pss.conv_output_avg[peak_idx], pss.fft_size+pss.frame_size-1, ce);
#endif
last_peak = peak_idx;
}
srslte_pss_synch_free(&pss);
free(buffer);
cuhd_close(uhd);
printf("Ok\n");
exit(0);
}
/** 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;
}
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) {
// Make sure we have enough space to estimate CFO
if (peak_pos + find_offset >= q->fft_size) {
float cfo = srslte_pss_synch_cfo_compute(&q->pss, &input[find_offset+peak_pos-q->fft_size]);
/* compute cumulative moving average CFO */
q->mean_cfo = SRSLTE_VEC_EMA(cfo, q->mean_cfo, CFO_EMA_ALPHA);
} else {
DEBUG("No space for CFO computation. Frame starts at \n",peak_pos);
}
/* Correct CFO with the averaged CFO estimation */
if (q->correct_cfo) {
srslte_cfo_correct(&q->cfocorr, input, input, -q->mean_cfo / q->fft_size);
}
// 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))) {
q->cp = 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->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;
}
/* Returns 1 if the subframe is synchronized in time, 0 otherwise */
int srslte_ue_sync_zerocopy(srslte_ue_sync_t *q, cf_t *input_buffer) {
int ret = SRSLTE_ERROR_INVALID_INPUTS;
uint32_t track_idx;
if (q != NULL &&
input_buffer != NULL)
{
if (q->file_mode) {
int n = srslte_filesource_read(&q->file_source, input_buffer, q->sf_len);
if (n < 0) {
fprintf(stderr, "Error reading input file\n");
return SRSLTE_ERROR;
}
if (n == 0) {
return 7;
// srslte_filesource_seek(&q->file_source, 0);
// q->sf_idx = 9;
// int n = srslte_filesource_read(&q->file_source, input_buffer, q->sf_len);
// if (n < 0) {
// fprintf(stderr, "Error reading input file\n");
// return SRSLTE_ERROR;
// }
}
if (q->correct_cfo) {
srslte_cfo_correct(&q->file_cfo_correct,
input_buffer,
input_buffer,
q->file_cfo / 15000 / q->fft_size);
}
q->sf_idx++;
if (q->sf_idx == 10) {
q->sf_idx = 0;
}
INFO("Reading %d samples. sf_idx = %d\n", q->sf_len, q->sf_idx);
ret = 1;
} else {
if (receive_samples(q, input_buffer)) {
fprintf(stderr, "Error receiving samples\n");
return SRSLTE_ERROR;
}
switch (q->state) {
case SF_FIND:
switch(srslte_sync_find(&q->sfind, input_buffer, 0, &q->peak_idx)) {
case SRSLTE_SYNC_ERROR:
ret = SRSLTE_ERROR;
fprintf(stderr, "Error finding correlation peak (%d)\n", ret);
return SRSLTE_ERROR;
case SRSLTE_SYNC_FOUND:
ret = find_peak_ok(q, input_buffer);
break;
case SRSLTE_SYNC_FOUND_NOSPACE:
/* If a peak was found but there is not enough space for SSS/CP detection, discard a few samples */
printf("No space for SSS/CP detection. Realigning frame...\n");
q->recv_callback(q->stream, dummy_offset_buffer, q->frame_len/2, NULL);
srslte_sync_reset(&q->sfind);
ret = SRSLTE_SUCCESS;
break;
default:
ret = SRSLTE_SUCCESS;
break;
}
if (q->do_agc) {
srslte_agc_process(&q->agc, input_buffer, q->sf_len);
}
break;
case SF_TRACK:
ret = 1;
srslte_sync_sss_en(&q->strack, q->decode_sss_on_track);
q->sf_idx = (q->sf_idx + q->nof_recv_sf) % 10;
/* Every SF idx 0 and 5, find peak around known position q->peak_idx */
if (q->sf_idx == 0 || q->sf_idx == 5) {
if (q->do_agc && (q->agc_period == 0 ||
(q->agc_period && (q->frame_total_cnt%q->agc_period) == 0)))
{
srslte_agc_process(&q->agc, input_buffer, q->sf_len);
}
#ifdef MEASURE_EXEC_TIME
struct timeval t[3];
gettimeofday(&t[1], NULL);
#endif
track_idx = 0;
/* Track PSS/SSS around the expected PSS position
* In tracking phase, the subframe carrying the PSS is always the last one of the frame
*/
switch(srslte_sync_find(&q->strack, input_buffer,
q->frame_len - q->sf_len/2 - q->fft_size - q->strack.max_offset/2,
&track_idx))
{
case SRSLTE_SYNC_ERROR:
ret = SRSLTE_ERROR;
fprintf(stderr, "Error tracking correlation peak\n");
return SRSLTE_ERROR;
case SRSLTE_SYNC_FOUND:
ret = track_peak_ok(q, track_idx);
break;
case SRSLTE_SYNC_FOUND_NOSPACE:
// It's very very unlikely that we fall here because this event should happen at FIND phase only
ret = 0;
q->state = SF_FIND;
printf("Warning: No space for SSS/CP while in tracking phase\n");
break;
case SRSLTE_SYNC_NOFOUND:
ret = track_peak_no(q);
break;
}
#ifdef MEASURE_EXEC_TIME
gettimeofday(&t[2], NULL);
get_time_interval(t);
q->mean_exec_time = (float) SRSLTE_VEC_CMA((float) t[0].tv_usec, q->mean_exec_time, q->frame_total_cnt);
#endif
if (ret == SRSLTE_ERROR) {
fprintf(stderr, "Error processing tracking peak\n");
q->state = SF_FIND;
return SRSLTE_SUCCESS;
}
q->frame_total_cnt++;
} else {
if (q->correct_cfo) {
srslte_cfo_correct(&q->sfind.cfocorr,
input_buffer,
input_buffer,
-srslte_sync_get_cfo(&q->strack) / q->fft_size);
}
}
break;
}
}
}
return ret;
}
/** 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 input signal is not modified. Any CFO correction is done in internal buffers
*
* The maximum of the correlation peak is always stored in *peak_position
*/
srslte_sync_find_ret_t srslte_sync_find(srslte_sync_t *q, const cf_t *input, uint32_t find_offset, uint32_t *peak_position)
{
srslte_sync_find_ret_t ret = SRSLTE_SYNC_ERROR;
int peak_pos = 0;
if (!q) {
return SRSLTE_ERROR_INVALID_INPUTS;
}
if (input != NULL &&
srslte_N_id_2_isvalid(q->N_id_2) &&
fft_size_isvalid(q->fft_size))
{
if (peak_position) {
*peak_position = 0;
}
const cf_t *input_ptr = input;
/* First CFO estimation stage is integer.
* Finds max PSS correlation for shifted +1/0/-1 integer versions.
* This should only used once N_id_2 is set
*/
if (q->cfo_i_enable) {
if (cfo_i_estimate(q, input_ptr, find_offset, &peak_pos, &q->cfo_i_value) < 0) {
fprintf(stderr, "Error calling finding PSS sequence at : %d \n", peak_pos);
return SRSLTE_ERROR;
}
// Correct it using precomputed signal and store in buffer (don't modify input signal)
if (q->cfo_i_value != 0) {
srslte_vec_prod_ccc((cf_t*) input_ptr, q->cfo_i_corr[q->cfo_i_value<0?0:1], q->temp, q->frame_size);
INFO("Compensating cfo_i=%d\n", q->cfo_i_value);
input_ptr = q->temp;
}
}
/* Second stage is coarse fractional CFO estimation using CP.
* In case of multi-cell, this can lead to incorrect estimations if CFO from different cells is different
*/
if (q->cfo_cp_enable) {
float cfo_cp = cfo_cp_estimate(q, input_ptr);
if (!q->cfo_cp_is_set) {
q->cfo_cp_mean = cfo_cp;
q->cfo_cp_is_set = true;
} else {
/* compute exponential moving average CFO */
q->cfo_cp_mean = SRSLTE_VEC_EMA(cfo_cp, q->cfo_cp_mean, q->cfo_ema_alpha);
}
INFO("CP-CFO: estimated=%f, mean=%f\n", cfo_cp, q->cfo_cp_mean);
/* Correct CFO with the averaged CFO estimation */
srslte_cfo_correct(&q->cfo_corr_frame, input_ptr, q->temp, -q->cfo_cp_mean / q->fft_size);
input_ptr = q->temp;
}
/* Find maximum of PSS correlation. If Integer CFO is enabled, correlation is already done
*/
if (!q->cfo_i_enable) {
srslte_pss_set_N_id_2(&q->pss, q->N_id_2);
peak_pos = srslte_pss_find_pss(&q->pss, &input_ptr[find_offset], q->threshold>0?&q->peak_value:NULL);
if (peak_pos < 0) {
fprintf(stderr, "Error calling finding PSS sequence at : %d \n", peak_pos);
return SRSLTE_ERROR;
}
}
INFO("PSS: id=%d, peak_pos=%d, peak_value=%f\n", q->N_id_2, peak_pos, q->peak_value);
// Save peak position
if (peak_position) {
*peak_position = (uint32_t) peak_pos;
}
// In case of decimation, this compensates for the constant time shift caused by the low pass filter
if(q->decimate && peak_pos < 0) {
peak_pos = 0 ;//peak_pos + q->decimate*(2);// replace 2 with q->filter_size -2;
}
/* If peak is over threshold, compute CFO and SSS */
if (q->peak_value >= q->threshold || q->threshold == 0) {
if (q->cfo_pss_enable && peak_pos >= q->fft_size) {
// Filter central bands before PSS-based CFO estimation
const cf_t *pss_ptr = &input_ptr[find_offset + peak_pos - q->fft_size];
if (q->pss_filtering_enabled) {
srslte_pss_filter(&q->pss, pss_ptr, q->pss_filt);
pss_ptr = q->pss_filt;
}
// PSS-based CFO estimation
q->cfo_pss = srslte_pss_cfo_compute(&q->pss, pss_ptr);
if (!q->cfo_pss_is_set) {
q->cfo_pss_mean = q->cfo_pss;
q->cfo_pss_is_set = true;
} else if (15000*fabsf(q->cfo_pss) < MAX_CFO_PSS_OFFSET) {
q->cfo_pss_mean = SRSLTE_VEC_EMA(q->cfo_pss, q->cfo_pss_mean, q->cfo_ema_alpha);
}
INFO("PSS-CFO: filter=%s, estimated=%f, mean=%f\n",
q->pss_filtering_enabled?"yes":"no", q->cfo_pss, q->cfo_pss_mean);
}
// If there is enough space for CP and SSS estimation
if (peak_pos + find_offset >= 2 * (q->fft_size + SRSLTE_CP_LEN_EXT(q->fft_size))) {
// If SSS search is enabled, correlate SSS sequence
if (q->sss_en) {
// Set an invalid N_id_1 indicating SSS is yet to be detected
q->N_id_1 = 1000;
int sss_idx = find_offset + peak_pos - 2 * q->fft_size -
SRSLTE_CP_LEN(q->fft_size, (SRSLTE_CP_ISNORM(q->cp) ? SRSLTE_CP_NORM_LEN : SRSLTE_CP_EXT_LEN));
const cf_t *sss_ptr = &input_ptr[sss_idx];
// Correct CFO if detected in PSS
if (q->cfo_pss_enable) {
srslte_cfo_correct(&q->cfo_corr_symbol, sss_ptr, q->sss_filt, -q->cfo_pss_mean / q->fft_size);
// Equalize channel if estimated in PSS
if (q->sss_channel_equalize && q->pss.chest_on_filter && q->pss_filtering_enabled) {
srslte_vec_prod_ccc(&q->sss_filt[q->fft_size/2-SRSLTE_PSS_LEN/2], q->pss.tmp_ce,
&q->sss_filt[q->fft_size/2-SRSLTE_PSS_LEN/2], SRSLTE_PSS_LEN);
}
sss_ptr = q->sss_filt;
}
if (sync_sss_symbol(q, sss_ptr) < 0) {
fprintf(stderr, "Error correlating SSS\n");
return -1;
}
}
// Detect CP length
if (q->detect_cp) {
srslte_sync_set_cp(q, srslte_sync_detect_cp(q, input_ptr, peak_pos + find_offset));
} else {
DEBUG("Not enough room to detect CP length. Peak position: %d\n", peak_pos);
}
ret = SRSLTE_SYNC_FOUND;
} else {
ret = SRSLTE_SYNC_FOUND_NOSPACE;
}
} else {
ret = SRSLTE_SYNC_NOFOUND;
}
DEBUG("SYNC ret=%d N_id_2=%d find_offset=%d frame_len=%d, pos=%d peak=%.2f threshold=%.2f sf_idx=%d, CFO=%.3f kHz\n",
ret, q->N_id_2, find_offset, q->frame_size, peak_pos, q->peak_value,
q->threshold, q->sf_idx, 15*(srslte_sync_get_cfo(q)));
} 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;
}
