RM turbo TX working with LUTs

srslte/include/srslte/fec/rm_turbo.h

@@ -57,6 +57,16 @@ SRSLTE_API int srslte_rm_turbo_tx(uint8_t *w_buff,
                                   uint32_t out_len, 
                                   uint32_t rv_idx);
 
+SRSLTE_API void srslte_rm_turbo_gentables(); 
+
+SRSLTE_API int srslte_rm_turbo_tx_lut(uint8_t *w_buff, 
+                                      uint8_t *systematic, 
+                                      uint8_t *parity, 
+                                      uint8_t *output, 
+                                      uint32_t cb_idx, 
+                                      uint32_t out_len, 
+                                      uint32_t rv_idx);
+
 SRSLTE_API int srslte_rm_turbo_rx(float *w_buff,
                                   uint32_t buff_len, 
                                   float *input, 

srslte/include/srslte/utils/bit.h

@@ -46,6 +46,18 @@ SRSLTE_API void srslte_bit_interleave(uint8_t *input,
                                       uint32_t *interleaver, 
                                       uint32_t nof_bits); 
 
+SRSLTE_API void srslte_bit_copy(uint8_t *dst, 
+                                uint32_t dst_offset, 
+                                uint8_t *src, 
+                                uint32_t src_offset, 
+                                uint32_t nof_bits);
+
+SRSLTE_API void srslte_bit_interleave_w_offset(uint8_t *input, 
+                                               uint8_t *output, 
+                                               uint32_t *interleaver, 
+                                               uint32_t nof_bits, 
+                                               uint32_t w_offset);
+
 SRSLTE_API void srslte_bit_unpack_vector(uint8_t *srslte_bit_packed, 
                                        uint8_t *bits_packed, 
                                        int nof_bits);

srslte/lib/fec/src/cbsegm.c

@@ -89,7 +89,7 @@ int srslte_cbsegm(srslte_cbsegm_t *s, uint32_t tbs) {
             s->C1 = s->C - s->C2;
           }
           s->F = s->C1 * s->K1 + s->C2 * s->K2 - Bp;
-          INFO("CB Segmentation: TBS: %d, C=%d, C+=%d K+=%d, C-=%d, K-=%d, F=%d, Bp=%d\n",
+          printf("CB Segmentation: TBS: %d, C=%d, C+=%d K+=%d, C-=%d, K-=%d, F=%d, Bp=%d\n",
               tbs, s->C, s->C1, s->K1, s->C2, s->K2, s->F, Bp);         
           ret = SRSLTE_SUCCESS; 
         }

srslte/lib/fec/src/rm_turbo.c

@@ -34,80 +34,165 @@
 
 #include "srslte/fec/rm_turbo.h"
 #include "srslte/utils/bit.h"
+#include "srslte/utils/vector.h"
+#include "srslte/fec/cbsegm.h"
+
+
 
 #define NCOLS 32
 #define NROWS_MAX NCOLS
 
-uint8_t RM_PERM_TC[NCOLS] = { 0, 16, 8, 24, 4, 20, 12, 28, 2, 18, 10, 26,
+static uint8_t RM_PERM_TC[NCOLS] = { 0, 16, 8, 24, 4, 20, 12, 28, 2, 18, 10, 26,
     6, 22, 14, 30, 1, 17, 9, 25, 5, 21, 13, 29, 3, 19, 11, 27, 7, 23, 15, 31 };
 
-uint32_t test_interleaver[64*1024];
+static uint32_t interleaver_systematic_bits[SRSLTE_NOF_TC_CB_SIZES][6148]; // 4 tail bits
+static uint32_t interleaver_parity_bits[SRSLTE_NOF_TC_CB_SIZES][2*6148];
+static uint32_t k0_vec[SRSLTE_NOF_TC_CB_SIZES][4][2];
 
-/* Turbo Code Rate Matching.
- * 3GPP TS 36.212 v10.1.0 section 5.1.4.1
- *
- * If rv_idx==0, the circular buffer w_buff is filled with all redundancy versions and 
- * the corresponding version of length out_len is saved in the output buffer.  
- * Otherwise, the corresponding version is directly obtained from w_buff and saved into output. 
- * 
- * Note that calling this function with rv_idx!=0 without having called it first with rv_idx=0
- * will produce unwanted results. 
- * 
- * TODO: Soft buffer size limitation according to UE category
- */
-//#define new 
 
-#ifdef new
-int srslte_rm_turbo_tx(uint8_t *w_buff, uint32_t w_buff_len, uint8_t *input, uint32_t in_len, uint8_t *output,
-    uint32_t out_len, uint32_t rv_idx) {
 
-  int ndummy, kidx; 
+void srslte_rm_turbo_gentable_systematic(uint32_t *table_bits, uint32_t k0_vec[4][2], uint32_t nrows, int ndummy) {
-  int nrows, K_p;
 
-  int i, j, k, s, N_cb, k0;
+  bool last_is_null=true;
+  int k_b=0, buff_idx=0;
+  for (int j = 0; j < NCOLS; j++) {
+    for (int i = 0; i < nrows; i++) {      
+      if (i * NCOLS + RM_PERM_TC[j] >= ndummy) {
+        table_bits[k_b] = i * NCOLS + RM_PERM_TC[j] - ndummy;
+        k_b++;        
+        last_is_null=false;
+      } else {
+        last_is_null=true;
+      }
+      for (int i=0;i<4;i++) {
+        if (k0_vec[i][1] == -1) {
+          if (k0_vec[i][0]%(3*nrows*NCOLS) <= buff_idx && !last_is_null) {
+            k0_vec[i][1] = k_b-1;
+          }
+        }
+      }
+      buff_idx++;
+    }
+  }
+}
 
-  if (in_len < 3) {
+void srslte_rm_turbo_gentable_parity(uint32_t *table_parity, uint32_t k0_vec[4][2], int offset, uint32_t nrows, int ndummy) {
-    fprintf(stderr, "Error minimum input length for rate matching is 3\n");
+  
-    return -1;
+  bool last_is_null=true;
+  int k_b=0, buff_idx0=0;
+  int K_p = nrows*NCOLS;
+  int buff_idx1=0;
+  for (int j = 0; j < NCOLS; j++) {
+    for (int i = 0; i < nrows; i++) {      
+      if (i * NCOLS + RM_PERM_TC[j] >= ndummy) {
+        table_parity[k_b] = i * NCOLS + RM_PERM_TC[j] - ndummy;
+        k_b++;
+        last_is_null=false;
+      } else {
+        last_is_null=true;
+      }
+      for (int i=0;i<4;i++) {
+        if (k0_vec[i][1] == -1) {
+          if (k0_vec[i][0]%(3*K_p) <= 2*buff_idx0+K_p && !last_is_null) {
+            k0_vec[i][1] = offset+k_b-1;
+          }
+        }
       }
+      buff_idx0++;
       
-  nrows = (uint32_t) (in_len / 3 - 1) / NCOLS + 1;
+      int kidx = (RM_PERM_TC[buff_idx1 / nrows] + NCOLS * (buff_idx1 % nrows) + 1) % K_p;
-  K_p = nrows * NCOLS;
+      if ((kidx - ndummy) >= 0) {
-  if (3 * K_p > w_buff_len) {
+        table_parity[k_b] = kidx-ndummy+offset;
-    fprintf(stderr,
+        k_b++;
-        "Input too large. Max input length including dummy bits is %d (3x%dx32, in_len %d, Kp=%d)\n",
+        last_is_null=false;
-        w_buff_len, nrows, in_len, K_p);
+      } else {
-    return -1;
+        last_is_null=true;
+      }
+      for (int i=0;i<4;i++) {
+        if (k0_vec[i][1] == -1) {
+          if (k0_vec[i][0]%(3*K_p) <= 2*buff_idx1+1+K_p && !last_is_null) {
+            k0_vec[i][1] = offset+k_b-1;
+          }
+        }
+      }
+      buff_idx1++;
+    }    
+  }
 }
 
-  ndummy = K_p - in_len / 3;
+void srslte_rm_turbo_gentables() {
+  for (int cb_idx=0;cb_idx<SRSLTE_NOF_TC_CB_SIZES;cb_idx++) {
+    int cb_len=srslte_cbsegm_cbsize(cb_idx);
+    int in_len=3*cb_len+12;
+    
+    int nrows = (in_len / 3 - 1) / NCOLS + 1;
+    int K_p = nrows * NCOLS;
+    int ndummy = K_p - in_len / 3;
     if (ndummy < 0) {
       ndummy = 0;
     }
 
-  if (rv_idx == 0) {
+    for (int i=0;i<4;i++) {
-    srslte_bit_interleave(input, w_buff, test_interleaver, in_len);
+      k0_vec[cb_idx][i][0] = nrows * (2 * (uint32_t) ceilf((float) (3*K_p) / (float) (8 * nrows)) * i + 2);
+      k0_vec[cb_idx][i][1] = -1; 
+    }
+    srslte_rm_turbo_gentable_systematic(interleaver_systematic_bits[cb_idx], k0_vec[cb_idx], nrows, ndummy);
+    srslte_rm_turbo_gentable_parity(interleaver_parity_bits[cb_idx], k0_vec[cb_idx], in_len/3, nrows, ndummy);
+ }
 }
 
-  /* Bit selection and transmission 5.1.4.1.2 */
+int srslte_rm_turbo_tx_lut(uint8_t *w_buff, uint8_t *systematic, uint8_t *parity, uint8_t *output, uint32_t cb_idx, uint32_t out_len, uint32_t rv_idx) {
-  N_cb = 3 * K_p;       // TODO: Soft buffer size limitation
 
-  k0 = nrows
-      * (2 * (uint32_t) ceilf((float) N_cb / (float) (8 * nrows)) * rv_idx + 2);
-  k = 0;
-  j = 0;
   
-  while (k < out_len) {
+  if (rv_idx < 4 && cb_idx < SRSLTE_NOF_TC_CB_SIZES) {
-    if (w_buff[(k0 + j) % N_cb] != SRSLTE_TX_NULL) {
+    
-      output[k] = w_buff[(k0 + j) % N_cb];
+    int in_len=3*srslte_cbsegm_cbsize(cb_idx)+12;
-      k++;
+    
+    /* Sub-block interleaver (5.1.4.1.1) and bit collection */
+    if (rv_idx == 0) {
+      
+      // Systematic bits 
+      srslte_bit_interleave(systematic, w_buff, interleaver_systematic_bits[cb_idx], in_len/3);
+
+      // Parity bits 
+      srslte_bit_interleave_w_offset(parity, &w_buff[in_len/24], interleaver_parity_bits[cb_idx], 2*in_len/3, 4);      
     }
-    j++;
+    
+    /* Bit selection and transmission 5.1.4.1.2 */    
+    int w_len = 0; 
+    int r_ptr = k0_vec[cb_idx][rv_idx][1]; 
+    while (w_len < out_len) {
+      int cp_len = out_len - w_len; 
+      if (cp_len + r_ptr >= in_len) {
+        cp_len = in_len - r_ptr;
+      }
+      srslte_bit_copy(output, w_len, w_buff, r_ptr, cp_len);
+      r_ptr += cp_len; 
+      if (r_ptr >= in_len) {
+        r_ptr -= in_len; 
+      }
+      w_len += cp_len; 
     }
+
     return 0;
+  } else {
+    return SRSLTE_ERROR_INVALID_INPUTS; 
+  }
 }
 
-#else
+
+/* Turbo Code Rate Matching.
+ * 3GPP TS 36.212 v10.1.0 section 5.1.4.1
+ *
+ * If rv_idx==0, the circular buffer w_buff is filled with all redundancy versions and 
+ * the corresponding version of length out_len is saved in the output buffer.  
+ * Otherwise, the corresponding version is directly obtained from w_buff and saved into output. 
+ * 
+ * Note that calling this function with rv_idx!=0 without having called it first with rv_idx=0
+ * will produce unwanted results. 
+ * 
+ * TODO: Soft buffer size limitation according to UE category
+ */
 int srslte_rm_turbo_tx(uint8_t *w_buff, uint32_t w_buff_len, uint8_t *input, uint32_t in_len, uint8_t *output,
     uint32_t out_len, uint32_t rv_idx) {
 
@@ -184,7 +269,6 @@ int srslte_rm_turbo_tx(uint8_t *w_buff, uint32_t w_buff_len, uint8_t *input, uin
   }
   return 0;
 }
-#endif
 
 /* Undoes Turbo Code Rate Matching.
  * 3GPP TS 36.212 v10.1.0 section 5.1.4.1

srslte/lib/fec/test/rm_turbo_test.c

@@ -36,21 +36,29 @@
 
 #include "srslte/srslte.h"
 
+
 int nof_tx_bits = -1, nof_rx_bits = -1;
 int nof_filler_bits = -1; 
 int rv_idx = 0;
+int cb_idx = -1; 
+
+uint8_t systematic[6148], parity[2*6148];
+uint8_t systematic_bytes[6148/8+1], parity_bytes[2*6148/8+1];
 
 void usage(char *prog) {
-  printf("Usage: %s -t nof_tx_bits -r nof_rx_bits [-i rv_idx -f nof_filler_bits]\n", prog);
+  printf("Usage: %s -t nof_tx_bits | -c cb_idx -r nof_rx_bits [-i rv_idx -f nof_filler_bits]\n", prog);
 }
 
 void parse_args(int argc, char **argv) {
   int opt;
-  while ((opt = getopt(argc, argv, "trif")) != -1) {
+  while ((opt = getopt(argc, argv, "tcrif")) != -1) {
     switch (opt) {
     case 'f':
       nof_filler_bits = atoi(argv[optind]);
       break;
+    case 'c':
+      cb_idx = atoi(argv[optind]);
+      break;
     case 't':
       nof_tx_bits = atoi(argv[optind]);
       break;
@@ -65,7 +73,7 @@ void parse_args(int argc, char **argv) {
       exit(-1);
     }
   }
-  if (nof_tx_bits == -1) {
+  if (nof_tx_bits == -1 && cb_idx == -1) {
     usage(argv[0]);
     exit(-1);
   }
@@ -77,12 +85,23 @@ void parse_args(int argc, char **argv) {
 
 int main(int argc, char **argv) {
   int i;
-  uint8_t *bits, *bits_out, *rm_bits, *w_buff_c;
+  uint8_t *bits, *bits_out, *rm_bits, *rm_bits2, *rm_bits2_bytes, *w_buff_c;
   float *rm_symbols, *unrm_symbols, *w_buff_f;
   int nof_errors;
 
   parse_args(argc, argv);
   
+  srslte_rm_turbo_gentables();
+
+ //for (cb_idx=0;cb_idx<188;cb_idx++) {
+  //  for (rv_idx=0;rv_idx<4;rv_idx++) {
+      printf("cb_len=%d, rv_idx=%d\n", cb_idx, rv_idx);
+  
+  
+  if (cb_idx != -1) {
+    nof_tx_bits = 3*srslte_cbsegm_cbsize(cb_idx)+12;
+  }
+  
   bits = malloc(sizeof(uint8_t) * nof_tx_bits);
   if (!bits) {
     perror("malloc");
@@ -103,6 +122,16 @@ int main(int argc, char **argv) {
     perror("malloc");
     exit(-1);
   }
+  rm_bits2 = malloc(sizeof(uint8_t) * nof_rx_bits);
+  if (!rm_bits2) {
+    perror("malloc");
+    exit(-1);
+  }
+  rm_bits2_bytes = malloc(sizeof(uint8_t) * nof_rx_bits/8 + 1);
+  if (!rm_bits2_bytes) {
+    perror("malloc");
+    exit(-1);
+  }
   rm_symbols = malloc(sizeof(float) * nof_rx_bits);
   if (!rm_symbols) {
     perror("malloc");
@@ -131,10 +160,47 @@ int main(int argc, char **argv) {
   bzero(w_buff_c, nof_tx_bits * 10 * sizeof(uint8_t));
   bzero(w_buff_f, nof_rx_bits * 10 * sizeof(float));
   
-  printf("BITS: ");
+  srslte_rm_turbo_tx(w_buff_c, nof_tx_bits * 10, bits, nof_tx_bits, rm_bits, nof_rx_bits, 0);
-  srslte_vec_fprint_b(stdout, bits, nof_tx_bits);
 
+  if (rv_idx > 0) {
     srslte_rm_turbo_tx(w_buff_c, nof_tx_bits * 10, bits, nof_tx_bits, rm_bits, nof_rx_bits, rv_idx);
+  }
+
+  for (i=0;i<nof_filler_bits;i++) {
+    bits[3*i+0] = 0;
+    bits[3*i+1] = 0;
+  }
+  
+  for (int i=0;i<nof_tx_bits/3;i++) {
+    systematic[i] = bits[3*i];
+    parity[i] = bits[3*i+1];
+    parity[i+nof_tx_bits/3] = bits[3*i+2];
+  }
+  
+  srslte_bit_pack_vector(systematic, systematic_bytes, nof_tx_bits/3);
+  srslte_bit_pack_vector(parity, parity_bytes, 2*nof_tx_bits/3);
+  
+  bzero(w_buff_c, nof_tx_bits * 10 * sizeof(uint8_t));
+
+  bzero(rm_bits2_bytes, nof_rx_bits/8);
+  srslte_rm_turbo_tx_lut(w_buff_c, systematic_bytes, parity_bytes, rm_bits2_bytes, cb_idx, nof_rx_bits, 0);
+  if (rv_idx > 0) {
+    bzero(rm_bits2_bytes, nof_rx_bits/8);
+    srslte_rm_turbo_tx_lut(w_buff_c, systematic_bytes, parity_bytes, rm_bits2_bytes, cb_idx, nof_rx_bits, rv_idx);
+  }
+
+  srslte_bit_unpack_vector(rm_bits2_bytes, rm_bits2, nof_rx_bits);
+  
+  for (int i=0;i<nof_rx_bits;i++) {
+    if (rm_bits[i] != rm_bits2[i]) {
+      printf("error in bit %d\n", i);
+      exit(-1);
+    }
+  }
+  //}    
+  //}
+  printf("OK\n");
+  exit(0);
   
   printf("RM: ");
   srslte_vec_fprint_b(stdout, rm_bits, nof_rx_bits);
@@ -159,7 +225,7 @@ int main(int argc, char **argv) {
 
   nof_errors = 0;
   for (i = 0; i < nof_tx_bits; i++) {
-    if (unrm_symbols[i] > 0 && ((unrm_symbols[i] > 0) != bits[i])) {
+    if (bits_out[i] != bits[i]) {
       nof_errors++;
     }
   }

srslte/lib/utils/src/bit.c

@@ -25,27 +25,191 @@
  *
  */
 
-
+#include <string.h>
 #include <stdint.h>
 #include <stdio.h>
+#include <limits.h>
+#include <string.h>
+#include <stddef.h>
 
 #include "srslte/utils/bit.h"
 
 void srslte_bit_interleave(uint8_t *input, uint8_t *output, uint32_t *interleaver, uint32_t nof_bits) {
-  for (uint32_t i=0;i<nof_bits/8;i++) {
+  srslte_bit_interleave_w_offset(input, output, interleaver, nof_bits, 0);
+}
+
+void srslte_bit_interleave_w_offset(uint8_t *input, uint8_t *output, uint32_t *interleaver, uint32_t nof_bits, uint32_t w_offset) {
+  uint32_t st=0, w_offset_p=0;
+  static const uint8_t mask[] = { 0x80, 0x40, 0x20, 0x10, 0x8, 0x4, 0x2, 0x1 };
+
+  if (w_offset < 8 && w_offset > 0) {
+    st=1;
+    for (uint32_t j=0;j<8-w_offset;j++) {
+      uint32_t i_p = interleaver[j];            
+      if (input[i_p/8] & mask[i_p%8]) {
+        output[0] |= mask[j+w_offset];
+      }
+    }
+    w_offset_p=8-w_offset;
+  }
+  for (uint32_t i=st;i<nof_bits/8;i++) {
     output[i] = 0; 
     for (uint32_t j=0;j<8;j++) {
-      uint32_t i_p = interleaver[i*8+j];      
+      uint32_t i_p = interleaver[i*8+j-w_offset_p];      
-      if (input[i_p/8] & (1<<(7-i_p%8))) {
+      if (input[i_p/8] & mask[i_p%8]) {        
-        output[i] |= 1<<(7-j);
+        output[i] |= mask[j];
       }      
     }
   }
   for (uint32_t j=0;j<nof_bits%8;j++) {
-    uint32_t i_p = interleaver[(nof_bits/8)*8+j];      
+    uint32_t i_p = interleaver[(nof_bits/8)*8+j-w_offset];          
+    if (input[i_p/8] & mask[i_p%8]) {
+      output[nof_bits/8] |= mask[j];
+    }
+  }
+  for (uint32_t j=0;j<w_offset;j++) {
+    uint32_t i_p = interleaver[(nof_bits/8)*8+j-w_offset];          
     if (input[i_p/8] & (1<<(7-i_p%8))) {
-      output[nof_bits/8] |= 1<<(7-j);
+      output[nof_bits/8] |= mask[j];
+    }
+  }
+}
+
+/* bitarray copy function taken from 
+ * http://stackoverflow.com/questions/3534535/whats-a-time-efficient-algorithm-to-copy-unaligned-bit-arrays
+ */
+
+
+#define PREPARE_FIRST_COPY()                                      \
+    do {                                                          \
+    if (src_len >= (CHAR_BIT - dst_offset_modulo)) {              \
+        *dst     &= reverse_mask[dst_offset_modulo];              \
+        src_len -= CHAR_BIT - dst_offset_modulo;                  \
+    } else {                                                      \
+        *dst     &= reverse_mask[dst_offset_modulo]               \
+              | reverse_mask_xor[dst_offset_modulo + src_len];    \
+         c       &= reverse_mask[dst_offset_modulo + src_len];    \
+        src_len = 0;                                              \
+    } } while (0)
+
+
+static void
+bitarray_copy(const unsigned char *src_org, int src_offset, int src_len,
+                    unsigned char *dst_org, int dst_offset)
+{
+    static const unsigned char reverse_mask[] =
+        { 0x00, 0x80, 0xc0, 0xe0, 0xf0, 0xf8, 0xfc, 0xfe, 0xff };
+    static const unsigned char reverse_mask_xor[] =
+        { 0xff, 0x7f, 0x3f, 0x1f, 0x0f, 0x07, 0x03, 0x01, 0x00 };
+
+    if (src_len) {
+        const unsigned char *src;
+              unsigned char *dst;
+        int                  src_offset_modulo,
+                             dst_offset_modulo;
+
+        src = src_org + (src_offset / CHAR_BIT);
+        dst = dst_org + (dst_offset / CHAR_BIT);
+
+        src_offset_modulo = src_offset % CHAR_BIT;
+        dst_offset_modulo = dst_offset % CHAR_BIT;
+
+        if (src_offset_modulo == dst_offset_modulo) {
+            int              byte_len;
+            int              src_len_modulo;
+            if (src_offset_modulo) {
+                unsigned char   c;
+
+                c = reverse_mask_xor[dst_offset_modulo]     & *src++;
+
+                PREPARE_FIRST_COPY();
+                *dst++ |= c;
+            }
+
+            byte_len = src_len / CHAR_BIT;
+            src_len_modulo = src_len % CHAR_BIT;
+
+            if (byte_len) {
+                memcpy(dst, src, byte_len);
+                src += byte_len;
+                dst += byte_len;
+            }
+            if (src_len_modulo) {
+                *dst     &= reverse_mask_xor[src_len_modulo];
+                *dst |= reverse_mask[src_len_modulo]     & *src;
+            }
+        } else {
+            int             bit_diff_ls,
+                            bit_diff_rs;
+            int             byte_len;
+            int             src_len_modulo;
+            unsigned char   c;
+            /*
+             * Begin: Line things up on destination. 
+             */
+            if (src_offset_modulo > dst_offset_modulo) {
+                bit_diff_ls = src_offset_modulo - dst_offset_modulo;
+                bit_diff_rs = CHAR_BIT - bit_diff_ls;
+
+                c = *src++ << bit_diff_ls;
+                c |= *src >> bit_diff_rs;
+                c     &= reverse_mask_xor[dst_offset_modulo];
+            } else {
+                bit_diff_rs = dst_offset_modulo - src_offset_modulo;
+                bit_diff_ls = CHAR_BIT - bit_diff_rs;
+
+                c = *src >> bit_diff_rs     &
+                    reverse_mask_xor[dst_offset_modulo];
+            }
+            PREPARE_FIRST_COPY();
+            *dst++ |= c;
+
+            /*
+             * Middle: copy with only shifting the source. 
+             */
+            byte_len = src_len / CHAR_BIT;
+
+            while (--byte_len >= 0) {
+                c = *src++ << bit_diff_ls;
+                c |= *src >> bit_diff_rs;
+                *dst++ = c;
+            }
+
+            /*
+             * End: copy the remaing bits; 
+             */
+            src_len_modulo = src_len % CHAR_BIT;
+            if (src_len_modulo) {
+                c = *src++ << bit_diff_ls;
+                c |= *src >> bit_diff_rs;
+                c     &= reverse_mask[src_len_modulo];
+
+                *dst     &= reverse_mask_xor[src_len_modulo];
+                *dst |= c;
+            }
+        }
+    }
+}
+
+void srslte_bit_copy(uint8_t *dst, uint32_t dst_offset, uint8_t *src, uint32_t src_offset, uint32_t nof_bits)
+{
+  static const uint8_t mask_src[] =
+        { 0x00, 0x1, 0x3, 0x7, 0xf, 0x1f, 0x3f, 0x7f, 0xff };
+  static const uint8_t mask_dst[] =
+        { 0x00, 0x80, 0xc0, 0xe0, 0xf0, 0xf8, 0xfc, 0xfe, 0xff };
+  if ((dst_offset%8) == (src_offset%8)) {
+    if (src_offset%8) {
+      // copy 1st word
+      dst[dst_offset/8] |= src[src_offset/8] & mask_src[src_offset%8];
+    }
+    // copy rest of words
+    memcpy(&dst[dst_offset/8], &src[src_offset/8], nof_bits/8);
+    // copy last word
+    if ((src_offset%8+nof_bits)%8) {
+      dst[dst_offset/8+nof_bits/8] = src[src_offset/8+nof_bits/8] & mask_dst[(src_offset%8+nof_bits)%8];
     }
+  } else {
+    bitarray_copy(src, src_offset, nof_bits, dst, dst_offset);
   }
 }
 
@@ -90,6 +254,7 @@ void srslte_bit_pack_vector(uint8_t *bits_packed, uint8_t *bits_unpacked, int no
   }
   if (nof_bits%8) {
     bits_unpacked[i] = srslte_bit_pack(&bits_packed, nof_bits%8);
+    bits_unpacked[i] <<= 8-(nof_bits%8);
   }
 }