git.gag.com Git - debian/gnuradio/blob - gnuradio-core/src/lib/general/gr_ofdm_frame_sink.cc

   1 /* -*- c++ -*- */
   2 /*
   3  * Copyright 2007 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 <gr_ofdm_frame_sink.h>
  28 #include <gr_io_signature.h>
  29 #include <cstdio>
  30 #include <stdexcept>
  31
  32 #define VERBOSE 0
  33
  34 inline void
  35 gr_ofdm_frame_sink::enter_search()
  36 {
  37   if (VERBOSE)
  38     fprintf(stderr, "@ enter_search\n");
  39
  40   d_state = STATE_SYNC_SEARCH;
  41
  42 }
  43
  44 inline void
  45 gr_ofdm_frame_sink::enter_have_sync()
  46 {
  47   if (VERBOSE)
  48     fprintf(stderr, "@ enter_have_sync\n");
  49
  50   d_state = STATE_HAVE_SYNC;
  51
  52   // clear state of demapper
  53   d_byte_offset = 0;
  54   d_partial_byte = 0;
  55
  56   d_header = 0;
  57   d_headerbytelen_cnt = 0;
  58 }
  59
  60 inline void
  61 gr_ofdm_frame_sink::enter_have_header()
  62 {
  63   d_state = STATE_HAVE_HEADER;
  64
  65   // header consists of two 16-bit shorts in network byte order
  66   // payload length is lower 12 bits
  67   // whitener offset is upper 4 bits
  68   d_packetlen = (d_header >> 16) & 0x0fff;
  69   d_packet_whitener_offset = (d_header >> 28) & 0x000f;
  70   d_packetlen_cnt = 0;
  71
  72   if (VERBOSE)
  73     fprintf(stderr, "@ enter_have_header (payload_len = %d) (offset = %d)\n",
  74             d_packetlen, d_packet_whitener_offset);
  75 }
  76
  77
  78 unsigned char gr_ofdm_frame_sink::slicer(const gr_complex x)
  79 {
  80   unsigned int table_size = d_sym_value_out.size();
  81   unsigned int min_index = 0;
  82   float min_euclid_dist = norm(x - d_sym_position[0]);
  83   float euclid_dist = 0;
  84
  85   for (unsigned int j = 1; j < table_size; j++){
  86     euclid_dist = norm(x - d_sym_position[j]);
  87     if (euclid_dist < min_euclid_dist){
  88       min_euclid_dist = euclid_dist;
  89       min_index = j;
  90     }
  91   }
  92   return d_sym_value_out[min_index];
  93 }
  94
  95 unsigned int gr_ofdm_frame_sink::demapper(const gr_complex *in,
  96                                           unsigned char *out)
  97 {
  98   unsigned int i=0, bytes_produced=0;
  99
 100   while(i < d_occupied_carriers) {
 101     if(d_nresid > 0) {
 102       d_partial_byte |= d_resid;
 103       d_byte_offset += d_nresid;
 104       d_nresid = 0;
 105       d_resid = 0;
 106     }
 107
 108     while((d_byte_offset < 8) && (i < d_occupied_carriers)) {
 109       //fprintf(stderr, "%f+j%f  = %d\n", in[i].real(), in[i].imag(), slicer(in[i]));
 110       unsigned char bits = slicer(in[i++]);
 111       if((8 - d_byte_offset) >= d_nbits) {
 112         d_partial_byte |= bits << (d_byte_offset);
 113         d_byte_offset += d_nbits;
 114       }
 115       else {
 116         d_nresid = d_nbits-(8-d_byte_offset);
 117         int mask = ((1<<(8-d_byte_offset))-1);
 118         d_partial_byte |= (bits & mask) << d_byte_offset;
 119         d_resid = bits >> (8-d_byte_offset);
 120         d_byte_offset += (d_nbits - d_nresid);
 121       }
 122       //printf("demod symbol: %.4f + j%.4f   bits: %x   partial_byte: %x   byte_offset: %d   resid: %x   nresid: %d\n",
 123       //     in[i-1].real(), in[i-1].imag(), bits, d_partial_byte, d_byte_offset, d_resid, d_nresid);
 124     }
 125
 126     if(d_byte_offset == 8) {
 127       //printf("demod byte: %x \n\n", d_partial_byte);
 128       out[bytes_produced++] = d_partial_byte;
 129       d_byte_offset = 0;
 130       d_partial_byte = 0;
 131     }
 132   }
 133
 134   return bytes_produced;
 135 }
 136
 137
 138 gr_ofdm_frame_sink_sptr
 139 gr_make_ofdm_frame_sink(const std::vector<gr_complex> &sym_position,
 140                         const std::vector<unsigned char> &sym_value_out,
 141                         gr_msg_queue_sptr target_queue, unsigned int occupied_carriers)
 142 {
 143   return gr_ofdm_frame_sink_sptr(new gr_ofdm_frame_sink(sym_position, sym_value_out,
 144                                                         target_queue, occupied_carriers));
 145 }
 146
 147
 148 gr_ofdm_frame_sink::gr_ofdm_frame_sink(const std::vector<gr_complex> &sym_position,
 149                                        const std::vector<unsigned char> &sym_value_out,
 150                                        gr_msg_queue_sptr target_queue, unsigned int occupied_carriers)
 151   : gr_sync_block ("ofdm_frame_sink",
 152                    gr_make_io_signature2 (2, 2, sizeof(gr_complex)*occupied_carriers, sizeof(char)),
 153                    gr_make_io_signature (0, 0, 0)),
 154     d_target_queue(target_queue), d_occupied_carriers(occupied_carriers),
 155     d_byte_offset(0), d_partial_byte(0),
 156     d_resid(0), d_nresid(0)
 157 {
 158   d_bytes_out = new unsigned char[d_occupied_carriers];
 159
 160   set_sym_value_out(sym_position, sym_value_out);
 161
 162   enter_search();
 163 }
 164
 165 gr_ofdm_frame_sink::~gr_ofdm_frame_sink ()
 166 {
 167   delete [] d_bytes_out;
 168 }
 169
 170 bool
 171 gr_ofdm_frame_sink::set_sym_value_out(const std::vector<gr_complex> &sym_position,
 172                                       const std::vector<unsigned char> &sym_value_out)
 173 {
 174   if (sym_position.size() != sym_value_out.size())
 175     return false;
 176
 177   if (sym_position.size()<1)
 178     return false;
 179
 180   d_sym_position  = sym_position;
 181   d_sym_value_out = sym_value_out;
 182   d_nbits = (unsigned long)(log10(d_sym_value_out.size()) / log10(2));
 183
 184   return true;
 185 }
 186
 187
 188 int
 189 gr_ofdm_frame_sink::work (int noutput_items,
 190                           gr_vector_const_void_star &input_items,
 191                           gr_vector_void_star &output_items)
 192 {
 193   const gr_complex *in = (const gr_complex *) input_items[0];
 194   const char *sig = (const char *) input_items[1];
 195   unsigned int j = 0;
 196   unsigned int bytes=0;
 197
 198   if (VERBOSE)
 199     fprintf(stderr,">>> Entering state machine\n");
 200
 201   bytes = demapper(&in[0], d_bytes_out);
 202
 203   switch(d_state) {
 204
 205   case STATE_SYNC_SEARCH:    // Look for flag indicating beginning of pkt
 206     if (VERBOSE)
 207       fprintf(stderr,"SYNC Search, noutput=%d\n", noutput_items);
 208
 209     if (sig[0]) {  // Found it, set up for header decode
 210       enter_have_sync();
 211     }
 212     break;
 213
 214   case STATE_HAVE_SYNC:
 215     if (VERBOSE) {
 216       if(sig[0])
 217         printf("ERROR -- Found SYNC in HAVE_SYNC\n");
 218       fprintf(stderr,"Header Search bitcnt=%d, header=0x%08x\n",
 219               d_headerbytelen_cnt, d_header);
 220     }
 221
 222     j = 0;
 223     while(j < bytes) {
 224       d_header = (d_header << 8) | (d_bytes_out[j] & 0xFF);
 225       j++;
 226
 227       if (++d_headerbytelen_cnt == HEADERBYTELEN) {
 228
 229         if (VERBOSE)
 230           fprintf(stderr, "got header: 0x%08x\n", d_header);
 231
 232         // we have a full header, check to see if it has been received properly
 233         if (header_ok()){
 234           enter_have_header();
 235
 236           if (VERBOSE)
 237             printf("\nPacket Length: %d\n", d_packetlen);
 238
 239           while((j < bytes) && (d_packetlen_cnt < d_packetlen)) {
 240             d_packet[d_packetlen_cnt++] = d_bytes_out[j++];
 241           }
 242
 243           if(d_packetlen_cnt == d_packetlen) {
 244             gr_message_sptr msg =
 245               gr_make_message(0, d_packet_whitener_offset, 0, d_packetlen);
 246             memcpy(msg->msg(), d_packet, d_packetlen_cnt);
 247             d_target_queue->insert_tail(msg);           // send it
 248             msg.reset();                                // free it up
 249
 250             enter_search();
 251           }
 252         }
 253         else {
 254           enter_search();                               // bad header
 255         }
 256       }
 257     }
 258     break;
 259
 260   case STATE_HAVE_HEADER:
 261     if (VERBOSE) {
 262       if(sig[0])
 263         printf("ERROR -- Found SYNC in HAVE_HEADER at %d, length of %d\n", d_packetlen_cnt, d_packetlen);
 264       fprintf(stderr,"Packet Build\n");
 265     }
 266
 267     j = 0;
 268     while(j < bytes) {
 269       d_packet[d_packetlen_cnt++] = d_bytes_out[j++];
 270
 271       if (d_packetlen_cnt == d_packetlen){              // packet is filled
 272         // build a message
 273         // NOTE: passing header field as arg1 is not scalable
 274         gr_message_sptr msg =
 275           gr_make_message(0, d_packet_whitener_offset, 0, d_packetlen_cnt);
 276         memcpy(msg->msg(), d_packet, d_packetlen_cnt);
 277
 278         d_target_queue->insert_tail(msg);               // send it
 279         msg.reset();                            // free it up
 280
 281         enter_search();
 282         break;
 283       }
 284     }
 285     break;
 286
 287   default:
 288     assert(0);
 289
 290   } // switch
 291
 292   return 1;
 293 }