SRSUE: fix condition number calculation

lib/include/srslte/phy/utils/mat.h

@@ -65,7 +65,7 @@ SRSLTE_API void srslte_mat_2x2_mmse_csi_gen(cf_t   y0,
                                             float  noise_estimate,
                                             float  norm);
 
-SRSLTE_API float srslte_mat_2x2_cn(cf_t h00, cf_t h01, cf_t h10, cf_t h11);
+SRSLTE_API int srslte_mat_2x2_cn(cf_t h00, cf_t h01, cf_t h10, cf_t h11, float* cn);
 
 #ifdef LV_HAVE_SSE
 

lib/src/phy/mimo/precoding.c

@@ -2822,9 +2822,11 @@ float srslte_precoding_2x2_cn_gen(cf_t* h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], u
     cf_t h10 = h[0][1][i];
     cf_t h11 = h[1][1][i];
 
-    cn_avg += srslte_mat_2x2_cn(h00, h01, h10, h11);
+    float cn = 0.0f;
-
+    if (srslte_mat_2x2_cn(h00, h01, h10, h11, &cn) == SRSLTE_SUCCESS) {
-    count++;
+      cn_avg += cn;
+      count++;
+    }
   }
 
   if (count) {

lib/src/phy/utils/mat.c

@@ -124,7 +124,7 @@ void srslte_mat_2x2_mmse_gen(cf_t  y0,
   srslte_mat_2x2_mmse_csi_gen(y0, y1, h00, h01, h10, h11, x0, x1, &csi0, &csi1, noise_estimate, norm);
 }
 
-inline float srslte_mat_2x2_cn(cf_t h00, cf_t h01, cf_t h10, cf_t h11)
+int srslte_mat_2x2_cn(cf_t h00, cf_t h01, cf_t h10, cf_t h11, float* cn)
 {
   // 1. A = H * H' (A = A')
   float a00 =
@@ -142,16 +142,20 @@ inline float srslte_mat_2x2_cn(cf_t h00, cf_t h01, cf_t h10, cf_t h11)
   float xmax = b + sqr;
   float xmin = b - sqr;
 
-  // 4. Bound xmin and xmax
+  // 4. Make sure NAN or INF are not propagated
-  if (!isnormal(xmin) || xmin < 1e-9) {
+  if (isnan(xmin) || isinf(xmin) || isnan(xmax) || isinf(xmax)) {
-    xmin = 1e-9;
+    return SRSLTE_ERROR;
-  }
-  if (!isnormal(xmax) || xmax > 1e+9) {
-    xmax = 1e+9;
   }
 
-  // 5. κ = sqrt(λ_max / λ_min)
+  // 5. Bound xmin and xmax
-  return 10.0f * log10f(xmax / xmin);
+  xmin = SRSLTE_MAX(xmin, 1e-9f);
+  xmax = SRSLTE_MIN(xmax, 1e+9f);
+
+  // 6. κ = sqrt(λ_max / λ_min)
+  if (cn != NULL) {
+    *cn = 10.0f * log10f(xmax / xmin);
+  }
+  return SRSLTE_SUCCESS;
 }
 
 #ifdef LV_HAVE_SSE
@@ -288,8 +292,8 @@ inline void srslte_mat_2x2_zf_avx(__m256  y0,
   *x0 = _MM256_PROD_PS(_MM256_PROD_SUB_PS(h11, y0, _MM256_PROD_PS(h01, y1)), detrec);
   *x1 = _MM256_PROD_PS(_MM256_PROD_SUB_PS(h00, y1, _MM256_PROD_PS(h10, y0)), detrec);
 #else
-  *x0        = _MM256_PROD_PS(_mm256_sub_ps(_MM256_PROD_PS(h11, y0), _MM256_PROD_PS(h01, y1)), detrec);
+  *x0                = _MM256_PROD_PS(_mm256_sub_ps(_MM256_PROD_PS(h11, y0), _MM256_PROD_PS(h01, y1)), detrec);
-  *x1        = _MM256_PROD_PS(_mm256_sub_ps(_MM256_PROD_PS(h00, y1), _MM256_PROD_PS(h10, y0)), detrec);
+  *x1                = _MM256_PROD_PS(_mm256_sub_ps(_MM256_PROD_PS(h00, y1), _MM256_PROD_PS(h10, y0)), detrec);
 #endif /* LV_HAVE_FMA */
 }
 
@@ -353,8 +357,8 @@ inline void srslte_mat_2x2_mmse_avx(__m256  y0,
   *x0 = _MM256_PROD_PS(_MM256_PROD_ADD_PS(y0, w00, _MM256_PROD_PS(y1, w01)), _norm);
   *x1 = _MM256_PROD_PS(_MM256_PROD_ADD_PS(y0, w10, _MM256_PROD_PS(y1, w11)), _norm);
 #else
-  *x0        = _MM256_PROD_PS(_mm256_add_ps(_MM256_PROD_PS(y0, w00), _MM256_PROD_PS(y1, w01)), _norm);
+  *x0                = _MM256_PROD_PS(_mm256_add_ps(_MM256_PROD_PS(y0, w00), _MM256_PROD_PS(y1, w01)), _norm);
-  *x1        = _MM256_PROD_PS(_mm256_add_ps(_MM256_PROD_PS(y0, w10), _MM256_PROD_PS(y1, w11)), _norm);
+  *x1                = _MM256_PROD_PS(_mm256_add_ps(_MM256_PROD_PS(y0, w10), _MM256_PROD_PS(y1, w11)), _norm);
 #endif /* LV_HAVE_FMA */
 }
 
@@ -446,9 +450,8 @@ static inline void srslte_vec_sub_ccc_simd_inline(const cf_t* x, const cf_t* y,
 
 static inline cf_t reciprocal(cf_t x)
 {
-  cf_t y = 0.0f;
+  cf_t  y   = 0.0f;
-
+  float mod = __real__ x * __real__ x + __imag__ x * __imag__ x;
-  float mod  = __real__ x * __real__ x + __imag__ x * __imag__ x;
 
   if (isnormal(mod)) {
     __real__ y = (__real__ x) / mod;