lte_est_freq_offset.c 9.44 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21
/*
 * Licensed to the OpenAirInterface (OAI) Software Alliance under one or more
 * contributor license agreements.  See the NOTICE file distributed with
 * this work for additional information regarding copyright ownership.
 * The OpenAirInterface Software Alliance licenses this file to You under
 * the OAI Public License, Version 1.0  (the "License"); you may not use this file
 * except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.openairinterface.org/?page_id=698
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *-------------------------------------------------------------------------------
 * For more information about the OpenAirInterface (OAI) Software Alliance:
 *      contact@openairinterface.org
 */

22
/*
23 24 25 26 27 28 29 30
   file: lte_est_freq_offset.c
   author (c): florian.kaltenberger@eurecom.fr
   date: 19.11.2009
*/

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

31
#if defined(__x86_64__) || defined(__i386__)
32
__m128i avg128F;
33 34 35
#elif defined(__arm__)
int32x4_t avg128F;
#endif
36 37

//compute average channel_level on each (TX,RX) antenna pair
38
int dl_channel_level(int16_t *dl_ch,
39 40
                     LTE_DL_FRAME_PARMS *frame_parms)
{
41

42
  int16_t rb;
43
#if defined(__x86_64__) || defined(__i386__)
44
  __m128i *dl_ch128;
45 46 47
#elif defined(__arm__)
  int16x4_t *dl_ch128;
#endif
48 49
  int avg;

50
  //clear average level
51
#if defined(__x86_64__) || defined(__i386__)
52
  avg128F = _mm_setzero_si128();
53 54
  dl_ch128=(__m128i *)dl_ch;

55 56
  for (rb=0; rb<frame_parms->N_RB_DL; rb++) {

57 58 59
    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]));
60 61 62

    dl_ch128+=3;

63
  }
64 65 66
#elif defined(__arm__)
  avg128F = vdupq_n_s32(0);
  dl_ch128=(int16x4_t *)dl_ch;
67

68 69 70 71 72 73 74 75 76 77 78 79 80 81 82
  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
83
  DevAssert( frame_parms->N_RB_DL );
84 85 86 87 88
  avg = (((int*)&avg128F)[0] +
         ((int*)&avg128F)[1] +
         ((int*)&avg128F)[2] +
         ((int*)&avg128F)[3])/(frame_parms->N_RB_DL*12);

89

90
#if defined(__x86_64__) || defined(__i386__)
91 92
  _mm_empty();
  _m_empty();
93
#endif
94 95 96 97
  return(avg);
}

int lte_est_freq_offset(int **dl_ch_estimates,
98 99
                        LTE_DL_FRAME_PARMS *frame_parms,
                        int l,
100 101
                        int* freq_offset,
			int reset)
102
{
103

104
  int ch_offset, omega, dl_ch_shift;
105
  struct complex16 omega_cpx;
106 107 108
  double phase_offset;
  int freq_offset_est;
  unsigned char aa;
109
  int16_t *dl_ch,*dl_ch_prev;
110 111 112 113
  static int first_run = 1;
  int coef = 1<<10;
  int ncoef =  32767 - coef;

114 115 116
  // initialize the averaging filter to initial value
  if (reset!=0)
    first_run=1;
117 118

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

120 121 122 123 124 125 126 127
  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++) {
128 129
  for (aa=0; aa<1; aa++) {

130
    dl_ch = (int16_t *)&dl_ch_estimates[aa][12+ch_offset];
131

knopp's avatar
knopp committed
132
    dl_ch_shift = 6+(log2_approx(dl_channel_level(dl_ch,frame_parms))/2);
133
    //    printf("dl_ch_shift: %d\n",dl_ch_shift);
134

135
    if (ch_offset == 0)
136
      dl_ch_prev = (int16_t *)&dl_ch_estimates[aa][12+(4-frame_parms->Ncp)*(frame_parms->ofdm_symbol_size)];
137
    else
138
      dl_ch_prev = (int16_t *)&dl_ch_estimates[aa][12+0];
139 140


141 142 143 144
    // 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);
145 146
    omega_cpx.r = ((struct complex16*) &omega)->r;
    omega_cpx.i = ((struct complex16*) &omega)->i;
147

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

150
    if (ch_offset == 0)
151
      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)];
152
    else
153
      dl_ch_prev = (int16_t *)&dl_ch_estimates[aa][((frame_parms->N_RB_DL/2) + 1)*12];
154

155 156
    // 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);
157 158 159 160

    omega_cpx.r += ((struct complex16*) &omega)->r;
    omega_cpx.i += ((struct complex16*) &omega)->i;

161
    //    phase_offset += atan2((double)omega_cpx->i,(double)omega_cpx->r);
162
    phase_offset += atan2((double)omega_cpx.i,(double)omega_cpx.r);
163
    //    LOG_I(PHY,"omega (%d,%d) -> %f\n",omega_cpx->r,omega_cpx->i,phase_offset);
164
  }
165 166

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

knopp's avatar
knopp committed
168
  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
169
  //  LOG_I(PHY,"symbol %d : freq_offset_est %d\n",l,freq_offset_est);
170 171 172 173 174

  // update freq_offset with phase_offset using a moving average filter
  if (first_run == 1) {
    *freq_offset = freq_offset_est;
    first_run = 0;
175
  } else
176 177 178 179
    *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);
180 181 182 183 184
  /*
  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
185 186 187 188 189 190 191

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

int lte_mbsfn_est_freq_offset(int **dl_ch_estimates,
192 193 194 195
                              LTE_DL_FRAME_PARMS *frame_parms,
                              int l,
                              int* freq_offset)
{
196

197 198
  int ch_offset, omega, dl_ch_shift;
  struct complex16 *omega_cpx;
199 200 201
  double phase_offset;
  int freq_offset_est;
  unsigned char aa;
202
  int16_t *dl_ch,*dl_ch_prev;
203 204 205 206 207 208
  static int first_run = 1;
  int coef = 1<<10;
  int ncoef =  32767 - coef;


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

210
  if ((l!=2) && (l!=6) && (l!=10)) {
211
    msg("lte_est_freq_offset: l (%d) must be 2 or 6 or 10", l);
212 213 214 215 216 217
    return(-1);
  }

  phase_offset = 0.0;

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

220
    dl_ch = (int16_t *)&dl_ch_estimates[aa][12+ch_offset];
221

222 223
    dl_ch_shift = 4+(log2_approx(dl_channel_level(dl_ch,frame_parms))/2);
    //    printf("dl_ch_shift: %d\n",dl_ch_shift);
224

225
    if (ch_offset == 0)
226
      dl_ch_prev = (int16_t *)&dl_ch_estimates[aa][12+(10*(frame_parms->ofdm_symbol_size))];
227
    else
228
      dl_ch_prev = (int16_t *)&dl_ch_estimates[aa][12+6];
229 230 231 232

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

233 234 235 236 237
    // 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;
238

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

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

243
    if (ch_offset == 0)
244
      dl_ch_prev = (int16_t *)&dl_ch_estimates[aa][(((frame_parms->N_RB_DL/2) + 1)*12)+10*(frame_parms->ofdm_symbol_size)];
245
    else
246
      dl_ch_prev = (int16_t *)&dl_ch_estimates[aa][((frame_parms->N_RB_DL/2) + 1)*12+6];
247

248 249 250 251 252 253 254 255
    // 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);
  }
256 257

  //  phase_offset /= (frame_parms->nb_antennas_rx*frame_parms->nb_antennas_tx);
258 259 260 261 262 263 264 265

  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;
266
  } else
267 268 269 270
    *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);
271 272 273 274 275
  /*
  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
276 277 278

  return(0);
}