serial_tun.c

tinyos-2.x.rar

      case IANA_UDP:
        transport_len = sizeof(struct udp_hdr); break;
      }

      g_hdr = (struct generic_header *)malloc(sizeof(struct generic_header));
      g_hdr->payload_malloced = 0;
      g_hdr->hdr.ext = ext;
      g_hdr->next = NULL;
      *g_list = g_hdr;
      g_list = &g_hdr->next;

      g_hdr->len = transport_len;
      ext = (struct ip6_ext *)(((uint8_t *)ext) + transport_len);
      hdr_len += transport_len;
  }

  msg->data = (uint8_t *)ext;
  msg->data_len = ntohs(msg->hdr.plen) - hdr_len;
}

#ifdef CENTRALIZED_ROUTING
/*
 * Given a source and destination, send a source-routed route install message
 * that will install the correct routes.
 *
 * NOTE: Make sure that this is the correct way to create a new packet
 */
time_t last_install;
void install_route(struct split_ip_msg *amsg, uint8_t flags) {
  uint8_t buf[sizeof(struct split_ip_msg) + INET_MTU];
  struct split_ip_msg *msg = (struct split_ip_msg *)buf;
  int offset = 0;


  struct ip6_ext *ext = (struct ip6_ext *)(msg->next);
  struct tlv_hdr *tlv = (struct tlv_hdr *)(ext + 1);
  struct rinstall_header *rih = (struct rinstall_header *)(tlv + 1);

  path_t* path = nw_get_route(ntohs(amsg->hdr.ip6_src.s6_addr16[7]), ntohs(amsg->hdr.ip6_dst.s6_addr16[7]));
  path_t* i;
  time_t current_time;

#if 0
  time(&current_time);

  if (current_time < last_install + 2) {
    debug("Not sending install\n");
    return;
  }
  time(&last_install);
#endif 


  if (path == NULL || path->isController) return;
  fprintf(stderr, "install_route for src: 0x%x, dest: 0x%x, flags: %x\n", 
          ntohs(amsg->hdr.ip6_src.s6_addr16[7]), ntohs(amsg->hdr.ip6_dst.s6_addr16[7]), path->length);
  if (path->length > 10) return;

  memset((uint8_t *)&msg->hdr, 0, sizeof(struct ip6_hdr));

  // Set IP Header options
  msg->hdr.hlim = 0x64; // CHECK THIS
  msg->hdr.nxt_hdr = IPV6_DEST;
  msg->hdr.vlfc[0] = IPV6_VERSION << 4;
  msg->flow_id = local_seqno++;

  memcpy(&msg->hdr.ip6_src, &__my_address, sizeof(struct in6_addr));
  memcpy(&msg->hdr.ip6_dst, &amsg->hdr.ip6_src, sizeof(struct in6_addr));
  msg->headers = NULL;

  ext->nxt_hdr = IPV6_NONEXT;
  ext->len = sizeof(struct ip6_ext) + sizeof(struct tlv_hdr) + sizeof(struct rinstall_header)
    + (path->length * sizeof(uint16_t));

  tlv->len = ext->len - sizeof(struct ip6_ext);
  tlv->type = TLV_TYPE_INSTALL;

  // Setup rinstall_header
 // Size is longer because we put the src in there
  rih->flags = flags;
  rih->match.src = htons(T_INVAL_NEIGH);
  rih->match.dest = amsg->hdr.ip6_dst.s6_addr16[7]; 
  rih->path_len = path->length;

  // Convert to host so add_headers_list works
  msg->hdr.plen = htons(ext->len); 

  info("install_route len: 0x%x\n", rih->path_len);

  fprintf(stderr, "from 0x%x to 0x%x [%i]: ", 
          ntohs(amsg->hdr.ip6_src.s6_addr16[7]), ntohs(amsg->hdr.ip6_dst.s6_addr16[7]), path->length);
  // rih->path[0] = amsg->hdr.ip6_src.s6_addr16[7]; //htons(l2fromIP(amsg->hdr.ip6_src.s6_addr));
  for (i = path; i != NULL; i = i->next) {
    fprintf(stderr, "0x%x ", i->node);
    rih->path[offset++] = htons(i->node);
  }

  nw_free_path(path);

  add_header_list(msg);
  print_ip_packet(msg);
  loglevel_t old_lvl = log_setlevel(LOGLVL_DEBUG);
  ip_to_pan(msg);
  log_setlevel(old_lvl);
  free_split_msg(msg);
}

void uninstall_route(uint16_t n1, uint16_t n2) {
  uint8_t buf[sizeof(struct split_ip_msg) + INET_MTU];
  struct split_ip_msg *msg = (struct split_ip_msg *)buf;
  struct ip6_ext *ext = (struct ip6_ext *)(msg->next);
  struct tlv_hdr *tlv = (struct tlv_hdr *)(ext + 1);
  struct rinstall_header *rih = (struct rinstall_header *)(tlv + 1);

  // Set IP Header options
  msg->hdr.hlim = 0x64; // CHECK THIS
  msg->hdr.nxt_hdr = IPV6_DEST;
  msg->hdr.vlfc[0] = IPV6_VERSION << 4;
  msg->flow_id = local_seqno++;

  memcpy(&msg->hdr.ip6_src, &__my_address, sizeof(struct in6_addr));
  memcpy(&msg->hdr.ip6_dst, &__my_address, sizeof(struct in6_addr));
  msg->hdr.ip6_dst.s6_addr16[7] = htons(n1);
  msg->headers = NULL;

  ext->nxt_hdr = IPV6_NONEXT;
  ext->len = sizeof(struct ip6_ext) + sizeof(struct tlv_hdr) + sizeof(struct rinstall_header);

  tlv->len = ext->len - sizeof(struct ip6_ext);
  tlv->type = TLV_TYPE_INSTALL;

  // Convert to host so add_headers_list works
  msg->hdr.plen = htons(ext->len); 

  rih->flags = HYDRO_INSTALL_UNINSTALL_MASK | HYDRO_METHOD_SOURCE;
  rih->match.src = htons(n1);
  rih->match.dest = htons(n2);
  rih->path_len = 0;
    

  add_header_list(msg);
  print_ip_packet(msg);
  loglevel_t old_lvl = log_setlevel(LOGLVL_DEBUG);
  ip_to_pan(msg);
  log_setlevel(old_lvl);
  free_split_msg(msg);
}

#endif

/*
 * read data from the tun device and send it to the serial port
 * does also fragmentation
 */
int tun_input()
{
  uint8_t buf[sizeof(struct split_ip_msg) + INET_MTU];
  struct split_ip_msg *msg = (struct split_ip_msg *)buf;
  int len;

  len = tun_read(tun_fd, (void *)(&msg->pi), INET_MTU + sizeof(struct ip6_hdr));

  if (len <= 0) {
    return 0;
  }
  debug("tun_read: read 0x%x bytes\n", len);

  if ((msg->hdr.vlfc[0] >> 4) != IPV6_VERSION) {
    warn("tun_read: discarding non-ip packet\n");
    goto fail;
  }
  if (ntohs(msg->hdr.plen) > INET_MTU - sizeof(struct ip6_hdr)) {
    debug("tun_input: dropping packet due to length: 0x%x\n", ntohs(msg->hdr.plen));
    goto fail;
  }
  if (msg->hdr.nxt_hdr == 0) {
    debug("tun_input: dropping packet with IPv6 options\n");
    goto fail;
  }
  
  add_header_list(msg);
  msg->flow_id = local_seqno++;

  ip_to_pan(msg);

  free_split_msg(msg);
  
  return 1;
 fail:
  /* error("Invalid packet or version received\n"); */
  return 1;
}

/* ------------------------------------------------------------------------- */
/* handling of data arriving on the serial port */

reconstruct_t reconstructions [N_RECONSTRUCTIONS];

void age_reconstructions() {
  int i;
  for (i = 0; i < N_RECONSTRUCTIONS; i++) {
    // switch "active" buffers to "zombie"
    if (reconstructions[i].timeout == T_ACTIVE) {
      reconstructions[i].timeout = T_ZOMBIE;
    } else if (reconstructions[i].timeout == T_ZOMBIE) {
      reconstructions[i].timeout = T_UNUSED;
      free(reconstructions[i].buf);
      reconstructions[i].buf = NULL;
    }
  }
}


reconstruct_t *getReassembly(packed_lowmsg_t *lowmsg) {
  int i, free_spot = N_RECONSTRUCTIONS + 1;
  uint16_t mytag, size;
  if (getFragDgramTag(lowmsg, &mytag)) return NULL;
  if (getFragDgramSize(lowmsg, &size)) return NULL;
  
  for (i = 0; i < N_RECONSTRUCTIONS; i++) {
    if (reconstructions[i].timeout > T_UNUSED && reconstructions[i].tag == mytag) {
      reconstructions[i].timeout = T_ACTIVE;
      return &(reconstructions[i]);
    }
    if (reconstructions[i].timeout == T_UNUSED) free_spot = i;
  }
  // allocate a new struct for doing reassembly.
  if (free_spot != N_RECONSTRUCTIONS + 1) {
    // if we don't get the packet with the protocol in it first, we
    // don't know who to ask for a buffer, and so give up.

    reconstructions[free_spot].tag = mytag;

    reconstructions[free_spot].size = size;
    reconstructions[free_spot].buf = malloc(size + offsetof(struct split_ip_msg, hdr));
    reconstructions[free_spot].bytes_rcvd = 0;
    reconstructions[free_spot].timeout = T_ACTIVE;

    debug("checking buffer size 0x%x\n", reconstructions[free_spot].size);
    if (reconstructions[free_spot].buf == NULL) {
      reconstructions[free_spot].timeout = T_UNUSED;
      return NULL;
    }
    return &(reconstructions[free_spot]);
  }
  return NULL;
}

/* 
 * read data on serial port and send it to the tun interface
 * does fragment reassembly
 */
int serial_input() {
    packed_lowmsg_t pkt;
    reconstruct_t *recon;
    struct split_ip_msg *msg;
    IEEE154_header_t *mac_hdr;
    
    uint8_t *ser_data = NULL;	        /* data read from serial port */
    int ser_len = 0;                    /* length of data read from serial port */
    uint8_t shortMsg[INET_MTU];
    uint8_t *payload;

#ifdef SF_SRC
    int rv = 0;
#else
    int rv = 1;
#endif

    /* read data from serial port */
    ser_data = (uint8_t *)read_pan_packet(&ser_len);

    /* process the packet we have received */
    if (ser_len && ser_data) {
      if (ser_data[0] != TOS_SERIAL_802_15_4_ID) {
        handle_other_pkt(ser_data, ser_len);
        goto discard_packet;
      }
      mac_hdr = (IEEE154_header_t *)(ser_data + 1);

      // size is  one for the length byte, minus two for the checksum
      pkt.len = mac_hdr->length - MAC_HEADER_SIZE - MAC_FOOTER_SIZE;
      // add one for the dispatch byte.
      pkt.data = ser_data + 1 + sizeof(IEEE154_header_t);

      // for some reason these are little endian so we don't do any conversion.
      pkt.src = mac_hdr->src;
      pkt.dst = mac_hdr->dest;

      log_dump_serial_packet(ser_data, ser_len);

      pkt.headers = getHeaderBitmap(&pkt);
      if (pkt.headers == LOWPAN_NALP_PATTERN) goto discard_packet;

      stats.rx_frags++;
      stats.rx_bytes += ser_len - 1;
      if (hasFrag1Header(&pkt) || hasFragNHeader(&pkt)) {
        unpack_info_t u_info;
        uint8_t amount_here;

        recon = getReassembly(&pkt);
        if (recon == NULL || recon->buf == NULL) goto discard_packet;
        msg = (struct split_ip_msg *)recon->buf;
        msg->prev_hop = pkt.src;

        if (hasFrag1Header(&pkt)) {
          if (unpackHeaders(&pkt, &u_info,
                            (uint8_t *)&msg->hdr, recon->size) == NULL) goto discard_packet;
          amount_here = pkt.len - (u_info.payload_start - pkt.data);
          // adjustPlen(&msg->hdr, &u_info);

          ip_memcpy(u_info.header_end, u_info.payload_start, amount_here);
          recon->bytes_rcvd = sizeof(struct ip6_hdr) + u_info.payload_offset + amount_here;
        } else {
          uint8_t offset_cmpr;
          uint16_t offset;
          if (getFragDgramOffset(&pkt, &offset_cmpr)) goto discard_packet;
          offset = offset_cmpr * 8;
          payload = getLowpanPayload(&pkt);
          amount_here = pkt.len - (payload - pkt.data);

          if (offset + amount_here > recon->size) goto discard_packet;
          ip_memcpy(((uint8_t *)&msg->hdr) + offset, payload, amount_here);
          recon->bytes_rcvd += amount_here;
          
          if (recon->size == recon->bytes_rcvd) {
            // got all the fragments...
            debug ("serial: reconstruction finished\n");
            add_header_list(msg);

            msg->metadata.sender = pkt.src;

            handle_serial_packet(msg);

            recon->timeout = T_UNUSED;
            free_split_msg(msg);
            free(recon->buf);
          }
        }

      } else {
        unpack_info_t u_info;
        // u_info.rih = NULL;
        msg = (struct split_ip_msg *)shortMsg;
        msg->prev_hop = pkt.src;

        if (unpackHeaders(&pkt, &u_info,
                          (uint8_t *)&msg->hdr, INET_MTU) == NULL) goto discard_packet;
        if (ntohs(msg->hdr.plen) > INET_MTU - sizeof(struct ip6_hdr)) goto discard_packet;
        // adjustPlen(&msg->hdr, &u_info);

        msg->metadata.sender = pkt.src;
/*         if (u_info.rih != NULL) */
/*           info("Has a rinstall_header for src 0x%x with match: 0x%x\n",  */
/*                pkt.src, ntohs(u_info.rih->match.dest));; */

        ip_memcpy(u_info.header_end, u_info.payload_start, ntohs(msg->hdr.plen));

        add_header_list(msg);
        
        handle_serial_packet(msg);
        free_split_msg(msg);
      }
    } else {
      //printf("no data on serial port, but FD triggered select\n");
      rv = 0;
    }
  discard_packet:
    // debug("serial_input: discard packet\n");
    free(ser_data);
    return rv;
}

void print_stats(int fd, int argc, char **argv) {
  VTY_HEAD;