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 2, 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 unsigned char gr_ofdm_frame_sink::bpsk_slicer(gr_complex x)
  78 {
  79   return (unsigned char)(x.real() > 0 ? 1 : 0);
  80 }
  81
  82 unsigned char gr_ofdm_frame_sink::qpsk_slicer(gr_complex x)
  83 {
  84   unsigned char i = (x.real() > 0 ? 1 : 0);
  85   unsigned char q = (x.imag() > 0 ? 1 : 0);
  86
  87   return (q << 1) | i;
  88 }
  89
  90 unsigned int gr_ofdm_frame_sink::bpsk_demapper(const gr_complex *in,
  91                                                unsigned char *out)
  92 {
  93   unsigned int i=0, bytes_produced=0;
  94
  95   while(i < d_occupied_carriers) {
  96
  97     while((d_byte_offset < 8) && (i < d_occupied_carriers)) {
  98       //fprintf(stderr, "%f+j%f\n", in[i].real(), in[i].imag());
  99       d_partial_byte |= bpsk_slicer(in[i++]) << (d_byte_offset++);
 100     }
 101
 102     if(d_byte_offset == 8) {
 103       out[bytes_produced++] = d_partial_byte;
 104       d_byte_offset = 0;
 105       d_partial_byte = 0;
 106     }
 107   }
 108
 109   return bytes_produced;
 110 }
 111
 112 unsigned int gr_ofdm_frame_sink::qpsk_demapper(const gr_complex *in,
 113                                                unsigned char *out)
 114 {
 115   unsigned int i=0, bytes_produced=0;
 116
 117   while(i < d_occupied_carriers) {
 118
 119     while((d_byte_offset < 8) && (i < d_occupied_carriers)) {
 120       //fprintf(stderr, "%f+j%f\n", in[i].real(), in[i].imag());
 121       d_partial_byte |= qpsk_slicer(in[i++]) << (d_byte_offset);
 122       d_byte_offset += 2;
 123     }
 124
 125     if(d_byte_offset == 8) {
 126       out[bytes_produced++] = d_partial_byte;
 127       d_byte_offset = 0;
 128       d_partial_byte = 0;
 129     }
 130   }
 131
 132   return bytes_produced;
 133 }
 134
 135 gr_ofdm_frame_sink_sptr
 136 gr_make_ofdm_frame_sink(gr_msg_queue_sptr target_queue, unsigned int occupied_carriers,
 137                         const std::string &mod)
 138 {
 139   return gr_ofdm_frame_sink_sptr(new gr_ofdm_frame_sink(target_queue, occupied_carriers, mod));
 140 }
 141
 142
 143 gr_ofdm_frame_sink::gr_ofdm_frame_sink(gr_msg_queue_sptr target_queue, unsigned int occupied_carriers,
 144                                        const std::string &mod)
 145   : gr_sync_block ("ofdm_frame_sink",
 146                    gr_make_io_signature2 (2, 2, sizeof(gr_complex)*occupied_carriers, sizeof(char)),
 147                    gr_make_io_signature (0, 0, 0)),
 148     d_target_queue(target_queue), d_occupied_carriers(occupied_carriers),
 149     d_byte_offset(0), d_partial_byte(0)
 150 {
 151   d_bytes_out = new unsigned char[(int)ceil(d_occupied_carriers/4.0)];
 152
 153   if(mod == "qpsk") {
 154     d_demapper = &gr_ofdm_frame_sink::qpsk_demapper;
 155   }
 156   else if(mod == "bpsk") {
 157     d_demapper = &gr_ofdm_frame_sink::bpsk_demapper;
 158   }
 159   else {
 160     throw std::invalid_argument("Modulation type must be BPSK or QPSK.");
 161   }
 162
 163   enter_search();
 164 }
 165
 166 gr_ofdm_frame_sink::~gr_ofdm_frame_sink ()
 167 {
 168   delete [] d_bytes_out;
 169 }
 170
 171 int
 172 gr_ofdm_frame_sink::work (int noutput_items,
 173                           gr_vector_const_void_star &input_items,
 174                           gr_vector_void_star &output_items)
 175 {
 176   const gr_complex *in = (const gr_complex *) input_items[0];
 177   const char *sig = (const char *) input_items[1];
 178   unsigned int j = 0;
 179   unsigned int bytes=0;
 180
 181   if (VERBOSE)
 182     fprintf(stderr,">>> Entering state machine\n");
 183
 184   //bytes = bpsk_demapper(&in[0], d_bytes_out);
 185   bytes = (this->*d_demapper)(&in[0], d_bytes_out);
 186
 187   switch(d_state) {
 188
 189   case STATE_SYNC_SEARCH:    // Look for flag indicating beginning of pkt
 190     if (VERBOSE)
 191       fprintf(stderr,"SYNC Search, noutput=%d\n", noutput_items);
 192
 193     if (sig[0]) {  // Found it, set up for header decode
 194       enter_have_sync();
 195     }
 196     break;
 197
 198   case STATE_HAVE_SYNC:
 199     if (VERBOSE) {
 200       if(sig[0])
 201         printf("ERROR -- Found SYNC in HAVE_SYNC\n");
 202       fprintf(stderr,"Header Search bitcnt=%d, header=0x%08x\n",
 203               d_headerbytelen_cnt, d_header);
 204     }
 205
 206     j = 0;
 207     while(j < bytes) {
 208       d_header = (d_header << 8) | (d_bytes_out[j] & 0xFF);
 209       j++;
 210
 211       if (++d_headerbytelen_cnt == HEADERBYTELEN) {
 212
 213         if (VERBOSE)
 214           fprintf(stderr, "got header: 0x%08x\n", d_header);
 215
 216         // we have a full header, check to see if it has been received properly
 217         if (header_ok()){
 218           enter_have_header();
 219
 220           if (VERBOSE)
 221             printf("\nPacket Length: %d\n", d_packetlen);
 222
 223           while((j < bytes) && (d_packetlen_cnt < d_packetlen)) {
 224             d_packet[d_packetlen_cnt++] = d_bytes_out[j++];
 225           }
 226
 227           if(d_packetlen_cnt == d_packetlen) {
 228             gr_message_sptr msg =
 229               gr_make_message(0, d_packet_whitener_offset, 0, d_packetlen);
 230             memcpy(msg->msg(), d_packet, d_packetlen_cnt);
 231             d_target_queue->insert_tail(msg);           // send it
 232             msg.reset();                                // free it up
 233
 234             enter_search();
 235           }
 236         }
 237         else {
 238           enter_search();                               // bad header
 239         }
 240       }
 241     }
 242     break;
 243
 244   case STATE_HAVE_HEADER:
 245     if (VERBOSE) {
 246       if(sig[0])
 247         printf("ERROR -- Found SYNC in HAVE_HEADER at %d, length of %d\n", d_packetlen_cnt, d_packetlen);
 248       fprintf(stderr,"Packet Build\n");
 249     }
 250
 251     j = 0;
 252     while(j < bytes) {
 253       d_packet[d_packetlen_cnt++] = d_bytes_out[j++];
 254
 255       if (d_packetlen_cnt == d_packetlen){              // packet is filled
 256         // build a message
 257         // NOTE: passing header field as arg1 is not scalable
 258         gr_message_sptr msg =
 259           gr_make_message(0, d_packet_whitener_offset, 0, d_packetlen_cnt);
 260         memcpy(msg->msg(), d_packet, d_packetlen_cnt);
 261
 262         d_target_queue->insert_tail(msg);               // send it
 263         msg.reset();                            // free it up
 264
 265         enter_search();
 266         break;
 267       }
 268     }
 269     break;
 270
 271   default:
 272     assert(0);
 273
 274   } // switch
 275
 276   return 1;
 277 }