custom allocator (VOLK) / filter update generalization

lib/adaptive_dfe_impl.cc

@@ -33,9 +33,8 @@
 
 #include "adaptive_dfe_impl.h"
 
-#define VOLK_SAFE_DELETE(x) \
+#include "lms.hpp"
-  volk_free(x);             \
+#include "rls.hpp"
-  x = nullptr
 
 namespace gr {
 namespace digitalhf {
@@ -71,9 +70,9 @@ adaptive_dfe_impl::adaptive_dfe_impl(int sps, // samples per symbol
   , _mu(mu)
   , _alpha(alpha)
   , _use_symbol_taps(true)
-  , _taps_samples(nullptr)
+  , _taps_samples()
-  , _taps_symbols(nullptr)
+  , _taps_symbols()
-  , _hist_symbols(nullptr)
+  , _hist_symbols()
   , _hist_symbol_index(0)
   , _constellations()
   , _npwr()
@@ -89,11 +88,12 @@ adaptive_dfe_impl::adaptive_dfe_impl(int sps, // samples per symbol
   , _msg_ports{{"soft_dec",   pmt::intern("soft_dec")},
                {"frame_info", pmt::intern("frame_info")}}
   , _msg_metadata(pmt::make_dict())
-  , _state(WAIT_FOR_PREAMBLE)
   , _num_samples_since_filter_update(0)
   , _rotated_samples()
   , _rotator()
   , _control_loop(2*M_PI/100, 5e-2, -5e-2)
+  , _state(WAIT_FOR_PREAMBLE)
+  , _filter_update()
 {
   GR_LOG_DECLARE_LOGPTR(d_logger);
   GR_LOG_ASSIGN_LOGPTR(d_logger, "adaptive_dfe");
@@ -115,9 +115,6 @@ adaptive_dfe_impl::adaptive_dfe_impl(int sps, // samples per symbol
 adaptive_dfe_impl::~adaptive_dfe_impl()
 {
   _msg_metadata = pmt::PMT_NIL;
-  VOLK_SAFE_DELETE(_taps_samples);
-  VOLK_SAFE_DELETE(_taps_symbols);
-  VOLK_SAFE_DELETE(_hist_symbols);
 }
 
 void
@@ -162,6 +159,7 @@ adaptive_dfe_impl::general_work(int noutput_items,
         _state = WAIT_FOR_FRAME_INFO;
         GR_LOG_DEBUG(d_logger, "got preamble tag > wait for frame info");
       }
+      _filter_update->reset();
       break;
     } // WAIT_FOR_PREAMBLE
     case WAIT_FOR_FRAME_INFO: {
@@ -208,7 +206,7 @@ adaptive_dfe_impl::general_work(int noutput_items,
         assert(i+_nF < nin && i-1-_nB >= 0);
         out_symb[nout] = filter(&_rotated_samples.front() + i - _nB,
                                 &_rotated_samples.front() + i + _nF+1);
-        std::memcpy(&out_taps[(_nB+_nF+1)*nout], _taps_samples, (_nB+_nF+1)*sizeof(gr_complex));
+        std::memcpy(&out_taps[(_nB+_nF+1)*nout], &_taps_samples.front(), (_nB+_nF+1)*sizeof(gr_complex));
         ++nout;
       } // next sample
       consume(0, ninput_processed);
@@ -221,23 +219,23 @@ adaptive_dfe_impl::general_work(int noutput_items,
 bool adaptive_dfe_impl::start()
 {
   gr::thread::scoped_lock lock(d_setlock);
-  _taps_samples      = (gr_complex*)(volk_malloc((_nB+_nF+1)*sizeof(gr_complex), volk_get_alignment()));
+  _taps_samples.resize(_nB+_nF+1);
-  _last_taps_samples = (gr_complex*)(volk_malloc((_nB+_nF+1)*sizeof(gr_complex), volk_get_alignment()));
+  _last_taps_samples.resize(_nB+_nF+1);
-  _taps_symbols      = (gr_complex*)(volk_malloc(        _nW*sizeof(gr_complex), volk_get_alignment()));
+  _taps_symbols.resize(_nW);
-  _hist_symbols      = (gr_complex*)(volk_malloc(      2*_nW*sizeof(gr_complex), volk_get_alignment()));
+  _hist_symbols.resize(2*_nW);
   reset_filter();
   GR_LOG_DEBUG(d_logger,str(boost::format("adaptive_dfe_impl::start() nB=%d nF=%d mu=%f alpha=%f")
                              % _nB % _nF % _mu % _alpha));
+
+  //_filter_update = lms::make(_mu);
+  _filter_update = rls::make(0.001, 0.9999);
   return true;
 }
 bool adaptive_dfe_impl::stop()
 {
   gr::thread::scoped_lock lock(d_setlock);
   GR_LOG_DEBUG(d_logger, "adaptive_dfe_impl::stop()");
-  VOLK_SAFE_DELETE(_taps_samples);
+  _filter_update.reset();
-  VOLK_SAFE_DELETE(_last_taps_samples);
-  VOLK_SAFE_DELETE(_taps_symbols);
-  VOLK_SAFE_DELETE(_hist_symbols);
   return true;
 }
 
@@ -249,7 +247,7 @@ gr_complex adaptive_dfe_impl::filter(gr_complex const* start, gr_complex const*
   // (1a) taps_samples
   volk_32fc_x2_dot_prod_32fc(&filter_output,
                              start,
-                             _taps_samples,
+                             &_taps_samples.front(),
                              _nB+_nF+1);
   // (1b) taps_symbols
   gr_complex dot_symbols(0);
@@ -287,13 +285,15 @@ gr_complex adaptive_dfe_impl::filter(gr_complex const* start, gr_complex const*
     _num_samples_since_filter_update += _sps;
 
     // (3a) update of adaptive filter taps
-    gr_complex const err =  filter_output - known_symbol;
+    gr_complex const err =  known_symbol - filter_output;
     if (std::abs(err)>0.7)
       std::cout << "err= " << std::abs(err) << std::endl;
     //       taps_samples
+    gr_complex const* gain = _filter_update->update(start, end);
     for (int j=0; j<_nB+_nF+1; ++j) {
       _last_taps_samples[j] = _taps_samples[j];
-      _taps_samples[j]     -= _mu*err*std::conj(start[j]);
+      _taps_samples[j]     += _mu*std::conj(start[j]) * err;
+//      _taps_samples[j]     += gain[j] * err;
     }
     //       taps_symbols
     if (_use_symbol_taps) {
@@ -335,8 +335,8 @@ gr_complex adaptive_dfe_impl::filter(gr_complex const* start, gr_complex const*
 int
 adaptive_dfe_impl::recenter_filter_taps() {
 #if 0
-  ssize_t const _idx_max = std::distance(_taps_samples,
+  ssize_t const _idx_max = std::distance(_taps_samples.begin(),
-                                        std::max_element(_taps_samples+_nB+1-3*_sps, _taps_samples+_nB+1+3*_sps,
+                                         std::max_element(_taps_samples.begin()+_nB+1-3*_sps, _taps_samples.begin()+_nB+1+3*_sps,
                                                          [](gr_complex a, gr_complex b) {
                                                            return std::norm(a) < std::norm(b);
                                                          }));
@@ -354,17 +354,17 @@ adaptive_dfe_impl::recenter_filter_taps() {
     // maximum is right of the center tap
     //   -> shift taps to the left left
     GR_LOG_DEBUG(d_logger, "shift left");
-    std::copy(_taps_samples+4*_sps, _taps_samples+_nB+_nF+1, _taps_samples);
+    std::copy(_taps_samples.begin()+4*_sps, _taps_samples.begin()+_nB+_nF+1, _taps_samples.begin());
-    std::fill_n(_taps_samples+_nB+_nF+1-4*_sps, 4*_sps, gr_complex(0));
+    std::fill_n(_taps_samples.begin()+_nB+_nF+1-4*_sps, 4*_sps, gr_complex(0));
     return +4*_sps;
   }
   if (idx_max-_nB-1 < -2*_sps) {
     // maximum is left of the center tap
     //   -> shift taps to the right
     GR_LOG_DEBUG(d_logger, "shift right");
-    std::copy_backward(_taps_samples, _taps_samples+_nB+_nF+1-4*_sps,
+    std::copy_backward(_taps_samples.begin(), _taps_samples.begin()+_nB+_nF+1-4*_sps,
-                       _taps_samples+_nB+_nF+1);
+                       _taps_samples.begin()+_nB+_nF+1);
-    std::fill_n(_taps_samples, 4*_sps, gr_complex(0));
+    std::fill_n(_taps_samples.begin(), 4*_sps, gr_complex(0));
     return -4*_sps;
   }
   return 0;
@@ -372,10 +372,10 @@ adaptive_dfe_impl::recenter_filter_taps() {
 
 void adaptive_dfe_impl::reset_filter()
 {
-  std::fill_n(_taps_samples,              _nB+_nF+1, gr_complex(0));
+  std::fill(_taps_samples.begin(),      _taps_samples.end(),      gr_complex(0));
-  std::fill_n(_last_taps_samples,         _nB+_nF+1, gr_complex(0));
+  std::fill(_last_taps_samples.begin(), _last_taps_samples.end(), gr_complex(0));
-  std::fill_n(_taps_symbols,                    _nW, gr_complex(0));
+  std::fill(_taps_symbols.begin(),      _taps_symbols.end(),      gr_complex(0));
-  std::fill_n(_hist_symbols,                  2*_nW, gr_complex(0));
+  std::fill(_hist_symbols.begin(),      _hist_symbols.end(),      gr_complex(0));
   _taps_symbols[0]     = 1;
   _hist_symbol_index   = 0;
   _num_samples_since_filter_update = 0;
@@ -387,11 +387,11 @@ void adaptive_dfe_impl::publish_frame_info()
   GR_LOG_DEBUG(d_logger, str(boost::format("publish_frame_info %d == %d") % _descrambled_symbols.size() % _symbols.size()));
   data = pmt::dict_add(data,
                        pmt::intern("symbols"),
-                       pmt::init_c32vector(_descrambled_symbols.size(), &_descrambled_symbols.front()));
+                       pmt::init_c32vector(_descrambled_symbols.size(), _descrambled_symbols));
   // for (int i=0; i<_vec_soft_decisions.size(); ++i)
   //   _vec_soft_decisions[i] = std::max(-1.0f, std::min(1.0f, _vec_soft_decisions[i]));
   data = pmt::dict_add(data,
-                       pmt::intern("soft_dec"), pmt::init_f32vector(_vec_soft_decisions.size(), &_vec_soft_decisions.front()));
+                       pmt::intern("soft_dec"), pmt::init_f32vector(_vec_soft_decisions.size(), _vec_soft_decisions));
   message_port_pub(_msg_ports["frame_info"], data);
   _descrambled_symbols.clear();
 }

lib/adaptive_dfe_impl.h

@@ -26,6 +26,9 @@
 #include <gnuradio/digital/constellation.h>
 #include <digitalhf/adaptive_dfe.h>
 
+#include "filter_update.hpp"
+#include "volk_allocator.hpp"
+
 namespace gr {
 namespace digitalhf {
 
@@ -56,6 +59,9 @@ private:
 
 class adaptive_dfe_impl : public adaptive_dfe {
 private:
+  typedef std::vector<gr_complex, volk_allocator<gr_complex> > gr_complex_vec_type;
+//  typedef std::vector<gr_complex> gr_complex_vec_type;
+
   int _sps;
   int _nB, _nF, _nW;
   int _nGuard;
@@ -65,25 +71,21 @@ private:
 
   bool _use_symbol_taps;
 
-  // module name w.r.t. digitalhf.physical_layer containing a PhysicalLayer class
+  gr_complex_vec_type _taps_samples;
-  // std::string           _py_module_name;
+  gr_complex_vec_type _taps_symbols;
-  // boost::python::object _physicalLayer; // class instance of physical layer description
+  gr_complex_vec_type _last_taps_samples;
 
-  gr_complex* _taps_samples;
+  gr_complex_vec_type _hist_symbols;
-  gr_complex* _taps_symbols;
-  gr_complex* _last_taps_samples;
-
-  gr_complex* _hist_symbols;
   int _hist_symbol_index;
 
   std::vector<gr::digital::constellation_sptr> _constellations;
   std::vector<constellation_distance_filter> _npwr;
   int _npwr_max_time_constant;
   int _constellation_index;
-  std::vector<gr_complex>   _symbols;
+  std::vector<gr_complex> _symbols;
-  std::vector<gr_complex>   _scramble; // PSK-8 scramble symbols
+  std::vector<gr_complex> _scramble; // PSK-8 scramble symbols
   std::vector<std::array<int,8> > _scramble_xor; // signs for XOR scrambling
-  std::vector<gr_complex>   _descrambled_symbols;
+  std::vector<gr_complex> _descrambled_symbols;
   int _symbol_counter;
 
   bool _save_soft_decisions;
@@ -92,7 +94,7 @@ private:
   pmt::pmt_t _msg_metadata;
 
   int _num_samples_since_filter_update;
-  std::vector<gr_complex> _rotated_samples;
+  gr_complex_vec_type _rotated_samples;
   blocks::rotator _rotator;
   gr::blocks::control_loop _control_loop;
 
@@ -102,6 +104,7 @@ private:
     DO_FILTER
   } _state;
 
+  filter_update::sptr _filter_update;
 //  void update_constellations(boost::python::object obj);
   void update_constellations(pmt::pmt_t );
   void update_frame_info(pmt::pmt_t );

lib/filter_update.hpp

@@ -0,0 +1,29 @@
+// -*- C++ -*-
+
+#ifndef _LIB_FILTER_UPDATE_HPP_
+#define _LIB_FILTER_UPDATE_HPP_
+
+#include <memory>
+#include <string>
+
+#include <boost/noncopyable.hpp>
+#include <gnuradio/gr_complex.h>
+
+namespace gr {
+namespace digitalhf {
+
+class filter_update : private boost::noncopyable {
+public:
+  typedef std::unique_ptr<filter_update> sptr;
+
+  virtual void reset() = 0;
+  virtual gr_complex const* update(gr_complex const*, gr_complex const*) = 0;
+
+protected:
+private:
+} ;
+
+
+} // namespace digitalhf
+} // namespace gr
+#endif // _LIB_FILTER_UPDATE_HPP_

lib/lms.cc

@@ -0,0 +1,45 @@
+// -*- C++ -*-
+
+#include <cassert>
+#include "lms.hpp"
+#include <volk/volk.h>
+
+namespace gr {
+namespace digitalhf {
+
+filter_update::sptr lms::make(float mu) {
+  return filter_update::sptr(new lms(mu));
+}
+
+lms::lms(float mu)
+  : _mu(mu)
+  , _gain() {
+}
+
+lms::~lms() {
+}
+
+void lms::resize(size_t n) {
+  if (_gain.size() == n)
+    return;
+  _gain.resize(n);
+  std::fill_n(_gain.begin(), n, 0);
+}
+
+void lms::reset() {
+  std::fill_n(_gain.begin(), _gain.size(), 0);
+}
+
+gr_complex const* lms::update(gr_complex const* beg,
+                              gr_complex const* end) {
+  assert(end-beg > 0);
+  size_t n = end - beg;
+  resize(n);
+  for (size_t i=0; i<n; ++i)
+    _gain[i] = _mu * std::conj(beg[i]);
+
+  return &_gain.front();
+}
+
+} // namespace digitalhf
+} // namespace gr

lib/lms.hpp

@@ -0,0 +1,35 @@
+// -*- c++ -*-
+
+#ifndef _LIB_LMS_HPP_
+#define _LIB_LMS_HPP_
+
+#include <vector>
+
+#include "filter_update.hpp"
+#include "volk_allocator.hpp"
+
+
+namespace gr {
+namespace digitalhf {
+
+class lms : public filter_update {
+public:
+  lms(float mu);
+  virtual ~lms();
+
+  static sptr make(float mu);
+
+  virtual void reset();
+  virtual gr_complex const* update(gr_complex const*, gr_complex const*);
+
+protected:
+  void resize(size_t);
+
+private:
+  float _mu;
+  std::vector<gr_complex, volk_allocator<gr_complex> > _gain;
+} ;
+
+} // namespace digitalhf
+} // namespace gr
+#endif // _LIB_LMS_HPP_

lib/rls.cc

@@ -0,0 +1,72 @@
+// -*- C++ -*-
+
+#include <cassert>
+#include "rls.hpp"
+#include <volk/volk.h>
+
+namespace gr {
+namespace digitalhf {
+
+filter_update::sptr rls::make(float delta, float lambda) {
+  return filter_update::sptr(new rls(delta, lambda));
+}
+
+rls::rls(float delta, float lambda)
+  : _delta(delta)
+  , _lambda(lambda)
+  , _gain()
+  , _inv_corr() {
+}
+
+rls::~rls() {
+}
+
+void rls::resize(size_t n) {
+  if (_gain.size() == n && _inv_corr.size() == n*n)
+    return;
+  _gain.resize(n);
+  _inv_corr.resize(n*n);
+  reset();
+}
+
+void rls::reset() {
+  size_t const n = _gain.size();
+  std::fill_n(_gain.begin(),     n,   0);
+  std::fill_n(_inv_corr.begin(), n*n, 0);
+  for (size_t i=0; i<n; ++i)
+    _inv_corr[n*i +i] = gr_complex(_delta, 0);
+}
+
+gr_complex const* rls::update(gr_complex const* beg,
+                              gr_complex const* end) {
+  assert(end-beg > 0);
+  unsigned const n = end - beg;
+  resize(n);
+
+  std::vector<gr_complex, volk_allocator<gr_complex> > pu(n), tmp(n);
+  //pu.resize(n);
+
+  for (unsigned i=0; i<n; ++i)
+    volk_32fc_x2_dot_prod_32fc(&pu[i], &_inv_corr[n*i], beg, n);
+
+  gr_complex uPu = 0;
+  volk_32fc_x2_conjugate_dot_prod_32fc(&uPu, &pu[0], beg, n);
+  for (unsigned i=0; i<n; ++i)
+    _gain[i] = std::conj(pu[i])/(_lambda + std::real(uPu));
+  for (unsigned i=0; i<n; ++i) {
+    unsigned const k = n*i;
+#if 0
+    for (unsigned j=0; j<n; ++j)
+      _inv_corr[k+j] = (_inv_corr[k+j] - pu[i]*_gain[j]) / _lambda;
+#else
+    volk_32fc_s32fc_multiply_32fc(&tmp[0], &_gain[0], -pu[i], n);
+    volk_32fc_x2_add_32fc(&_inv_corr[k], &_inv_corr[k], &tmp[0], n);
+    volk_32f_s32f_multiply_32f((float*)&_inv_corr[k], (float const*)&_inv_corr[k], 1.0f/_lambda, 2*n);
+#endif
+    _inv_corr[k+i] += _delta;
+  }
+  return &_gain.front();
+}
+
+} // namespace digitalhf
+} // namespace gr

lib/rls.hpp

@@ -0,0 +1,37 @@
+// -*- c++ -*-
+
+#ifndef _LIB_RLS_HPP_
+#define _LIB_RLS_HPP_
+
+#include <vector>
+
+#include "filter_update.hpp"
+#include "volk_allocator.hpp"
+
+
+namespace gr {
+namespace digitalhf {
+
+class rls : public filter_update {
+public:
+  rls(float delta, float lambda);
+  virtual ~rls();
+
+  static sptr make(float delta, float lambda);
+
+  virtual void reset();
+  virtual gr_complex const* update(gr_complex const*, gr_complex const*);
+
+protected:
+  void resize(size_t);
+
+private:
+  float _delta;
+  float _lambda;
+  std::vector<gr_complex, volk_allocator<gr_complex> > _gain;
+  std::vector<gr_complex, volk_allocator<gr_complex> > _inv_corr;
+} ;
+
+} // namespace digitalhf
+} // namespace gr
+#endif // _LIB_RLS_HPP_

lib/volk_allocator.hpp

@@ -0,0 +1,43 @@
+// -*- c++ -*-
+
+#ifndef _LIB_VOLK_ALLOCATOR_HPP_
+#define _LIB_VOLK_ALLOCATOR_HPP_
+
+#include <cstdlib>
+#include <limits>
+#include <volk/volk.h>
+
+namespace gr {
+namespace digitalhf {
+
+// see https://en.cppreference.com/w/cpp/named_req/Allocator
+template <class T>
+struct volk_allocator {
+  typedef T value_type;
+
+  volk_allocator() = default;
+
+  template <class U> constexpr volk_allocator(const volk_allocator<U>&) noexcept {}
+
+  T* allocate(std::size_t n) {
+    if (n > std::numeric_limits<std::size_t>::max() / sizeof(T)) throw std::bad_alloc();
+
+    if (auto p = static_cast<T*>(volk_malloc(n*sizeof(T), volk_get_alignment())))
+      return p;
+
+    throw std::bad_alloc();
+  }
+
+  void deallocate(T* p, std::size_t) noexcept { volk_free(p); }
+
+} ;
+
+template <class T, class U>
+bool operator==(const volk_allocator<T>&, const volk_allocator<U>&) { return true; }
+
+template <class T, class U>
+bool operator!=(const volk_allocator<T>&, const volk_allocator<U>&) { return false; }
+
+} // namespace digitalhf
+} // namespace gr
+#endif // _LIB_VOLK_ALLOCATOR_HPP_