nr_pbch.c 22.8 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 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)
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
  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;
Raymond Knopp's avatar
Raymond Knopp committed
69
 
70 71 72
  AssertFatal(symbol>=1 && symbol<5, 
	      "symbol %d illegal for PBCH extraction\n",
	      symbol);
Raymond Knopp's avatar
Raymond Knopp committed
73

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

Hongzhi Wang's avatar
Hongzhi 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
Hongzhi Wang's avatar
Hongzhi Wang committed
90 91

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

94
      if (symbol==1 || symbol==3) {
Hongzhi Wang's avatar
Hongzhi Wang committed
95 96 97 98
        for (i=0; i<12; i++) {
          if ((i!=nushiftmod4) &&
              (i!=(nushiftmod4+4)) &&
              (i!=(nushiftmod4+8))) {
Raymond Knopp's avatar
Raymond 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++;
Hongzhi Wang's avatar
Hongzhi Wang committed
109
          }
Raymond Knopp's avatar
Raymond Knopp committed
110
	  rx_offset=(rx_offset+1)&(frame_parms->ofdm_symbol_size-1);
Hongzhi Wang's avatar
Hongzhi Wang committed
111
        }
hongzhi wang's avatar
hongzhi wang committed
112
        rxF_ext+=9;
Hongzhi Wang's avatar
Hongzhi Wang committed
113
      } else { //symbol 2
hongzhi wang's avatar
hongzhi wang committed
114
    	if ((rb < 4) || (rb >15)){
Hongzhi Wang's avatar
Hongzhi Wang committed
115
    	  for (i=0; i<12; i++) {
Raymond Knopp's avatar
Raymond 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);
Hongzhi Wang's avatar
Hongzhi Wang committed
131
    	  }
Raymond Knopp's avatar
Raymond Knopp committed
132
	  rxF_ext+=9;
hongzhi wang's avatar
hongzhi wang committed
133
	}
Raymond Knopp's avatar
Raymond Knopp committed
134 135
	else rx_offset = (rx_offset+12)&(frame_parms->ofdm_symbol_size-1);
        
Hongzhi Wang's avatar
Hongzhi Wang committed
136 137 138
      }
    }

Raymond Knopp's avatar
Raymond Knopp committed
139
    if (high_speed_flag == 1)
140
      dl_ch0     = &dl_ch_estimates[aarx][((symbol+s_offset)*(frame_parms->ofdm_symbol_size))];
Raymond Knopp's avatar
Raymond 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)];
Raymond Knopp's avatar
Raymond Knopp committed
147 148 149
    
    for (rb=0; rb<20; rb++) {
      j=0;
150
      if (symbol==1 || symbol==3) {
Raymond Knopp's avatar
Raymond 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;
Hongzhi Wang's avatar
Hongzhi Wang committed
171
      }
Raymond Knopp's avatar
Raymond 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;
      }
    }
Hongzhi Wang's avatar
Hongzhi 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,
Raymond Knopp's avatar
Raymond Knopp committed
204 205
			  NR_DL_FRAME_PARMS *frame_parms,
			  uint32_t symbol)
Hongzhi Wang's avatar
Hongzhi Wang committed
206 207
{

hongzhi wang's avatar
hongzhi wang committed
208
  int16_t rb, nb_rb=20;
Raymond Knopp's avatar
Raymond Knopp committed
209
  uint8_t aarx;
Hongzhi Wang's avatar
Hongzhi 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;

Raymond Knopp's avatar
Raymond Knopp committed
220 221 222
  for (aarx=0; aarx<frame_parms->nb_antennas_rx; aarx++) {
    //clear average level
    
Hongzhi Wang's avatar
Hongzhi Wang committed
223
#if defined(__x86_64__) || defined(__i386__)
Raymond Knopp's avatar
Raymond Knopp committed
224 225
    avg128 = _mm_setzero_si128();
    dl_ch128=(__m128i *)&dl_ch_estimates_ext[aarx][symbol*20*12];
Hongzhi Wang's avatar
Hongzhi Wang committed
226
#elif defined(__arm__)
Raymond Knopp's avatar
Raymond Knopp committed
227 228 229
    avg128 = vdupq_n_s32(0);
    dl_ch128=(int16x8_t *)&dl_ch_estimates_ext[aarx][symbol*20*12];
    
Hongzhi Wang's avatar
Hongzhi Wang committed
230
#endif
Raymond Knopp's avatar
Raymond Knopp committed
231
    for (rb=0; rb<nb_rb; rb++) {
Hongzhi Wang's avatar
Hongzhi Wang committed
232
#if defined(__x86_64__) || defined(__i386__)
Raymond Knopp's avatar
Raymond 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]));
Hongzhi Wang's avatar
Hongzhi Wang committed
236 237 238
#elif defined(__arm__)
// to be filled in
#endif
Raymond Knopp's avatar
Raymond 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]);
	}*/
      
Hongzhi Wang's avatar
Hongzhi Wang committed
247
    }
Raymond Knopp's avatar
Raymond 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);
  }
Hongzhi Wang's avatar
Hongzhi 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,
Raymond Knopp's avatar
Raymond 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)
Hongzhi Wang's avatar
Hongzhi 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};
Raymond Knopp's avatar
Raymond 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

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

#endif

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


298

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

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

Raymond Knopp's avatar
Raymond 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]); 
    */
Hongzhi Wang's avatar
Hongzhi Wang committed
314 315 316 317
#elif defined(__arm__)
// to be filled in
#endif

Raymond Knopp's avatar
Raymond Knopp committed
318
    for (int re=0; re<nb_re; re+=12) {
319
      //            printf("******re %d\n",re);
Hongzhi Wang's avatar
Hongzhi Wang committed
320
#if defined(__x86_64__) || defined(__i386__)
Raymond Knopp's avatar
Raymond 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;
      
Hongzhi Wang's avatar
Hongzhi Wang committed
383
#elif defined(__arm__)
Raymond Knopp's avatar
Raymond Knopp committed
384
      // to be filled in
Hongzhi Wang's avatar
Hongzhi Wang committed
385 386
#endif
    }
Raymond Knopp's avatar
Raymond Knopp committed
387
  }
Hongzhi Wang's avatar
Hongzhi 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)
{

Raymond Knopp's avatar
Raymond Knopp committed
399
  uint8_t symbol_mod;
Hongzhi Wang's avatar
Hongzhi 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__)
Raymond Knopp's avatar
Raymond 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];
Hongzhi Wang's avatar
Hongzhi Wang committed
412
#elif defined(__arm__)
Raymond Knopp's avatar
Raymond 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];
Hongzhi Wang's avatar
Hongzhi 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
      }
    }
Raymond Knopp's avatar
Raymond Knopp committed
427

Hongzhi Wang's avatar
Hongzhi 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,
Raymond Knopp's avatar
Raymond 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)
Hongzhi Wang's avatar
Hongzhi Wang committed
441
{
442
  uint8_t reset, offset;
Hongzhi Wang's avatar
Hongzhi 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

Hongzhi Wang's avatar
Hongzhi Wang committed
448 449
  reset = 1;
  // x1 is set in first call to lte_gold_generic
450
  x2 = Nid; //this is c_init
Hongzhi Wang's avatar
Hongzhi 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 461 462 463 464
    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++) {
      if (((i+offset)&0x1f)==0) {
        s = lte_gold_generic(&x1, &x2, reset);
        reset = 0;
      }
Raymond Knopp's avatar
Raymond Knopp committed
465
#ifdef DEBUG_PBCH_ENCODING
hongzhi wang's avatar
hongzhi wang committed
466
      if (i<8)
Raymond Knopp's avatar
Raymond Knopp committed
467 468 469
	printf("s: %04x\t", s);
      printf("pbch_a_interleaved 0x%08x\n", pbch->pbch_a_interleaved);
#endif
470
      if (bitwise) {
471
	
Guy De Souza's avatar
Guy De Souza committed
472 473
        (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);
474
      }
Hongzhi Wang's avatar
Hongzhi Wang committed
475

476 477
      else {
    	  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,30,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 526
{

  NR_UE_COMMON *nr_ue_common_vars = &ue->common_vars;

  int max_h=0;

  int symbol,i;
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;
hongzhi wang's avatar
hongzhi wang committed
537
  unsigned short idx_demod =0;
Hongzhi Wang's avatar
Hongzhi Wang committed
538 539
  int8_t decoderState=0;
  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

Raymond Knopp's avatar
Raymond Knopp committed
555 556
  int first_symbol=1;
  if (ue->is_synchronized > 0) first_symbol+=4;
Hongzhi Wang's avatar
Hongzhi Wang committed
557

Florian Kaltenberger's avatar
WIP  
Florian Kaltenberger committed
558
#ifdef DEBUG_PBCH
hongzhi wang's avatar
hongzhi wang committed
559
    //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
560 561
    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
562
#endif
Raymond Knopp's avatar
Raymond Knopp committed
563 564 565

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

hongzhi wang's avatar
hongzhi wang committed
566
  
Hongzhi Wang's avatar
Hongzhi Wang committed
567
    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
568 569 570
		    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,
571
		    symbol-first_symbol+1,
Raymond Knopp's avatar
Raymond Knopp committed
572 573 574
		    high_speed_flag,
		    ue->is_synchronized,
		    frame_parms);
Hongzhi Wang's avatar
Hongzhi Wang committed
575
#ifdef DEBUG_PBCH
Raymond Knopp's avatar
Raymond Knopp committed
576 577
    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
578 579
#endif

Raymond Knopp's avatar
Raymond Knopp committed
580 581 582 583 584 585
    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);
    }
Hongzhi Wang's avatar
Hongzhi Wang committed
586 587

#ifdef DEBUG_PBCH
Raymond Knopp's avatar
Raymond Knopp committed
588
    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
589 590 591
#endif

    nr_pbch_channel_compensation(nr_ue_pbch_vars->rxdataF_ext,
Raymond Knopp's avatar
Raymond Knopp committed
592 593 594 595 596 597
				 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
Hongzhi Wang's avatar
Hongzhi Wang committed
598 599 600 601 602 603

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

604
/*
Hongzhi Wang's avatar
Hongzhi Wang committed
605 606 607
    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
608
      LOG_I(PHY,"[PBCH][RX] Unsupported MIMO mode\n");
Hongzhi Wang's avatar
Hongzhi Wang committed
609 610
      return(-1);
    }
611
*/
Raymond Knopp's avatar
Raymond Knopp committed
612
    if (symbol==(first_symbol+1)) {
Hongzhi Wang's avatar
Hongzhi Wang committed
613
      nr_pbch_quantize(pbch_e_rx,
Hongzhi Wang's avatar
Hongzhi Wang committed
614
                    (short*)&(nr_ue_pbch_vars->rxdataF_comp[0][symbol*240]),
Hongzhi Wang's avatar
Hongzhi Wang committed
615
                    144);
Hongzhi Wang's avatar
Hongzhi Wang committed
616

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

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


  }

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

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

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

Raymond Knopp's avatar
Raymond Knopp committed
641 642 643
  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));
Hongzhi Wang's avatar
Hongzhi Wang committed
644

Hongzhi Wang's avatar
Hongzhi Wang committed
645
#endif
646

hongzhi wang's avatar
hongzhi wang committed
647

648 649 650
  //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
651 652
  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
653

654 655


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


Raymond Knopp's avatar
Raymond Knopp committed
659 660 661
  AssertFatal(ue->nrPolar_params != NULL,"ue->nrPolar_params is null\n");

  t_nrPolar_params *currentPtr = nr_polar_params(ue->nrPolar_params, NR_POLAR_PBCH_MESSAGE_TYPE, NR_POLAR_PBCH_PAYLOAD_BITS, NR_POLAR_PBCH_AGGREGATION_LEVEL);
662

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

665

666 667
  if(decoderState == -1)
  	return(decoderState);
Turker YILMAZ's avatar
Turker YILMAZ committed
668
  	
Guy De Souza's avatar
Guy De Souza committed
669
  	printf("polar decoder output 0x%08x\n",nr_ue_pbch_vars->pbch_a_prime);
hongzhi wang's avatar
hongzhi wang committed
670
  
671
  //payload un-scrambling
672
  memset(&nr_ue_pbch_vars->pbch_a_interleaved, 0, sizeof(uint32_t) );
673
  M = (Lmax == 64)? (NR_POLAR_PBCH_PAYLOAD_BITS - 6) : (NR_POLAR_PBCH_PAYLOAD_BITS - 3);
Guy De Souza's avatar
Guy De Souza committed
674 675 676
  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);
  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) );
677 678

  //payload deinterleaving
679 680
  //uint32_t in=0;
  uint32_t out=0;
681
  for (int i=0; i<32; i++) {
682
    out |= ((nr_ue_pbch_vars->pbch_a_interleaved>>i)&1)<<(pbch_deinterleaving_pattern[i]);
683
#ifdef DEBUG_PBCH
684
    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);
685
#endif
686 687
  }

Guy De Souza's avatar
Guy De Souza committed
688 689 690 691 692 693 694 695 696
 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
697

698
#ifdef DEBUG_PBCH
Guy De Souza's avatar
Guy De Souza committed
699
 printf("xtra_byte %x payload %x\n", xtra_byte, payload);
hongzhi wang's avatar
hongzhi wang committed
700
  for (i=0; i<(NR_POLAR_PBCH_PAYLOAD_BITS>>3); i++){
701
    //  	  printf("unscrambling pbch_a[%d] = %x \n", i,pbch_a[i]);
hongzhi wang's avatar
hongzhi wang committed
702
  	  printf("[PBCH] decoder payload[%d] = %x\n",i,decoded_output[i]);
hongzhi wang's avatar
hongzhi wang committed
703
  }
704
#endif
705

706 707 708
    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;
709 710
    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;
711
    ue->rx_ind.rx_indication_body->mib_pdu.pdu = &decoded_output[0];
Guy De Souza's avatar
Guy De Souza committed
712
    ue->rx_ind.rx_indication_body->mib_pdu.additional_bits = xtra_byte;
713 714 715
    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
716

717 718
    if (ue->if_inst && ue->if_inst->dl_indication)
      ue->if_inst->dl_indication(&ue->dl_indication);
719 720

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