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

   1 /* -*- c++ -*- */
   2 /*
   3  * Copyright 2004 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_packet_sink.h>
  28 #include <gr_io_signature.h>
  29 #include <cstdio>
  30 #include <errno.h>
  31 #include <sys/types.h>
  32 #include <sys/stat.h>
  33 #include <fcntl.h>
  34 #include <stdexcept>
  35 #include <gr_count_bits.h>
  36
  37 #define VERBOSE 0
  38
  39 static const int DEFAULT_THRESHOLD = 12;  // detect access code with up to DEFAULT_THRESHOLD bits wrong
  40
  41 inline void
  42 gr_packet_sink::enter_search()
  43 {
  44   if (VERBOSE)
  45     fprintf(stderr, "@ enter_search\n");
  46
  47   d_state = STATE_SYNC_SEARCH;
  48   d_shift_reg = 0;
  49 }
  50
  51 inline void
  52 gr_packet_sink::enter_have_sync()
  53 {
  54   if (VERBOSE)
  55     fprintf(stderr, "@ enter_have_sync\n");
  56
  57   d_state = STATE_HAVE_SYNC;
  58   d_header = 0;
  59   d_headerbitlen_cnt = 0;
  60 }
  61
  62 inline void
  63 gr_packet_sink::enter_have_header(int payload_len)
  64 {
  65   if (VERBOSE)
  66     fprintf(stderr, "@ enter_have_header (payload_len = %d)\n", payload_len);
  67
  68   d_state = STATE_HAVE_HEADER;
  69   d_packetlen  = payload_len;
  70   d_packetlen_cnt = 0;
  71   d_packet_byte = 0;
  72   d_packet_byte_index = 0;
  73 }
  74
  75 gr_packet_sink_sptr
  76 gr_make_packet_sink (const std::vector<unsigned char>& sync_vector,
  77                      gr_msg_queue_sptr target_queue, int threshold)
  78 {
  79   return gr_packet_sink_sptr (new gr_packet_sink (sync_vector, target_queue, threshold));
  80 }
  81
  82
  83 gr_packet_sink::gr_packet_sink (const std::vector<unsigned char>& sync_vector,
  84                                 gr_msg_queue_sptr target_queue, int threshold)
  85   : gr_sync_block ("packet_sink",
  86                    gr_make_io_signature (1, 1, sizeof(float)),
  87                    gr_make_io_signature (0, 0, 0)),
  88     d_target_queue(target_queue), d_threshold(threshold == -1 ? DEFAULT_THRESHOLD : threshold)
  89 {
  90   d_sync_vector = 0;
  91   for(int i=0;i<8;i++){
  92     d_sync_vector <<= 8;
  93     d_sync_vector |= sync_vector[i];
  94   }
  95
  96   enter_search();
  97 }
  98
  99 gr_packet_sink::~gr_packet_sink ()
 100 {
 101 }
 102
 103 int
 104 gr_packet_sink::work (int noutput_items,
 105                       gr_vector_const_void_star &input_items,
 106                       gr_vector_void_star &output_items)
 107 {
 108   float *inbuf = (float *) input_items[0];
 109   int count=0;
 110
 111   if (VERBOSE)
 112     fprintf(stderr,">>> Entering state machine\n"),fflush(stderr);
 113
 114   while (count<noutput_items) {
 115     switch(d_state) {
 116
 117     case STATE_SYNC_SEARCH:    // Look for sync vector
 118       if (VERBOSE)
 119         fprintf(stderr,"SYNC Search, noutput=%d\n",noutput_items),fflush(stderr);
 120
 121       while (count < noutput_items) {
 122         if(slice(inbuf[count++]))
 123           d_shift_reg = (d_shift_reg << 1) | 1;
 124         else
 125           d_shift_reg = d_shift_reg << 1;
 126
 127         // Compute popcnt of putative sync vector
 128         if(gr_count_bits64 (d_shift_reg ^ d_sync_vector) <= d_threshold) {
 129           // Found it, set up for header decode
 130           enter_have_sync();
 131           break;
 132         }
 133       }
 134       break;
 135
 136     case STATE_HAVE_SYNC:
 137       if (VERBOSE)
 138         fprintf(stderr,"Header Search bitcnt=%d, header=0x%08x\n", d_headerbitlen_cnt, d_header),
 139           fflush(stderr);
 140
 141       while (count < noutput_items) {           // Shift bits one at a time into header
 142         if(slice(inbuf[count++]))
 143           d_header = (d_header << 1) | 1;
 144         else
 145           d_header = d_header << 1;
 146
 147         if (++d_headerbitlen_cnt == HEADERBITLEN) {
 148
 149           if (VERBOSE)
 150             fprintf(stderr, "got header: 0x%08x\n", d_header);
 151
 152           // we have a full header, check to see if it has been received properly
 153           if (header_ok()){
 154             int payload_len = header_payload_len();
 155             if (payload_len <= MAX_PKT_LEN)             // reasonable?
 156               enter_have_header(payload_len);           // yes.
 157             else
 158               enter_search();                           // no.
 159           }
 160           else
 161             enter_search();                             // no.
 162           break;                        // we're in a new state
 163         }
 164       }
 165       break;
 166
 167     case STATE_HAVE_HEADER:
 168       if (VERBOSE)
 169         fprintf(stderr,"Packet Build\n"),fflush(stderr);
 170
 171       while (count < noutput_items) {   // shift bits into bytes of packet one at a time
 172         if(slice(inbuf[count++]))
 173           d_packet_byte = (d_packet_byte << 1) | 1;
 174         else
 175           d_packet_byte = d_packet_byte << 1;
 176
 177         if (d_packet_byte_index++ == 7) {               // byte is full so move to next byte
 178           d_packet[d_packetlen_cnt++] = d_packet_byte;
 179           d_packet_byte_index = 0;
 180
 181           if (d_packetlen_cnt == d_packetlen){          // packet is filled
 182
 183             // build a message
 184             gr_message_sptr msg = gr_make_message(0, 0, 0, d_packetlen_cnt);
 185             memcpy(msg->msg(), d_packet, d_packetlen_cnt);
 186
 187             d_target_queue->insert_tail(msg);           // send it
 188             msg.reset();                                // free it up
 189
 190             enter_search();
 191             break;
 192           }
 193         }
 194       }
 195       break;
 196
 197     default:
 198       assert(0);
 199
 200     } // switch
 201
 202   }   // while
 203
 204   return noutput_items;
 205 }
 206