Gonzalo Arcos
0f56254df3
SSE2 and improves the performance significantly. The decoder was taken from the gr-dvbt module from GNU Radio. - Disabled some logging functions in the ofdm_mac and ofdm_decode_signal blocks that were degrading the performance significantly. [Gonzalo Arcos | Rodrigo Ferreri]
501 lines
12 KiB
C++
501 lines
12 KiB
C++
/*
|
|
* Copyright (C) 2013 Bastian Bloessl <bloessl@ccs-labs.org>
|
|
*
|
|
* This program 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 of the License, or
|
|
* (at your option) any later version.
|
|
*
|
|
* This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
#include <ieee802-11/ofdm_decode_mac.h>
|
|
|
|
#include "utils.h"
|
|
|
|
#include <boost/crc.hpp>
|
|
#include <gnuradio/io_signature.h>
|
|
#include <itpp/itcomm.h>
|
|
#include <iomanip>
|
|
|
|
|
|
static __m128i metric0[4] __attribute__ ((aligned(16)));
|
|
static __m128i metric1[4] __attribute__ ((aligned(16)));
|
|
static __m128i path0[4] __attribute__ ((aligned(16)));
|
|
static __m128i path1[4] __attribute__ ((aligned(16)));
|
|
struct viterbi_state state0[64];
|
|
struct viterbi_state state1[64];
|
|
static int d_init = 0;
|
|
|
|
int decoded_count;
|
|
|
|
using namespace gr::ieee802_11;
|
|
using namespace itpp;
|
|
|
|
class ofdm_decode_mac_impl : public ofdm_decode_mac {
|
|
|
|
public:
|
|
ofdm_decode_mac_impl(bool log, bool debug) : block("ofdm_decode_mac",
|
|
gr::io_signature::make(1, 1, 48 * sizeof(gr_complex)),
|
|
gr::io_signature::make(0, 0, 0)),
|
|
d_log(log),
|
|
d_debug(debug),
|
|
d_ofdm(BPSK_1_2),
|
|
d_tx(d_ofdm, 0),
|
|
d_frame_complete(true) {
|
|
|
|
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) {
|
|
|
|
const gr_complex *in = (const gr_complex*)input_items[0];
|
|
|
|
int i = 0;
|
|
|
|
std::vector<gr::tag_t> tags;
|
|
const uint64_t nread = this->nitems_read(0);
|
|
|
|
dout << "Decode MAC: input " << ninput_items[0] << std::endl;
|
|
|
|
while(i < ninput_items[0]) {
|
|
|
|
get_tags_in_range(tags, 0, nread + i, nread + i + 1,
|
|
pmt::string_to_symbol("ofdm_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_tx.n_sym << " symbols of last frame" << std::endl;
|
|
}
|
|
d_frame_complete = false;
|
|
|
|
pmt::pmt_t tuple = tags[0].value;
|
|
int len_data = pmt::to_uint64(pmt::car(tuple));
|
|
int encoding = pmt::to_uint64(pmt::cdr(tuple));
|
|
|
|
ofdm_param ofdm = ofdm_param((Encoding)encoding);
|
|
tx_param tx = tx_param(ofdm, len_data);
|
|
|
|
// sanity check, more than MAX_SYM may indicate garbage
|
|
if(tx.n_sym <= MAX_SYM && tx.n_encoded_bits <= MAX_BITS) {
|
|
d_ofdm = ofdm;
|
|
d_tx = tx;
|
|
copied = 0;
|
|
dout << "Decode MAC: frame start -- len " << len_data
|
|
<< " symbols " << tx.n_sym << " encoding "
|
|
<< encoding << std::endl;
|
|
} else {
|
|
dout << "Dropping frame which is too large (symbols or bits)" << std::endl;
|
|
}
|
|
}
|
|
|
|
if(copied < d_tx.n_sym) {
|
|
dout << "copy one symbol, copied " << copied << " out of " << d_tx.n_sym << std::endl;
|
|
std::memcpy(sym + (copied * 48), in, 48 * sizeof(gr_complex));
|
|
copied++;
|
|
|
|
if(copied == d_tx.n_sym) {
|
|
dout << "received complete frame - decoding" << std::endl;
|
|
decode();
|
|
in += 48;
|
|
i++;
|
|
d_frame_complete = true;
|
|
break;
|
|
}
|
|
}
|
|
|
|
in += 48;
|
|
i++;
|
|
}
|
|
|
|
consume(0, i);
|
|
return 0;
|
|
}
|
|
|
|
void decode() {
|
|
demodulate();
|
|
deinterleave();
|
|
decode_conv();
|
|
descramble();
|
|
print_output();
|
|
|
|
// skip service field
|
|
boost::crc_32_type result;
|
|
result.process_bytes(out_bytes + 2, d_tx.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_tx.psdu_size % d_tx.n_sym);
|
|
|
|
// create PDU
|
|
pmt::pmt_t blob = pmt::make_blob(out_bytes + 2, d_tx.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);
|
|
message_port_pub(pmt::mp("out"), pmt::cons(dict, blob));
|
|
}
|
|
|
|
/***** Auxiliary functions used for demodulation *****/
|
|
// BPSK
|
|
bvec demodulate_bpsk(const cvec &signal) const
|
|
{
|
|
|
|
bvec out(signal.size());
|
|
out.set_size(signal.size(), false);
|
|
|
|
for (int i = 0; i < signal.length(); i++) {
|
|
out(i) = bin(signal(i).real() > 0);
|
|
}
|
|
|
|
return out;
|
|
}
|
|
|
|
// QPSK
|
|
bvec demodulate_qpsk(const cvec &signal) const
|
|
{
|
|
|
|
int k = 2;
|
|
|
|
bvec out(k * signal.size());
|
|
out.set_size(k * signal.size(), false);
|
|
|
|
for (int i = 0; i < signal.size(); i++) {
|
|
out(k*i) = bin(signal(i).real() > 0);
|
|
out(k*i + 1) = bin(signal(i).imag() > 0);
|
|
}
|
|
|
|
return out;
|
|
}
|
|
|
|
// 16QAM
|
|
bvec demodulate_16qam(const cvec &signal) const
|
|
{
|
|
|
|
int k = 4;
|
|
|
|
bvec out(k * signal.size());
|
|
out.set_size(k * signal.size(), false);
|
|
|
|
for (int i = 0; i < signal.size(); i++) {
|
|
out(k*i) = bin(signal(i).real() > 0);
|
|
out(k*i + 1) = bin(std::abs(signal(i).real()) < 0.63245);
|
|
out(k*i + 2) = bin(signal(i).imag() > 0);
|
|
out(k*i + 3) = bin(std::abs(signal(i).imag()) < 0.63245);
|
|
}
|
|
|
|
return out;
|
|
}
|
|
|
|
// 64QAM
|
|
bvec demodulate_64qam(const cvec &signal) const
|
|
{
|
|
|
|
int k = 6;
|
|
|
|
bvec out(k * signal.size());
|
|
out.set_size(k * signal.size(), false);
|
|
|
|
for (int i = 0; i < signal.size(); i++) {
|
|
out(k*i) = bin(signal(i).real() > 0);
|
|
out(k*i + 1) = bin(std::abs(signal(i).real()) < 0.6172);
|
|
out(k*i + 2) = bin(std::abs(signal(i).real()) < 0.9258 && std::abs(signal(i).real()) > 0.3086);
|
|
out(k*i + 3) = bin(signal(i).imag() > 0);
|
|
out(k*i + 4) = bin(std::abs(signal(i).imag()) < 0.6172);
|
|
out(k*i + 5) = bin(std::abs(signal(i).imag()) < 0.9258 && std::abs(signal(i).imag()) > 0.3086);
|
|
}
|
|
|
|
return out;
|
|
}
|
|
|
|
|
|
void demodulate() {
|
|
|
|
cvec symbols;
|
|
symbols.set_length(d_tx.n_sym * 48);
|
|
for(int i = 0; i < d_tx.n_sym * 48; i++) {
|
|
symbols[i] = std::complex<double>(sym[i]);
|
|
}
|
|
|
|
switch(d_ofdm.encoding) {
|
|
case BPSK_1_2:
|
|
case BPSK_3_4:
|
|
bits = to_vec(demodulate_bpsk(symbols));
|
|
break;
|
|
|
|
case QPSK_1_2:
|
|
case QPSK_3_4:
|
|
bits = to_vec(demodulate_qpsk(symbols));
|
|
break;
|
|
|
|
case QAM16_1_2:
|
|
case QAM16_3_4:
|
|
bits = to_vec(demodulate_16qam(symbols));
|
|
break;
|
|
case QAM64_2_3:
|
|
case QAM64_3_4:
|
|
bits = to_vec(demodulate_64qam(symbols));
|
|
break;
|
|
}
|
|
}
|
|
|
|
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);
|
|
|
|
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));
|
|
}
|
|
|
|
int count = 0;
|
|
for(int i = 0; i < d_tx.n_sym; i++) {
|
|
for(int k = 0; k < n_cbps; k++) {
|
|
if (bits[i * n_cbps + k] > 0)
|
|
{
|
|
d_tmp_inbits[i * n_cbps + second[first[k]]] = 0x01;
|
|
}
|
|
else
|
|
{
|
|
d_tmp_inbits[i * n_cbps + second[first[k]]] = 0x00;
|
|
}
|
|
}
|
|
}
|
|
|
|
// Set the appropiate coding rate for
|
|
// the convolutional decoder.
|
|
set_coding_rate();
|
|
|
|
if (d_ntraceback == 5)
|
|
{
|
|
count = d_tx.n_sym * n_cbps;
|
|
d_inbits = d_tmp_inbits;
|
|
}
|
|
else
|
|
{
|
|
d_inbits = d_inbits_punctured;
|
|
count = 0;
|
|
for(int i = 0; i < d_tx.n_sym; i++) {
|
|
for(int k = 0; k < n_cbps; k++) {
|
|
while (d_puncture[count % (2 * d_k)] == 0)
|
|
{
|
|
d_inbits[count] = 2;
|
|
count++;
|
|
}
|
|
|
|
// Insert received bits
|
|
d_inbits[count] = d_tmp_inbits[i * n_cbps + k];
|
|
count++;
|
|
|
|
while (d_puncture[count % (2 * d_k)] == 0)
|
|
{
|
|
d_inbits[count] = 2;
|
|
count++;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
for (int i = count; i < NeededBits(); i++)
|
|
d_inbits[i] = 0x00;
|
|
}
|
|
|
|
int NeededBits()
|
|
{
|
|
int return_value;
|
|
if (d_ntraceback == 5)
|
|
return_value = d_tx.n_encoded_bits / 2;
|
|
else if (d_ntraceback == 9)
|
|
{
|
|
return_value = (d_tx.n_encoded_bits / 3) * 2;
|
|
}
|
|
else // si d_ntraceback == 10
|
|
{
|
|
return_value = (d_tx.n_encoded_bits / 4) * 3;
|
|
}
|
|
return return_value;
|
|
}
|
|
|
|
void decode_conv_viterbi() {
|
|
|
|
d_viterbi_chunks_init(state0);
|
|
d_viterbi_chunks_init_sse2(metric0, path0);
|
|
|
|
int out_count = 0;
|
|
decoded_count = 0;
|
|
|
|
int in_count = 0;
|
|
int neededBits = NeededBits();
|
|
while(decoded_count < neededBits)
|
|
{
|
|
if ((in_count % 4) == 0) //0 or 3
|
|
{
|
|
d_viterbi_butterfly2_sse2(&d_inbits[in_count & 0xfffffffc], metric0, metric1, path0, path1);
|
|
|
|
if ((in_count > 0) && (in_count % 16) == 8) // 8 or 11
|
|
{
|
|
unsigned char c;
|
|
|
|
d_viterbi_get_output_sse2(metric0, path0, d_ntraceback, &c);
|
|
|
|
if (d_init == 0)
|
|
{
|
|
if (out_count >= d_ntraceback)
|
|
{
|
|
for (int i= 0; i < 8; i++)
|
|
{
|
|
out_bytes_viterbi[(out_count - d_ntraceback) * 8 + i] = (c >> (7 - i)) & 0x1;
|
|
decoded_count++;
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
out_bytes_viterbi[out_count] = c;
|
|
for (int i= 0; i < 8; i++)
|
|
{
|
|
out_bytes_viterbi[out_count * 8 + i] = (c >> (7 - i)) & 0x1;
|
|
decoded_count++;
|
|
}
|
|
}
|
|
out_count++;
|
|
}
|
|
}
|
|
in_count++;
|
|
}
|
|
}
|
|
|
|
void set_coding_rate()
|
|
{
|
|
switch(d_ofdm.encoding) {
|
|
case BPSK_1_2:
|
|
case QPSK_1_2:
|
|
case QAM16_1_2:
|
|
// Code rate 1/2
|
|
d_ntraceback = 5;
|
|
d_puncture = d_puncture_1_2;
|
|
d_k = 1;
|
|
break;
|
|
case QAM64_2_3:
|
|
// Code rate 2/3
|
|
d_ntraceback = 9;
|
|
d_puncture = d_puncture_2_3;
|
|
d_k = 2;
|
|
break;
|
|
case BPSK_3_4:
|
|
case QPSK_3_4:
|
|
case QAM16_3_4:
|
|
case QAM64_3_4:
|
|
// Code rate 3/4
|
|
d_ntraceback = 10;
|
|
d_puncture = d_puncture_3_4;
|
|
d_k = 3;
|
|
break;
|
|
}
|
|
}
|
|
|
|
void decode_conv() {
|
|
decode_conv_viterbi();
|
|
}
|
|
|
|
void descramble () {
|
|
int state = 0;
|
|
for(int i = 0; i < 7; i++) {
|
|
if(out_bytes_viterbi[i]) {
|
|
state |= 1 << (6 - i);
|
|
}
|
|
}
|
|
|
|
int feedback;
|
|
|
|
for(int i = 7; i < decoded_count; i++) {
|
|
feedback = ((!!(state & 64))) ^ (!!(state & 8));
|
|
out_bits[i] = feedback ^ (out_bytes_viterbi[i] & 0x1);
|
|
state = ((state << 1) & 0x7e) | feedback;
|
|
}
|
|
|
|
for(int i = 0; i < decoded_count; i++) {
|
|
int bit = i % 8;
|
|
int byte = i / 8;
|
|
if(bit == 0) {
|
|
out_bytes[byte] = 0;
|
|
}
|
|
|
|
if(out_bits[i]) {
|
|
out_bytes[byte] |= (1 << bit);
|
|
}
|
|
}
|
|
}
|
|
|
|
void print_output() {
|
|
|
|
dout << std::endl;
|
|
for(int i = 0; i < decoded_bits.size() / 8; 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 = 0; i < decoded_bits.size() / 8; i++) {
|
|
if((out_bytes[i] > 31) && (out_bytes[i] < 127)) {
|
|
dout << ((char) out_bytes[i]);
|
|
} else {
|
|
dout << ".";
|
|
}
|
|
}
|
|
dout << std::endl;
|
|
}
|
|
|
|
private:
|
|
gr_complex sym[1000 * 48 * 100];
|
|
vec bits;
|
|
unsigned char* d_inbits;
|
|
unsigned char d_inbits_punctured[1000 * 48];
|
|
unsigned char d_tmp_inbits[1000 * 48];
|
|
char out_bits[40000];
|
|
char out_bytes[40000];
|
|
char out_bytes_viterbi[40000 * 8];
|
|
bvec decoded_bits;
|
|
|
|
int d_ntraceback;
|
|
|
|
// number of input bits in the convolutional encoder.
|
|
int d_k;
|
|
|
|
const unsigned char * d_puncture;
|
|
const unsigned char d_puncture_1_2[2] = {1, 1};
|
|
const unsigned char d_puncture_2_3[4] = {1, 1, 1, 0};
|
|
const unsigned char d_puncture_3_4[6] = {1, 1, 1, 0, 0, 1};
|
|
|
|
bool d_debug;
|
|
bool d_log;
|
|
tx_param d_tx;
|
|
ofdm_param d_ofdm;
|
|
int copied;
|
|
bool d_frame_complete;
|
|
};
|
|
|
|
ofdm_decode_mac::sptr
|
|
ofdm_decode_mac::make(bool log, bool debug) {
|
|
return gnuradio::get_initial_sptr(new ofdm_decode_mac_impl(log, debug));
|
|
}
|
|
|