1 /* -*- c++ -*- */
2 /*
3 * Copyright 2014,2017,2018 Free Software Foundation, Inc.
4 *
5 * This file is part of GNU Radio
6 *
7 * GNU Radio is free software; you can redistribute it and/or modify
8 * it under the terms of the GNU General Public License as published by
9 * the Free Software Foundation; either version 3, or (at your option)
10 * any later version.
11 *
12 * GNU Radio is distributed in the hope that it will be useful,
13 * but WITHOUT ANY WARRANTY; without even the implied warranty of
14 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15 * GNU General Public License for more details.
16 *
17 * You should have received a copy of the GNU General Public License
18 * along with GNU Radio; see the file COPYING. If not, write to
19 * the Free Software Foundation, Inc., 51 Franklin Street,
20 * Boston, MA 02110-1301, USA.
21 */
22
23 #ifdef HAVE_CONFIG_H
24 #include "config.h"
25 #endif
26
27 #include "multiply_matrix_impl.h"
28 #include <gnuradio/io_signature.h>
29 #include <volk/volk.h>
30
31 namespace gr {
32 namespace blocks {
33
34 // Copy tags from input k to output l if A[l][k] is not zero
35 template <>
propagate_tags_by_A(int noutput_items,size_t ninput_ports,size_t noutput_ports)36 void multiply_matrix_impl<gr_complex>::propagate_tags_by_A(int noutput_items,
37 size_t ninput_ports,
38 size_t noutput_ports)
39 {
40 std::vector<gr::tag_t> tags;
41 for (size_t in_idx = 0; in_idx < ninput_ports; in_idx++) {
42 this->get_tags_in_window(tags, in_idx, 0, noutput_items);
43
44 for (size_t out_idx = 0; out_idx < noutput_ports; out_idx++) {
45 if (d_A[out_idx][in_idx] == std::complex<float>(0, 0)) {
46 continue;
47 }
48 for (size_t i = 0; i < tags.size(); i++) {
49 this->add_item_tag(out_idx, tags[i]);
50 }
51 }
52 }
53 }
54
55 // Check dimensions before copying
56 template <>
set_A(const std::vector<std::vector<gr_complex>> & new_A)57 bool multiply_matrix_impl<gr_complex>::set_A(
58 const std::vector<std::vector<gr_complex>>& new_A)
59 {
60 if (d_A.size() != new_A.size()) {
61 GR_LOG_ALERT(d_logger, "Attempted to set matrix with invalid dimensions.");
62 return false;
63 }
64 for (size_t i = 0; i < d_A.size(); i++) {
65 if (d_A[i].size() != new_A[i].size()) {
66 GR_LOG_ALERT(d_logger, "Attempted to set matrix with invalid dimensions.");
67 return false;
68 }
69 }
70 d_A = new_A;
71 return true;
72 }
73
74 template <>
msg_handler_A(pmt::pmt_t A)75 void multiply_matrix_impl<gr_complex>::msg_handler_A(pmt::pmt_t A)
76 {
77 if (!pmt::is_vector(A) && !pmt::is_tuple(A)) {
78 GR_LOG_ALERT(d_logger, "Invalid message to set A (wrong type).");
79 return;
80 }
81 if (pmt::length(A) != d_A.size()) {
82 GR_LOG_ALERT(d_logger, "Invalid message to set A (wrong size).");
83 return;
84 }
85
86 std::vector<std::vector<gr_complex>> new_A(d_A);
87 for (size_t i = 0; i < pmt::length(A); i++) {
88 pmt::pmt_t row;
89 if (pmt::is_vector(A)) {
90 row = pmt::vector_ref(A, i);
91 } else if (pmt::is_tuple(A)) {
92 row = pmt::tuple_ref(A, i);
93 }
94 if (pmt::is_vector(row) || pmt::is_tuple(row)) {
95 if (pmt::length(row) != d_A[0].size()) {
96 GR_LOG_ALERT(d_logger,
97 "Invalid message to set A (wrong number of columns).");
98 return;
99 }
100 for (size_t k = 0; k < pmt::length(row); k++) {
101 new_A[i][k] =
102 pmt::to_complex(pmt::is_vector(row) ? pmt::vector_ref(row, k)
103 : pmt::tuple_ref(row, k));
104 }
105 } else if (pmt::is_c32vector(row)) {
106 size_t row_len = 0;
107 const gr_complex* elements = pmt::c32vector_elements(row, row_len);
108 if (row_len != d_A[0].size()) {
109 GR_LOG_ALERT(d_logger,
110 "Invalid message to set A (wrong number of columns).");
111 return;
112 }
113 new_A[i].assign(elements, elements + row_len);
114 }
115 }
116
117 if (!set_A(new_A)) {
118 GR_LOG_ALERT(d_logger, "Invalid message to set A.");
119 }
120 }
121
122 // Copy tags from input k to output l if A[l][k] is not zero
123 template <>
propagate_tags_by_A(int noutput_items,size_t ninput_ports,size_t noutput_ports)124 void multiply_matrix_impl<float>::propagate_tags_by_A(int noutput_items,
125 size_t ninput_ports,
126 size_t noutput_ports)
127 {
128 std::vector<gr::tag_t> tags;
129 for (size_t in_idx = 0; in_idx < ninput_ports; in_idx++) {
130 get_tags_in_window(tags, in_idx, 0, noutput_items);
131
132 for (size_t out_idx = 0; out_idx < noutput_ports; out_idx++) {
133 if (d_A[out_idx][in_idx] == 0) {
134 continue;
135 }
136 for (size_t i = 0; i < tags.size(); i++) {
137 add_item_tag(out_idx, tags[i]);
138 }
139 }
140 }
141 }
142
143 // Check dimensions before copying
144 template <>
set_A(const std::vector<std::vector<float>> & new_A)145 bool multiply_matrix_impl<float>::set_A(const std::vector<std::vector<float>>& new_A)
146 {
147 if (d_A.size() != new_A.size()) {
148 GR_LOG_ALERT(d_logger, "Attempted to set matrix with invalid dimensions.");
149 return false;
150 }
151 for (size_t i = 0; i < d_A.size(); i++) {
152 if (d_A[i].size() != new_A[i].size()) {
153 GR_LOG_ALERT(d_logger, "Attempted to set matrix with invalid dimensions.");
154 return false;
155 }
156 }
157 d_A = new_A;
158 return true;
159 }
160
161 template <>
msg_handler_A(pmt::pmt_t A)162 void multiply_matrix_impl<float>::msg_handler_A(pmt::pmt_t A)
163 {
164 if (!pmt::is_vector(A) && !pmt::is_tuple(A)) {
165 GR_LOG_ALERT(d_logger, "Invalid message to set A (wrong type).");
166 return;
167 }
168 if (pmt::length(A) != d_A.size()) {
169 GR_LOG_ALERT(d_logger, "Invalid message to set A (wrong size).");
170 return;
171 }
172
173 std::vector<std::vector<float>> new_A(d_A);
174 for (size_t i = 0; i < pmt::length(A); i++) {
175 pmt::pmt_t row;
176 if (pmt::is_vector(A)) {
177 row = pmt::vector_ref(A, i);
178 } else if (pmt::is_tuple(A)) {
179 row = pmt::tuple_ref(A, i);
180 }
181 if (pmt::is_vector(row) || pmt::is_tuple(row)) {
182 if (pmt::length(row) != d_A[0].size()) {
183 GR_LOG_ALERT(d_logger,
184 "Invalid message to set A (wrong number of columns).");
185 return;
186 }
187 for (size_t k = 0; k < pmt::length(row); k++) {
188 new_A[i][k] =
189 pmt::to_double(pmt::is_vector(row) ? pmt::vector_ref(row, k)
190 : pmt::tuple_ref(row, k));
191 }
192 } else if (pmt::is_f32vector(row)) {
193 size_t row_len = 0;
194 const float* elements = pmt::f32vector_elements(row, row_len);
195 if (row_len != d_A[0].size()) {
196 GR_LOG_ALERT(d_logger,
197 "Invalid message to set A (wrong number of columns).");
198 return;
199 }
200 new_A[i].assign(elements, elements + row_len);
201 }
202 }
203
204 if (!set_A(new_A)) {
205 GR_LOG_ALERT(d_logger, "Invalid message to set A.");
206 }
207 }
208
209
210 template <class T>
211 typename multiply_matrix<T>::sptr
make(std::vector<std::vector<T>> A,gr::block::tag_propagation_policy_t tag_propagation_policy)212 multiply_matrix<T>::make(std::vector<std::vector<T>> A,
213 gr::block::tag_propagation_policy_t tag_propagation_policy)
214 {
215 if (A.empty() || A[0].empty()) {
216 throw std::invalid_argument("matrix A has invalid dimensions.");
217 }
218 return gnuradio::get_initial_sptr(
219 new multiply_matrix_impl<T>(A, tag_propagation_policy));
220 }
221
222 template <>
multiply_matrix_impl(std::vector<std::vector<gr_complex>> A,gr::block::tag_propagation_policy_t tag_propagation_policy)223 multiply_matrix_impl<gr_complex>::multiply_matrix_impl(
224 std::vector<std::vector<gr_complex>> A,
225 gr::block::tag_propagation_policy_t tag_propagation_policy)
226 : gr::sync_block("multiply_matrix_cc",
227 gr::io_signature::make(A[0].size(), A[0].size(), sizeof(gr_complex)),
228 gr::io_signature::make(A.size(), A.size(), sizeof(gr_complex))),
229 d_A(A)
230 {
231 this->MSG_PORT_NAME_SET_A = "set_A";
232 this->set_tag_propagation_policy(tag_propagation_policy);
233 const int alignment_multiple = volk_get_alignment() / sizeof(gr_complex);
234 set_alignment(std::max(1, alignment_multiple));
235
236 pmt::pmt_t port_name = pmt::string_to_symbol("set_A");
237 message_port_register_in(port_name);
238 set_msg_handler(port_name, [this](pmt::pmt_t msg) { this->msg_handler_A(msg); });
239 }
240
241 template <>
multiply_matrix_impl(std::vector<std::vector<float>> A,gr::block::tag_propagation_policy_t tag_propagation_policy)242 multiply_matrix_impl<float>::multiply_matrix_impl(
243 std::vector<std::vector<float>> A,
244 gr::block::tag_propagation_policy_t tag_propagation_policy)
245 : gr::sync_block("multiply_matrix_ff",
246 gr::io_signature::make(A[0].size(), A[0].size(), sizeof(float)),
247 gr::io_signature::make(A.size(), A.size(), sizeof(float))),
248 d_A(A)
249 {
250 this->MSG_PORT_NAME_SET_A = "set_A";
251 this->set_tag_propagation_policy(tag_propagation_policy);
252 const int alignment_multiple = volk_get_alignment() / sizeof(float);
253 set_alignment(std::max(1, alignment_multiple));
254
255 pmt::pmt_t port_name = pmt::string_to_symbol("set_A");
256 message_port_register_in(port_name);
257 set_msg_handler(port_name, [this](pmt::pmt_t msg) { this->msg_handler_A(msg); });
258 }
259
260
261 template <class T>
~multiply_matrix_impl()262 multiply_matrix_impl<T>::~multiply_matrix_impl()
263 {
264 }
265
266 template <>
work(int noutput_items,gr_vector_const_void_star & input_items,gr_vector_void_star & output_items)267 int multiply_matrix_impl<gr_complex>::work(int noutput_items,
268 gr_vector_const_void_star& input_items,
269 gr_vector_void_star& output_items)
270 {
271 for (size_t out_idx = 0; out_idx < output_items.size(); out_idx++) {
272 gr_complex* out = reinterpret_cast<gr_complex*>(output_items[out_idx]);
273 // Do input 0 first, this saves a memset
274 const gr_complex* in = reinterpret_cast<const gr_complex*>(input_items[0]);
275 volk_32fc_s32fc_multiply_32fc(out, in, d_A[out_idx][0], noutput_items);
276 // Then do inputs 1 through N
277 for (size_t in_idx = 1; in_idx < input_items.size(); in_idx++) {
278 in = reinterpret_cast<const gr_complex*>(input_items[in_idx]);
279 // Yeah, this needs VOLK-ifying (TODO)
280 for (int i = 0; i < noutput_items; i++) {
281 out[i] += in[i] * d_A[out_idx][in_idx];
282 }
283 }
284 }
285 if (tag_propagation_policy() == TPP_CUSTOM) {
286 propagate_tags_by_A(noutput_items, input_items.size(), output_items.size());
287 }
288 return noutput_items;
289 }
290
291 template <>
work(int noutput_items,gr_vector_const_void_star & input_items,gr_vector_void_star & output_items)292 int multiply_matrix_impl<float>::work(int noutput_items,
293 gr_vector_const_void_star& input_items,
294 gr_vector_void_star& output_items)
295 {
296 for (size_t out_idx = 0; out_idx < output_items.size(); out_idx++) {
297 float* out = reinterpret_cast<float*>(output_items[out_idx]);
298 // Do input 0 first, this saves a memset
299 const float* in = reinterpret_cast<const float*>(input_items[0]);
300 volk_32f_s32f_multiply_32f(out, in, d_A[out_idx][0], noutput_items);
301 // Then do inputs 1 through N
302 for (size_t in_idx = 1; in_idx < input_items.size(); in_idx++) {
303 in = reinterpret_cast<const float*>(input_items[in_idx]);
304 // Yeah, this needs VOLK-ifying (TODO)
305 for (int i = 0; i < noutput_items; i++) {
306 out[i] += in[i] * d_A[out_idx][in_idx];
307 }
308 }
309 }
310 if (tag_propagation_policy() == TPP_CUSTOM) {
311 propagate_tags_by_A(noutput_items, input_items.size(), output_items.size());
312 }
313 return noutput_items;
314 }
315
316
317 template class multiply_matrix<float>;
318 template class multiply_matrix<gr_complex>;
319
320 } /* namespace blocks */
321 } /* namespace gr */
322