1 /* -*- c++ -*- */
2 /*
3 * Copyright 2014 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 "correlate_access_code_ff_ts_impl.h"
28 #include <gnuradio/io_signature.h>
29 #include <gnuradio/math.h>
30 #include <volk/volk.h>
31 #include <boost/format.hpp>
32 #include <cstdio>
33 #include <iostream>
34 #include <stdexcept>
35
36 namespace gr {
37 namespace digital {
38
make(const std::string & access_code,int threshold,const std::string & tag_name)39 correlate_access_code_ff_ts::sptr correlate_access_code_ff_ts::make(
40 const std::string& access_code, int threshold, const std::string& tag_name)
41 {
42 return gnuradio::get_initial_sptr(
43 new correlate_access_code_ff_ts_impl(access_code, threshold, tag_name));
44 }
45
correlate_access_code_ff_ts_impl(const std::string & access_code,int threshold,const std::string & tag_name)46 correlate_access_code_ff_ts_impl::correlate_access_code_ff_ts_impl(
47 const std::string& access_code, int threshold, const std::string& tag_name)
48 : block("correlate_access_code_ff_ts",
49 io_signature::make(1, 1, sizeof(float)),
50 io_signature::make(1, 1, sizeof(float))),
51 d_data_reg(0),
52 d_mask(0),
53 d_threshold(threshold),
54 d_len(0)
55 {
56 set_tag_propagation_policy(TPP_DONT);
57
58 if (!set_access_code(access_code)) {
59 GR_LOG_ERROR(d_logger, "access_code is > 64 bits");
60 throw std::out_of_range("access_code is > 64 bits");
61 }
62
63 std::stringstream str;
64 str << name() << unique_id();
65 d_me = pmt::string_to_symbol(str.str());
66 d_key = pmt::string_to_symbol(tag_name);
67
68 d_state = STATE_SYNC_SEARCH;
69 d_pkt_len = 0;
70 d_pkt_count = 0;
71 d_hdr_reg = 0;
72 d_hdr_count = 0;
73 }
74
~correlate_access_code_ff_ts_impl()75 correlate_access_code_ff_ts_impl::~correlate_access_code_ff_ts_impl() {}
76
set_access_code(const std::string & access_code)77 bool correlate_access_code_ff_ts_impl::set_access_code(const std::string& access_code)
78 {
79 d_len = access_code.length(); // # of bytes in string
80 if (d_len > 64)
81 return false;
82
83 // set len least significant bits to 1.
84 d_mask = ((~0ULL) >> (64 - d_len));
85
86 d_access_code = 0;
87 for (unsigned i = 0; i < d_len; i++) {
88 d_access_code = (d_access_code << 1) | (access_code[i] & 1);
89 }
90
91 GR_LOG_DEBUG(d_logger, boost::format("Access code: %llx") % d_access_code);
92 GR_LOG_DEBUG(d_logger, boost::format("Mask: %llx") % d_mask);
93
94 return true;
95 }
96
access_code() const97 unsigned long long correlate_access_code_ff_ts_impl::access_code() const
98 {
99 return d_access_code;
100 }
101
enter_search()102 inline void correlate_access_code_ff_ts_impl::enter_search()
103 {
104 d_state = STATE_SYNC_SEARCH;
105 d_data_reg_bits = 0;
106 }
107
enter_have_sync()108 inline void correlate_access_code_ff_ts_impl::enter_have_sync()
109 {
110 d_state = STATE_HAVE_SYNC;
111 d_hdr_reg = 0;
112 d_hdr_count = 0;
113 }
114
enter_have_header(int payload_len)115 inline void correlate_access_code_ff_ts_impl::enter_have_header(int payload_len)
116 {
117 d_state = STATE_HAVE_HEADER;
118 d_pkt_len = 8 * payload_len;
119 d_pkt_count = 0;
120 }
121
header_ok()122 bool correlate_access_code_ff_ts_impl::header_ok()
123 {
124 // confirm that two copies of header info are identical
125 return ((d_hdr_reg >> 16) ^ (d_hdr_reg & 0xffff)) == 0;
126 }
127
header_payload()128 int correlate_access_code_ff_ts_impl::header_payload()
129 {
130 return (d_hdr_reg >> 16) & 0x0fff;
131 }
132
general_work(int noutput_items,gr_vector_int & ninput_items,gr_vector_const_void_star & input_items,gr_vector_void_star & output_items)133 int correlate_access_code_ff_ts_impl::general_work(int noutput_items,
134 gr_vector_int& ninput_items,
135 gr_vector_const_void_star& input_items,
136 gr_vector_void_star& output_items)
137 {
138 const float* in = (const float*)input_items[0];
139 float* out = (float*)output_items[0];
140
141 uint64_t abs_out_sample_cnt = nitems_written(0);
142
143 int nprod = 0;
144
145 int count = 0;
146 while (count < noutput_items) {
147 switch (d_state) {
148 case STATE_SYNC_SEARCH: // Look for the access code correlation
149
150 while (count < noutput_items) {
151 // shift in new data
152 d_data_reg =
153 (d_data_reg << 1) | (gr::branchless_binary_slicer(in[count++]) & 0x1);
154 if (d_data_reg_bits + 1 < d_len) {
155 d_data_reg_bits++;
156 continue;
157 }
158 // compute hamming distance between desired access code and current
159 // data
160 uint64_t wrong_bits = 0;
161 uint64_t nwrong = d_threshold + 1;
162
163 wrong_bits = (d_data_reg ^ d_access_code) & d_mask;
164 volk_64u_popcnt(&nwrong, wrong_bits);
165
166 if (nwrong <= d_threshold) {
167 enter_have_sync();
168 break;
169 }
170 }
171 break;
172
173 case STATE_HAVE_SYNC:
174 while (count < noutput_items) { // Shift bits one at a time into header
175 d_hdr_reg =
176 (d_hdr_reg << 1) | (gr::branchless_binary_slicer(in[count++]) & 0x1);
177 d_hdr_count++;
178
179 if (d_hdr_count == 32) {
180 // we have a full header, check to see if it has been received
181 // properly
182 if (header_ok()) {
183 int payload_len = header_payload();
184 enter_have_header(payload_len);
185 } else {
186 enter_search(); // bad header
187 }
188 break;
189 }
190 }
191 break;
192
193 case STATE_HAVE_HEADER:
194 if (d_pkt_count == 0) {
195 // MAKE A TAG OUT OF THIS AND UPDATE OFFSET
196 add_item_tag(0, // stream ID
197 abs_out_sample_cnt + nprod, // sample
198 d_key, // length key
199 pmt::from_long(d_pkt_len), // length data
200 d_me); // block src id
201 }
202
203 while (count < noutput_items) {
204 if (d_pkt_count < d_pkt_len) {
205 out[nprod++] = in[count++];
206 d_pkt_count++;
207 } else {
208 enter_search();
209 break;
210 }
211 }
212 break;
213 }
214 }
215
216 consume_each(noutput_items);
217 return nprod;
218 }
219
220 } /* namespace digital */
221 } /* namespace gr */
222