nr_pbch.c 23.2 KB
Newer Older
Hongzhi Wang's avatar
Hongzhi Wang committed
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35
/*
 * 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
 */

/*! \file PHY/LTE_TRANSPORT/pbch.c
* \brief Top-level routines for generating and decoding  the PBCH/BCH physical/transport channel V8.6 2009-03
* \author R. Knopp, F. Kaltenberger
* \date 2011
* \version 0.1
* \company Eurecom
* \email: knopp@eurecom.fr,florian.kaltenberger.fr
* \note
* \warning
*/
#include "PHY/defs_nr_UE.h"
#include "PHY/CODING/coding_extern.h"
#include "PHY/phy_extern_nr_ue.h"
#include "PHY/sse_intrin.h"
hongzhi wang's avatar
hongzhi wang committed
36
#include "PHY/LTE_REFSIG/lte_refsig.h"
Hongzhi Wang's avatar
Hongzhi Wang committed
37

Raymond Knopp's avatar
Raymond Knopp committed
38
//#define DEBUG_PBCH 1
Hongzhi Wang's avatar
Hongzhi Wang committed
39 40 41 42 43 44 45 46
//#define DEBUG_PBCH_ENCODING

#ifdef OPENAIR2
//#include "PHY_INTERFACE/defs.h"
#endif

#define PBCH_A 24

Raymond Knopp's avatar
Raymond Knopp committed
47
#define print_shorts(s,x) printf("%s : %d,%d,%d,%d,%d,%d,%d,%d\n",s,((int16_t*)x)[0],((int16_t*)x)[1],((int16_t*)x)[2],((int16_t*)x)[3],((int16_t*)x)[4],((int16_t*)x)[5],((int16_t*)x)[6],((int16_t*)x)[7])
Hongzhi Wang's avatar
Hongzhi Wang committed
48 49

uint16_t nr_pbch_extract(int **rxdataF,
Raymond Knopp's avatar
Raymond Knopp committed
50 51 52 53
			 int **dl_ch_estimates,
			 int **rxdataF_ext,
			 int **dl_ch_estimates_ext,
			 uint32_t symbol,
54
			 uint32_t s_offset,
Raymond Knopp's avatar
Raymond Knopp committed
55 56
			 uint32_t high_speed_flag,
			 NR_DL_FRAME_PARMS *frame_parms)
Hongzhi Wang's avatar
Hongzhi Wang committed
57 58 59 60
{


  uint16_t rb;
Raymond Knopp's avatar
Raymond Knopp committed
61
  uint8_t i,j,aarx;
hongzhi wang's avatar
hongzhi wang committed
62
  int32_t *dl_ch0,*dl_ch0_ext,*rxF,*rxF_ext;
Hongzhi Wang's avatar
Hongzhi Wang committed
63

hongzhi wang's avatar
hongzhi wang committed
64
  int nushiftmod4 = frame_parms->nushift;
Hongzhi Wang's avatar
Hongzhi Wang committed
65

Raymond Knopp's avatar
Raymond Knopp committed
66 67 68
  unsigned int  rx_offset = frame_parms->first_carrier_offset + frame_parms->ssb_start_subcarrier; //and
  if (rx_offset>= frame_parms->ofdm_symbol_size) rx_offset-=frame_parms->ofdm_symbol_size;
 
69 70 71
  AssertFatal(symbol>=1 && symbol<5, 
	      "symbol %d illegal for PBCH extraction\n",
	      symbol);
Raymond Knopp's avatar
Raymond Knopp committed
72

Hongzhi Wang's avatar
Hongzhi Wang committed
73
  for (aarx=0; aarx<frame_parms->nb_antennas_rx; aarx++) {
hongzhi wang's avatar
hongzhi wang committed
74
        
75
    rxF        = &rxdataF[aarx][(symbol+s_offset)*frame_parms->ofdm_symbol_size];
76
    rxF_ext    = &rxdataF_ext[aarx][symbol*20*12];
hongzhi wang's avatar
hongzhi wang committed
77
#ifdef DEBUG_PBCH
hongzhi wang's avatar
hongzhi wang committed
78
     printf("extract_rbs (nushift %d): rx_offset=%d, symbol %d\n",frame_parms->nushift,
79
	    (rx_offset + ((symbol+s_offset)*(frame_parms->ofdm_symbol_size))),symbol);
hongzhi wang's avatar
hongzhi wang committed
80
     int16_t *p = (int16_t *)rxF;
hongzhi wang's avatar
hongzhi wang committed
81
     for (int i =0; i<8;i++){
hongzhi wang's avatar
hongzhi wang committed
82
        printf("rxF [%d]= %d\n",i,rxF[i]);
hongzhi wang's avatar
hongzhi wang committed
83 84
        printf("pbch extract rxF  %d %d addr %p\n", p[2*i], p[2*i+1], &p[2*i]);
        printf("rxF ext addr %p\n", &rxF_ext[i]);
hongzhi wang's avatar
hongzhi wang committed
85 86
     }
#endif
Hongzhi Wang's avatar
Hongzhi Wang committed
87 88

    for (rb=0; rb<20; rb++) {
hongzhi wang's avatar
hongzhi wang committed
89
      j=0;
Raymond Knopp's avatar
Raymond Knopp committed
90

91
      if (symbol==1 || symbol==3) {
Hongzhi Wang's avatar
Hongzhi Wang committed
92 93 94 95
        for (i=0; i<12; i++) {
          if ((i!=nushiftmod4) &&
              (i!=(nushiftmod4+4)) &&
              (i!=(nushiftmod4+8))) {
Raymond Knopp's avatar
Raymond Knopp committed
96 97 98 99 100 101 102 103 104
            rxF_ext[j]=rxF[rx_offset];
#ifdef DEBUG_PBCH
	    printf("rxF ext[%d] = (%d,%d) rxF [%d]= (%d,%d)\n",(9*rb) + j,
		   ((int16_t*)&rxF_ext[j])[0],
		   ((int16_t*)&rxF_ext[j])[1],
		   rx_offset,
		   ((int16_t*)&rxF[rx_offset])[0],
		   ((int16_t*)&rxF[rx_offset])[1]);
#endif
hongzhi wang's avatar
hongzhi wang committed
105
	    j++;
Hongzhi Wang's avatar
Hongzhi Wang committed
106
          }
Raymond Knopp's avatar
Raymond Knopp committed
107
	  rx_offset=(rx_offset+1)&(frame_parms->ofdm_symbol_size-1);
Hongzhi Wang's avatar
Hongzhi Wang committed
108
        }
hongzhi wang's avatar
hongzhi wang committed
109
        rxF_ext+=9;
Hongzhi Wang's avatar
Hongzhi Wang committed
110
      } else { //symbol 2
hongzhi wang's avatar
hongzhi wang committed
111
    	if ((rb < 4) || (rb >15)){
Hongzhi Wang's avatar
Hongzhi Wang committed
112
    	  for (i=0; i<12; i++) {
Raymond Knopp's avatar
Raymond Knopp committed
113 114 115 116 117 118 119 120 121 122 123 124 125 126 127
	    if ((i!=nushiftmod4) &&
		(i!=(nushiftmod4+4)) &&
		(i!=(nushiftmod4+8))) {
	      rxF_ext[j]=rxF[rx_offset];
#ifdef DEBUG_PBCH
	      printf("rxF ext[%d] = (%d,%d) rxF [%d]= (%d,%d)\n",(rb<4) ? (9*rb) + j : (9*(rb-12))+j,
		     ((int16_t*)&rxF_ext[j])[0],
		     ((int16_t*)&rxF_ext[j])[1],
		     rx_offset,
		     ((int16_t*)&rxF[rx_offset])[0],
		     ((int16_t*)&rxF[rx_offset])[1]);
#endif    
	      j++;
	    }
	    rx_offset=(rx_offset+1)&(frame_parms->ofdm_symbol_size-1);
Hongzhi Wang's avatar
Hongzhi Wang committed
128
    	  }
Raymond Knopp's avatar
Raymond Knopp committed
129
	  rxF_ext+=9;
hongzhi wang's avatar
hongzhi wang committed
130
	}
Raymond Knopp's avatar
Raymond Knopp committed
131 132
	else rx_offset = (rx_offset+12)&(frame_parms->ofdm_symbol_size-1);
        
Hongzhi Wang's avatar
Hongzhi Wang committed
133 134 135
      }
    }

Raymond Knopp's avatar
Raymond Knopp committed
136
    if (high_speed_flag == 1)
137
      dl_ch0     = &dl_ch_estimates[aarx][((symbol+s_offset)*(frame_parms->ofdm_symbol_size))];
Raymond Knopp's avatar
Raymond Knopp committed
138 139 140 141 142
    else
      dl_ch0     = &dl_ch_estimates[aarx][0];
    
    //printf("dl_ch0 addr %p\n",dl_ch0);
    
143
    dl_ch0_ext = &dl_ch_estimates_ext[aarx][symbol*20*12];
Raymond Knopp's avatar
Raymond Knopp committed
144 145 146
    
    for (rb=0; rb<20; rb++) {
      j=0;
147
      if (symbol==1 || symbol==3) {
Raymond Knopp's avatar
Raymond Knopp committed
148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167
	for (i=0; i<12; i++) {
	  if ((i!=nushiftmod4) &&
	      (i!=(nushiftmod4+4)) &&
	      (i!=(nushiftmod4+8))) {
	    dl_ch0_ext[j]=dl_ch0[i];
#ifdef DEBUG_PBCH
	    if ((rb==0) && (i<2))
	      printf("dl ch0 ext[%d] = (%d,%d)  dl_ch0 [%d]= (%d,%d)\n",j,
		     ((int16_t*)&dl_ch0_ext[j])[0],
		     ((int16_t*)&dl_ch0_ext[j])[1],
		     i,
		     ((int16_t*)&dl_ch0[i])[0],
		     ((int16_t*)&dl_ch0[i])[1]);
#endif
	    j++;
	  }
	}
	
	dl_ch0+=12;
	dl_ch0_ext+=9;
Hongzhi Wang's avatar
Hongzhi Wang committed
168
      }
Raymond Knopp's avatar
Raymond Knopp committed
169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191
      else { 
	if ((rb < 4) || (rb >15)){
	  for (i=0; i<12; i++) {
	    if ((i!=nushiftmod4) &&
		(i!=(nushiftmod4+4)) &&
		(i!=(nushiftmod4+8))) {
	      dl_ch0_ext[j]=dl_ch0[i];
#ifdef DEBUG_PBCH
	      printf("dl ch0 ext[%d] = (%d,%d)  dl_ch0 [%d]= (%d,%d)\n",j,
		     ((int16_t*)&dl_ch0_ext[j])[0],
		     ((int16_t*)&dl_ch0_ext[j])[1],
		     i,
		     ((int16_t*)&dl_ch0[i])[0],
		     ((int16_t*)&dl_ch0[i])[1]);
#endif
	      j++;
	    }
	  }
	  dl_ch0_ext+=9;
	}
	dl_ch0+=12;
      }
    }
Hongzhi Wang's avatar
Hongzhi Wang committed
192 193 194 195 196 197 198 199 200
  }

  return(0);
}

//__m128i avg128;

//compute average channel_level on each (TX,RX) antenna pair
int nr_pbch_channel_level(int **dl_ch_estimates_ext,
Raymond Knopp's avatar
Raymond Knopp committed
201 202
			  NR_DL_FRAME_PARMS *frame_parms,
			  uint32_t symbol)
Hongzhi Wang's avatar
Hongzhi Wang committed
203 204
{

hongzhi wang's avatar
hongzhi wang committed
205
  int16_t rb, nb_rb=20;
Raymond Knopp's avatar
Raymond Knopp committed
206
  uint8_t aarx;
Hongzhi Wang's avatar
Hongzhi Wang committed
207 208 209 210 211 212 213 214 215 216

#if defined(__x86_64__) || defined(__i386__)
  __m128i avg128;
  __m128i *dl_ch128;
#elif defined(__arm__)
  int32x4_t avg128;
  int16x8_t *dl_ch128;
#endif
  int avg1=0,avg2=0;

Raymond Knopp's avatar
Raymond Knopp committed
217 218 219
  for (aarx=0; aarx<frame_parms->nb_antennas_rx; aarx++) {
    //clear average level
    
Hongzhi Wang's avatar
Hongzhi Wang committed
220
#if defined(__x86_64__) || defined(__i386__)
Raymond Knopp's avatar
Raymond Knopp committed
221 222
    avg128 = _mm_setzero_si128();
    dl_ch128=(__m128i *)&dl_ch_estimates_ext[aarx][symbol*20*12];
Hongzhi Wang's avatar
Hongzhi Wang committed
223
#elif defined(__arm__)
Raymond Knopp's avatar
Raymond Knopp committed
224 225 226
    avg128 = vdupq_n_s32(0);
    dl_ch128=(int16x8_t *)&dl_ch_estimates_ext[aarx][symbol*20*12];
    
Hongzhi Wang's avatar
Hongzhi Wang committed
227
#endif
Raymond Knopp's avatar
Raymond Knopp committed
228
    for (rb=0; rb<nb_rb; rb++) {
Hongzhi Wang's avatar
Hongzhi Wang committed
229
#if defined(__x86_64__) || defined(__i386__)
Raymond Knopp's avatar
Raymond Knopp committed
230 231 232
      avg128 = _mm_add_epi32(avg128,_mm_madd_epi16(dl_ch128[0],dl_ch128[0]));
      avg128 = _mm_add_epi32(avg128,_mm_madd_epi16(dl_ch128[1],dl_ch128[1]));
      avg128 = _mm_add_epi32(avg128,_mm_madd_epi16(dl_ch128[2],dl_ch128[2]));
Hongzhi Wang's avatar
Hongzhi Wang committed
233 234 235
#elif defined(__arm__)
// to be filled in
#endif
Raymond Knopp's avatar
Raymond Knopp committed
236 237 238 239 240 241 242 243
      dl_ch128+=3;
      /*
      if (rb==0) {
	print_shorts("dl_ch128",&dl_ch128[0]);
	print_shorts("dl_ch128",&dl_ch128[1]);
	print_shorts("dl_ch128",&dl_ch128[2]);
	}*/
      
Hongzhi Wang's avatar
Hongzhi Wang committed
244
    }
Raymond Knopp's avatar
Raymond Knopp committed
245 246 247 248 249 250 251 252 253 254 255
    
    avg1 = (((int*)&avg128)[0] +
	    ((int*)&avg128)[1] +
	    ((int*)&avg128)[2] +
	    ((int*)&avg128)[3])/(nb_rb*12);
    
    if (avg1>avg2)
      avg2 = avg1;
    
    //LOG_I(PHY,"Channel level : %d, %d\n",avg1, avg2);
  }
Hongzhi Wang's avatar
Hongzhi Wang committed
256 257 258 259 260 261 262 263 264
#if defined(__x86_64__) || defined(__i386__)
  _mm_empty();
  _m_empty();
#endif
  return(avg2);

}

void nr_pbch_channel_compensation(int **rxdataF_ext,
Raymond Knopp's avatar
Raymond Knopp committed
265 266 267
				  int **dl_ch_estimates_ext,
				  int **rxdataF_comp,
				  NR_DL_FRAME_PARMS *frame_parms,
268
				  uint32_t symbol,
Raymond Knopp's avatar
Raymond Knopp committed
269
				  uint8_t output_shift)
Hongzhi Wang's avatar
Hongzhi Wang committed
270 271
{

Florian Kaltenberger's avatar
WIP  
Florian Kaltenberger committed
272
  short conjugate[8]__attribute__((aligned(16))) = {-1,1,-1,1,-1,1,-1,1};
273
  //short conjugate2[8]__attribute__((aligned(16))) = {1,-1,1,-1,1,-1,1,-1};
Raymond Knopp's avatar
Raymond Knopp committed
274 275 276 277 278
#if defined(__x86_64__) || defined(__i386__)
  __m128i mmtmpP0,mmtmpP1,mmtmpP2,mmtmpP3;
#elif defined(__arm__)
  int16x8_t mmtmpP0,mmtmpP1,mmtmpP2,mmtmpP3;
#endif
Florian Kaltenberger's avatar
WIP  
Florian Kaltenberger committed
279

Raymond Knopp's avatar
Raymond Knopp committed
280 281
  uint16_t nb_re=180;
  uint8_t aarx;
282

Hongzhi Wang's avatar
Hongzhi Wang committed
283 284 285 286 287 288
#if defined(__x86_64__) || defined(__i386__)
  __m128i *dl_ch128,*rxdataF128,*rxdataF_comp128;
#elif defined(__arm__)

#endif

289 290 291
  AssertFatal((symbol > 0 && symbol < 4),
	      "symbol %d is illegal for PBCH DM-RS\n",
	      symbol);
292

293
  if (symbol == 2) nb_re = 72;
Hongzhi Wang's avatar
Hongzhi Wang committed
294

295 296
  //  printf("comp: symbol %d : nb_re %d\n",symbol,nb_re);

Raymond Knopp's avatar
Raymond Knopp committed
297 298 299 300 301 302 303 304 305 306 307
  for (aarx=0; aarx<frame_parms->nb_antennas_rx; aarx++) {
    
#if defined(__x86_64__) || defined(__i386__)
    dl_ch128          = (__m128i *)&dl_ch_estimates_ext[aarx][symbol*20*12];
    rxdataF128        = (__m128i *)&rxdataF_ext[aarx][symbol*20*12];
    rxdataF_comp128   = (__m128i *)&rxdataF_comp[aarx][symbol*20*12];
    /*
    printf("ch compensation dl_ch ext addr %p \n", &dl_ch_estimates_ext[aarx][symbol*20*12]);
    printf("rxdataf ext addr %p symbol %d\n", &rxdataF_ext[aarx][symbol*20*12], symbol);
    printf("rxdataf_comp addr %p\n",&rxdataF_comp[aarx][symbol*20*12]); 
    */
Hongzhi Wang's avatar
Hongzhi Wang committed
308 309 310 311
#elif defined(__arm__)
// to be filled in
#endif

Raymond Knopp's avatar
Raymond Knopp committed
312
    for (int re=0; re<nb_re; re+=12) {
313
      //            printf("******re %d\n",re);
Hongzhi Wang's avatar
Hongzhi Wang committed
314
#if defined(__x86_64__) || defined(__i386__)
Raymond Knopp's avatar
Raymond Knopp committed
315 316 317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 339 340 341 342 343 344 345 346 347 348 349 350 351 352 353 354 355 356 357 358 359 360 361 362 363 364 365 366 367 368 369 370 371 372 373 374 375 376
      // multiply by conjugated channel
      mmtmpP0 = _mm_madd_epi16(dl_ch128[0],rxdataF128[0]);
      //  print_ints("re",&mmtmpP0);
      // mmtmpP0 contains real part of 4 consecutive outputs (32-bit)
      mmtmpP1 = _mm_shufflelo_epi16(dl_ch128[0],_MM_SHUFFLE(2,3,0,1));
      mmtmpP1 = _mm_shufflehi_epi16(mmtmpP1,_MM_SHUFFLE(2,3,0,1));
      mmtmpP1 = _mm_sign_epi16(mmtmpP1,*(__m128i*)&conjugate[0]);
      //  print_ints("im",&mmtmpP1);
      mmtmpP1 = _mm_madd_epi16(mmtmpP1,rxdataF128[0]);
      // mmtmpP1 contains imag part of 4 consecutive outputs (32-bit)
      mmtmpP0 = _mm_srai_epi32(mmtmpP0,output_shift);
      //  print_ints("re(shift)",&mmtmpP0);
      mmtmpP1 = _mm_srai_epi32(mmtmpP1,output_shift);
      //  print_ints("im(shift)",&mmtmpP1);
      mmtmpP2 = _mm_unpacklo_epi32(mmtmpP0,mmtmpP1);
      mmtmpP3 = _mm_unpackhi_epi32(mmtmpP0,mmtmpP1);
      //      print_ints("c0",&mmtmpP2);
      //  print_ints("c1",&mmtmpP3);
      rxdataF_comp128[0] = _mm_packs_epi32(mmtmpP2,mmtmpP3);
      /*
        print_shorts("rx:",rxdataF128);
        print_shorts("ch:",dl_ch128);
        print_shorts("pack:",rxdataF_comp128);
      */
      // multiply by conjugated channel
      mmtmpP0 = _mm_madd_epi16(dl_ch128[1],rxdataF128[1]);
      // mmtmpP0 contains real part of 4 consecutive outputs (32-bit)
      mmtmpP1 = _mm_shufflelo_epi16(dl_ch128[1],_MM_SHUFFLE(2,3,0,1));
      mmtmpP1 = _mm_shufflehi_epi16(mmtmpP1,_MM_SHUFFLE(2,3,0,1));
      mmtmpP1 = _mm_sign_epi16(mmtmpP1,*(__m128i*)&conjugate[0]);
      mmtmpP1 = _mm_madd_epi16(mmtmpP1,rxdataF128[1]);
      // mmtmpP1 contains imag part of 4 consecutive outputs (32-bit)
      mmtmpP0 = _mm_srai_epi32(mmtmpP0,output_shift);
      mmtmpP1 = _mm_srai_epi32(mmtmpP1,output_shift);
      mmtmpP2 = _mm_unpacklo_epi32(mmtmpP0,mmtmpP1);
      mmtmpP3 = _mm_unpackhi_epi32(mmtmpP0,mmtmpP1);
      rxdataF_comp128[1] = _mm_packs_epi32(mmtmpP2,mmtmpP3);
      //  print_shorts("rx:",rxdataF128+1);
      //  print_shorts("ch:",dl_ch128+1);
      //  print_shorts("pack:",rxdataF_comp128+1);
      
      // multiply by conjugated channel
      mmtmpP0 = _mm_madd_epi16(dl_ch128[2],rxdataF128[2]);
      // mmtmpP0 contains real part of 4 consecutive outputs (32-bit)
      mmtmpP1 = _mm_shufflelo_epi16(dl_ch128[2],_MM_SHUFFLE(2,3,0,1));
      mmtmpP1 = _mm_shufflehi_epi16(mmtmpP1,_MM_SHUFFLE(2,3,0,1));
      mmtmpP1 = _mm_sign_epi16(mmtmpP1,*(__m128i*)&conjugate[0]);
      mmtmpP1 = _mm_madd_epi16(mmtmpP1,rxdataF128[2]);
      // mmtmpP1 contains imag part of 4 consecutive outputs (32-bit)
      mmtmpP0 = _mm_srai_epi32(mmtmpP0,output_shift);
      mmtmpP1 = _mm_srai_epi32(mmtmpP1,output_shift);
      mmtmpP2 = _mm_unpacklo_epi32(mmtmpP0,mmtmpP1);
      mmtmpP3 = _mm_unpackhi_epi32(mmtmpP0,mmtmpP1);
      rxdataF_comp128[2] = _mm_packs_epi32(mmtmpP2,mmtmpP3);
      //  print_shorts("rx:",rxdataF128+2);
      //  print_shorts("ch:",dl_ch128+2);
      //      print_shorts("pack:",rxdataF_comp128+2);
      
      dl_ch128+=3;
      rxdataF128+=3;
      rxdataF_comp128+=3;
      
Hongzhi Wang's avatar
Hongzhi Wang committed
377
#elif defined(__arm__)
Raymond Knopp's avatar
Raymond Knopp committed
378
      // to be filled in
Hongzhi Wang's avatar
Hongzhi Wang committed
379 380
#endif
    }
Raymond Knopp's avatar
Raymond Knopp committed
381
  }
Hongzhi Wang's avatar
Hongzhi Wang committed
382 383 384 385 386 387 388 389 390 391 392
#if defined(__x86_64__) || defined(__i386__)
  _mm_empty();
  _m_empty();
#endif
}

void nr_pbch_detection_mrc(NR_DL_FRAME_PARMS *frame_parms,
                        int **rxdataF_comp,
                        uint8_t symbol)
{

Raymond Knopp's avatar
Raymond Knopp committed
393
  uint8_t symbol_mod;
Hongzhi Wang's avatar
Hongzhi Wang committed
394 395 396 397 398 399 400 401 402 403
  int i, nb_rb=6;
#if defined(__x86_64__) || defined(__i386__)
  __m128i *rxdataF_comp128_0,*rxdataF_comp128_1;
#elif defined(__arm__)
  int16x8_t *rxdataF_comp128_0,*rxdataF_comp128_1;
#endif
  symbol_mod = (symbol>=(7-frame_parms->Ncp)) ? symbol-(7-frame_parms->Ncp) : symbol;

  if (frame_parms->nb_antennas_rx>1) {
#if defined(__x86_64__) || defined(__i386__)
Raymond Knopp's avatar
Raymond Knopp committed
404 405
      rxdataF_comp128_0   = (__m128i *)&rxdataF_comp[0][symbol_mod*6*12];
      rxdataF_comp128_1   = (__m128i *)&rxdataF_comp[1][symbol_mod*6*12];
Hongzhi Wang's avatar
Hongzhi Wang committed
406
#elif defined(__arm__)
Raymond Knopp's avatar
Raymond Knopp committed
407 408
      rxdataF_comp128_0   = (int16x8_t *)&rxdataF_comp[0][symbol_mod*6*12];
      rxdataF_comp128_1   = (int16x8_t *)&rxdataF_comp[1][symbol_mod*6*12];
Hongzhi Wang's avatar
Hongzhi Wang committed
409 410 411 412 413 414 415 416 417 418 419 420

#endif
      // MRC on each re of rb, both on MF output and magnitude (for 16QAM/64QAM llr computation)
      for (i=0; i<nb_rb*3; i++) {
#if defined(__x86_64__) || defined(__i386__)
        rxdataF_comp128_0[i] = _mm_adds_epi16(_mm_srai_epi16(rxdataF_comp128_0[i],1),_mm_srai_epi16(rxdataF_comp128_1[i],1));
#elif defined(__arm__)
        rxdataF_comp128_0[i] = vhaddq_s16(rxdataF_comp128_0[i],rxdataF_comp128_1[i]);

#endif
      }
    }
Raymond Knopp's avatar
Raymond Knopp committed
421

Hongzhi Wang's avatar
Hongzhi Wang committed
422 423 424 425 426 427
#if defined(__x86_64__) || defined(__i386__)
  _mm_empty();
  _m_empty();
#endif
}

428
void nr_pbch_unscrambling(NR_UE_PBCH *pbch,
Raymond Knopp's avatar
Raymond Knopp committed
429 430 431 432
			  uint16_t Nid,
			  uint8_t nushift,
			  uint16_t M,
			  uint16_t length,
Guy De Souza's avatar
Guy De Souza committed
433 434
			  uint8_t bitwise,
        uint32_t unscrambling_mask)
Hongzhi Wang's avatar
Hongzhi Wang committed
435
{
436
  uint8_t reset, offset;
Hongzhi Wang's avatar
Hongzhi Wang committed
437
  uint32_t x1, x2, s=0;
Guy De Souza's avatar
Guy De Souza committed
438
  uint8_t k=0;
439
  int16_t *demod_pbch_e = pbch->llr;
440

hongzhi wang's avatar
hongzhi wang committed
441

Hongzhi Wang's avatar
Hongzhi Wang committed
442 443
  reset = 1;
  // x1 is set in first call to lte_gold_generic
444
  x2 = Nid; //this is c_init
Hongzhi Wang's avatar
Hongzhi Wang committed
445

446
  // The Gold sequence is shifted by nushift* M, so we skip (nushift*M /32) double words
447
  for (int i=0; i<(uint16_t)ceil(((float)nushift*M)/32); i++) {
448 449 450 451 452 453 454
    s = lte_gold_generic(&x1, &x2, reset);
    reset = 0;
  }
  // Scrambling is now done with offset (nushift*M)%32
  offset = (nushift*M)&0x1f;

  for (int i=0; i<length; i++) {
455 456 457 458 459 460 461
      /*if (((i+offset)&0x1f)==0) {
        s = lte_gold_generic(&x1, &x2, reset);
        reset = 0;
      }*/

      if (bitwise) {
      if (((k+offset)&0x1f)==0 && (!((unscrambling_mask>>i)&1))) {
462 463 464
        s = lte_gold_generic(&x1, &x2, reset);
        reset = 0;
      }
465 466
        (pbch->pbch_a_interleaved) ^= ((unscrambling_mask>>i)&1)? ((pbch->pbch_a_prime>>i)&1)<<i : (((pbch->pbch_a_prime>>i)&1) ^ ((s>>((k+offset)&0x1f))&1))<<i;
        k += (!((unscrambling_mask>>i)&1));
Raymond Knopp's avatar
Raymond Knopp committed
467
#ifdef DEBUG_PBCH_ENCODING
468
    printf("i %d k %d offset %d (unscrambling_mask>>i)&1) %d s: %08x\t  pbch_a_interleaved 0x%08x (!((unscrambling_mask>>i)&1)) %d\n", i, k, offset, (unscrambling_mask>>i)&1, s, pbch->pbch_a_interleaved, (!((unscrambling_mask>>i)&1)));
Raymond Knopp's avatar
Raymond Knopp committed
469
#endif
470
      }
Hongzhi Wang's avatar
Hongzhi Wang committed
471

472
      else {
473 474 475 476
      if (((i+offset)&0x1f)==0) {
        s = lte_gold_generic(&x1, &x2, reset);
        reset = 0;
      }
477
    	  if (((s>>((i+offset)&0x1f))&1)==1)
hongzhi wang's avatar
hongzhi wang committed
478
    		  demod_pbch_e[i] = -demod_pbch_e[i];
479 480 481 482
#ifdef DEBUG_PBCH_ENCODING
		if (i<8)
	printf("s %d demod_pbch_e[i] %d\n", ((s>>((i+offset)&0x1f))&1), demod_pbch_e[i]);
#endif
483
      }
Hongzhi Wang's avatar
Hongzhi Wang committed
484
  }
485

Hongzhi Wang's avatar
Hongzhi Wang committed
486 487
}

488 489 490
void nr_pbch_quantize(int16_t *pbch_llr8,
                      int16_t *pbch_llr,
                      uint16_t len)
Hongzhi Wang's avatar
Hongzhi Wang committed
491 492 493 494 495
{

  uint16_t i;

  for (i=0; i<len; i++) {
496 497 498 499
    if (pbch_llr[i]>31)
      pbch_llr8[i]=32;
    else if (pbch_llr[i]<-31)
      pbch_llr8[i]=-32;
500
    else
hongzhi wang's avatar
hongzhi wang committed
501
      pbch_llr8[i] = (char)(pbch_llr[i]);
Hongzhi Wang's avatar
Hongzhi Wang committed
502 503 504

  }
}
505
/*
hongzhi wang's avatar
hongzhi wang committed
506 507
unsigned char sign(int8_t x) {
  return (unsigned char)x >> 7;
hongzhi wang's avatar
hongzhi wang committed
508
}
509
*/
hongzhi wang's avatar
hongzhi wang committed
510

Guy De Souza's avatar
Guy De Souza committed
511
uint8_t pbch_deinterleaving_pattern[32] = {28,0,31,30,7,29,25,27,5,8,24,9,10,11,12,13,1,4,3,14,15,16,17,2,26,18,19,20,21,22,6,23};
512
 
513
int nr_rx_pbch( PHY_VARS_NR_UE *ue,
Raymond Knopp's avatar
Raymond Knopp committed
514 515 516 517 518 519
		UE_nr_rxtx_proc_t *proc,
		NR_UE_PBCH *nr_ue_pbch_vars,
		NR_DL_FRAME_PARMS *frame_parms,
		uint8_t eNB_id,
		MIMO_mode_t mimo_mode,
		uint32_t high_speed_flag)
Hongzhi Wang's avatar
Hongzhi Wang committed
520 521 522 523 524 525
{

  NR_UE_COMMON *nr_ue_common_vars = &ue->common_vars;

  int max_h=0;

526
  int symbol;
hongzhi wang's avatar
hongzhi wang committed
527
  //uint8_t pbch_a[64];
528
  uint8_t *pbch_a = malloc(sizeof(uint8_t) * 32);
529
  //uint32_t pbch_a_prime;
Raymond Knopp's avatar
Raymond Knopp committed
530
  int16_t *pbch_e_rx;
Hongzhi Wang's avatar
Hongzhi Wang committed
531
  uint8_t *decoded_output = nr_ue_pbch_vars->decoded_output;
532 533 534 535
  uint8_t nushift;
  uint16_t M;
  uint8_t Lmax=8; //to update
  uint8_t ssb_index=0;
hongzhi wang's avatar
hongzhi wang committed
536
  //uint16_t crc;
537
  //unsigned short idx_demod =0;
538
  uint32_t decoderState=0;
539
  //uint8_t decoderListSize = 8, pathMetricAppr = 0;
hongzhi wang's avatar
hongzhi wang committed
540

541 542
  //time_stats_t polar_decoder_init,polar_rate_matching,decoding,bit_extraction,deinterleaving;
  //time_stats_t path_metric,sorting,update_LLR;
Hongzhi Wang's avatar
Hongzhi Wang committed
543
  memset(&pbch_a[0], 0, sizeof(uint8_t) * NR_POLAR_PBCH_PAYLOAD_BITS);
hongzhi wang's avatar
hongzhi wang committed
544

Hongzhi Wang's avatar
Hongzhi Wang committed
545
  //printf("nr_pbch_ue nid_cell %d\n",frame_parms->Nid_cell);
hongzhi wang's avatar
hongzhi wang committed
546

Hongzhi Wang's avatar
Hongzhi Wang committed
547 548

  int subframe_rx = proc->subframe_rx;
hongzhi wang's avatar
hongzhi wang committed
549
  
Hongzhi Wang's avatar
Hongzhi Wang committed
550
  pbch_e_rx = &nr_ue_pbch_vars->llr[0];
Hongzhi Wang's avatar
Hongzhi Wang committed
551 552

  // clear LLR buffer
Hongzhi Wang's avatar
Hongzhi Wang committed
553
  memset(nr_ue_pbch_vars->llr,0,NR_POLAR_PBCH_E);
Hongzhi Wang's avatar
Hongzhi Wang committed
554

555 556 557 558 559
  int symbol_offset=1;
  if (ue->is_synchronized > 0)
    symbol_offset=4;
  else
    symbol_offset=0;
Hongzhi Wang's avatar
Hongzhi Wang committed
560

Florian Kaltenberger's avatar
WIP  
Florian Kaltenberger committed
561
#ifdef DEBUG_PBCH
hongzhi wang's avatar
hongzhi wang committed
562
    //printf("address dataf %p",nr_ue_common_vars->common_vars_rx_data_per_thread[ue->current_thread_id[subframe_rx]].rxdataF);
Raymond Knopp's avatar
Raymond Knopp committed
563
    write_output("rxdataF0_pbch.m","rxF0pbch",
564
		 &nr_ue_common_vars->common_vars_rx_data_per_thread[ue->current_thread_id[subframe_rx]].rxdataF[0][(symbol_offset+1)*frame_parms->ofdm_symbol_size],frame_parms->ofdm_symbol_size*3,1,1);
Florian Kaltenberger's avatar
WIP  
Florian Kaltenberger committed
565
#endif
Raymond Knopp's avatar
Raymond Knopp committed
566

567 568
    // symbol refers to symbol within SSB. symbol_offset is the offset of the SSB wrt start of slot
  for (symbol=1; symbol<4; symbol++) {
Raymond Knopp's avatar
Raymond Knopp committed
569

Hongzhi Wang's avatar
Hongzhi Wang committed
570
    nr_pbch_extract(nr_ue_common_vars->common_vars_rx_data_per_thread[ue->current_thread_id[subframe_rx]].rxdataF,
Raymond Knopp's avatar
Raymond Knopp committed
571 572 573
		    nr_ue_common_vars->common_vars_rx_data_per_thread[ue->current_thread_id[subframe_rx]].dl_ch_estimates[eNB_id],
		    nr_ue_pbch_vars->rxdataF_ext,
		    nr_ue_pbch_vars->dl_ch_estimates_ext,
574 575
		    symbol,
		    symbol_offset,
Raymond Knopp's avatar
Raymond Knopp committed
576 577
		    high_speed_flag,
		    frame_parms);
Hongzhi Wang's avatar
Hongzhi Wang committed
578
#ifdef DEBUG_PBCH
Raymond Knopp's avatar
Raymond Knopp committed
579 580
    LOG_I(PHY,"[PHY] PBCH Symbol %d ofdm size %d\n",symbol, frame_parms->ofdm_symbol_size );
    LOG_I(PHY,"[PHY] PBCH starting channel_level\n");
Hongzhi Wang's avatar
Hongzhi Wang committed
581 582
#endif

583
    if (symbol == 1) {
Raymond Knopp's avatar
Raymond Knopp committed
584 585 586 587 588
      max_h = nr_pbch_channel_level(nr_ue_pbch_vars->dl_ch_estimates_ext,
				    frame_parms,
				    symbol);
      nr_ue_pbch_vars->log2_maxh = 3+(log2_approx(max_h)/2);
    }
Hongzhi Wang's avatar
Hongzhi Wang committed
589 590

#ifdef DEBUG_PBCH
Raymond Knopp's avatar
Raymond Knopp committed
591
    LOG_I(PHY,"[PHY] PBCH log2_maxh = %d (%d)\n",nr_ue_pbch_vars->log2_maxh,max_h);
Hongzhi Wang's avatar
Hongzhi Wang committed
592 593 594
#endif

    nr_pbch_channel_compensation(nr_ue_pbch_vars->rxdataF_ext,
Raymond Knopp's avatar
Raymond Knopp committed
595 596 597 598 599
				 nr_ue_pbch_vars->dl_ch_estimates_ext,
				 nr_ue_pbch_vars->rxdataF_comp,
				 frame_parms,
				 symbol,
				 nr_ue_pbch_vars->log2_maxh); // log2_maxh+I0_shift
Hongzhi Wang's avatar
Hongzhi Wang committed
600 601 602 603 604 605

    /*if (frame_parms->nb_antennas_rx > 1)
      pbch_detection_mrc(frame_parms,
                         nr_ue_pbch_vars->rxdataF_comp,
                         symbol);*/

606
/*
Hongzhi Wang's avatar
Hongzhi Wang committed
607 608 609
    if (mimo_mode == ALAMOUTI) {
      nr_pbch_alamouti(frame_parms,nr_ue_pbch_vars->rxdataF_comp,symbol);
    } else if (mimo_mode != SISO) {
Raymond Knopp's avatar
Raymond Knopp committed
610
      LOG_I(PHY,"[PBCH][RX] Unsupported MIMO mode\n");
Hongzhi Wang's avatar
Hongzhi Wang committed
611 612
      return(-1);
    }
613
*/
614
    if (symbol==1) {
Hongzhi Wang's avatar
Hongzhi Wang committed
615
      nr_pbch_quantize(pbch_e_rx,
Hongzhi Wang's avatar
Hongzhi Wang committed
616
                    (short*)&(nr_ue_pbch_vars->rxdataF_comp[0][symbol*240]),
Hongzhi Wang's avatar
Hongzhi Wang committed
617
                    144);
Hongzhi Wang's avatar
Hongzhi Wang committed
618

Hongzhi Wang's avatar
Hongzhi Wang committed
619
      pbch_e_rx+=144;
Hongzhi Wang's avatar
Hongzhi Wang committed
620 621
    } else {
      nr_pbch_quantize(pbch_e_rx,
Hongzhi Wang's avatar
Hongzhi Wang committed
622 623
                    (short*)&(nr_ue_pbch_vars->rxdataF_comp[0][symbol*240]),
                    360);
Hongzhi Wang's avatar
Hongzhi Wang committed
624

Hongzhi Wang's avatar
Hongzhi Wang committed
625
      pbch_e_rx+=360;
Hongzhi Wang's avatar
Hongzhi Wang committed
626 627 628 629 630
    }


  }

Raymond Knopp's avatar
Raymond Knopp committed
631
#ifdef DEBUG_PBCH
632
  write_output("rxdataF_comp.m","rxFcomp",&nr_ue_pbch_vars->rxdataF_comp[0][240],240*3,1,1);
Raymond Knopp's avatar
Raymond Knopp committed
633 634 635
#endif
    

Hongzhi Wang's avatar
Hongzhi Wang committed
636
  pbch_e_rx = nr_ue_pbch_vars->llr;
637
  //demod_pbch_e = nr_ue_pbch_vars->demod_pbch_e;
638
  pbch_a = nr_ue_pbch_vars->pbch_a;
Hongzhi Wang's avatar
Hongzhi Wang committed
639

640
#ifdef DEBUG_PBCH
hongzhi wang's avatar
hongzhi wang committed
641
  //pbch_e_rx = &nr_ue_pbch_vars->llr[0];
Hongzhi Wang's avatar
Hongzhi Wang committed
642

643
  short *p = (short *)&(nr_ue_pbch_vars->rxdataF_comp[0][20*12]);
Raymond Knopp's avatar
Raymond Knopp committed
644 645
  for (int cnt = 0; cnt < 864  ; cnt++)
    printf("pbch rx llr %d\n",*(pbch_e_rx+cnt));
Hongzhi Wang's avatar
Hongzhi Wang committed
646

Hongzhi Wang's avatar
Hongzhi Wang committed
647
#endif
648

hongzhi wang's avatar
hongzhi wang committed
649

650 651 652
  //un-scrambling
  M =  NR_POLAR_PBCH_E;
  nushift = (Lmax==4)? ssb_index&3 : ssb_index&7;
Guy De Souza's avatar
Guy De Souza committed
653 654
  uint32_t unscrambling_mask = (Lmax==64)?0x100006D:0x1000041;
  nr_pbch_unscrambling(nr_ue_pbch_vars,frame_parms->Nid_cell,nushift,M,NR_POLAR_PBCH_E,0,0);
Hongzhi Wang's avatar
Hongzhi Wang committed
655

656 657


Hongzhi Wang's avatar
Hongzhi Wang committed
658
  //polar decoding de-rate matching
659

Raymond Knopp's avatar
Raymond Knopp committed
660 661 662 663
  nr_polar_init(&nr_ue_pbch_vars->nrPolar_params,
		NR_POLAR_PBCH_MESSAGE_TYPE,
		NR_POLAR_PBCH_PAYLOAD_BITS,
		NR_POLAR_PBCH_AGGREGATION_LEVEL);
664

Raymond Knopp's avatar
Raymond Knopp committed
665
  AssertFatal(nr_ue_pbch_vars->nrPolar_params != NULL,"nr_ue_pbch_vars->nrPolar_params is null\n");
Raymond Knopp's avatar
Raymond Knopp committed
666

Raymond Knopp's avatar
Raymond Knopp committed
667
  t_nrPolar_params *currentPtr = nr_polar_params(nr_ue_pbch_vars->nrPolar_params, NR_POLAR_PBCH_MESSAGE_TYPE, NR_POLAR_PBCH_PAYLOAD_BITS, NR_POLAR_PBCH_AGGREGATION_LEVEL);
668

669 670 671
  decoderState = polar_decoder_int16(pbch_e_rx,(uint64_t*)&nr_ue_pbch_vars->pbch_a_prime,currentPtr);
 
  if(decoderState) return(decoderState);
Turker YILMAZ's avatar
Turker YILMAZ committed
672
  	
673
  //  printf("polar decoder output 0x%08x\n",nr_ue_pbch_vars->pbch_a_prime);
674 675 676 677 678 679 680

  // Decoder reversal
  uint32_t a_reversed=0;
  for (int i=0; i<NR_POLAR_PBCH_PAYLOAD_BITS; i++)
    a_reversed |= (((uint64_t)nr_ue_pbch_vars->pbch_a_prime>>i)&1)<<(31-i);

  nr_ue_pbch_vars->pbch_a_prime = a_reversed;
hongzhi wang's avatar
hongzhi wang committed
681
  
682
  //payload un-scrambling
683
  memset(&nr_ue_pbch_vars->pbch_a_interleaved, 0, sizeof(uint32_t) );
684
  M = (Lmax == 64)? (NR_POLAR_PBCH_PAYLOAD_BITS - 6) : (NR_POLAR_PBCH_PAYLOAD_BITS - 3);
Guy De Souza's avatar
Guy De Souza committed
685 686
  nushift = ((nr_ue_pbch_vars->pbch_a_prime>>24)&1) ^ (((nr_ue_pbch_vars->pbch_a_prime>>6)&1)<<1);
  nr_pbch_unscrambling(nr_ue_pbch_vars,frame_parms->Nid_cell,nushift,M,NR_POLAR_PBCH_PAYLOAD_BITS,1,unscrambling_mask);
687
  //printf("nushift %d sfn 3rd %d 2nd %d", nushift,((nr_ue_pbch_vars->pbch_a_prime>>6)&1), ((nr_ue_pbch_vars->pbch_a_prime>>24)&1) );
688 689

  //payload deinterleaving
690 691
  //uint32_t in=0;
  uint32_t out=0;
692
  for (int i=0; i<32; i++) {
693
    out |= ((nr_ue_pbch_vars->pbch_a_interleaved>>i)&1)<<(pbch_deinterleaving_pattern[i]);
694
#ifdef DEBUG_PBCH
695
    printf("i %d in 0x%08x out 0x%08x ilv %d (in>>i)&1) 0x%08x\n", i, nr_ue_pbch_vars->pbch_a_interleaved, out, pbch_deinterleaving_pattern[i], (nr_ue_pbch_vars->pbch_a_interleaved>>i)&1);
696
#endif
697 698
  }

Guy De Souza's avatar
Guy De Souza committed
699 700 701 702 703 704 705 706 707
 uint32_t payload = 0;
 uint8_t xtra_byte = 0;
 xtra_byte = (out>>24)&0xff;

  for (int i=0; i<NR_POLAR_PBCH_PAYLOAD_BITS; i++)
	  payload |= ((out>>i)&1)<<(NR_POLAR_PBCH_PAYLOAD_BITS-i-1);

 for (int i=0; i<3; i++)
  decoded_output[i] = (uint8_t)((payload>>((3-i)<<3))&0xff);
hongzhi wang's avatar
hongzhi wang committed
708

709
#ifdef DEBUG_PBCH
Guy De Souza's avatar
Guy De Souza committed
710
 printf("xtra_byte %x payload %x\n", xtra_byte, payload);
hongzhi wang's avatar
hongzhi wang committed
711
  for (i=0; i<(NR_POLAR_PBCH_PAYLOAD_BITS>>3); i++){
712
    //  	  printf("unscrambling pbch_a[%d] = %x \n", i,pbch_a[i]);
hongzhi wang's avatar
hongzhi wang committed
713
  	  printf("[PBCH] decoder payload[%d] = %x\n",i,decoded_output[i]);
hongzhi wang's avatar
hongzhi wang committed
714
  }
715
#endif
716

717 718
    ue->dl_indication.rx_ind = &ue->rx_ind; //  hang on rx_ind instance
    //ue->rx_ind.sfn_slot = 0;  //should be set by higher-1-layer, i.e. clean_and_set_if_instance()
719 720 721 722 723 724 725
    ue->rx_ind.rx_indication_body[0].pdu_type = FAPI_NR_RX_PDU_TYPE_MIB;
    ue->rx_ind.rx_indication_body[0].mib_pdu.pdu = &decoded_output[0];
    ue->rx_ind.rx_indication_body[0].mib_pdu.additional_bits = xtra_byte;
    ue->rx_ind.rx_indication_body[0].mib_pdu.ssb_index = ssb_index;            //  confirm with TCL
    ue->rx_ind.rx_indication_body[0].mib_pdu.ssb_length = Lmax;                //  confirm with TCL
    ue->rx_ind.rx_indication_body[0].mib_pdu.cell_id = frame_parms->Nid_cell;  //  confirm with TCL
    ue->rx_ind.number_pdus = 1;
726

727 728
    if (ue->if_inst && ue->if_inst->dl_indication)
      ue->if_inst->dl_indication(&ue->dl_indication);
729 730

    return 0;    
Hongzhi Wang's avatar
Hongzhi Wang committed
731
}