nr_pbch.c 22.7 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;
444
  int16_t *demod_pbch_e = pbch->llr;
445

hongzhi wang's avatar
hongzhi wang committed
446

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

451
  // The Gold sequence is shifted by nushift* M, so we skip (nushift*M /32) double words
452
  for (int i=0; i<(uint16_t)ceil(((float)nushift*M)/32); i++) {
453
454
455
456
457
458
459
460
461
462
463
    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;
      }
knopp's avatar
knopp committed
464
#ifdef DEBUG_PBCH_ENCODING
hongzhi wang's avatar
hongzhi wang committed
465
      if (i<8)
knopp's avatar
knopp committed
466
467
468
	printf("s: %04x\t", s);
      printf("pbch_a_interleaved 0x%08x\n", pbch->pbch_a_interleaved);
#endif
469
      if (bitwise) {
470
471
	
        (pbch->pbch_a_interleaved) ^= ((unscrambling_mask>>i)&1)? ((pbch->pbch_a_prime>>i)&1)<<i : (((pbch->pbch_a_prime>>i)&1) ^ ((s>>((i+offset)&0x1f))&1))<<i;
472
      }
Wang's avatar
Wang committed
473

474
475
      else {
    	  if (((s>>((i+offset)&0x1f))&1)==1)
hongzhi wang's avatar
hongzhi wang committed
476
    		  demod_pbch_e[i] = -demod_pbch_e[i];
hongzhi wang's avatar
hongzhi wang committed
477
478
479
480
#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
481
      }
Wang's avatar
Wang committed
482
  }
483

Wang's avatar
Wang committed
484
485
}

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

  uint16_t i;

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

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

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

Hongzhi Wang's avatar
Hongzhi Wang committed
511
int nr_rx_pbch( PHY_VARS_NR_UE *ue,
knopp's avatar
knopp committed
512
513
514
515
516
517
		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
518
519
520
521
522
523
524
{

  NR_UE_COMMON *nr_ue_common_vars = &ue->common_vars;

  int max_h=0;

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

539
540
  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
541
  memset(&pbch_a[0], 0, sizeof(uint8_t) * NR_POLAR_PBCH_PAYLOAD_BITS);
hongzhi wang's avatar
hongzhi wang committed
542

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

Wang's avatar
Wang committed
545
546

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

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

knopp's avatar
knopp committed
553
554
  int first_symbol=1;
  if (ue->is_synchronized > 0) first_symbol+=4;
Wang's avatar
Wang committed
555

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

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

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

knopp's avatar
knopp committed
578
579
580
581
582
583
    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
584
585

#ifdef DEBUG_PBCH
knopp's avatar
knopp committed
586
    LOG_I(PHY,"[PHY] PBCH log2_maxh = %d (%d)\n",nr_ue_pbch_vars->log2_maxh,max_h);
Wang's avatar
Wang committed
587
588
589
#endif

    nr_pbch_channel_compensation(nr_ue_pbch_vars->rxdataF_ext,
knopp's avatar
knopp committed
590
591
592
593
594
595
				 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
596
597
598
599
600
601

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

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

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

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


  }

knopp's avatar
knopp committed
627
628
629
630
631
#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
632
  pbch_e_rx = nr_ue_pbch_vars->llr;
633
  //demod_pbch_e = nr_ue_pbch_vars->demod_pbch_e;
634
  pbch_a = nr_ue_pbch_vars->pbch_a;
Hongzhi Wang's avatar
Hongzhi Wang committed
635

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

knopp's avatar
knopp committed
639
640
641
  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
642

Hongzhi Wang's avatar
Hongzhi Wang committed
643
#endif
644

hongzhi wang's avatar
hongzhi wang committed
645

646
647
648
  //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
649
650
  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
651

652
653


Hongzhi Wang's avatar
Hongzhi Wang committed
654
  //polar decoding de-rate matching
655
656


knopp's avatar
knopp committed
657
658
659
  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);
660

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

663

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

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

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

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

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

715
716
    if (ue->if_inst && ue->if_inst->dl_indication)
      ue->if_inst->dl_indication(&ue->dl_indication);
717
718

    return 0;    
Wang's avatar
Wang committed
719
}