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