1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
|
/* -*- c++ -*- */
/* Copyright 2015-2016 Free Software Foundation, Inc.
*
* This file is part of GNU Radio
*
* SPDX-License-Identifier: GPL-3.0-or-later
*
*/
#ifdef HAVE_CONFIG_H
#include "config.h"
#endif
#include <gnuradio/digital/header_format_default.h>
#include <gnuradio/math.h>
#include <volk/volk_alloc.hh>
#include <cstring>
namespace gr {
namespace digital {
header_format_default::sptr
header_format_default::make(const std::string& access_code, int threshold, int bps)
{
return header_format_default::sptr(
new header_format_default(access_code, threshold, bps));
}
header_format_default::header_format_default(const std::string& access_code,
int threshold,
int bps)
: header_format_base(),
d_bps(bps),
d_data_reg(0),
d_mask(0),
d_threshold(0),
d_pkt_len(0),
d_pkt_count(0),
d_nbits(0)
{
if (!set_access_code(access_code)) {
throw std::runtime_error("header_format_default: Setting access code failed");
}
set_threshold(threshold);
}
header_format_default::~header_format_default() {}
bool header_format_default::set_access_code(const std::string& access_code)
{
d_access_code_len = access_code.length(); // # of bits in the access code
if (access_code.size() > 64) {
return false;
}
// set len top bits to 1.
d_mask = ((~0ULL) >> (64 - d_access_code_len));
d_access_code = 0;
for (unsigned i = 0; i < d_access_code_len; i++) {
d_access_code = (d_access_code << 1) | (access_code[i] & 1);
}
return true;
}
unsigned long long header_format_default::access_code() const { return d_access_code; }
void header_format_default::set_threshold(unsigned int thresh)
{
if (d_threshold > d_access_code_len) {
throw std::runtime_error("header_format_default: Cannot set threshold "
"larger than the access code length.");
}
d_threshold = thresh;
}
unsigned int header_format_default::threshold() const { return d_threshold; }
bool header_format_default::format(int nbytes_in,
const unsigned char* input,
pmt::pmt_t& output,
pmt::pmt_t& info)
{
// Creating the output pmt copies data; free our own here when done.
volk::vector<uint8_t> bytes_out(header_nbytes());
header_buffer header(bytes_out.data());
header.add_field64(d_access_code, d_access_code_len);
header.add_field16((uint16_t)(nbytes_in));
header.add_field16((uint16_t)(nbytes_in));
// Package output data into a PMT vector
output = pmt::init_u8vector(header_nbytes(), bytes_out.data());
return true;
}
bool header_format_default::parse(int nbits_in,
const unsigned char* input,
std::vector<pmt::pmt_t>& info,
int& nbits_processed)
{
nbits_processed = 0;
while (nbits_processed < nbits_in) {
switch (d_state) {
case STATE_SYNC_SEARCH: // Look for the access code correlation
while (nbits_processed < nbits_in) {
// shift in new data
d_data_reg = (d_data_reg << 1) | ((input[nbits_processed++]) & 0x1);
// compute hamming distance between desired access code and current data
uint64_t wrong_bits = 0;
uint64_t nwrong = d_threshold + 1;
wrong_bits = (d_data_reg ^ d_access_code) & d_mask;
volk_64u_popcnt(&nwrong, wrong_bits);
if (nwrong <= d_threshold) {
enter_have_sync();
break;
}
}
break;
case STATE_HAVE_SYNC:
while (nbits_processed <= nbits_in) { // Shift bits one at a time into header
d_hdr_reg.insert_bit(input[nbits_processed++]);
if (d_hdr_reg.length() == (header_nbits() - d_access_code_len)) {
// we have a full header, check to see if it has been received
// properly
if (header_ok()) {
int payload_len = header_payload();
enter_have_header(payload_len);
info.push_back(d_info);
return true;
} else {
enter_search(); // bad header
return false;
}
break;
}
}
break;
}
}
return false;
}
size_t header_format_default::header_nbits() const
{
return d_access_code_len + 8 * 2 * sizeof(uint16_t);
}
inline void header_format_default::enter_have_sync()
{
d_state = STATE_HAVE_SYNC;
d_hdr_reg.clear();
}
inline void header_format_default::enter_have_header(int payload_len)
{
d_state = STATE_SYNC_SEARCH;
d_pkt_len = payload_len;
d_pkt_count = 0;
}
bool header_format_default::header_ok()
{
// confirm that two copies of header info are identical
uint16_t len0 = d_hdr_reg.extract_field16(0, 16);
uint16_t len1 = d_hdr_reg.extract_field16(16, 16);
return (len0 ^ len1) == 0;
}
int header_format_default::header_payload()
{
uint16_t len = d_hdr_reg.extract_field16(0, 16);
d_info = pmt::make_dict();
d_info = pmt::dict_add(
d_info, pmt::intern("payload symbols"), pmt::from_long(8 * len / d_bps));
return static_cast<int>(len);
}
} /* namespace digital */
} /* namespace gr */
|
