ctest: add unit test for new simd function

Casting to int16_t when the intermediate product is higher than int16 max value
will truncate MSB bits and leads to undefined behavior. Hence the values should
be saturated to stay closer to the actual value.

common/platform_types.h

@@ -10,6 +10,7 @@
 
 #define ALIGNARRAYSIZE(a, b) (((a + b - 1) / b) * b)
 #define ALNARS_16_4(a) ALIGNARRAYSIZE(a, 4)
+#define ALNARS_32_8(a) ALIGNARRAYSIZE(a, 8)
 
 typedef struct complexd {
   double r;

common/utils/utils.h

@@ -51,6 +51,12 @@ extern "C" {
 #define IS_BIT_SET(a, b) ((a >> b) & 1)
 #define SET_BIT(a, b) (a | (1 << b))
 
+// Gives number of bytes to next 32 byte mem boundary from an element in array
+#define PTR_ALIGN_OFFSET_BYTES(ptr, bytes) (((bytes) - ((uintptr_t)(ptr) & (bytes - 1))) & (bytes - 1))
+#define PTR_ALIGN_OFFSET_ELEMS(ptr, bytes) (PTR_ALIGN_OFFSET_BYTES(ptr, bytes) / sizeof(*(ptr)))
+
+#define SATURATE_S16(x) ((int16_t)((x) > INT16_MAX ? INT16_MAX : ((x) < INT16_MIN ? INT16_MIN : (int16_t)(x))))
+
 #ifdef __cplusplus
 #ifdef min
 #undef min

openair1/PHY/MODULATION/ul_7_5_kHz.c

@@ -56,7 +56,7 @@ void remove_7_5_kHz(RU_t *ru,uint8_t slot)
     c16_t *rxptr = rxdata[aa]+slot_offset;
     c16_t *rxptr_7_5kHz = rxdata_7_5kHz[aa]+slot_offset2;
     // apply 7.5 kHz
-    mult_complex_vectors((c16_t*)kHz7_5,rxptr,rxptr_7_5kHz,len, 15);
+    mult_cpx_vector((c16_t*)kHz7_5,rxptr,rxptr_7_5kHz,len, 15);
     // undo 7.5 kHz offset for symbol 3 in case RU is slave (for OTA synchronization)
     if (ru->is_slave == 1 && slot == 2){
       int offset=3*frame_parms->ofdm_symbol_size+2*frame_parms->nb_prefix_samples+frame_parms->nb_prefix_samples0;

openair1/PHY/NR_TRANSPORT/pucch_rx.c

@@ -1303,7 +1303,7 @@ void nr_decode_pucch2(PHY_VARS_gNB *gNB,
     {
       c16_t rdmrs_gold[nb_re_dmrs] __attribute__((aligned(32)));
       for (int aa = 0; aa < Prx; aa++) {
-        mult_complex_vectors(rdmrs_ext[aa], pil_dmrs, rdmrs_gold, nb_re_dmrs, 0);
+        mult_cpx_vector(rdmrs_ext[aa], pil_dmrs, rdmrs_gold, nb_re_dmrs, 0);
         c16_t *ch_ls_ptr = ch_ls;
         c16_t *end = ch_ls_ptr + 128;
         for (int i = 0; i < nb_re_dmrs; i++)
@@ -1320,7 +1320,7 @@ void nr_decode_pucch2(PHY_VARS_gNB *gNB,
       int delay_idx = get_delay_idx(delay.est_delay, MAX_DELAY_COMP);
       c16_t *delay_table = frame_parms->delay_table128[delay_idx];
       for (int aa = 0; aa < Prx; aa++)
-        mult_complex_vectors(rp[aa][symb], delay_table, rp[aa][symb], nb_re_pucch, 8);
+        mult_cpx_vector(rp[aa][symb], delay_table, rp[aa][symb], nb_re_pucch, 8);
     }
     
     // extract again DMRS, and signal, after delay compensation
@@ -1350,7 +1350,7 @@ void nr_decode_pucch2(PHY_VARS_gNB *gNB,
 #endif
     c16_t rdmrs_gold[Prx][nb_re_dmrs] __attribute__((aligned(32)));
     for (int aa = 0; aa < Prx; aa++)
-      mult_complex_vectors(rdmrs_ext[aa], pil_dmrs, rdmrs_gold[aa], nb_re_dmrs, 0);
+      mult_cpx_vector(rdmrs_ext[aa], pil_dmrs, rdmrs_gold[aa], nb_re_dmrs, 0);
     for (int aa = 0; aa < Prx; aa++) {
       c16_t *pil_ptr = pil_dmrs;
       for (int group = 0; group < ngroup; group++) {

openair1/PHY/NR_UE_TRANSPORT/nr_dlsch_demodulation.c

@@ -506,7 +506,7 @@ static void nr_determin(int size,
                   nb_rb,
                   ((rtx & 1) == 1 ? -1 : 1) * ((ctx & 1) == 1 ? -1 : 1) * sign,
                   shift0);
-      mult_complex_vectors(a44[ctx][rtx], outtemp, rtx == 0 ? ad_bc : outtemp1, sizeofArray(outtemp1), shift0);
+      mult_cpx_vector(a44[ctx][rtx], outtemp, rtx == 0 ? ad_bc : outtemp1, sizeofArray(outtemp1), shift0);
 
       if (rtx != 0)
         nr_a_sum_b(ad_bc, outtemp1, nb_rb);
@@ -707,10 +707,11 @@ static void nr_dlsch_mmse(uint32_t rx_size_symbol,
                           uint32_t noise_var)
 {
   uint32_t nb_rb_0 = (length + 11) / 12;
-  c16_t determ_fin[12 * nb_rb_0] __attribute__((aligned(32)));
+  uint32_t nb_re_avx2 = ALNARS_32_8(12 * nb_rb_0);
+  c16_t determ_fin[nb_re_avx2] __attribute__((aligned(32)));
 
   ///Allocate H^*H matrix elements and sub elements
-  c16_t conjH_H_elements_data[n_rx][nl][nl][12 * nb_rb_0];
+  c16_t conjH_H_elements_data[n_rx][nl][nl][nb_re_avx2] __attribute__((aligned(32)));
   memset(conjH_H_elements_data, 0, sizeof(conjH_H_elements_data));
   c16_t *conjH_H_elements[n_rx][nl][nl];
   for (int aarx = 0; aarx < n_rx; aarx++)
@@ -750,7 +751,7 @@ static void nr_dlsch_mmse(uint32_t rx_size_symbol,
   //Compute the inverse and determinant of the H^*H matrix
   //Allocate the inverse matrix
   c16_t *inv_H_h_H[nl][nl];
-  c16_t inv_H_h_H_data[nl][nl][12 * nb_rb_0];
+  c16_t inv_H_h_H_data[nl][nl][nb_re_avx2] __attribute__((aligned(32)));
   memset(inv_H_h_H_data, 0, sizeof(inv_H_h_H_data));
   for (int rtx = 0; rtx < nl; rtx++)
     for (int ctx = 0; ctx < nl; ctx++)
@@ -766,9 +767,9 @@ static void nr_dlsch_mmse(uint32_t rx_size_symbol,
                     shift - (fp_flag == 1 ? 1 : 0)); // the out put is Q15
 
   // multiply Matrix inversion pf H_h_H by the rx signal vector
-  c16_t outtemp[12 * nb_rb_0] __attribute__((aligned(32)));
+  c16_t outtemp[nb_re_avx2] __attribute__((aligned(32)));
   //Allocate rxdataF for zforcing out
-  c16_t rxdataF_zforcing[nl][12 * nb_rb_0];
+  c16_t rxdataF_zforcing[nl][nb_re_avx2];
   memset(rxdataF_zforcing, 0, sizeof(rxdataF_zforcing));
 
   for (int rtx = 0; rtx < nl; rtx++) {//Output Layers row
@@ -777,11 +778,7 @@ static void nr_dlsch_mmse(uint32_t rx_size_symbol,
       // printf("Computing r_%d c_%d\n",rtx,ctx);
       // print_shorts(" H_h_H=",(int16_t*)&conjH_H_elements[ctx*nl+rtx][0][0]);
       // print_shorts(" Inv_H_h_H=",(int16_t*)&inv_H_h_H[ctx*nl+rtx][0]);
-      mult_complex_vectors(inv_H_h_H[ctx][rtx],
-                           rxdataF_comp[symbol][ctx][0],
-                           outtemp,
-                           sizeofArray(outtemp),
-                           shift - (fp_flag == 1 ? 1 : 0));
+      mult_cpx_vector(inv_H_h_H[ctx][rtx], rxdataF_comp[symbol][ctx][0], outtemp, length, shift - (fp_flag == 1 ? 1 : 0));
       nr_a_sum_b(rxdataF_zforcing[rtx], outtemp, nb_rb_0); // a = a + b
     }
 #ifdef DEBUG_DLSCH_DEMOD

openair1/PHY/NR_UE_TRANSPORT/nr_prach.c

@@ -311,7 +311,7 @@ int32_t generate_nr_prach(PHY_VARS_NR_UE *ue, uint8_t gNB_id, int frame, uint8_t
     for (int offset = 0, offset2 = 0; offset < N_ZC; offset++, offset2 += preamble_shift) {
       if (offset2 >= N_ZC)
         offset2 -= N_ZC;
-      const c16_t Xu_t = c16xmulConstShift(Xu[offset], amp, 15);
+      const c16_t Xu_t = c16mulRealShift(Xu[offset], amp, 15);
       const double w = 2 * M_PI * (double)offset2 / N_ZC;
       const c16_t ru = {.r = (int16_t)(floor(32767.0 * cos(w))), .i = (int16_t)(floor(32767.0 * sin(w)))};
       const c16_t p = c16mulShift(Xu_t, ru, 15);

openair1/PHY/NR_UE_TRANSPORT/nr_psbch_tx.c

@@ -243,7 +243,7 @@ void sl_generate_and_map_psbch(c16_t *txF,
 #endif
 
       if (m % 4 == 0) {
-        txF[offset] = c16xmulConstShift(psbch_dmrs[dmrs_index], scaling_factor, 15);
+        txF[offset] = c16mulRealShift(psbch_dmrs[dmrs_index], scaling_factor, 15);
 
 #ifdef SL_DEBUG
         printf("txF[%d]:%d,%d, psbch_dmrs[%d]:%d,%d ",
@@ -258,7 +258,7 @@ void sl_generate_and_map_psbch(c16_t *txF,
         dmrs_index++;
 
       } else {
-        txF[offset] = c16xmulConstShift(psbch_modsym[index], scaling_factor, 15);
+        txF[offset] = c16mulRealShift(psbch_modsym[index], scaling_factor, 15);
 
 #ifdef SL_DEBUG
         printf("txF[%d]:%d,%d, psbch_modsym[%d]:%d,%d\n",

openair1/PHY/TOOLS/tests/test_vector_op.cpp

@@ -23,8 +23,100 @@ void exit_function(const char *file, const char *function, const int line, const
 #include "common/utils/LOG/log.h"
 #include "openair1/PHY/TOOLS/phy_test_tools.hpp"
 
+static float error_pct(c16_t fixed, cd_t ref) {
+  float err_r = (float)fixed.r - (float)ref.r;
+  float err_i = (float)fixed.i - (float)ref.i;
+  float err_mag = sqrtf(err_r * err_r + err_i * err_i);
+  float ref_mag = sqrtf(ref.r * ref.r + ref.i * ref.i);
+  float denom = (ref_mag > 1.0f) ? ref_mag : 32767.0f; /* guard /0 */
+  return 100.0f * err_mag / denom;
+}
+
+static int c16mul_tol_check(const char *label,
+                     c16_t a, c16_t b, uint16_t q,
+                     float tol_pct)
+{
+  /* fixed-point result */
+  c16_t fp = c16mulShift(a, b, q);
+
+  /* double reference – scale Q<q> inputs to [-1, 1] range */
+  const double S = (double)((1 << q) - 1);
+  cd_t ad = {a.r / S, a.i / S};
+  cd_t bd = {b.r / S, b.i / S};
+  cd_t rd = cdMul(ad, bd);
+
+  /* scale reference back to Q<q> integer range for comparison */
+  cd_t ref = {rd.r * S, rd.i * S};
+
+  float pct = error_pct(fp, ref);
+
+  if (pct > tol_pct)
+    printf(
+        "%-30s  a=(%6d,%6d)  b=(%6d,%6d)"
+        "  got=(%6d,%6d)  ref=(%.1f,%.1f)  err=%.4f%%  %s\n",
+        label,
+        a.r,
+        a.i,
+        b.r,
+        b.i,
+        fp.r,
+        fp.i,
+        ref.r,
+        ref.i,
+        pct,
+        pct > tol_pct ? "FAIL" : "PASS");
+
+  return (pct > tol_pct);
+}
+
 int main()
 {
+  const float TOL = 1.0f;
+  const uint16_t q = 15;
+  int res = 0;
+
+  const uint16_t maxq = (1 << q) - 1;
+  const uint16_t one_sqrt2 = maxq * (1 / sqrt(2));
+  const uint16_t halfq = maxq >> 1;
+
+  /* unity × unity -> (1,0) */
+  res |= c16mul_tol_check("unity x unity", (c16_t){maxq, 0}, (c16_t){maxq, 0}, q, TOL);
+
+  /* unity × j -> (0,1) */
+  res |= c16mul_tol_check("unity x j", (c16_t){maxq, 0}, (c16_t){0, maxq}, q, TOL);
+
+  /* j × j -> (-1,0) */
+  res |= c16mul_tol_check("j x j", (c16_t){0, maxq}, (c16_t){0, maxq}, q, TOL);
+
+  /* conjugate pair -> purely real */
+  res |= c16mul_tol_check("conjugate pair", (c16_t){one_sqrt2, one_sqrt2}, (c16_t){one_sqrt2, -one_sqrt2}, q, TOL);
+
+  /* negative real × positive real */
+  res |= c16mul_tol_check("neg x pos real", (c16_t){-maxq, 0}, (c16_t){halfq, 0}, q, TOL);
+
+  /* small values (quantisation dominates) */
+  res |= c16mul_tol_check("small values", (c16_t){100, 50}, (c16_t){200, -75}, q, TOL);
+
+  /* full-scale worst case. The best we can do is 50% when saturating the result */
+  res |= c16mul_tol_check("full scale", (c16_t){maxq, maxq}, (c16_t){maxq, maxq}, q, 51);
+
+  /* zero inputs */
+  res |= c16mul_tol_check("zero x anything", (c16_t){0, 0}, (c16_t){12345, -6789}, q, TOL);
+
+  /* 45-degree phasor squared */
+  res |= c16mul_tol_check("45deg phasor squared", (c16_t){one_sqrt2, one_sqrt2}, (c16_t){one_sqrt2, one_sqrt2}, q, TOL);
+  res |= c16mul_tol_check("45deg phasor squared with noise",
+                          (c16_t){one_sqrt2 + 100, -one_sqrt2},
+                          (c16_t){one_sqrt2 + 100, -one_sqrt2},
+                          q,
+                          TOL);
+
+  /* mixed signs */
+  res |= c16mul_tol_check("mixed signs", (c16_t){-halfq, halfq}, (c16_t){halfq, -halfq}, q, TOL);
+
+  if (res > 0)
+    return 1;
+
   const int shift = 15; // it should always be 15 to keep int16 in same range
   for (int vector_size = 1237; vector_size < 1237 + 8; vector_size++) {
     auto input1 = generate_random_c16(vector_size);
@@ -40,36 +132,102 @@ int main()
     // so when we sum, it overflows
     // _mm256_madd_epi16() overflows also, as do regular addition instruction
     // so the result will be the same (the same wrong value)
-    
-    for(auto it = input1.begin(); it != input1.end(); it++ ) {
+
+    for (auto it = input1.begin(); it != input1.end(); it++) {
       if (it->r == -32768)
-	it->r=-32767;
+        it->r = -32767;
       if (it->i == -32768)
-	it->i=-32767;
+        it->i = -32767;
     }
-    for(auto it = input2.begin(); it != input2.end(); it++ ) {
+    for (auto it = input2.begin(); it != input2.end(); it++) {
       if (it->r == -32768)
-	it->r=-32767;
+        it->r = -32767;
       if (it->i == -32768)
-	it->i=-32767;
+        it->i = -32767;
     }
+
     AlignedVector512<c16_t> output;
     output.resize(vector_size);
-    mult_complex_vectors(input1.data(), input2.data(), output.data(), vector_size, shift);
-    for (int i = 0; i < vector_size; i++) {
-      c16_t res = c16mulShift(input1[i], input2[i], shift);
-      if (output[i].r != res.r || output[i].i != res.i) {
-        printf("Error at %d: (%d,%d) * (%d,%d) = (%d,%d) (should be (%d,%d))\n",
-               i,
-               input1[i].r,
-               input1[i].i,
-               input2[i].r,
-               input2[i].i,
-               output[i].r,
-               output[i].i,
-               res.r,
-               res.i);
-        return 1;
+
+    const int test_offset = 8;
+    // Test multadd_cpx_vector()
+    for (int off = 0; off < test_offset; off++) {
+      std::fill(output.begin(), output.end(), c16_t{0, 0});
+      multadd_cpx_vector(input1.data() + off, input2.data() + off, output.data() + off, vector_size - off, shift);
+      for (int i = off; i < vector_size - off; i++) {
+        const c16_t res = c16maddShift(input1[i], input2[i], (c16_t){0, 0}, shift);
+        if (output[i].r != res.r || output[i].i != res.i) {
+          printf("Error at %d, offset %d, size %d: (%d,%d) * (%d,%d) = (%d,%d) (should be (%d,%d))\n",
+                 i,
+                 off,
+                 vector_size,
+                 input1[i].r,
+                 input1[i].i,
+                 input2[i].r,
+                 input2[i].i,
+                 output[i].r,
+                 output[i].i,
+                 res.r,
+                 res.i);
+          return 1;
+        }
+      }
+    }
+
+    // Test mult_cpx_vector()
+    for (int off = 0; off < test_offset; off++) {
+      std::fill(output.begin(), output.end(), c16_t{0, 0});
+      mult_cpx_vector(input1.data() + off, input2.data() + off, output.data() + off, vector_size - off, shift);
+      for (int i = off; i < vector_size - off; i++) {
+        const c16_t res = c16mulShift(input1[i], input2[i], shift);
+        if (output[i].r != res.r || output[i].i != res.i) {
+          printf("Error at %d, offset %d, size %d: (%d,%d) * (%d,%d) = (%d,%d) (should be (%d,%d))\n",
+                 i,
+                 off,
+                 vector_size,
+                 input1[i].r,
+                 input1[i].i,
+                 input2[i].r,
+                 input2[i].i,
+                 output[i].r,
+                 output[i].i,
+                 res.r,
+                 res.i);
+          return 1;
+        }
+      }
+    }
+
+    // Test multadd_cpx_vector_cpx_scalar()
+    const c16_t alpha_cases[] = {
+        {16384, 0}, // real only (0.5 in Q15)
+        {0, 16384}, // imag only
+        {23170, 23170}, // ~1/sqrt(2) + j/sqrt(2)
+        {-16384, 8192}, // negative real
+        {32767, 32767}, // near max
+    };
+    // Buffer start offset used to test unaligned inputs
+    for (int off = 0; off < test_offset; off++) {
+      const c16_t alpha = alpha_cases[off % sizeofArray(alpha_cases)];
+      std::fill(output.begin(), output.end(), c16_t{0, 0});
+      multadd_cpx_vector_cpx_scalar(input1.data() + off, alpha, output.data() + off, vector_size - off, shift);
+      for (int i = off; i < vector_size - off; i++) {
+        const c16_t res = c16maddShift(input1[i], alpha, (c16_t){0, 0}, shift);
+        if (output[i].r != res.r || output[i].i != res.i) {
+          printf("Error at %d, offset %d, size %d: (%d,%d) * (%d,%d) = (%d,%d) (should be (%d,%d))\n",
+                 i,
+                 off,
+                 vector_size,
+                 input1[i].r,
+                 input1[i].i,
+                 alpha.r,
+                 alpha.i,
+                 output[i].r,
+                 output[i].i,
+                 res.r,
+                 res.i);
+          return 1;
+        }
       }
     }
   }

openair1/PHY/TOOLS/tools_defs.h

@@ -159,38 +159,28 @@ extern "C" {
   }
 
   __attribute__((always_inline)) inline c16_t c16mulShift(const c16_t a, const c16_t b, const int Shift) {
-    return (c16_t) {
-      .r = (int16_t)((a.r * b.r - a.i * b.i) >> Shift),
-      .i = (int16_t)((a.r * b.i + a.i * b.r) >> Shift)
-    };
+    return (c16_t){.r = SATURATE_S16((a.r * b.r - a.i * b.i) >> Shift), .i = SATURATE_S16((a.r * b.i + a.i * b.r) >> Shift)};
   }
 
   __attribute__((always_inline)) inline c16_t c16mulRealShift(const c16_t a, const int32_t b, const int Shift)
   {
-    return (c16_t){.r = (int16_t)((a.r * b) >> Shift), .i = (int16_t)((a.i * b) >> Shift)};
+    return (c16_t){.r = SATURATE_S16((a.r * b) >> Shift), .i = SATURATE_S16((a.i * b) >> Shift)};
   }
 
   __attribute__((always_inline)) inline c16_t c16MulConjShift(const c16_t a, const c16_t b, const int Shift)
   {
-    return (c16_t) {
-      .r = (int16_t)((a.r * b.r + a.i * b.i) >> Shift),
-      .i = (int16_t)((a.r * b.i - a.i * b.r) >> Shift)
-    };
+    return (c16_t){.r = SATURATE_S16((a.r * b.r + a.i * b.i) >> Shift), .i = SATURATE_S16((a.r * b.i - a.i * b.r) >> Shift)};
   }
 
   __attribute__((always_inline)) inline c16_t c16maddShift(const c16_t a, const c16_t b, c16_t c, const int Shift) {
-    return (c16_t) {
-      .r = (int16_t)(((a.r * b.r - a.i * b.i ) >> Shift) + c.r),
-      .i = (int16_t)(((a.r * b.i + a.i * b.r ) >> Shift) + c.i)
-    };
+    return (c16_t){.r = SATURATE_S16(((a.r * b.r - a.i * b.i) >> Shift) + c.r),
+                   .i = SATURATE_S16(((a.r * b.i + a.i * b.r) >> Shift) + c.i)};
   }
 
   __attribute__((always_inline)) inline c16_t c16maddConjShift(const c16_t a, const c16_t b, c16_t c, const int Shift)
   {
-    return (c16_t) {
-      .r = (int16_t)(((a.r * b.r + a.i * b.i ) >> Shift) + c.r),
-      .i = (int16_t)(((a.r * b.i - a.i * b.r ) >> Shift) + c.i)
-    };
+    return (c16_t){.r = SATURATE_S16(((a.r * b.r + a.i * b.i) >> Shift) + c.r),
+                   .i = SATURATE_S16(((a.r * b.i - a.i * b.r) >> Shift) + c.i)};
   }
 
   __attribute__((always_inline)) inline c32_t c32x16mulShift(const c16_t a, const c16_t b, const int Shift) {
@@ -200,11 +190,6 @@ extern "C" {
     };
   }
 
-  __attribute__((always_inline)) inline c16_t c16xmulConstShift(const c16_t a, const int b, const int Shift)
-  {
-    return (c16_t){.r = (int16_t)((a.r * b) >> Shift), .i = (int16_t)((a.i * b) >> Shift)};
-  }
-
   __attribute__((always_inline)) inline c32_t c32x16maddShift(const c16_t a, const c16_t b, const c32_t c, const int Shift) {
     return (c32_t) {
       .r = ((a.r * b.r - a.i * b.i) >> Shift) + c.r,
@@ -312,74 +297,6 @@ static __attribute__((always_inline)) inline void multadd_real_four_symbols_vect
     simde_mm_storeu_si128((simd_q15_t *)y, y_128);
 }
 
-// Multiply two vectors of complex int16 and take the most significant bits (shift by 15 in normal case)
-// works only with little endian storage (for big endian, modify the srai/ssli at the end)
-static __attribute__((always_inline)) inline void mult_complex_vectors(const c16_t *in1,
-                                                                       const c16_t *in2,
-                                                                       c16_t *out,
-                                                                       const int size,
-                                                                       const int shift)
-{
-  const simde__m256i complex_shuffle256 = simde_mm256_set_epi8(29,
-                                                               28,
-                                                               31,
-                                                               30,
-                                                               25,
-                                                               24,
-                                                               27,
-                                                               26,
-                                                               21,
-                                                               20,
-                                                               23,
-                                                               22,
-                                                               17,
-                                                               16,
-                                                               19,
-                                                               18,
-                                                               13,
-                                                               12,
-                                                               15,
-                                                               14,
-                                                               9,
-                                                               8,
-                                                               11,
-                                                               10,
-                                                               5,
-                                                               4,
-                                                               7,
-                                                               6,
-                                                               1,
-                                                               0,
-                                                               3,
-                                                               2);
-  const simde__m256i conj256 = simde_mm256_set_epi16(-1, 1, -1, 1, -1, 1, -1, 1, -1, 1, -1, 1, -1, 1, -1, 1);
-  int i;
-  // do 8 multiplications at a time
-  for (i = 0; i < size - 7; i += 8) {
-    const simde__m256i i1 = simde_mm256_loadu_si256((simde__m256i *)(in1 + i));
-    const simde__m256i i2 = simde_mm256_loadu_si256((simde__m256i *)(in2 + i));
-    const simde__m256i i2swap = simde_mm256_shuffle_epi8(i2, complex_shuffle256);
-    const simde__m256i i2conj = simde_mm256_sign_epi16(i2, conj256);
-    const simde__m256i re = simde_mm256_madd_epi16(i1, i2conj);
-    const simde__m256i im = simde_mm256_madd_epi16(i1, i2swap);
-    simde_mm256_storeu_si256(
-        (simde__m256i *)(out + i),
-        simde_mm256_blend_epi16(simde_mm256_srai_epi32(re, shift), simde_mm256_slli_epi32(im, 16 - shift), 0xAA));
-  }
-  if (size - i > 4) {
-    const simde__m128i i1 = simde_mm_loadu_si128((simde__m128i *)(in1 + i));
-    const simde__m128i i2 = simde_mm_loadu_si128((simde__m128i *)(in2 + i));
-    const simde__m128i i2swap = simde_mm_shuffle_epi8(i2, *(simde__m128i *)&complex_shuffle256);
-    const simde__m128i i2conj = simde_mm_sign_epi16(i2, *(simde__m128i *)&conj256);
-    const simde__m128i re = simde_mm_madd_epi16(i1, i2conj);
-    const simde__m128i im = simde_mm_madd_epi16(i1, i2swap);
-    simde_mm_storeu_si128((simde__m128i *)(out + i),
-                          simde_mm_blend_epi16(simde_mm_srai_epi32(re, shift), simde_mm_slli_epi32(im, 16 - shift), 0xAA));
-    i += 4;
-  }
-  for (; i < size; i++)
-    out[i] = c16mulShift(in1[i], in2[i], shift);
-}
 /*!\fn void multadd_complex_vector_real_scalar(int16_t *x,int16_t alpha,int16_t *y,uint8_t zero_flag,uint32_t N)
 This function performs componentwise multiplication and accumulation of a real scalar and a complex vector.
 @param x Vector input (Q1.15) in the format |Re0 Im0|Re1 Im 1| ...
@@ -451,8 +368,154 @@ static inline void mult_cpx_conj_vector(const c16_t *x1, const c16_t *x2, c16_t
     y_128[i] = oai_mm_cpx_mult_conj(x1_128[i], x2_128[i], output_shift);
 }
 
+static inline void mult_cpx_vector_scalar(const c16_t *x1, const c16_t *x2, c16_t *y, const uint32_t N, const int output_shift)
+{
+  for (uint_fast32_t i = 0; i < N; i++)
+    y[i] = c16mulShift(x1[i], x2[i], output_shift);
+}
+
+static inline void multadd_cpx_vector_scalar(const c16_t *x1, const c16_t *x2, c16_t *y, const uint32_t N, const int output_shift)
+{
+  for (uint_fast32_t i = 0; i < N; i++)
+    y[i] = c16maddShift(x1[i], x2[i], y[i], output_shift);
+}
+
+static inline void mult_cpx_vector_128(const c16_t *x1, // Q15
+                                       const c16_t *x2, // Q13
+                                       c16_t *y,
+                                       const uint32_t N,
+                                       const int output_shift)
+{
+  // Handle unaligned memory
+  const size_t m128_sz = sizeof(simde__m128i);
+  const uintptr_t m = PTR_ALIGN_OFFSET_ELEMS(x1, m128_sz);
+  // All arrays must have same unaligned offset
+  DevAssert(PTR_ALIGN_OFFSET_ELEMS(x2, m128_sz) == m && PTR_ALIGN_OFFSET_ELEMS(y, m128_sz) == m);
+  if (m)
+    mult_cpx_vector_scalar(x1, x2, y, m, output_shift);
+
+  const simde__m128i *x1_128 = (simde__m128i *)(x1 + m);
+  const simde__m128i *x2_128 = (simde__m128i *)(x2 + m);
+  simde__m128i *y_128 = (simde__m128i *)(y + m);
+  // SSE compute 4 cpx multiply for each loop
+  uint_fast32_t i = 0;
+  for (; i < ((N - m) / 4); i++) {
+    y_128[i] = oai_mm_cpx_mult(x1_128[i], x2_128[i], output_shift);
+  }
+  // Remaining elements
+  i = i * 4 + m;
+  if (i < N) {
+    mult_cpx_vector_scalar(x1 + i, x2 + i, y + i, N - i, output_shift);
+  }
+}
+
+static inline void multadd_cpx_vector_128(const c16_t *x1, // Q15
+                                          const c16_t *x2, // Q13
+                                          c16_t *y,
+                                          const uint32_t N,
+                                          const int output_shift)
+{
+  // Handle unaligned memory
+  const size_t m128_sz = sizeof(simde__m128i);
+  const uintptr_t m = PTR_ALIGN_OFFSET_ELEMS(x1, m128_sz);
+  // All arrays must have same unaligned offset
+  DevAssert(PTR_ALIGN_OFFSET_ELEMS(x2, m128_sz) == m && PTR_ALIGN_OFFSET_ELEMS(y, m128_sz) == m);
+  if (m)
+    multadd_cpx_vector_scalar(x1, x2, y, m, output_shift);
+
+  const simde__m128i *x1_128 = (simde__m128i *)(x1 + m);
+  const simde__m128i *x2_128 = (simde__m128i *)(x2 + m);
+  simde__m128i *y_128 = (simde__m128i *)(y + m);
+  // SSE compute 4 cpx multiply for each loop
+  uint_fast32_t i = 0;
+  for (; i < ((N - m) / 4); i++) {
+    simde__m128i result = oai_mm_cpx_mult(x1_128[i], x2_128[i], output_shift);
+    y_128[i] = simde_mm_adds_epi16(y_128[i], result);
+  }
+  // Remaining elements
+  i = i * 4 + m;
+  if (i < N)
+    multadd_cpx_vector_scalar(x1 + i, x2 + i, y + i, N - i, output_shift);
+}
+
+static inline void mult_cpx_vector_256(const c16_t *x1, // Q15
+                                       const c16_t *x2, // Q13
+                                       c16_t *y,
+                                       const uint32_t N,
+                                       const int output_shift)
+{
+  // Handle unaligned memory
+  const size_t m256_sz = sizeof(simde__m256i);
+  const uintptr_t m = PTR_ALIGN_OFFSET_ELEMS(x1, m256_sz);
+  // All arrays must have same unaligned offset
+  DevAssert(PTR_ALIGN_OFFSET_ELEMS(x2, m256_sz) == m && PTR_ALIGN_OFFSET_ELEMS(y, m256_sz) == m);
+  if (m)
+    mult_cpx_vector_128(x1, x2, y, m, output_shift);
+
+  const simde__m256i *x1_256 = (simde__m256i *)(x1 + m);
+  const simde__m256i *x2_256 = (simde__m256i *)(x2 + m);
+  simde__m256i *y_256 = (simde__m256i *)(y + m);
+  // AVX2 compute 8 cpx multiply for each loop
+  uint_fast32_t i = 0;
+  for (; i < ((N - m) / 8); i++) {
+    y_256[i] = oai_mm256_cpx_mult(x1_256[i], x2_256[i], output_shift);
+  }
+  // Remaining elements
+  i = i * 8 + m;
+  if (i < N)
+    mult_cpx_vector_128(x1 + i, x2 + i, y + i, N - i, output_shift);
+}
+
+static inline void multadd_cpx_vector_256(const c16_t *x1, // Q15
+                                          const c16_t *x2, // Q13
+                                          c16_t *y,
+                                          const uint32_t N,
+                                          const int output_shift)
+{
+  // Handle unaligned memory
+  const size_t m256_sz = sizeof(simde__m256i);
+  const uintptr_t m = PTR_ALIGN_OFFSET_ELEMS(x1, m256_sz);
+  // All arrays must have same unaligned offset
+  DevAssert(PTR_ALIGN_OFFSET_ELEMS(x2, m256_sz) == m && PTR_ALIGN_OFFSET_ELEMS(y, m256_sz) == m);
+  if (m)
+    multadd_cpx_vector_128(x1, x2, y, m, output_shift);
+
+  const simde__m256i *x1_256 = (simde__m256i *)(x1 + m);
+  const simde__m256i *x2_256 = (simde__m256i *)(x2 + m);
+  simde__m256i *y_256 = (simde__m256i *)(y + m);
+  // AVX2 compute 8 cpx multiply for each loop
+  uint_fast32_t i = 0;
+  for (; i < ((N - m) / 8); i++) {
+    simde__m256i result = oai_mm256_cpx_mult(x1_256[i], x2_256[i], output_shift);
+    y_256[i] = simde_mm256_adds_epi16(y_256[i], result);
+  }
+  // Remaining elements
+  i = i * 8 + m;
+  if (i < N)
+    multadd_cpx_vector_128(x1 + i, x2 + i, y + i, N - i, output_shift);
+}
+
+#define CPX_VECTOR_DISPATCH(x1, x2, y, N, output_shift, fn256, fn128, fn_scalar) \
+  do {                                                                           \
+    const size_t m256_sz = sizeof(simde__m256i);                                 \
+    const uintptr_t mx1 = PTR_ALIGN_OFFSET_ELEMS(x1, m256_sz);                   \
+    const uintptr_t mx2 = PTR_ALIGN_OFFSET_ELEMS(x2, m256_sz);                   \
+    const uintptr_t my = PTR_ALIGN_OFFSET_ELEMS(y, m256_sz);                     \
+    if ((mx1 == mx2) && (mx2 == my)) {                                           \
+      fn256(x1, x2, y, N, output_shift);                                         \
+    } else if (((mx1 ^ mx2 ^ my) == 4) || ((mx1 ^ mx2 ^ my) == 0)) {             \
+      fn128(x1, x2, y, N, output_shift);                                         \
+    } else {                                                                     \
+      LOG_W(PHY,                                                                 \
+            "Input arrays have different memory alignments. "                    \
+            "Consider proper alignment for better performance\n");               \
+      fn_scalar(x1, x2, y, N, output_shift);                                     \
+    }                                                                            \
+  } while (0)
+
 /*!
   Element-wise multiplication and accumulation of two complex vectors x1 and x2.
+  The function checks input memory for alignment and calls the right SIMD function.
   @param x1       - input 1    in the format  |Re0 Im0 Re1 Im1|,......,|Re(N-2)  Im(N-2) Re(N-1) Im(N-1)|
               We assume x1 with a dinamic of 15 bit maximum
   @param x2       - input 2    in the format  |Re0 Im0 Re1 Im1|,......,|Re(N-2)  Im(N-2) Re(N-1) Im(N-1)|
@@ -468,27 +531,116 @@ static inline void mult_cpx_vector(const c16_t *x1, // Q15
                                    const uint32_t N,
                                    const int output_shift)
 {
-  const simde__m128i *x1_128 = (simde__m128i *)x1;
-  const simde__m128i *x2_128 = (simde__m128i *)x2;
-  simde__m128i *y_128 = (simde__m128i *)y;
-
-  // right shift by 13 while p_a * x0 and 15 while
-  //  SSE compute 4 cpx multiply for each loop
-  for (uint32_t i = 0; i < (N >> 2); i++) {
-    y_128[i] = oai_mm_cpx_mult(x1_128[i], x2_128[i], output_shift);
-  }
+  CPX_VECTOR_DISPATCH(x1, x2, y, N, output_shift, mult_cpx_vector_256, mult_cpx_vector_128, mult_cpx_vector_scalar);
 }
 
 static inline void multadd_cpx_vector(const c16_t *x1, const c16_t *x2, c16_t *y, const uint32_t N, const int output_shift)
 {
-  const simde__m128i *x1_128 = (simde__m128i *)x1;
-  const simde__m128i *x2_128 = (simde__m128i *)x2;
-  simde__m128i *y_128 = (simde__m128i *)y;
+  CPX_VECTOR_DISPATCH(x1, x2, y, N, output_shift, multadd_cpx_vector_256, multadd_cpx_vector_128, multadd_cpx_vector_scalar);
+}
+
+static inline void multadd_cpx_vector_cpx_scalar_scalar(const c16_t *x1,
+                                                        const c16_t alpha,
+                                                        c16_t *y,
+                                                        const uint32_t N,
+                                                        const int output_shift)
+{
+  for (uint_fast32_t i = 0; i < N; i++)
+    y[i] = c16maddShift(x1[i], alpha, y[i], output_shift);
+}
+
+static inline void multadd_cpx_vector_cpx_scalar_128(const c16_t *x1,
+                                                     const c16_t alpha,
+                                                     c16_t *y,
+                                                     const uint32_t N,
+                                                     const int output_shift)
+{
+  // Handle unaligned memory
+  const size_t m128_sz = sizeof(simde__m128i);
+  const uintptr_t m = PTR_ALIGN_OFFSET_ELEMS(x1, m128_sz);
+  // All arrays must have same unaligned offset
+  DevAssert(PTR_ALIGN_OFFSET_ELEMS(y, m128_sz) == m);
+  if (m)
+    multadd_cpx_vector_cpx_scalar_scalar(x1, alpha, y, m, output_shift);
+
+  const simde__m128i *x1_128 = (simde__m128i *)(x1 + m);
+  simde__m128i alpha_128 = simde_mm_set1_epi32(*(int32_t *)&alpha);
+  simde__m128i *y_128 = (simde__m128i *)(y + m);
   // SSE compute 4 cpx multiply for each loop
-  for (uint32_t i = 0; i < (N >> 2); i++) {
-    simde__m128i result = oai_mm_cpx_mult(x1_128[i], x2_128[i], output_shift);
+  uint_fast32_t i = 0;
+  for (; i < ((N - m) / 4); i++) {
+    simde__m128i result = oai_mm_cpx_mult(x1_128[i], alpha_128, output_shift);
     y_128[i] = simde_mm_adds_epi16(y_128[i], result);
   }
+  // Remaining elements
+  i = i * 4 + m;
+  if (i < N) {
+    multadd_cpx_vector_cpx_scalar_scalar(x1 + i, alpha, y + i, N - i, output_shift);
+  }
+}
+
+static inline void multadd_cpx_vector_cpx_scalar_256(const c16_t *x1,
+                                                     const c16_t alpha,
+                                                     c16_t *y,
+                                                     const uint32_t N,
+                                                     const int output_shift)
+{
+  // Handle unaligned memory
+  const size_t m256_sz = sizeof(simde__m256i);
+  const uintptr_t m = PTR_ALIGN_OFFSET_ELEMS(x1, m256_sz);
+  // All arrays must have same unaligned offset
+  DevAssert(PTR_ALIGN_OFFSET_ELEMS(y, m256_sz) == m);
+  if (m) {
+    multadd_cpx_vector_cpx_scalar_128(x1, alpha, y, m, output_shift);
+  }
+
+  const simde__m256i *x1_256 = (simde__m256i *)(x1 + m);
+  simde__m256i *y_256 = (simde__m256i *)(y + m);
+  // AVX2 compute 8 cpx multiply for each loop
+  uint_fast32_t i = 0;
+  simde__m256i alpha_256 = simde_mm256_set1_epi32(*(int32_t *)&alpha);
+  for (; i < ((N - m) / 8); i++) {
+    simde__m256i result = oai_mm256_cpx_mult(x1_256[i], alpha_256, output_shift);
+    y_256[i] = simde_mm256_adds_epi16(y_256[i], result);
+  }
+  // Remaining elements
+  i = i * 8 + m;
+  if (i < N) {
+    multadd_cpx_vector_cpx_scalar_128(x1 + i, alpha, y + i, N - i, output_shift);
+  }
+}
+
+#define CPX_VECTOR_SCALAR_DISPATCH(x1, alpha, y, N, output_shift, fn256, fn128, fn_scalar) \
+  do {                                                                                     \
+    const size_t m256_sz = sizeof(simde__m256i);                                           \
+    const uintptr_t mx1 = PTR_ALIGN_OFFSET_ELEMS(x1, m256_sz);                             \
+    const uintptr_t my = PTR_ALIGN_OFFSET_ELEMS(y, m256_sz);                               \
+    if (mx1 == my) {                                                                       \
+      fn256(x1, alpha, y, N, output_shift);                                                \
+    } else if (((mx1 ^ my) == 4) || ((mx1 ^ my) == 0)) {                                   \
+      fn128(x1, alpha, y, N, output_shift);                                                \
+    } else {                                                                               \
+      LOG_W(PHY,                                                                           \
+            "Input arrays have different memory alignments. "                              \
+            "Consider proper alignment for better performance\n");                         \
+      fn_scalar(x1, alpha, y, N, output_shift);                                            \
+    }                                                                                      \
+  } while (0)
+
+static inline void multadd_cpx_vector_cpx_scalar(const c16_t *x1,
+                                                 const c16_t alpha,
+                                                 c16_t *y,
+                                                 const uint32_t N,
+                                                 const int output_shift)
+{
+  CPX_VECTOR_SCALAR_DISPATCH(x1,
+                             alpha,
+                             y,
+                             N,
+                             output_shift,
+                             multadd_cpx_vector_cpx_scalar_256,
+                             multadd_cpx_vector_cpx_scalar_128,
+                             multadd_cpx_vector_cpx_scalar_scalar);
 }
 
 static const int16_t ones_epi16[16] __attribute__((aligned(32))) = {1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1};

openair1/PHY/defs_nr_common.h

@@ -178,7 +178,7 @@ typedef struct NR_DL_FRAME_PARMS_s {
   c16_t symbol_rotation[3][224];
   /// sequence used to compensate the phase rotation due to timeshifted OFDM symbols
   /// First dimenstion is for different CP lengths
-  c16_t timeshift_symbol_rotation[4096*2] __attribute__ ((aligned (16)));
+  c16_t timeshift_symbol_rotation[4096 * 2] __attribute__((aligned(32)));
   /// Table used to apply the delay compensation in DL/UL
   c16_t delay_table[2 * MAX_DELAY_COMP + 1][NR_MAX_OFDM_SYMBOL_SIZE];
   /// Table used to apply the delay compensation in PUCCH2

openair1/PHY/nr_phy_common/src/nr_phy_common.c

@@ -446,13 +446,13 @@ void nr_fo_compensation(double fo_Hz, int samples_per_ms, int sample_offset, con
   }
   const c16_t rot_vec = get_sin_cos(CHUNK * phase_inc);
   while (size > CHUNK) {
-    mult_complex_vectors(rxdata_in, rot, rxdata_out, CHUNK, 14);
+    mult_cpx_vector(rxdata_in, rot, rxdata_out, CHUNK, 14);
     rotate_cpx_vector(rot, &rot_vec, rot, CHUNK, 14);
     rxdata_in += CHUNK;
     rxdata_out += CHUNK;
     size -= CHUNK;
   }
-  mult_complex_vectors(rxdata_in, rot, rxdata_out, size, 14);
+  mult_cpx_vector(rxdata_in, rot, rxdata_out, size, 14);
 #else
   // This code path computes the complex rotation values for the complete OFDM symbol using get_sin_cos().
   // This is more accurate, but also slower than the code path above.
@@ -461,7 +461,7 @@ void nr_fo_compensation(double fo_Hz, int samples_per_ms, int sample_offset, con
     rot[i] = get_sin_cos(phase);
     phase += phase_inc;
   }
-  mult_complex_vectors(rxdata_in, rot, rxdata_out, size, 14);
+  mult_cpx_vector(rxdata_in, rot, rxdata_out, size, 14);
 #endif
 }