diff options
Diffstat (limited to 'gr-digital/lib/digital_packet_sink.cc')
| -rw-r--r-- | gr-digital/lib/digital_packet_sink.cc | 207 |
1 files changed, 207 insertions, 0 deletions
diff --git a/gr-digital/lib/digital_packet_sink.cc b/gr-digital/lib/digital_packet_sink.cc new file mode 100644 index 000000000..92521376f --- /dev/null +++ b/gr-digital/lib/digital_packet_sink.cc @@ -0,0 +1,207 @@ +/* -*- c++ -*- */ +/* + * Copyright 2004,2010,2012 Free Software Foundation, Inc. + * + * This file is part of GNU Radio + * + * GNU Radio 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, or (at your option) + * any later version. + * + * GNU Radio 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 GNU Radio; see the file COPYING. If not, write to + * the Free Software Foundation, Inc., 51 Franklin Street, + * Boston, MA 02110-1301, USA. + */ + +#ifdef HAVE_CONFIG_H +#include "config.h" +#endif + +#include <digital_packet_sink.h> +#include <gr_io_signature.h> +#include <cstdio> +#include <errno.h> +#include <sys/types.h> +#include <sys/stat.h> +#include <fcntl.h> +#include <stdexcept> +#include <gr_count_bits.h> +#include <string.h> + +#define VERBOSE 0 + +static const int DEFAULT_THRESHOLD = 12; // detect access code with up to DEFAULT_THRESHOLD bits wrong + +inline void +digital_packet_sink::enter_search() +{ + if (VERBOSE) + fprintf(stderr, "@ enter_search\n"); + + d_state = STATE_SYNC_SEARCH; + d_shift_reg = 0; +} + +inline void +digital_packet_sink::enter_have_sync() +{ + if (VERBOSE) + fprintf(stderr, "@ enter_have_sync\n"); + + d_state = STATE_HAVE_SYNC; + d_header = 0; + d_headerbitlen_cnt = 0; +} + +inline void +digital_packet_sink::enter_have_header(int payload_len) +{ + if (VERBOSE) + fprintf(stderr, "@ enter_have_header (payload_len = %d)\n", payload_len); + + d_state = STATE_HAVE_HEADER; + d_packetlen = payload_len; + d_packetlen_cnt = 0; + d_packet_byte = 0; + d_packet_byte_index = 0; +} + +digital_packet_sink_sptr +digital_make_packet_sink (const std::vector<unsigned char>& sync_vector, + gr_msg_queue_sptr target_queue, int threshold) +{ + return gnuradio::get_initial_sptr(new digital_packet_sink (sync_vector, target_queue, threshold)); +} + + +digital_packet_sink::digital_packet_sink (const std::vector<unsigned char>& sync_vector, + gr_msg_queue_sptr target_queue, int threshold) + : gr_sync_block ("packet_sink", + gr_make_io_signature (1, 1, sizeof(float)), + gr_make_io_signature (0, 0, 0)), + d_target_queue(target_queue), d_threshold(threshold == -1 ? DEFAULT_THRESHOLD : threshold) +{ + d_sync_vector = 0; + for(int i=0;i<8;i++){ + d_sync_vector <<= 8; + d_sync_vector |= sync_vector[i]; + } + + enter_search(); +} + +digital_packet_sink::~digital_packet_sink () +{ +} + +int +digital_packet_sink::work (int noutput_items, + gr_vector_const_void_star &input_items, + gr_vector_void_star &output_items) +{ + float *inbuf = (float *) input_items[0]; + int count=0; + + if (VERBOSE) + fprintf(stderr,">>> Entering state machine\n"),fflush(stderr); + + while (count<noutput_items) { + switch(d_state) { + + case STATE_SYNC_SEARCH: // Look for sync vector + if (VERBOSE) + fprintf(stderr,"SYNC Search, noutput=%d\n",noutput_items),fflush(stderr); + + while (count < noutput_items) { + if(slice(inbuf[count++])) + d_shift_reg = (d_shift_reg << 1) | 1; + else + d_shift_reg = d_shift_reg << 1; + + // Compute popcnt of putative sync vector + if(gr_count_bits64 (d_shift_reg ^ d_sync_vector) <= d_threshold) { + // Found it, set up for header decode + enter_have_sync(); + break; + } + } + break; + + case STATE_HAVE_SYNC: + if (VERBOSE) + fprintf(stderr,"Header Search bitcnt=%d, header=0x%08x\n", d_headerbitlen_cnt, d_header), + fflush(stderr); + + while (count < noutput_items) { // Shift bits one at a time into header + if(slice(inbuf[count++])) + d_header = (d_header << 1) | 1; + else + d_header = d_header << 1; + + if (++d_headerbitlen_cnt == HEADERBITLEN) { + + if (VERBOSE) + fprintf(stderr, "got header: 0x%08x\n", d_header); + + // we have a full header, check to see if it has been received properly + if (header_ok()){ + int payload_len = header_payload_len(); + if (payload_len <= MAX_PKT_LEN) // reasonable? + enter_have_header(payload_len); // yes. + else + enter_search(); // no. + } + else + enter_search(); // no. + break; // we're in a new state + } + } + break; + + case STATE_HAVE_HEADER: + if (VERBOSE) + fprintf(stderr,"Packet Build\n"),fflush(stderr); + + while (count < noutput_items) { // shift bits into bytes of packet one at a time + if(slice(inbuf[count++])) + d_packet_byte = (d_packet_byte << 1) | 1; + else + d_packet_byte = d_packet_byte << 1; + + if (d_packet_byte_index++ == 7) { // byte is full so move to next byte + d_packet[d_packetlen_cnt++] = d_packet_byte; + d_packet_byte_index = 0; + + if (d_packetlen_cnt == d_packetlen){ // packet is filled + + // build a message + gr_message_sptr msg = gr_make_message(0, 0, 0, d_packetlen_cnt); + memcpy(msg->msg(), d_packet, d_packetlen_cnt); + + d_target_queue->insert_tail(msg); // send it + msg.reset(); // free it up + + enter_search(); + break; + } + } + } + break; + + default: + assert(0); + + } // switch + + } // while + + return noutput_items; +} + |