winsng2.cc

定向扩散路由协议

//
// WINSng2.cc    : WINSng2 Radio Support for Diffusion
// Authors       : Fabio Silva
//
// Copyright (C) 2000-2002 by the University of Southern California
// $Id: WINSng2.cc,v 1.12 2002/08/21 20:10:21 fabio Exp $
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License,
// version 2, as published by the Free Software Foundation.
//
// This program 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 this program; if not, write to the Free Software Foundation, Inc.,
// 59 Temple Place, Suite 330, Boston, MA 02111-1307, USA.
//
//

#include <stdio.h>
#include <stdlib.h>

#include "WINSng2.hh"

WINSNG2::WINSNG2() : DiffusionIO()
{
  rf_filter_t rf_filter;
  int status;
  int fd1, fd2;

  // Open RF radio 1
  if ((fd1 = open(WINS_RF_DEVICE_0, O_RDWR)) < 0){
    DiffPrint(DEBUG_ALWAYS, "Cannot open " WINS_RF_DEVICE_0 " !\n");
    exit(-1);
  }

  DiffPrint(DEBUG_IMPORTANT, "WINSNG 2.0: Radio 1 fd: %d\n", fd1);

  // Open RF radio 2
  if ((fd2 = open(WINS_RF_DEVICE_1, O_RDWR)) < 0){
    DiffPrint(DEBUG_ALWAYS, "Cannot open " WINS_RF_DEVICE_1 " !\n");
    exit(-1);
  }

  DiffPrint(DEBUG_IMPORTANT, "WINSNG 2.0: Radio 2 fd: %d\n", fd2);

  in_fds_.push_back(fd1);
  out_fds_.push_back(fd1);
  in_fds_.push_back(fd2);
  out_fds_.push_back(fd2);

  if (fd1 > fd2)
    max_in_descriptor_ = fd1;
  else
    max_in_descriptor_ = fd2;

  // Add filters to the radio devices so we just receive our packets
  rf_filter.data[0] = DIFFUSION_PACKET_TYPE;
  rf_filter.len = 1;

  status = ioctl(fd1, RF_SET_FILTER, &rf_filter);
  if (status < 0){
    DiffPrint(DEBUG_ALWAYS,
	      "Cannot set filter for radio " WINS_RF_DEVICE_0 " !\n");
    exit(-1);
  }

  // Repeat process for radio 2
  rf_filter.data[0] = DIFFUSION_PACKET_TYPE;
  rf_filter.len = 1;

  status = ioctl(fd2, RF_SET_FILTER, &rf_filter);
  if (status < 0){
    DiffPrint(DEBUG_ALWAYS,
	      "Cannot set filter for radio " WINS_RF_DEVICE_1 " !\n");
    exit(-1);
  }
}

DiffPacket WINSNG2::recvPacket(int fd)
{
  struct rf_pkt *rf_pkt_hdr;
  uint8_t rf_packet_type;
  if_id_t radio_id;
  int diffusion_packet_size;
  int32_t diffusion_id;
  DiffPacket rf_packet = NULL;
  DiffPacket diffusion_packet = NULL;
  int flag;
  char *rf_payload;
  int rf_packet_size;
  struct hdr_diff *dfh;

  // Determine packet size
  flag = ioctl(fd, RF_GET_FRONT_SIZE, &rf_packet_size);
  if (flag != 0){
    DiffPrint(DEBUG_ALWAYS, "Cannot determine packet size ! Error Code = %d\n",
	      flag);
    exit(-1);
  }

  // Convert from network to host order
  rf_packet_size = ntohl(rf_packet_size);

  if (rf_packet_size <= 0){
    DiffPrint(DEBUG_ALWAYS, "Incoming packet has size <= 0 !\n");
    return NULL;
  }

  // Allocate memory for the incoming packet
  rf_packet = new int [(rf_packet_size / sizeof(int)) + 1];

  if (rf_packet == NULL){
    DiffPrint(DEBUG_ALWAYS, "Cannot allocate memory for the incoming radio packet !\n");
    exit(-1);
  }

  // Read the entire Sensoria Packet
  read(fd, rf_packet, rf_packet_size);
  rf_pkt_hdr = (struct rf_pkt *) rf_packet;
  rf_payload = (char *) rf_pkt_hdr->data;

  // Check if packet is a diffusion packet
  rf_packet_type = rf_pkt_hdr->type;
  if (rf_packet_type != DIFFUSION_PACKET_TYPE){
    // Print message
    DiffPrint(DEBUG_IMPORTANT,
	      "Received a packet type = %d, discarding...\n",
	      rf_packet_type);

    delete [] rf_packet;
    return NULL;
  }

  // Allocate memory for the diffusion packet
  diffusion_packet_size = rf_packet_size - sizeof(struct rf_pkt);
  diffusion_packet = new int [(diffusion_packet_size / sizeof(int)) + 1];

  if (diffusion_packet == NULL){
    DiffPrint(DEBUG_ALWAYS, "Cannot allocate memory for incoming packet !\n");
    exit(-1);
  }

  // Copy diffusion packet out of the sensoria packet
  memcpy(diffusion_packet, rf_payload, diffusion_packet_size);

  // Extract Sensoria's radio ID and diffusion's ID from packet
  dfh = HDR_DIFF(diffusion_packet);
  diffusion_id = ntohl(LAST_HOP(dfh));
  radio_id = rf_pkt_hdr->src;

  // Add radio Id to Cache
  addCache(fd, diffusion_id, radio_id);

  // We don't need the sensoria packet anymore
  delete rf_packet;

  DiffPrint(DEBUG_LOTS_DETAILS,
	    "Received %d bytes from node %d !\n",
	    diffusion_packet_size, diffusion_id);

  return diffusion_packet;
}

void WINSNG2::sendPacket(DiffPacket pkt, int len, int dst)
{
  if_id_t radio_id;
  int fd;
  DiffPacket outgoing_packet = NULL;
  list<int>::iterator itr;

  if (dst != BROADCAST_ADDR){
    fd = findInCache(dst, &radio_id);
    if (fd < 0){
      DiffPrint(DEBUG_ALWAYS, "Warning: Destination not in cache !\n");
      return;
    }

    outgoing_packet = assemblePacket(radio_id, len, pkt);
    write(fd, outgoing_packet, (len + sizeof(struct rf_pkt)));

    DiffPrint(DEBUG_LOTS_DETAILS, "Sending %d bytes to node %d !\n", len, dst);

  }
  else{
    // We have a broadcast message
    radio_id = RF_BROADCAST;

    DiffPrint(DEBUG_LOTS_DETAILS,
	      "Sending %d bytes to broadcast address !\n", len);

    outgoing_packet = assemblePacket(radio_id, len, pkt);
    for (itr = out_fds_.begin(); itr != out_fds_.end(); ++itr){
      write(*itr, outgoing_packet, (len + sizeof(struct rf_pkt)));
    }
  }
  delete [] outgoing_packet;
}

void WINSNG2::addCache(int fd, int32_t diffusion_id, if_id_t radio_id)
{
  RFIdCache::iterator itr;
  RFIdCacheEntry *entry;

  for (itr = RfCache_.begin(); itr != RfCache_.end(); ++itr){
    entry = *itr;
    if (radio_id == entry->radio_id_){
      if ((fd == entry->radio_fd_) &&
	  (diffusion_id == entry->diffusion_id_)){
	return;
      }
      // Something changed for this WINS address
      entry->radio_fd_ = fd;
      entry->diffusion_id_ = diffusion_id;
      return;
    }
  }

  // Create a new Cache entry
  entry = new RFIdCacheEntry(diffusion_id, radio_id, fd);
  RfCache_.push_back(entry);
}

int WINSNG2::findInCache(int32_t diffusion_id, if_id_t *radio_id)
{
  RFIdCache::iterator itr;
  RFIdCacheEntry *entry;

  for (itr = RfCache_.begin(); itr != RfCache_.end(); ++itr){
    entry = *itr;

    if (diffusion_id == entry->diffusion_id_){
      // Entry found !
      *radio_id = entry->radio_id_;
      return entry->radio_fd_;
    }
  }

  // Entry not in the cache
  return -1;
}

DiffPacket WINSNG2::assemblePacket(if_id_t radio_id, int len, DiffPacket pkt)
{
  DiffPacket outgoing_packet = NULL;
  char *packet_payload;
  struct rf_pkt *rf_pkt_hdr;

  // Allocate memory for packet + header
  outgoing_packet = new int[((len + sizeof(struct rf_pkt)) / sizeof(int))];
  rf_pkt_hdr = (struct rf_pkt *) outgoing_packet;
  packet_payload = (char *) rf_pkt_hdr->data;

  // Fill rf header
  rf_pkt_hdr->dst = radio_id;
  rf_pkt_hdr->type = (uint8_t) DIFFUSION_PACKET_TYPE;

  // Copy packet
  memcpy(packet_payload, pkt, len);

  return outgoing_packet;
}