lte_est_freq_offset.c 9.68 KB
Newer Older
ghaddab's avatar
ghaddab committed
1
/*******************************************************************************
2
    OpenAirInterface
ghaddab's avatar
ghaddab committed
3 4 5 6 7 8 9 10 11 12 13 14 15 16
    Copyright(c) 1999 - 2014 Eurecom

    OpenAirInterface is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.


    OpenAirInterface is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
17 18
    along with OpenAirInterface.The full GNU General Public License is
   included in this distribution in the file called "COPYING". If not,
ghaddab's avatar
ghaddab committed
19 20 21 22 23 24
   see <http://www.gnu.org/licenses/>.

  Contact Information
  OpenAirInterface Admin: openair_admin@eurecom.fr
  OpenAirInterface Tech : openair_tech@eurecom.fr
  OpenAirInterface Dev  : openair4g-devel@eurecom.fr
25

ghaddab's avatar
ghaddab committed
26
  Address      : Eurecom, Campus SophiaTech, 450 Route des Chappes, CS 50193 - 06904 Biot Sophia Antipolis cedex, FRANCE
ghaddab's avatar
ghaddab committed
27 28

 *******************************************************************************/
29
/*
30 31 32 33 34 35 36 37
   file: lte_est_freq_offset.c
   author (c): florian.kaltenberger@eurecom.fr
   date: 19.11.2009
*/

#include "PHY/defs.h"
//#define DEBUG_PHY

38
#if defined(__x86_64__) || defined(__i386__)
39
__m128i avg128F;
40 41 42
#elif defined(__arm__)
int32x4_t avg128F;
#endif
43 44

//compute average channel_level on each (TX,RX) antenna pair
45
int dl_channel_level(int16_t *dl_ch,
46 47
                     LTE_DL_FRAME_PARMS *frame_parms)
{
48

49
  int16_t rb;
50
#if defined(__x86_64__) || defined(__i386__)
51
  __m128i *dl_ch128;
52 53 54
#elif defined(__arm__)
  int16x4_t *dl_ch128;
#endif
55 56
  int avg;

57
  //clear average level
58
#if defined(__x86_64__) || defined(__i386__)
59
  avg128F = _mm_setzero_si128();
60 61
  dl_ch128=(__m128i *)dl_ch;

62 63
  for (rb=0; rb<frame_parms->N_RB_DL; rb++) {

64 65 66
    avg128F = _mm_add_epi32(avg128F,_mm_madd_epi16(dl_ch128[0],dl_ch128[0]));
    avg128F = _mm_add_epi32(avg128F,_mm_madd_epi16(dl_ch128[1],dl_ch128[1]));
    avg128F = _mm_add_epi32(avg128F,_mm_madd_epi16(dl_ch128[2],dl_ch128[2]));
67 68 69

    dl_ch128+=3;

70
  }
71 72 73
#elif defined(__arm__)
  avg128F = vdupq_n_s32(0);
  dl_ch128=(int16x4_t *)dl_ch;
74

75 76 77 78 79 80 81 82 83 84 85 86 87 88 89
  for (rb=0; rb<frame_parms->N_RB_DL; rb++) {

       avg128F = vqaddq_s32(avg128F,vmull_s16(dl_ch128[0],dl_ch128[0]));
       avg128F = vqaddq_s32(avg128F,vmull_s16(dl_ch128[1],dl_ch128[1]));
       avg128F = vqaddq_s32(avg128F,vmull_s16(dl_ch128[2],dl_ch128[2]));
       avg128F = vqaddq_s32(avg128F,vmull_s16(dl_ch128[3],dl_ch128[3]));
       avg128F = vqaddq_s32(avg128F,vmull_s16(dl_ch128[4],dl_ch128[4]));
       avg128F = vqaddq_s32(avg128F,vmull_s16(dl_ch128[5],dl_ch128[5]));
       dl_ch128+=6;


  }


#endif
90
  DevAssert( frame_parms->N_RB_DL );
91 92 93 94 95
  avg = (((int*)&avg128F)[0] +
         ((int*)&avg128F)[1] +
         ((int*)&avg128F)[2] +
         ((int*)&avg128F)[3])/(frame_parms->N_RB_DL*12);

96

97
#if defined(__x86_64__) || defined(__i386__)
98 99
  _mm_empty();
  _m_empty();
100
#endif
101 102 103 104
  return(avg);
}

int lte_est_freq_offset(int **dl_ch_estimates,
105 106
                        LTE_DL_FRAME_PARMS *frame_parms,
                        int l,
107 108
                        int* freq_offset,
			int reset)
109
{
110

111 112
  int ch_offset, omega, dl_ch_shift;
  struct complex16 *omega_cpx;
113 114 115
  double phase_offset;
  int freq_offset_est;
  unsigned char aa;
116
  int16_t *dl_ch,*dl_ch_prev;
117 118 119 120
  static int first_run = 1;
  int coef = 1<<10;
  int ncoef =  32767 - coef;

121 122 123
  // initialize the averaging filter to initial value
  if (reset!=0)
    first_run=1;
124 125

  ch_offset = (l*(frame_parms->ofdm_symbol_size));
126

127 128 129 130 131 132 133 134
  if ((l!=0) && (l!=(4-frame_parms->Ncp))) {
    msg("lte_est_freq_offset: l (%d) must be 0 or %d\n",l,4-frame_parms->Ncp);
    return(-1);
  }

  phase_offset = 0.0;

  //  for (aa=0;aa<frame_parms->nb_antennas_rx*frame_parms->nb_antennas_tx;aa++) {
135 136
  for (aa=0; aa<1; aa++) {

137
    dl_ch = (int16_t *)&dl_ch_estimates[aa][12+ch_offset];
138

139
    dl_ch_shift = 6+(log2_approx(dl_channel_level(dl_ch,frame_parms))/2);
140
    //    printf("dl_ch_shift: %d\n",dl_ch_shift);
141

142
    if (ch_offset == 0)
143
      dl_ch_prev = (int16_t *)&dl_ch_estimates[aa][12+(4-frame_parms->Ncp)*(frame_parms->ofdm_symbol_size)];
144
    else
145
      dl_ch_prev = (int16_t *)&dl_ch_estimates[aa][12+0];
146 147


148 149 150 151 152
    // calculate omega = angle(conj(dl_ch)*dl_ch_prev))
    //    printf("Computing freq_offset\n");
    omega = dot_product(dl_ch,dl_ch_prev,(frame_parms->N_RB_DL/2 - 1)*12,dl_ch_shift);
    //omega = dot_product(dl_ch,dl_ch_prev,frame_parms->ofdm_symbol_size,15);
    omega_cpx = (struct complex16*) &omega;
153

154
    //    printf("omega (%d,%d)\n",omega_cpx->r,omega_cpx->i);
155

156
    dl_ch = (int16_t *)&dl_ch_estimates[aa][(((frame_parms->N_RB_DL/2) + 1)*12) + ch_offset];
157

158
    if (ch_offset == 0)
159
      dl_ch_prev = (int16_t *)&dl_ch_estimates[aa][(((frame_parms->N_RB_DL/2) + 1)*12)+(4-frame_parms->Ncp)*(frame_parms->ofdm_symbol_size)];
160
    else
161
      dl_ch_prev = (int16_t *)&dl_ch_estimates[aa][((frame_parms->N_RB_DL/2) + 1)*12];
162

163 164 165 166 167 168
    // calculate omega = angle(conj(dl_ch)*dl_ch_prev))
    omega = dot_product(dl_ch,dl_ch_prev,((frame_parms->N_RB_DL/2) - 1)*12,dl_ch_shift);
    omega_cpx->r += ((struct complex16*) &omega)->r;
    omega_cpx->i += ((struct complex16*) &omega)->i;
    //    phase_offset += atan2((double)omega_cpx->i,(double)omega_cpx->r);
    phase_offset += atan2((double)omega_cpx->i,(double)omega_cpx->r);
169
    //    LOG_I(PHY,"omega (%d,%d) -> %f\n",omega_cpx->r,omega_cpx->i,phase_offset);
170
  }
171 172

  //  phase_offset /= (frame_parms->nb_antennas_rx*frame_parms->nb_antennas_tx);
173

174
  freq_offset_est = (int) (phase_offset/(2*M_PI)/(frame_parms->Ncp==NORMAL ? (285.8e-6):(2.5e-4))); //2.5e-4 is the time between pilot symbols
175
  //  LOG_I(PHY,"symbol %d : freq_offset_est %d\n",l,freq_offset_est);
176 177 178 179 180

  // update freq_offset with phase_offset using a moving average filter
  if (first_run == 1) {
    *freq_offset = freq_offset_est;
    first_run = 0;
181
  } else
182 183 184 185
    *freq_offset = ((freq_offset_est * coef) + (*freq_offset * ncoef)) >> 15;

  //#ifdef DEBUG_PHY
  //    msg("l=%d, phase_offset = %f (%d,%d), freq_offset_est = %d Hz, freq_offset_filt = %d \n",l,phase_offset,omega_cpx->r,omega_cpx->i,freq_offset_est,*freq_offset);
186 187 188 189 190
  /*
  for (i=0;i<150;i++)
    msg("i %d : %d,%d <=> %d,%d\n",i,dl_ch[2*i],dl_ch[(2*i)+1],dl_ch_prev[2*i],dl_ch_prev[(2*i)+1]);
  */
  //#endif
191 192 193 194 195 196 197

  return(0);
}
//******************************************************************************************************
// LTE MBSFN Frequency offset estimation

int lte_mbsfn_est_freq_offset(int **dl_ch_estimates,
198 199 200 201
                              LTE_DL_FRAME_PARMS *frame_parms,
                              int l,
                              int* freq_offset)
{
202

203 204
  int ch_offset, omega, dl_ch_shift;
  struct complex16 *omega_cpx;
205 206 207
  double phase_offset;
  int freq_offset_est;
  unsigned char aa;
208
  int16_t *dl_ch,*dl_ch_prev;
209 210 211 212 213 214
  static int first_run = 1;
  int coef = 1<<10;
  int ncoef =  32767 - coef;


  ch_offset = (l*(frame_parms->ofdm_symbol_size));
215

216
  if ((l!=2) && (l!=6) && (l!=10)) {
217
    msg("lte_est_freq_offset: l (%d) must be 2 or 6 or 10", l);
218 219 220 221 222 223
    return(-1);
  }

  phase_offset = 0.0;

  //  for (aa=0;aa<frame_parms->nb_antennas_rx*frame_parms->nb_antennas_tx;aa++) {
224 225
  for (aa=0; aa<1; aa++) {

226
    dl_ch = (int16_t *)&dl_ch_estimates[aa][12+ch_offset];
227

228 229
    dl_ch_shift = 4+(log2_approx(dl_channel_level(dl_ch,frame_parms))/2);
    //    printf("dl_ch_shift: %d\n",dl_ch_shift);
230

231
    if (ch_offset == 0)
232
      dl_ch_prev = (int16_t *)&dl_ch_estimates[aa][12+(10*(frame_parms->ofdm_symbol_size))];
233
    else
234
      dl_ch_prev = (int16_t *)&dl_ch_estimates[aa][12+6];
235 236 237 238

    //else if
    // dl_ch_prev = (int16_t *)&dl_ch_estimates[aa][12+0];

239 240 241 242 243
    // calculate omega = angle(conj(dl_ch)*dl_ch_prev))
    //    printf("Computing freq_offset\n");
    omega = dot_product(dl_ch,dl_ch_prev,(frame_parms->N_RB_DL/2 - 1)*12,dl_ch_shift);
    //omega = dot_product(dl_ch,dl_ch_prev,frame_parms->ofdm_symbol_size,15);
    omega_cpx = (struct complex16*) &omega;
244

245
    //    printf("omega (%d,%d)\n",omega_cpx->r,omega_cpx->i);
246

247
    dl_ch = (int16_t *)&dl_ch_estimates[aa][(((frame_parms->N_RB_DL/2) + 1)*12) + ch_offset];
248

249
    if (ch_offset == 0)
250
      dl_ch_prev = (int16_t *)&dl_ch_estimates[aa][(((frame_parms->N_RB_DL/2) + 1)*12)+10*(frame_parms->ofdm_symbol_size)];
251
    else
252
      dl_ch_prev = (int16_t *)&dl_ch_estimates[aa][((frame_parms->N_RB_DL/2) + 1)*12+6];
253

254 255 256 257 258 259 260 261
    // calculate omega = angle(conj(dl_ch)*dl_ch_prev))
    omega = dot_product(dl_ch,dl_ch_prev,((frame_parms->N_RB_DL/2) - 1)*12,dl_ch_shift);
    omega_cpx->r += ((struct complex16*) &omega)->r;
    omega_cpx->i += ((struct complex16*) &omega)->i;
    //    phase_offset += atan2((double)omega_cpx->i,(double)omega_cpx->r);
    phase_offset += atan2((double)omega_cpx->i,(double)omega_cpx->r);
    //    printf("omega (%d,%d) -> %f\n",omega_cpx->r,omega_cpx->i,phase_offset);
  }
262 263

  //  phase_offset /= (frame_parms->nb_antennas_rx*frame_parms->nb_antennas_tx);
264 265 266 267 268 269 270 271

  freq_offset_est = (int) (phase_offset/(2*M_PI)/2.5e-4); //2.5e-4 is the time between pilot symbols
  //  printf("symbol %d : freq_offset_est %d\n",l,freq_offset_est);

  // update freq_offset with phase_offset using a moving average filter
  if (first_run == 1) {
    *freq_offset = freq_offset_est;
    first_run = 0;
272
  } else
273 274 275 276
    *freq_offset = ((freq_offset_est * coef) + (*freq_offset * ncoef)) >> 15;

  //#ifdef DEBUG_PHY
  //    msg("l=%d, phase_offset = %f (%d,%d), freq_offset_est = %d Hz, freq_offset_filt = %d \n",l,phase_offset,omega_cpx->r,omega_cpx->i,freq_offset_est,*freq_offset);
277 278 279 280 281
  /*
  for (i=0;i<150;i++)
    msg("i %d : %d,%d <=> %d,%d\n",i,dl_ch[2*i],dl_ch[(2*i)+1],dl_ch_prev[2*i],dl_ch_prev[(2*i)+1]);
  */
  //#endif
282 283 284

  return(0);
}