nr_pbch.c 23.2 KB
Newer Older
Wang's avatar
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"
Wang's avatar
Wang committed
37

knopp's avatar
knopp committed
38
//#define DEBUG_PBCH 1
Wang's avatar
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

knopp's avatar
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])
Wang's avatar
Wang committed
48 49

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


  uint16_t rb;
knopp's avatar
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;
Wang's avatar
Wang committed
63

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

knopp's avatar
knopp committed
66 67
  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;
68
  int s_offset=0;
knopp's avatar
knopp committed
69
 
70 71 72
  AssertFatal(symbol>=1 && symbol<5, 
	      "symbol %d illegal for PBCH extraction\n",
	      symbol);
knopp's avatar
knopp committed
73

74
  if (is_synchronized==1) s_offset=4;
knopp's avatar
knopp committed
75

Wang's avatar
Wang committed
76
  for (aarx=0; aarx<frame_parms->nb_antennas_rx; aarx++) {
hongzhi wang's avatar
hongzhi wang committed
77
        
78 79
    rxF        = &rxdataF[aarx][(symbol+s_offset)*frame_parms->ofdm_symbol_size];
    rxF_ext    = &rxdataF_ext[aarx][(symbol+s_offset)*(20*12)];
hongzhi wang's avatar
hongzhi wang committed
80
#ifdef DEBUG_PBCH
hongzhi wang's avatar
hongzhi wang committed
81
     printf("extract_rbs (nushift %d): rx_offset=%d, symbol %d\n",frame_parms->nushift,
82
	    (rx_offset + ((symbol+s_offset)*(frame_parms->ofdm_symbol_size))),symbol);
hongzhi wang's avatar
hongzhi wang committed
83
     int16_t *p = (int16_t *)rxF;
hongzhi wang's avatar
hongzhi wang committed
84
     for (int i =0; i<8;i++){
hongzhi wang's avatar
hongzhi wang committed
85
        printf("rxF [%d]= %d\n",i,rxF[i]);
hongzhi wang's avatar
hongzhi wang committed
86 87
        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
88 89
     }
#endif
Wang's avatar
Wang committed
90 91

    for (rb=0; rb<20; rb++) {
hongzhi wang's avatar
hongzhi wang committed
92
      j=0;
knopp's avatar
knopp committed
93

94
      if (symbol==1 || symbol==3) {
Wang's avatar
Wang committed
95 96 97 98
        for (i=0; i<12; i++) {
          if ((i!=nushiftmod4) &&
              (i!=(nushiftmod4+4)) &&
              (i!=(nushiftmod4+8))) {
knopp's avatar
knopp committed
99 100 101 102 103 104 105 106 107
            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
108
	    j++;
Wang's avatar
Wang committed
109
          }
knopp's avatar
knopp committed
110
	  rx_offset=(rx_offset+1)&(frame_parms->ofdm_symbol_size-1);
Wang's avatar
Wang committed
111
        }
hongzhi wang's avatar
hongzhi wang committed
112
        rxF_ext+=9;
Wang's avatar
Wang committed
113
      } else { //symbol 2
hongzhi wang's avatar
hongzhi wang committed
114
    	if ((rb < 4) || (rb >15)){
Wang's avatar
Wang committed
115
    	  for (i=0; i<12; i++) {
knopp's avatar
knopp committed
116 117 118 119 120 121 122 123 124 125 126 127 128 129 130
	    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);
Wang's avatar
Wang committed
131
    	  }
knopp's avatar
knopp committed
132
	  rxF_ext+=9;
hongzhi wang's avatar
hongzhi wang committed
133
	}
knopp's avatar
knopp committed
134 135
	else rx_offset = (rx_offset+12)&(frame_parms->ofdm_symbol_size-1);
        
Wang's avatar
Wang committed
136 137 138
      }
    }

knopp's avatar
knopp committed
139
    if (high_speed_flag == 1)
140
      dl_ch0     = &dl_ch_estimates[aarx][((symbol+s_offset)*(frame_parms->ofdm_symbol_size))];
knopp's avatar
knopp committed
141 142 143 144 145
    else
      dl_ch0     = &dl_ch_estimates[aarx][0];
    
    //printf("dl_ch0 addr %p\n",dl_ch0);
    
146
    dl_ch0_ext = &dl_ch_estimates_ext[aarx][(symbol+s_offset)*(20*12)];
knopp's avatar
knopp committed
147 148 149
    
    for (rb=0; rb<20; rb++) {
      j=0;
150
      if (symbol==1 || symbol==3) {
knopp's avatar
knopp committed
151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170
	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;
Wang's avatar
Wang committed
171
      }
knopp's avatar
knopp committed
172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194
      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;
      }
    }
Wang's avatar
Wang committed
195 196 197 198 199 200 201 202 203
  }

  return(0);
}

//__m128i avg128;

//compute average channel_level on each (TX,RX) antenna pair
int nr_pbch_channel_level(int **dl_ch_estimates_ext,
knopp's avatar
knopp committed
204 205
			  NR_DL_FRAME_PARMS *frame_parms,
			  uint32_t symbol)
Wang's avatar
Wang committed
206 207
{

hongzhi wang's avatar
hongzhi wang committed
208
  int16_t rb, nb_rb=20;
knopp's avatar
knopp committed
209
  uint8_t aarx;
Wang's avatar
Wang committed
210 211 212 213 214 215 216 217 218 219

#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;

knopp's avatar
knopp committed
220 221 222
  for (aarx=0; aarx<frame_parms->nb_antennas_rx; aarx++) {
    //clear average level
    
Wang's avatar
Wang committed
223
#if defined(__x86_64__) || defined(__i386__)
knopp's avatar
knopp committed
224 225
    avg128 = _mm_setzero_si128();
    dl_ch128=(__m128i *)&dl_ch_estimates_ext[aarx][symbol*20*12];
Wang's avatar
Wang committed
226
#elif defined(__arm__)
knopp's avatar
knopp committed
227 228 229
    avg128 = vdupq_n_s32(0);
    dl_ch128=(int16x8_t *)&dl_ch_estimates_ext[aarx][symbol*20*12];
    
Wang's avatar
Wang committed
230
#endif
knopp's avatar
knopp committed
231
    for (rb=0; rb<nb_rb; rb++) {
Wang's avatar
Wang committed
232
#if defined(__x86_64__) || defined(__i386__)
knopp's avatar
knopp committed
233 234 235
      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]));
Wang's avatar
Wang committed
236 237 238
#elif defined(__arm__)
// to be filled in
#endif
knopp's avatar
knopp committed
239 240 241 242 243 244 245 246
      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]);
	}*/
      
Wang's avatar
Wang committed
247
    }
knopp's avatar
knopp committed
248 249 250 251 252 253 254 255 256 257 258
    
    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);
  }
Wang's avatar
Wang committed
259 260 261 262 263 264 265 266 267
#if defined(__x86_64__) || defined(__i386__)
  _mm_empty();
  _m_empty();
#endif
  return(avg2);

}

void nr_pbch_channel_compensation(int **rxdataF_ext,
knopp's avatar
knopp committed
268 269 270 271 272 273
				  int **dl_ch_estimates_ext,
				  int **rxdataF_comp,
				  NR_DL_FRAME_PARMS *frame_parms,
				  uint8_t symbol,
				  int is_synchronized,
				  uint8_t output_shift)
Wang's avatar
Wang committed
274 275
{

Florian Kaltenberger's avatar
WIP  
Florian Kaltenberger committed
276
  short conjugate[8]__attribute__((aligned(16))) = {-1,1,-1,1,-1,1,-1,1};
277
  //short conjugate2[8]__attribute__((aligned(16))) = {1,-1,1,-1,1,-1,1,-1};
knopp's avatar
knopp committed
278 279 280 281 282
#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
283

knopp's avatar
knopp committed
284 285
  uint16_t nb_re=180;
  uint8_t aarx;
Wang's avatar
Wang committed
286 287 288 289 290 291
#if defined(__x86_64__) || defined(__i386__)
  __m128i *dl_ch128,*rxdataF128,*rxdataF_comp128;
#elif defined(__arm__)

#endif

knopp's avatar
knopp committed
292
  AssertFatal((symbol > 0 && symbol < 4 && is_synchronized == 0) || 
293
	      (symbol > 4 && symbol < 8 && is_synchronized == 1),
knopp's avatar
knopp committed
294 295
	      "symbol %d is illegal for PBCH DM-RS (is_synchronized %d)\n",
	      symbol,is_synchronized);
Wang's avatar
Wang committed
296 297


298

knopp's avatar
knopp committed
299
  if (symbol == 2 || symbol == 6) nb_re = 72;
Wang's avatar
Wang committed
300

301 302
  //  printf("comp: symbol %d : nb_re %d\n",symbol,nb_re);

knopp's avatar
knopp committed
303 304 305 306 307 308 309 310 311 312 313
  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]); 
    */
Wang's avatar
Wang committed
314 315 316 317
#elif defined(__arm__)
// to be filled in
#endif

knopp's avatar
knopp committed
318
    for (int re=0; re<nb_re; re+=12) {
319
      //            printf("******re %d\n",re);
Wang's avatar
Wang committed
320
#if defined(__x86_64__) || defined(__i386__)
knopp's avatar
knopp committed
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 377 378 379 380 381 382
      // 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;
      
Wang's avatar
Wang committed
383
#elif defined(__arm__)
knopp's avatar
knopp committed
384
      // to be filled in
Wang's avatar
Wang committed
385 386
#endif
    }
knopp's avatar
knopp committed
387
  }
Wang's avatar
Wang committed
388 389 390 391 392 393 394 395 396 397 398
#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)
{

knopp's avatar
knopp committed
399
  uint8_t symbol_mod;
Wang's avatar
Wang committed
400 401 402 403 404 405 406 407 408 409
  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__)
knopp's avatar
knopp committed
410 411
      rxdataF_comp128_0   = (__m128i *)&rxdataF_comp[0][symbol_mod*6*12];
      rxdataF_comp128_1   = (__m128i *)&rxdataF_comp[1][symbol_mod*6*12];
Wang's avatar
Wang committed
412
#elif defined(__arm__)
knopp's avatar
knopp committed
413 414
      rxdataF_comp128_0   = (int16x8_t *)&rxdataF_comp[0][symbol_mod*6*12];
      rxdataF_comp128_1   = (int16x8_t *)&rxdataF_comp[1][symbol_mod*6*12];
Wang's avatar
Wang committed
415 416 417 418 419 420 421 422 423 424 425 426

#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
      }
    }
knopp's avatar
knopp committed
427

Wang's avatar
Wang committed
428 429 430 431 432 433
#if defined(__x86_64__) || defined(__i386__)
  _mm_empty();
  _m_empty();
#endif
}

434
void nr_pbch_unscrambling(NR_UE_PBCH *pbch,
knopp's avatar
knopp committed
435 436 437 438
			  uint16_t Nid,
			  uint8_t nushift,
			  uint16_t M,
			  uint16_t length,
Guy De Souza's avatar
Guy De Souza committed
439 440
			  uint8_t bitwise,
        uint32_t unscrambling_mask)
Wang's avatar
Wang committed
441
{
442
  uint8_t reset, offset;
Wang's avatar
Wang committed
443
  uint32_t x1, x2, s=0;
Guy De Souza's avatar
Guy De Souza committed
444
  uint8_t k=0;
445
  int16_t *demod_pbch_e = pbch->llr;
446

hongzhi wang's avatar
hongzhi wang committed
447

Wang's avatar
Wang committed
448 449
  reset = 1;
  // x1 is set in first call to lte_gold_generic
450
  x2 = Nid; //this is c_init
Wang's avatar
Wang committed
451

452
  // The Gold sequence is shifted by nushift* M, so we skip (nushift*M /32) double words
453
  for (int i=0; i<(uint16_t)ceil(((float)nushift*M)/32); i++) {
454 455 456 457 458 459 460
    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++) {
461 462 463 464 465 466 467
      /*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))) {
468 469 470
        s = lte_gold_generic(&x1, &x2, reset);
        reset = 0;
      }
471 472
        (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));
knopp's avatar
knopp committed
473
#ifdef DEBUG_PBCH_ENCODING
474
    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)));
knopp's avatar
knopp committed
475
#endif
476
      }
Wang's avatar
Wang committed
477

478
      else {
479 480 481 482
      if (((i+offset)&0x1f)==0) {
        s = lte_gold_generic(&x1, &x2, reset);
        reset = 0;
      }
483
    	  if (((s>>((i+offset)&0x1f))&1)==1)
hongzhi wang's avatar
hongzhi wang committed
484
    		  demod_pbch_e[i] = -demod_pbch_e[i];
hongzhi wang's avatar
hongzhi wang committed
485 486 487 488
#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
489
      }
Wang's avatar
Wang committed
490
  }
491

Wang's avatar
Wang committed
492 493
}

494 495 496
void nr_pbch_quantize(int16_t *pbch_llr8,
                      int16_t *pbch_llr,
                      uint16_t len)
Wang's avatar
Wang committed
497 498 499 500 501
{

  uint16_t i;

  for (i=0; i<len; i++) {
502 503 504 505
    if (pbch_llr[i]>31)
      pbch_llr8[i]=32;
    else if (pbch_llr[i]<-31)
      pbch_llr8[i]=-32;
506
    else
hongzhi wang's avatar
hongzhi wang committed
507
      pbch_llr8[i] = (char)(pbch_llr[i]);
Wang's avatar
Wang committed
508 509 510

  }
}
511
/*
hongzhi wang's avatar
hongzhi wang committed
512 513
unsigned char sign(int8_t x) {
  return (unsigned char)x >> 7;
hongzhi wang's avatar
hongzhi wang committed
514
}
515
*/
hongzhi wang's avatar
hongzhi wang committed
516

Guy De Souza's avatar
Guy De Souza committed
517
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};
518

Hongzhi Wang's avatar
Hongzhi Wang committed
519
int nr_rx_pbch( PHY_VARS_NR_UE *ue,
knopp's avatar
knopp committed
520 521 522 523 524 525
		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)
Wang's avatar
Wang committed
526 527 528 529 530 531 532
{

  NR_UE_COMMON *nr_ue_common_vars = &ue->common_vars;

  int max_h=0;

  int symbol,i;
hongzhi wang's avatar
hongzhi wang committed
533
  //uint8_t pbch_a[64];
534
  uint8_t *pbch_a = malloc(sizeof(uint8_t) * 32);
535
  uint32_t pbch_a_prime;
knopp's avatar
knopp committed
536
  int16_t *pbch_e_rx;
Wang's avatar
Wang committed
537
  uint8_t *decoded_output = nr_ue_pbch_vars->decoded_output;
538 539 540 541
  uint8_t nushift;
  uint16_t M;
  uint8_t Lmax=8; //to update
  uint8_t ssb_index=0;
hongzhi wang's avatar
hongzhi wang committed
542
  //uint16_t crc;
hongzhi wang's avatar
hongzhi wang committed
543
  unsigned short idx_demod =0;
Hongzhi Wang's avatar
Hongzhi Wang committed
544 545
  int8_t decoderState=0;
  uint8_t decoderListSize = 8, pathMetricAppr = 0;
hongzhi wang's avatar
hongzhi wang committed
546

547 548
  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
549
  memset(&pbch_a[0], 0, sizeof(uint8_t) * NR_POLAR_PBCH_PAYLOAD_BITS);
hongzhi wang's avatar
hongzhi wang committed
550

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

Wang's avatar
Wang committed
553 554

  int subframe_rx = proc->subframe_rx;
hongzhi wang's avatar
hongzhi wang committed
555
  
Hongzhi Wang's avatar
Hongzhi Wang committed
556
  pbch_e_rx = &nr_ue_pbch_vars->llr[0];
Wang's avatar
Wang committed
557 558

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

knopp's avatar
knopp committed
561 562
  int first_symbol=1;
  if (ue->is_synchronized > 0) first_symbol+=4;
Wang's avatar
Wang committed
563

Florian Kaltenberger's avatar
WIP  
Florian Kaltenberger committed
564
#ifdef DEBUG_PBCH
hongzhi wang's avatar
hongzhi wang committed
565
    //printf("address dataf %p",nr_ue_common_vars->common_vars_rx_data_per_thread[ue->current_thread_id[subframe_rx]].rxdataF);
knopp's avatar
knopp committed
566 567
    write_output("rxdataF0_pbch.m","rxF0pbch",
		 &nr_ue_common_vars->common_vars_rx_data_per_thread[ue->current_thread_id[subframe_rx]].rxdataF[0][first_symbol*frame_parms->ofdm_symbol_size],frame_parms->ofdm_symbol_size*3,1,1);
Florian Kaltenberger's avatar
WIP  
Florian Kaltenberger committed
568
#endif
knopp's avatar
knopp committed
569 570 571

  for (symbol=first_symbol; symbol<(first_symbol+3); symbol++) {

hongzhi wang's avatar
hongzhi wang committed
572
  
Wang's avatar
Wang committed
573
    nr_pbch_extract(nr_ue_common_vars->common_vars_rx_data_per_thread[ue->current_thread_id[subframe_rx]].rxdataF,
knopp's avatar
knopp committed
574 575 576
		    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,
577
		    symbol-first_symbol+1,
knopp's avatar
knopp committed
578 579 580
		    high_speed_flag,
		    ue->is_synchronized,
		    frame_parms);
Wang's avatar
Wang committed
581
#ifdef DEBUG_PBCH
knopp's avatar
knopp committed
582 583
    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");
Wang's avatar
Wang committed
584 585
#endif

knopp's avatar
knopp committed
586 587 588 589 590 591
    if (symbol == 1 || symbol == 5) {
      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);
    }
Wang's avatar
Wang committed
592 593

#ifdef DEBUG_PBCH
knopp's avatar
knopp committed
594
    LOG_I(PHY,"[PHY] PBCH log2_maxh = %d (%d)\n",nr_ue_pbch_vars->log2_maxh,max_h);
Wang's avatar
Wang committed
595 596 597
#endif

    nr_pbch_channel_compensation(nr_ue_pbch_vars->rxdataF_ext,
knopp's avatar
knopp committed
598 599 600 601 602 603
				 nr_ue_pbch_vars->dl_ch_estimates_ext,
				 nr_ue_pbch_vars->rxdataF_comp,
				 frame_parms,
				 symbol,
				 ue->is_synchronized,
				 nr_ue_pbch_vars->log2_maxh); // log2_maxh+I0_shift
Wang's avatar
Wang committed
604 605 606 607 608 609

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

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

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

Hongzhi Wang's avatar
Hongzhi Wang committed
629
      pbch_e_rx+=360;
Wang's avatar
Wang committed
630 631 632 633 634
    }


  }

knopp's avatar
knopp committed
635 636 637 638 639
#ifdef DEBUG_PBCH
  write_output("rxdataF_comp.m","rxFcomp",&nr_ue_pbch_vars->rxdataF_comp[0][240*first_symbol],240*3,1,1);
#endif
    

Wang's avatar
Wang committed
640
  pbch_e_rx = nr_ue_pbch_vars->llr;
641
  //demod_pbch_e = nr_ue_pbch_vars->demod_pbch_e;
642
  pbch_a = nr_ue_pbch_vars->pbch_a;
Hongzhi Wang's avatar
Hongzhi Wang committed
643

644
#ifdef DEBUG_PBCH
hongzhi wang's avatar
hongzhi wang committed
645
  //pbch_e_rx = &nr_ue_pbch_vars->llr[0];
Wang's avatar
Wang committed
646

knopp's avatar
knopp committed
647 648 649
  short *p = (short *)&(nr_ue_pbch_vars->rxdataF_comp[0][first_symbol*20*12]);
  for (int cnt = 0; cnt < 864  ; cnt++)
    printf("pbch rx llr %d\n",*(pbch_e_rx+cnt));
Wang's avatar
Wang committed
650

Hongzhi Wang's avatar
Hongzhi Wang committed
651
#endif
652

hongzhi wang's avatar
hongzhi wang committed
653

654 655 656
  //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
657 658
  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
659

660 661


Hongzhi Wang's avatar
Hongzhi Wang committed
662
  //polar decoding de-rate matching
663

knopp's avatar
knopp committed
664 665 666 667 668
  nr_ue_pbch_vars->nrPolar_params = NULL;
  nr_polar_init(&nr_ue_pbch_vars->nrPolar_params,
		NR_POLAR_PBCH_MESSAGE_TYPE,
		NR_POLAR_PBCH_PAYLOAD_BITS,
		NR_POLAR_PBCH_AGGREGATION_LEVEL);
669

knopp's avatar
knopp committed
670
  AssertFatal(nr_ue_pbch_vars->nrPolar_params != NULL,"nr_ue_pbch_vars->nrPolar_params is null\n");
knopp's avatar
knopp committed
671

knopp's avatar
knopp committed
672
  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);
673

674
  decoderState = polar_decoder_int16(pbch_e_rx,(uint8_t*)&nr_ue_pbch_vars->pbch_a_prime,currentPtr);
675

676

677
  if(decoderState > 0)	return(decoderState);
Turker YILMAZ's avatar
Turker YILMAZ committed
678
  	
679
  //printf("polar decoder output 0x%08x\n",nr_ue_pbch_vars->pbch_a_prime);
hongzhi wang's avatar
hongzhi wang committed
680
  
681
  //payload un-scrambling
682
  memset(&nr_ue_pbch_vars->pbch_a_interleaved, 0, sizeof(uint32_t) );
683
  M = (Lmax == 64)? (NR_POLAR_PBCH_PAYLOAD_BITS - 6) : (NR_POLAR_PBCH_PAYLOAD_BITS - 3);
Guy De Souza's avatar
Guy De Souza committed
684 685
  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);
686 687

  //payload deinterleaving
688 689
  //uint32_t in=0;
  uint32_t out=0;
690
  for (int i=0; i<32; i++) {
691
    out |= ((nr_ue_pbch_vars->pbch_a_interleaved>>i)&1)<<(pbch_deinterleaving_pattern[i]);
692
#ifdef DEBUG_PBCH
693
    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);
694
#endif
695 696
  }

Guy De Souza's avatar
Guy De Souza committed
697 698 699 700 701 702 703 704 705
 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
706

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

715 716 717
    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()
    ue->rx_ind.number_pdus = ue->rx_ind.number_pdus + 1;
718 719
    ue->rx_ind.rx_indication_body = (fapi_nr_rx_indication_body_t *)malloc(sizeof(fapi_nr_rx_indication_body_t));
    ue->rx_ind.rx_indication_body->pdu_type = FAPI_NR_RX_PDU_TYPE_MIB;
720
    ue->rx_ind.rx_indication_body->mib_pdu.pdu = &decoded_output[0];
Guy De Souza's avatar
Guy De Souza committed
721
    ue->rx_ind.rx_indication_body->mib_pdu.additional_bits = xtra_byte;
722 723 724
    ue->rx_ind.rx_indication_body->mib_pdu.ssb_index = ssb_index;            //  confirm with TCL
    ue->rx_ind.rx_indication_body->mib_pdu.ssb_length = Lmax;                //  confirm with TCL
    ue->rx_ind.rx_indication_body->mib_pdu.cell_id = frame_parms->Nid_cell;  //  confirm with TCL
725

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

    return 0;    
Wang's avatar
Wang committed
730
}