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,10 +2822,12 @@ 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) {
+      cn_avg += cn;
       count++;
     }
+  }
 
   if (count) {
     cn_avg /= 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
-  if (!isnormal(xmin) || xmin < 1e-9) {
-    xmin = 1e-9;
-  }
-  if (!isnormal(xmax) || xmax > 1e+9) {
-    xmax = 1e+9;
+  // 4. Make sure NAN or INF are not propagated
+  if (isnan(xmin) || isinf(xmin) || isnan(xmax) || isinf(xmax)) {
+    return SRSLTE_ERROR;
   }
 
-  // 5. κ = sqrt(λ_max / λ_min)
-  return 10.0f * log10f(xmax / xmin);
+  // 5. Bound xmin and xmax
+  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
@@ -447,7 +451,6 @@ 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;
-
   float mod = __real__ x * __real__ x + __imag__ x * __imag__ x;
 
   if (isnormal(mod)) {