From 984830505bcfb676c3ddcb4daac8c4dddde651aa Mon Sep 17 00:00:00 2001
From: Sagar Parsawar <sagar.parsawar@eurecom.fr>
Date: Mon, 28 Mar 2022 10:58:32 +0200
Subject: [PATCH] 256-bit alignment fix in trx_usrp_write/trx_usrp_write_thread
 functions

---
 targets/ARCH/USRP/USERSPACE/LIB/usrp_lib.cpp | 29 +++++++++++++++++---
 1 file changed, 25 insertions(+), 4 deletions(-)

diff --git a/targets/ARCH/USRP/USERSPACE/LIB/usrp_lib.cpp b/targets/ARCH/USRP/USERSPACE/LIB/usrp_lib.cpp
index 2c880ed919..6b52407a70 100644
--- a/targets/ARCH/USRP/USERSPACE/LIB/usrp_lib.cpp
+++ b/targets/ARCH/USRP/USERSPACE/LIB/usrp_lib.cpp
@@ -402,7 +402,14 @@ static int trx_usrp_write(openair0_device *device,
       for (int j=0; j<nsamps2; j++) {
 #if defined(__x86_64__) || defined(__i386__)
 #ifdef __AVX2__
-        buff_tx[i][j] = _mm256_slli_epi16(((__m256i *)buff[i])[j],4);
+        if ((((uintptr_t) buff[i])&0x1F)==0) {
+          buff_tx[i][j] = _mm256_slli_epi16(((__m256i *)buff[i])[j],4);
+        }
+        else
+        {
+          ((__m128i *)buff_tx[i])[2*j]   = _mm_slli_epi16(((__m128i *)buff[i])[2*j],4);
+          ((__m128i *)buff_tx[i])[2*j+1] = _mm_slli_epi16(((__m128i *)buff[i])[2*j+1],4);
+        }
 #else
         buff_tx[i][j] = _mm_slli_epi16(((__m128i *)buff[i])[j],4);
 #endif
@@ -540,7 +547,14 @@ void *trx_usrp_write_thread(void * arg){
       for (int j=0; j<nsamps2; j++) {
         #if defined(__x86_64__) || defined(__i386__)
           #ifdef __AVX2__
-            buff_tx[i][j] = _mm256_slli_epi16(((__m256i *)buff[i])[j],4);
+            if ((((uintptr_t) buff[i])&0x1F)==0) {
+              buff_tx[i][j] = _mm256_slli_epi16(((__m256i *)buff[i])[j],4);
+            }
+            else
+            {
+              ((__m128i *)buff_tx[i])[2*j]   = _mm_slli_epi16(((__m128i *)buff[i])[2*j],4);
+              ((__m128i *)buff_tx[i])[2*j+1] = _mm_slli_epi16(((__m128i *)buff[i])[2*j+1],4);
+            }
           #else
             buff_tx[i][j] = _mm_slli_epi16(((__m128i *)buff[i])[j],4);
           #endif
@@ -1127,8 +1141,8 @@ extern "C" {
         // from usrp_time_offset
         //openair0_cfg[0].samples_per_packet    = 2048;
         openair0_cfg[0].tx_sample_advance     = 15; //to be checked
-        openair0_cfg[0].tx_bw                 = 80e6;
-        openair0_cfg[0].rx_bw                 = 80e6;
+        //openair0_cfg[0].tx_bw                 = 80e6;
+        //openair0_cfg[0].rx_bw                 = 80e6;
         break;
 
       case 61440000:
@@ -1154,6 +1168,13 @@ extern "C" {
         openair0_cfg[0].rx_bw                 = 20e6;
         break;
 
+      case 23040000:
+        //openair0_cfg[0].samples_per_packet    = 2048;
+        openair0_cfg[0].tx_sample_advance     = 15;
+        openair0_cfg[0].tx_bw                 = 20e6;
+        openair0_cfg[0].rx_bw                 = 20e6;
+        break;
+
       case 15360000:
         //openair0_cfg[0].samples_per_packet    = 2048;
         openair0_cfg[0].tx_sample_advance     = 45;