clang format, similar to GNU Radio
This commit is contained in:
Bastian Bloessl
+201
-180
@@ -19,234 +19,255 @@
|
||||
#include "utils.h"
|
||||
#include "viterbi_decoder/viterbi_decoder.h"
|
||||
|
||||
#include <boost/crc.hpp>
|
||||
#include <gnuradio/io_signature.h>
|
||||
#include <boost/crc.hpp>
|
||||
|
||||
using namespace gr::ieee802_11;
|
||||
|
||||
#define LINKTYPE_IEEE802_11 105 /* http://www.tcpdump.org/linktypes.html */
|
||||
|
||||
class decode_mac_impl : public decode_mac {
|
||||
class decode_mac_impl : public decode_mac
|
||||
{
|
||||
|
||||
public:
|
||||
decode_mac_impl(bool log, bool debug) :
|
||||
block("decode_mac",
|
||||
gr::io_signature::make(1, 1, 48),
|
||||
gr::io_signature::make(0, 0, 0)),
|
||||
d_log(log),
|
||||
d_debug(debug),
|
||||
d_snr(0),
|
||||
d_nom_freq(0.0),
|
||||
d_freq_offset(0.0),
|
||||
d_ofdm(BPSK_1_2),
|
||||
d_frame(d_ofdm, 0),
|
||||
d_frame_complete(true) {
|
||||
decode_mac_impl(bool log, bool debug)
|
||||
: block("decode_mac",
|
||||
gr::io_signature::make(1, 1, 48),
|
||||
gr::io_signature::make(0, 0, 0)),
|
||||
d_log(log),
|
||||
d_debug(debug),
|
||||
d_snr(0),
|
||||
d_nom_freq(0.0),
|
||||
d_freq_offset(0.0),
|
||||
d_ofdm(BPSK_1_2),
|
||||
d_frame(d_ofdm, 0),
|
||||
d_frame_complete(true)
|
||||
{
|
||||
|
||||
message_port_register_out(pmt::mp("out"));
|
||||
}
|
||||
message_port_register_out(pmt::mp("out"));
|
||||
}
|
||||
|
||||
int general_work (int noutput_items, gr_vector_int& ninput_items,
|
||||
gr_vector_const_void_star& input_items,
|
||||
gr_vector_void_star& output_items) {
|
||||
int general_work(int noutput_items,
|
||||
gr_vector_int& ninput_items,
|
||||
gr_vector_const_void_star& input_items,
|
||||
gr_vector_void_star& output_items)
|
||||
{
|
||||
|
||||
const uint8_t *in = (const uint8_t*)input_items[0];
|
||||
const uint8_t* in = (const uint8_t*)input_items[0];
|
||||
|
||||
int i = 0;
|
||||
int i = 0;
|
||||
|
||||
std::vector<gr::tag_t> tags;
|
||||
const uint64_t nread = this->nitems_read(0);
|
||||
std::vector<gr::tag_t> tags;
|
||||
const uint64_t nread = this->nitems_read(0);
|
||||
|
||||
dout << "Decode MAC: input " << ninput_items[0] << std::endl;
|
||||
dout << "Decode MAC: input " << ninput_items[0] << std::endl;
|
||||
|
||||
while(i < ninput_items[0]) {
|
||||
while (i < ninput_items[0]) {
|
||||
|
||||
get_tags_in_range(tags, 0, nread + i, nread + i + 1,
|
||||
pmt::string_to_symbol("wifi_start"));
|
||||
get_tags_in_range(
|
||||
tags, 0, nread + i, nread + i + 1, pmt::string_to_symbol("wifi_start"));
|
||||
|
||||
if(tags.size()) {
|
||||
if (d_frame_complete == false) {
|
||||
dout << "Warning: starting to receive new frame before old frame was complete" << std::endl;
|
||||
dout << "Already copied " << copied << " out of " << d_frame.n_sym << " symbols of last frame" << std::endl;
|
||||
}
|
||||
d_frame_complete = false;
|
||||
if (tags.size()) {
|
||||
if (d_frame_complete == false) {
|
||||
dout << "Warning: starting to receive new frame before old frame was "
|
||||
"complete"
|
||||
<< std::endl;
|
||||
dout << "Already copied " << copied << " out of " << d_frame.n_sym
|
||||
<< " symbols of last frame" << std::endl;
|
||||
}
|
||||
d_frame_complete = false;
|
||||
|
||||
pmt::pmt_t dict = tags[0].value;
|
||||
int len_data = pmt::to_uint64(pmt::dict_ref(dict, pmt::mp("frame_bytes"), pmt::from_uint64(MAX_PSDU_SIZE+1)));
|
||||
int encoding = pmt::to_uint64(pmt::dict_ref(dict, pmt::mp("encoding"), pmt::from_uint64(0)));
|
||||
d_snr = pmt::to_double(pmt::dict_ref(dict, pmt::mp("snr"), pmt::from_double(0)));
|
||||
d_nom_freq = pmt::to_double(pmt::dict_ref(dict, pmt::mp("freq"), pmt::from_double(0)));
|
||||
d_freq_offset = pmt::to_double(pmt::dict_ref(dict, pmt::mp("freq_offset"), pmt::from_double(0)));
|
||||
d_beta = pmt::to_double(pmt::dict_ref(dict, pmt::mp("beta"), pmt::from_double(0)));
|
||||
d_csi = pmt::c32vector_elements(pmt::dict_ref(dict, pmt::mp("csi"), pmt::make_c32vector(52, 0)));
|
||||
ofdm_param ofdm = ofdm_param((Encoding)encoding);
|
||||
frame_param frame = frame_param(ofdm, len_data);
|
||||
pmt::pmt_t dict = tags[0].value;
|
||||
int len_data = pmt::to_uint64(pmt::dict_ref(
|
||||
dict, pmt::mp("frame_bytes"), pmt::from_uint64(MAX_PSDU_SIZE + 1)));
|
||||
int encoding = pmt::to_uint64(
|
||||
pmt::dict_ref(dict, pmt::mp("encoding"), pmt::from_uint64(0)));
|
||||
d_snr = pmt::to_double(
|
||||
pmt::dict_ref(dict, pmt::mp("snr"), pmt::from_double(0)));
|
||||
d_nom_freq = pmt::to_double(
|
||||
pmt::dict_ref(dict, pmt::mp("freq"), pmt::from_double(0)));
|
||||
d_freq_offset = pmt::to_double(
|
||||
pmt::dict_ref(dict, pmt::mp("freq_offset"), pmt::from_double(0)));
|
||||
d_beta = pmt::to_double(
|
||||
pmt::dict_ref(dict, pmt::mp("beta"), pmt::from_double(0)));
|
||||
d_csi = pmt::c32vector_elements(
|
||||
pmt::dict_ref(dict, pmt::mp("csi"), pmt::make_c32vector(52, 0)));
|
||||
ofdm_param ofdm = ofdm_param((Encoding)encoding);
|
||||
frame_param frame = frame_param(ofdm, len_data);
|
||||
|
||||
// check for maximum frame size
|
||||
if(frame.n_sym <= MAX_SYM && frame.psdu_size <= MAX_PSDU_SIZE) {
|
||||
d_ofdm = ofdm;
|
||||
d_frame = frame;
|
||||
copied = 0;
|
||||
dout << "Decode MAC: frame start -- len " << len_data
|
||||
<< " symbols " << frame.n_sym << " encoding "
|
||||
<< encoding << std::endl;
|
||||
} else {
|
||||
dout << "Dropping frame which is too large (symbols or bits)" << std::endl;
|
||||
}
|
||||
}
|
||||
// check for maximum frame size
|
||||
if (frame.n_sym <= MAX_SYM && frame.psdu_size <= MAX_PSDU_SIZE) {
|
||||
d_ofdm = ofdm;
|
||||
d_frame = frame;
|
||||
copied = 0;
|
||||
dout << "Decode MAC: frame start -- len " << len_data << " symbols "
|
||||
<< frame.n_sym << " encoding " << encoding << std::endl;
|
||||
} else {
|
||||
dout << "Dropping frame which is too large (symbols or bits)"
|
||||
<< std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
if(copied < d_frame.n_sym) {
|
||||
dout << "copy one symbol, copied " << copied << " out of " << d_frame.n_sym << std::endl;
|
||||
std::memcpy(d_rx_symbols + (copied * 48), in, 48);
|
||||
copied++;
|
||||
if (copied < d_frame.n_sym) {
|
||||
dout << "copy one symbol, copied " << copied << " out of "
|
||||
<< d_frame.n_sym << std::endl;
|
||||
std::memcpy(d_rx_symbols + (copied * 48), in, 48);
|
||||
copied++;
|
||||
|
||||
if(copied == d_frame.n_sym) {
|
||||
dout << "received complete frame - decoding" << std::endl;
|
||||
decode();
|
||||
in += 48;
|
||||
i++;
|
||||
d_frame_complete = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (copied == d_frame.n_sym) {
|
||||
dout << "received complete frame - decoding" << std::endl;
|
||||
decode();
|
||||
in += 48;
|
||||
i++;
|
||||
d_frame_complete = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
in += 48;
|
||||
i++;
|
||||
}
|
||||
in += 48;
|
||||
i++;
|
||||
}
|
||||
|
||||
consume(0, i);
|
||||
consume(0, i);
|
||||
|
||||
return 0;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
void decode() {
|
||||
void decode()
|
||||
{
|
||||
|
||||
for(int i = 0; i < d_frame.n_sym * 48; i++) {
|
||||
for(int k = 0; k < d_ofdm.n_bpsc; k++) {
|
||||
d_rx_bits[i*d_ofdm.n_bpsc + k] = !!(d_rx_symbols[i] & (1 << k));
|
||||
}
|
||||
}
|
||||
for (int i = 0; i < d_frame.n_sym * 48; i++) {
|
||||
for (int k = 0; k < d_ofdm.n_bpsc; k++) {
|
||||
d_rx_bits[i * d_ofdm.n_bpsc + k] = !!(d_rx_symbols[i] & (1 << k));
|
||||
}
|
||||
}
|
||||
|
||||
deinterleave();
|
||||
uint8_t *decoded = d_decoder.decode(&d_ofdm, &d_frame, d_deinterleaved_bits);
|
||||
descramble(decoded);
|
||||
print_output();
|
||||
deinterleave();
|
||||
uint8_t* decoded = d_decoder.decode(&d_ofdm, &d_frame, d_deinterleaved_bits);
|
||||
descramble(decoded);
|
||||
print_output();
|
||||
|
||||
// skip service field
|
||||
boost::crc_32_type result;
|
||||
result.process_bytes(out_bytes + 2, d_frame.psdu_size);
|
||||
if(result.checksum() != 558161692) {
|
||||
dout << "checksum wrong -- dropping" << std::endl;
|
||||
return;
|
||||
}
|
||||
// skip service field
|
||||
boost::crc_32_type result;
|
||||
result.process_bytes(out_bytes + 2, d_frame.psdu_size);
|
||||
if (result.checksum() != 558161692) {
|
||||
dout << "checksum wrong -- dropping" << std::endl;
|
||||
return;
|
||||
}
|
||||
|
||||
mylog(boost::format("encoding: %1% - length: %2% - symbols: %3%")
|
||||
% d_ofdm.encoding % d_frame.psdu_size % d_frame.n_sym);
|
||||
mylog(boost::format("encoding: %1% - length: %2% - symbols: %3%") %
|
||||
d_ofdm.encoding % d_frame.psdu_size % d_frame.n_sym);
|
||||
|
||||
// create PDU
|
||||
pmt::pmt_t blob = pmt::make_blob(out_bytes + 2, d_frame.psdu_size - 4);
|
||||
pmt::pmt_t enc = pmt::from_uint64(d_ofdm.encoding);
|
||||
pmt::pmt_t dict = pmt::make_dict();
|
||||
dict = pmt::dict_add(dict, pmt::mp("encoding"), enc);
|
||||
dict = pmt::dict_add(dict, pmt::mp("snr"), pmt::from_double(d_snr));
|
||||
dict = pmt::dict_add(dict, pmt::mp("nomfreq"), pmt::from_double(d_nom_freq));
|
||||
dict = pmt::dict_add(dict, pmt::mp("freqofs"), pmt::from_double(d_freq_offset));
|
||||
dict = pmt::dict_add(dict, pmt::mp("beta"), pmt::from_double(d_beta));
|
||||
dict = pmt::dict_add(dict, pmt::mp("csi"), pmt::init_c32vector(52, d_csi));
|
||||
dict = pmt::dict_add(dict, pmt::mp("dlt"), pmt::from_long(LINKTYPE_IEEE802_11));
|
||||
message_port_pub(pmt::mp("out"), pmt::cons(dict, blob));
|
||||
}
|
||||
// create PDU
|
||||
pmt::pmt_t blob = pmt::make_blob(out_bytes + 2, d_frame.psdu_size - 4);
|
||||
pmt::pmt_t enc = pmt::from_uint64(d_ofdm.encoding);
|
||||
pmt::pmt_t dict = pmt::make_dict();
|
||||
dict = pmt::dict_add(dict, pmt::mp("encoding"), enc);
|
||||
dict = pmt::dict_add(dict, pmt::mp("snr"), pmt::from_double(d_snr));
|
||||
dict = pmt::dict_add(dict, pmt::mp("nomfreq"), pmt::from_double(d_nom_freq));
|
||||
dict = pmt::dict_add(dict, pmt::mp("freqofs"), pmt::from_double(d_freq_offset));
|
||||
dict = pmt::dict_add(dict, pmt::mp("beta"), pmt::from_double(d_beta));
|
||||
dict = pmt::dict_add(dict, pmt::mp("csi"), pmt::init_c32vector(52, d_csi));
|
||||
dict = pmt::dict_add(dict, pmt::mp("dlt"), pmt::from_long(LINKTYPE_IEEE802_11));
|
||||
message_port_pub(pmt::mp("out"), pmt::cons(dict, blob));
|
||||
}
|
||||
|
||||
void deinterleave() {
|
||||
void deinterleave()
|
||||
{
|
||||
|
||||
int n_cbps = d_ofdm.n_cbps;
|
||||
int first[n_cbps];
|
||||
int second[n_cbps];
|
||||
int s = std::max(d_ofdm.n_bpsc / 2, 1);
|
||||
int n_cbps = d_ofdm.n_cbps;
|
||||
int first[n_cbps];
|
||||
int second[n_cbps];
|
||||
int s = std::max(d_ofdm.n_bpsc / 2, 1);
|
||||
|
||||
for(int j = 0; j < n_cbps; j++) {
|
||||
first[j] = s * (j / s) + ((j + int(floor(16.0 * j / n_cbps))) % s);
|
||||
}
|
||||
for (int j = 0; j < n_cbps; j++) {
|
||||
first[j] = s * (j / s) + ((j + int(floor(16.0 * j / n_cbps))) % s);
|
||||
}
|
||||
|
||||
for(int i = 0; i < n_cbps; i++) {
|
||||
second[i] = 16 * i - (n_cbps - 1) * int(floor(16.0 * i / n_cbps));
|
||||
}
|
||||
for (int i = 0; i < n_cbps; i++) {
|
||||
second[i] = 16 * i - (n_cbps - 1) * int(floor(16.0 * i / n_cbps));
|
||||
}
|
||||
|
||||
int count = 0;
|
||||
for(int i = 0; i < d_frame.n_sym; i++) {
|
||||
for(int k = 0; k < n_cbps; k++) {
|
||||
d_deinterleaved_bits[i * n_cbps + second[first[k]]] = d_rx_bits[i * n_cbps + k];
|
||||
}
|
||||
}
|
||||
}
|
||||
int count = 0;
|
||||
for (int i = 0; i < d_frame.n_sym; i++) {
|
||||
for (int k = 0; k < n_cbps; k++) {
|
||||
d_deinterleaved_bits[i * n_cbps + second[first[k]]] =
|
||||
d_rx_bits[i * n_cbps + k];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void descramble (uint8_t *decoded_bits) {
|
||||
void descramble(uint8_t* decoded_bits)
|
||||
{
|
||||
|
||||
int state = 0;
|
||||
std::memset(out_bytes, 0, d_frame.psdu_size+2);
|
||||
int state = 0;
|
||||
std::memset(out_bytes, 0, d_frame.psdu_size + 2);
|
||||
|
||||
for(int i = 0; i < 7; i++) {
|
||||
if(decoded_bits[i]) {
|
||||
state |= 1 << (6 - i);
|
||||
}
|
||||
}
|
||||
out_bytes[0] = state;
|
||||
for (int i = 0; i < 7; i++) {
|
||||
if (decoded_bits[i]) {
|
||||
state |= 1 << (6 - i);
|
||||
}
|
||||
}
|
||||
out_bytes[0] = state;
|
||||
|
||||
int feedback;
|
||||
int bit;
|
||||
int feedback;
|
||||
int bit;
|
||||
|
||||
for(int i = 7; i < d_frame.psdu_size*8+16; i++) {
|
||||
feedback = ((!!(state & 64))) ^ (!!(state & 8));
|
||||
bit = feedback ^ (decoded_bits[i] & 0x1);
|
||||
out_bytes[i/8] |= bit << (i%8);
|
||||
state = ((state << 1) & 0x7e) | feedback;
|
||||
}
|
||||
}
|
||||
for (int i = 7; i < d_frame.psdu_size * 8 + 16; i++) {
|
||||
feedback = ((!!(state & 64))) ^ (!!(state & 8));
|
||||
bit = feedback ^ (decoded_bits[i] & 0x1);
|
||||
out_bytes[i / 8] |= bit << (i % 8);
|
||||
state = ((state << 1) & 0x7e) | feedback;
|
||||
}
|
||||
}
|
||||
|
||||
void print_output() {
|
||||
void print_output()
|
||||
{
|
||||
|
||||
dout << std::endl;
|
||||
dout << "psdu size" << d_frame.psdu_size << std::endl;
|
||||
for(int i = 2; i < d_frame.psdu_size+2; i++) {
|
||||
dout << std::setfill('0') << std::setw(2) << std::hex << ((unsigned int)out_bytes[i] & 0xFF) << std::dec << " ";
|
||||
if(i % 16 == 15) {
|
||||
dout << std::endl;
|
||||
}
|
||||
}
|
||||
dout << std::endl;
|
||||
for(int i = 2; i < d_frame.psdu_size+2; i++) {
|
||||
if((out_bytes[i] > 31) && (out_bytes[i] < 127)) {
|
||||
dout << ((char) out_bytes[i]);
|
||||
} else {
|
||||
dout << ".";
|
||||
}
|
||||
}
|
||||
dout << std::endl;
|
||||
}
|
||||
dout << std::endl;
|
||||
dout << "psdu size" << d_frame.psdu_size << std::endl;
|
||||
for (int i = 2; i < d_frame.psdu_size + 2; i++) {
|
||||
dout << std::setfill('0') << std::setw(2) << std::hex
|
||||
<< ((unsigned int)out_bytes[i] & 0xFF) << std::dec << " ";
|
||||
if (i % 16 == 15) {
|
||||
dout << std::endl;
|
||||
}
|
||||
}
|
||||
dout << std::endl;
|
||||
for (int i = 2; i < d_frame.psdu_size + 2; i++) {
|
||||
if ((out_bytes[i] > 31) && (out_bytes[i] < 127)) {
|
||||
dout << ((char)out_bytes[i]);
|
||||
} else {
|
||||
dout << ".";
|
||||
}
|
||||
}
|
||||
dout << std::endl;
|
||||
}
|
||||
|
||||
private:
|
||||
bool d_debug;
|
||||
bool d_log;
|
||||
bool d_debug;
|
||||
bool d_log;
|
||||
|
||||
frame_param d_frame;
|
||||
ofdm_param d_ofdm;
|
||||
double d_snr; // dB
|
||||
double d_nom_freq; // nominal frequency, Hz
|
||||
double d_freq_offset; // frequency offset, Hz
|
||||
double d_beta;
|
||||
std::vector<gr_complex> d_csi;
|
||||
viterbi_decoder d_decoder;
|
||||
frame_param d_frame;
|
||||
ofdm_param d_ofdm;
|
||||
double d_snr; // dB
|
||||
double d_nom_freq; // nominal frequency, Hz
|
||||
double d_freq_offset; // frequency offset, Hz
|
||||
double d_beta;
|
||||
std::vector<gr_complex> d_csi;
|
||||
viterbi_decoder d_decoder;
|
||||
|
||||
uint8_t d_rx_symbols[48 * MAX_SYM];
|
||||
uint8_t d_rx_bits[MAX_ENCODED_BITS];
|
||||
uint8_t d_deinterleaved_bits[MAX_ENCODED_BITS];
|
||||
uint8_t out_bytes[MAX_PSDU_SIZE + 2]; // 2 for signal field
|
||||
uint8_t d_rx_symbols[48 * MAX_SYM];
|
||||
uint8_t d_rx_bits[MAX_ENCODED_BITS];
|
||||
uint8_t d_deinterleaved_bits[MAX_ENCODED_BITS];
|
||||
uint8_t out_bytes[MAX_PSDU_SIZE + 2]; // 2 for signal field
|
||||
|
||||
int copied;
|
||||
bool d_frame_complete;
|
||||
int copied;
|
||||
bool d_frame_complete;
|
||||
};
|
||||
|
||||
decode_mac::sptr
|
||||
decode_mac::make(bool log, bool debug) {
|
||||
return gnuradio::get_initial_sptr(new decode_mac_impl(log, debug));
|
||||
decode_mac::sptr decode_mac::make(bool log, bool debug)
|
||||
{
|
||||
return gnuradio::get_initial_sptr(new decode_mac_impl(log, debug));
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user