5 files changed, 273 insertions, 14 deletions
diff --git a/gr-blocks/grc/blocks_multiply_matrix_xx.xml b/gr-blocks/grc/blocks_multiply_matrix_xx.xml
index b0ba41df60..070221446e 100644
--- a/gr-blocks/grc/blocks_multiply_matrix_xx.xml
+++ b/gr-blocks/grc/blocks_multiply_matrix_xx.xml
@@ -14,6 +14,11 @@
 		<key>float</key>
 		<opt>fcn:ff</opt>
 	</option>
+	<option>
+		<name>Complex</name>
+		<key>complex</key>
+		<opt>fcn:cc</opt>
+	</option>
   </param>
   <param>
     <name>Matrix A</name>
diff --git a/gr-blocks/include/gnuradio/blocks/CMakeLists.txt b/gr-blocks/include/gnuradio/blocks/CMakeLists.txt
index c0e20ae015..bf0c1e8e4f 100644
--- a/gr-blocks/include/gnuradio/blocks/CMakeLists.txt
+++ b/gr-blocks/include/gnuradio/blocks/CMakeLists.txt
@@ -78,6 +78,7 @@ expand_h(moving_average_XX  ss ii ff cc)
 expand_h(multiply_XX        ss ii)
 expand_h(multiply_const_XX  ss ii)
 expand_h(multiply_const_vXX ss ii ff cc)
+expand_h(multiply_matrix_XX ff cc)
 expand_h(mute_XX            ss ii ff cc)
 expand_h(not_XX             bb ss ii)
 expand_h(or_XX              bb ss ii)
@@ -219,7 +220,7 @@ install(FILES
     vector_to_streams.h
     wavfile_sink.h
     wavfile_source.h
-    multiply_matrix_ff.h DESTINATION ${GR_INCLUDE_DIR}/gnuradio/blocks
+    DESTINATION ${GR_INCLUDE_DIR}/gnuradio/blocks
     COMPONENT "blocks_devel"
 )
 
diff --git a/gr-blocks/include/gnuradio/blocks/multiply_matrix_ff.h b/gr-blocks/include/gnuradio/blocks/multiply_matrix_XX.h.t
index 90bb68824d..25e7212105 100644
--- a/gr-blocks/include/gnuradio/blocks/multiply_matrix_ff.h
+++ b/gr-blocks/include/gnuradio/blocks/multiply_matrix_XX.h.t
@@ -20,8 +20,10 @@
  * Boston, MA 02110-1301, USA.
  */
 
-#ifndef INCLUDED_BLOCKS_MULTIPLY_MATRIX_FF_H
-#define INCLUDED_BLOCKS_MULTIPLY_MATRIX_FF_H
+// @WARNING@
+
+#ifndef @GUARD_NAME@
+#define @GUARD_NAME@
 
 #include <gnuradio/blocks/api.h>
 #include <gnuradio/sync_block.h>
@@ -36,9 +38,9 @@ namespace gr {
      * This block is similar to gr::blocks::multiply_const_ff, the difference
      * being it can handle several inputs and outputs, and the input-to-output
      * relation can be described by the following mathematical equation:
-     * \[
+     * \f[
      *    y(k) = A x(k) \, , \, y \in \mathbb{R}^N, x \in \mathbb{R}^M, A \in \mathbb{R}^{N \times M}
-     * \]
+     * \f]
      * \$y(k)\$ and \$x(i)\$ are column-vectors describing the elements on the input port
      * at time step \$k\$ (this is a sync block with no memory).
      *
@@ -47,16 +49,16 @@ namespace gr {
      * - Simulation of static MIMO-Channels (in that case, \$A\$ is the channel matrix)
      *
      * This block features a special tag propagation mode: When setting the tag propagation policy
-     * to gr::blocks::multiply_matrix_ff::TPP_SELECT_BY_MATRIX, a tag is propagated from input k
-     * to output l if \$(A)_{l,k} \neq 0\$.
+     * to gr::blocks::@NAME@::TPP_SELECT_BY_MATRIX, a tag is propagated from input k
+     * to output l if \f$(A)_{l,k} \neq 0\f$.
      *
      * A message port (\p set_A) allows to set the matrix. *Note*: It is not possible to change
      * the dimension of the matrix after initialization, as this affects the I/O signature!
      */
-    class BLOCKS_API multiply_matrix_ff : virtual public gr::sync_block
+    class BLOCKS_API @NAME@ : virtual public gr::sync_block
     {
      public:
-      typedef boost::shared_ptr<multiply_matrix_ff> sptr;
+      typedef boost::shared_ptr<@NAME@> sptr;
 
       /*!
        * \param A The matrix
@@ -64,14 +66,14 @@ namespace gr {
        *                               Note this can be any gr::block::tag_propagation_policy_t value, or TPP_SELECT_BY_MATRIX.
        */
       static sptr make(
-          std::vector<std::vector<float> > A,
+          std::vector<std::vector<@O_TYPE@> > A,
           gr::block::tag_propagation_policy_t tag_propagation_policy=gr::block::TPP_ALL_TO_ALL
       );
 
       //! Returns the current matrix
-      virtual const std::vector<std::vector<float> >& get_A() const = 0;
+      virtual const std::vector<std::vector<@O_TYPE@> >& get_A() const = 0;
       //! Sets the matrix to a new value \p new_A. Returns true if the new matrix was valid and could be changed.
-      virtual bool set_A(const std::vector<std::vector<float> > &new_A) = 0;
+      virtual bool set_A(const std::vector<std::vector<@O_TYPE@> > &new_A) = 0;
 
       /*!
        * \brief Set the policy by the scheduler to determine how tags are moved downstream.
@@ -83,10 +85,10 @@ namespace gr {
       static const int TPP_SELECT_BY_MATRIX = 999;
       static const std::string MSG_PORT_NAME_SET_A;
     };
-    const std::string multiply_matrix_ff::MSG_PORT_NAME_SET_A = "set_A";
+    const std::string @NAME@::MSG_PORT_NAME_SET_A = "set_A";
 
   } // namespace blocks
 } // namespace gr
 
-#endif /* INCLUDED_BLOCKS_MULTIPLY_MATRIX_FF_H */
+#endif /* @GUARD_NAME */
 
diff --git a/gr-blocks/lib/multiply_matrix_cc_impl.cc b/gr-blocks/lib/multiply_matrix_cc_impl.cc
new file mode 100644
index 0000000000..f9fc30ee6d
--- /dev/null
+++ b/gr-blocks/lib/multiply_matrix_cc_impl.cc
@@ -0,0 +1,192 @@
+/* -*- c++ -*- */
+/* 
+ * Copyright 2014 Free Software Foundation, Inc.
+ * 
+ * This file is part of GNU Radio
+ * 
+ * GNU Radio is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3, or (at your option)
+ * any later version.
+ * 
+ * GNU Radio is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ * 
+ * You should have received a copy of the GNU General Public License
+ * along with GNU Radio; see the file COPYING.  If not, write to
+ * the Free Software Foundation, Inc., 51 Franklin Street,
+ * Boston, MA 02110-1301, USA.
+ */
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include <gnuradio/io_signature.h>
+#include <volk/volk.h>
+#include "multiply_matrix_cc_impl.h"
+
+namespace gr {
+  namespace blocks {
+
+    multiply_matrix_cc::sptr
+    multiply_matrix_cc::make(std::vector<std::vector<gr_complex> > A, gr::block::tag_propagation_policy_t tag_propagation_policy)
+    {
+      if (A.empty() or A[0].size() == 0) {
+        throw std::invalid_argument("matrix A has invalid dimensions.");
+      }
+      return gnuradio::get_initial_sptr
+        (new multiply_matrix_cc_impl(A, tag_propagation_policy));
+    }
+
+    multiply_matrix_cc_impl::multiply_matrix_cc_impl(std::vector<std::vector<gr_complex> > A, gr::block::tag_propagation_policy_t tag_propagation_policy)
+      : gr::sync_block("multiply_matrix_cc",
+              gr::io_signature::make(A[0].size(), A[0].size(), sizeof(gr_complex)),
+              gr::io_signature::make(A.size(), A.size(), sizeof(gr_complex))),
+      d_A(A)
+    {
+      this->set_tag_propagation_policy(tag_propagation_policy);
+      const int alignment_multiple = volk_get_alignment() / sizeof(gr_complex);
+      set_alignment(std::max(1, alignment_multiple));
+
+      pmt::pmt_t port_name = pmt::string_to_symbol("set_A");
+      message_port_register_in(port_name);
+      set_msg_handler(
+          port_name,
+          boost::bind(&multiply_matrix_cc_impl::msg_handler_A, this, _1)
+      );
+    }
+
+    multiply_matrix_cc_impl::~multiply_matrix_cc_impl()
+    {
+    }
+
+    int
+    multiply_matrix_cc_impl::work(int noutput_items,
+                          gr_vector_const_void_star &input_items,
+                          gr_vector_void_star &output_items)
+    {
+      for (size_t out_idx = 0; out_idx < output_items.size(); out_idx++) {
+        gr_complex *out = reinterpret_cast<gr_complex *>(output_items[out_idx]);
+        // Do input 0 first, this saves a memset
+        const gr_complex *in = reinterpret_cast<const gr_complex *>(input_items[0]);
+        volk_32fc_s32fc_multiply_32fc(out, in, d_A[out_idx][0], noutput_items);
+        // Then do inputs 1 through N
+        for (size_t in_idx = 1; in_idx < input_items.size(); in_idx++) {
+          in = reinterpret_cast<const gr_complex *>(input_items[in_idx]);
+          // Yeah, this needs VOLK-ifying (TODO)
+          for (int i = 0; i < noutput_items; i++) {
+            out[i] += in[i] * d_A[out_idx][in_idx];
+          }
+        }
+      }
+      if (d_tag_prop_select) {
+        propagate_tags_by_A(noutput_items, input_items.size(), output_items.size());
+      }
+      return noutput_items;
+    }
+
+
+    // Copy tags from input k to output l if A[l][k] is not zero
+    void
+    multiply_matrix_cc_impl::propagate_tags_by_A(int noutput_items, size_t ninput_ports, size_t noutput_ports)
+    {
+      std::vector<gr::tag_t> tags;
+      for (size_t in_idx = 0; in_idx < ninput_ports; in_idx++) {
+        get_tags_in_window(
+            tags,
+            in_idx,
+            0,
+            noutput_items
+        );
+
+        for (size_t out_idx = 0; out_idx < noutput_ports; out_idx++) {
+          if (d_A[out_idx][in_idx] == 0) {
+            continue;
+          }
+          for (size_t i = 0; i < tags.size(); i++) {
+            add_item_tag(out_idx, tags[i]);
+          }
+        }
+      }
+    }
+
+    // Check dimensions before copying
+    bool
+    multiply_matrix_cc_impl::set_A(const std::vector<std::vector<gr_complex> > &new_A)
+    {
+      if (d_A.size() != new_A.size()) {
+        GR_LOG_ALERT(d_logger, "Attempted to set matrix with invalid dimensions.");
+        return false;
+      }
+      for (size_t i = 0; i < d_A.size(); i++) {
+        if (d_A[i].size() != new_A[i].size()) {
+          GR_LOG_ALERT(d_logger, "Attempted to set matrix with invalid dimensions.");
+          return false;
+        }
+      }
+      d_A = new_A;
+      return true;
+    }
+
+    void
+    multiply_matrix_cc_impl::msg_handler_A(pmt::pmt_t A)
+    {
+      if (not pmt::is_vector(A) and not pmt::is_tuple(A)) {
+          GR_LOG_ALERT(d_logger, "Invalid message to set A (wrong type).");
+          return;
+      }
+      if (not pmt::length(A) == d_A.size()) {
+          GR_LOG_ALERT(d_logger, "Invalid message to set A (wrong size).");
+          return;
+      }
+
+      std::vector<std::vector<gr_complex> > new_A(d_A);
+      for (size_t i = 0; i < pmt::length(A); i++) {
+        pmt::pmt_t row;
+        if (pmt::is_vector(A)) {
+          row = pmt::vector_ref(A, i);
+        } else if (pmt::is_tuple(A)) {
+          row = pmt::tuple_ref(A, i);
+        }
+        if (pmt::is_vector(row) or pmt::is_tuple(row)) {
+          if (pmt::length(row) != d_A[0].size()) {
+            GR_LOG_ALERT(d_logger, "Invalid message to set A (wrong number of columns).");
+            return;
+          }
+          for (size_t k = 0; k < pmt::length(row); k++) {
+            new_A[i][k] = pmt::to_double(pmt::is_vector(row) ? pmt::vector_ref(row, k) : pmt::tuple_ref(row, k));
+          }
+        } else if (pmt::is_c32vector(row)) {
+          size_t row_len = 0;
+          const gr_complex *elements = pmt::c32vector_elements(row, row_len);
+          if (row_len != d_A[0].size()) {
+            GR_LOG_ALERT(d_logger, "Invalid message to set A (wrong number of columns).");
+            return;
+          }
+          new_A[i].assign(elements, elements + row_len);
+        }
+      }
+
+      if (not set_A(new_A)) {
+          GR_LOG_ALERT(d_logger, "Invalid message to set A.");
+      }
+    }
+
+    void
+    multiply_matrix_cc_impl::set_tag_propagation_policy(gr::block::tag_propagation_policy_t tpp)
+    {
+      if (tpp == TPP_SELECT_BY_MATRIX) {
+        set_tag_propagation_policy(TPP_DONT);
+        d_tag_prop_select = true;
+      } else {
+        gr::block::set_tag_propagation_policy(tpp);
+        d_tag_prop_select = false;
+      }
+    }
+
+  } /* namespace blocks */
+} /* namespace gr */
+
diff --git a/gr-blocks/lib/multiply_matrix_cc_impl.h b/gr-blocks/lib/multiply_matrix_cc_impl.h
new file mode 100644
index 0000000000..b357dde2cb
--- /dev/null
+++ b/gr-blocks/lib/multiply_matrix_cc_impl.h
@@ -0,0 +1,59 @@
+/* -*- c++ -*- */
+/* 
+ * Copyright 2014 Free Software Foundation, Inc.
+ * 
+ * This file is part of GNU Radio
+ * 
+ * GNU Radio is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3, or (at your option)
+ * any later version.
+ * 
+ * GNU Radio is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ * 
+ * You should have received a copy of the GNU General Public License
+ * along with GNU Radio; see the file COPYING.  If not, write to
+ * the Free Software Foundation, Inc., 51 Franklin Street,
+ * Boston, MA 02110-1301, USA.
+ */
+
+#ifndef INCLUDED_BLOCKS_MULTIPLY_matrix_cc_IMPL_H
+#define INCLUDED_BLOCKS_MULTIPLY_matrix_cc_IMPL_H
+
+#include <gnuradio/blocks/multiply_matrix_cc.h>
+
+namespace gr {
+  namespace blocks {
+
+    class multiply_matrix_cc_impl : public multiply_matrix_cc
+    {
+     private:
+      std::vector<std::vector<gr_complex> > d_A;
+      bool d_tag_prop_select; //!< If true, handle the tag propagation ourselves
+
+      void propagate_tags_by_A(int noutput_items, size_t ninput_ports, size_t noutput_ports);
+
+      void msg_handler_A(pmt::pmt_t A);
+
+     public:
+      multiply_matrix_cc_impl(std::vector<std::vector<gr_complex> > A, gr::block::tag_propagation_policy_t tag_propagation_policy);
+      ~multiply_matrix_cc_impl();
+
+      const std::vector<std::vector<gr_complex> >& get_A() const { return d_A; };
+      bool set_A(const std::vector<std::vector<gr_complex> > &new_A);
+
+      void set_tag_propagation_policy(gr::block::tag_propagation_policy_t p);
+
+      int work(int noutput_items,
+               gr_vector_const_void_star &input_items,
+               gr_vector_void_star &output_items);
+    };
+
+  } // namespace blocks
+} // namespace gr
+
+#endif /* INCLUDED_BLOCKS_MULTIPLY_matrix_cc_IMPL_H */
+