ipdispatchp.nc

tinyos-2.x.rar

    table_map(&recon_cache, reconstruct_age);
    table_map(&forward_cache, forward_age);

    /*
    printfUART("Frag pool size: %i\n", call FragPool.size());
    printfUART("SendInfo pool size: %i\n", call SendInfoPool.size());
    printfUART("SendEntry pool size: %i\n", call SendEntryPool.size());
    printfUART("Forward queue length: %i\n", call SendQueue.size());
    */
    ip_print_heap();
  }

  /*
   * allocate a structure for recording information about incomming fragments.
   */

  reconstruct_t *get_reconstruct(ieee154_saddr_t src, uint16_t tag) {
    reconstruct_t *ret = NULL;
    int i;
    for (i = 0; i < N_RECONSTRUCTIONS; i++) {
      reconstruct_t *recon = (reconstruct_t *)&recon_data[i];
      dbg("IPDispatch", " 0x%x 0x%x 0x%x\n",  recon->timeout, recon->metadata.sender, recon->tag);

      if (recon->tag == tag &&
          recon->metadata.sender == src) {

        if (recon->timeout > T_UNUSED) {
          
          recon->timeout = T_ACTIVE;
          return recon;

        } else if (recon->timeout < T_UNUSED) {
          // if we have already tried and failed to get a buffer, we
          // need to drop remaining fragments.
          return NULL;
        }
      }
      if (recon->timeout == T_UNUSED) 
        ret = recon;
    }
    return ret;
  }
  
  /*
   * This is called before a receive on packets with a source routing header.
   *
   * it updates the path stored in the header to remove our address
   * and include our predicessor.
   *
   * However, if this is not a source record path and we are not in the current
   *  spot, this means we are along the default path and so should invalidate this
   *  source header.
   */
  void updateSourceRoute(ieee154_saddr_t prev_hop, struct ip6_route *sh) {
    uint16_t my_address = call IPAddress.getShortAddr();
    uint16_t target_hop = sh->hops[ROUTE_NENTRIES(sh) - sh->segs_remain];
    if ((sh->type & ~IP6ROUTE_FLAG_MASK) == IP6ROUTE_TYPE_INVAL || sh->segs_remain == 0) return;

    if (target_hop != htons(my_address)) {
      printfUART("invalidating source route\n");

      if (ROUTE_NENTRIES(sh) >= 2) {
        sh->hops[0] = htons(prev_hop);
        sh->hops[1] = target_hop;
      }
      sh->type = (sh->type & IP6ROUTE_FLAG_MASK) | IP6ROUTE_TYPE_INVAL;
    } else {
      sh->hops[ROUTE_NENTRIES(sh) - sh->segs_remain] = htons(prev_hop);
      sh->segs_remain--;
      printfUART("updating source route with prev: 0x%x remaining: %i\n",
                 prev_hop, sh->segs_remain);
    }
  }

  message_t *handle1stFrag(message_t *msg, packed_lowmsg_t *lowmsg) {
    uint8_t *unpack_buf;
    struct ip6_hdr *ip;

    uint16_t real_payload_length;// , real_offset = sizeof(struct ip6_hdr);

    unpack_info_t u_info;

    unpack_buf = ip_malloc(LIB6LOWPAN_MAX_LEN + LOWPAN_LINK_MTU);
    if (unpack_buf == NULL) return msg;

    // unpack all the compressed headers.  this means the IP headers,
    // and possibly also the UDP ones if there are no hop-by-hop
    // options.
    ip_memclr(unpack_buf, LIB6LOWPAN_MAX_LEN + LOWPAN_LINK_MTU);
    if (unpackHeaders(lowmsg, &u_info,
                      unpack_buf, LIB6LOWPAN_MAX_LEN) == NULL) {
      ip_free(unpack_buf);
      return msg;
    }
    
    ip = (struct ip6_hdr *)unpack_buf;


    if (u_info.hdr_route != NULL) {
      // this updates the source route in the message_t, if it
      // exists...
      updateSourceRoute(call Ieee154Packet.source(msg),
                        u_info.hdr_route);
    }

    // we handle the extension headers generically now
    signal IPExtensions.handleExtensions(current_local_label++,
                                         ip,
                                         u_info.hdr_hop,
                                         u_info.hdr_dest,
                                         u_info.hdr_route,
                                         u_info.nxt_hdr);
    
    // first check if we forward or consume it
    if (call IPRouting.isForMe(ip)) {
      struct ip_metadata metadata;
      dbg("IPDispatch", "is for me!\n");
      // consume it:
      //   - get a buffer
      //   - if fragmented, wait for remaining fragments
      //   - if not, dispatch from here.

      metadata.sender = call Ieee154Packet.source(msg);
      metadata.lqi = call ReadLqi.read(msg);

      real_payload_length = ntohs(ip->plen);
      adjustPlen(ip, &u_info);

      if (!hasFrag1Header(lowmsg)) {
        uint16_t amount_here = lowmsg->len - (u_info.payload_start - lowmsg->data);

#if 0
        int i;
        for (i = 0; i < 48; i++)
          printfUART("0x%x ", ((uint8_t *)ip)[i]);
        printfUART("\n");
        for (i = 0; i < 8; i++)
          printfUART("0x%x ", ((uint8_t *)u_info.payload_start)[i]);
        printfUART("\n");
#endif

        // we can fill in the data and deliver the packet from here.
        // this is the easy case...
        // we malloc'ed a bit extra in this case so we don't have to
        //  copy the IP header; we can just add the payload after the unpacked
        //  buffers.
        // if (rcv_buf == NULL) goto done;
        ip_memcpy(u_info.header_end, u_info.payload_start, amount_here);

        printfUART("IP.recv[%i] here: %i\n", amount_here);
        signal IP.recv[u_info.nxt_hdr](ip, u_info.transport_ptr, &metadata);
      } else {
        // in this case, we need to set up a reconstruction
        // structure so when the next packets come in, they can be
        // filled in.
        reconstruct_t *recon;
        uint16_t tag, amount_here = lowmsg->len - (u_info.payload_start - lowmsg->data);
        void *rcv_buf;

        if (getFragDgramTag(lowmsg, &tag)) goto fail;
        
        dbg("IPDispatch", "looking up frag tag: 0x%x\n", tag);
        recon = get_reconstruct(lowmsg->src, tag);
        
        // allocate a new struct for doing reassembly.
        if (recon == NULL) {
          goto fail;
        }
        
        // the total size of the IP packet
        rcv_buf = ip_malloc(real_payload_length + sizeof(struct ip6_hdr));

        recon->metadata.sender = lowmsg->src;
        recon->tag = tag;
        recon->size = real_payload_length + sizeof(struct ip6_hdr);
        recon->buf = rcv_buf;
        recon->nxt_hdr = u_info.nxt_hdr;
        recon->transport_hdr = ((uint8_t *)rcv_buf) + (u_info.transport_ptr - unpack_buf);
        recon->bytes_rcvd = u_info.payload_offset + amount_here + sizeof(struct ip6_hdr);
        recon->timeout = T_ACTIVE;

        if (rcv_buf == NULL) {
          // if we didn't get a buffer better not memcopy anything
          recon->timeout = T_FAILED1;
          recon->size = 0;
          goto fail;
        }
        if (amount_here > recon->size - sizeof(struct ip6_hdr)) {
          call Leds.led1Toggle();
          recon->timeout = T_FAILED1;
          recon->size = 0;
          ip_free(rcv_buf);
          recon->buf = NULL;
          goto fail;
        }

        ip_memcpy(rcv_buf, unpack_buf, u_info.payload_offset + sizeof(struct ip6_hdr));
        ip_memcpy(rcv_buf + u_info.payload_offset + sizeof(struct ip6_hdr), 
                  u_info.payload_start, amount_here);
        ip_memcpy(&recon->metadata, &metadata, sizeof(struct ip_metadata));

        goto done;
        // that's it, we just filled in the first piece of the fragment
      } 
    } else {
      // otherwise set up forwarding information for the next
      // fragments and enqueue this message_t on its merry way.
      send_info_t *s_info;
      send_entry_t *s_entry;
      forward_entry_t *fwd;
      message_t *msg_replacement;
      
      // this is a pointer to the hop-limit field in the packed fragment
      *u_info.hlim = *u_info.hlim - 1;
      if (*u_info.hlim == 0) {
#ifndef NO_ICMP_TIME_EXCEEDED
        uint16_t amount_here = lowmsg->len - (u_info.payload_start - lowmsg->data);
        call ICMP.sendTimeExceeded(ip, &u_info, amount_here);
#endif
        // by bailing here and not setting up an entry in the
        // forwarding cache, following fragments will be dropped like
        // they should be.  we don't strictly follow the RFC that says
        // we should return at least 64 bytes of payload.
        ip_free(unpack_buf);
        return msg;
      }
      s_info = getSendInfo();
      s_entry = call SendEntryPool.get();
      msg_replacement = call FragPool.get();
      if (s_info == NULL || s_entry == NULL || msg_replacement == NULL) {
        if (s_info != NULL) 
          SENDINFO_DECR(s_info);
        if (s_entry != NULL)
          call SendEntryPool.put(s_entry);
        if (msg_replacement != NULL) 
          call FragPool.put(msg_replacement);
        goto fail;
      }

      if (call IPRouting.getNextHop(ip, u_info.hdr_route, 
                                    lowmsg->src, &s_info->policy) != SUCCESS)
        goto fwd_fail;

      dbg("IPDispatch", "next hop is: 0x%x\n", s_info->policy.dest[0]);

      if (hasFrag1Header(lowmsg)) {
        fwd = table_search(&forward_cache, forward_unused);
        if (fwd == NULL) {
          goto fwd_fail;
        }

        fwd->timeout = T_ACTIVE;
        fwd->l2_src = call Ieee154Packet.source(msg);
        getFragDgramTag(lowmsg, &fwd->old_tag);
        fwd->new_tag = ++lib6lowpan_frag_tag;
        // forward table gets a reference
        SENDINFO_INCR(s_info);
        fwd->s_info = s_info;
        setFragDgramTag(lowmsg, lib6lowpan_frag_tag);
      } 

      // give a reference to the send_entry
      SENDINFO_INCR(s_info);
      s_info->local_flow_label = current_local_label - 1;
      s_entry->msg = msg;
      s_entry->info = s_info;

      if (call SendQueue.enqueue(s_entry) != SUCCESS)
        BLIP_STATS_INCR(stats.encfail);
      post sendTask();

      BLIP_STATS_INCR(stats.forwarded);

      // s_info leaves lexical scope;
      SENDINFO_DECR(s_info);
      ip_free(unpack_buf);
      return msg_replacement;

    fwd_fail:
      call FragPool.put(msg_replacement);
      call SendInfoPool.put(s_info);
      call SendEntryPool.put(s_entry);
    }
    
   

  fail:
  done:
    ip_free(unpack_buf);
    return msg;
  }

  event message_t *Ieee154Receive.receive(message_t *msg, void *msg_payload, uint8_t len) {
    packed_lowmsg_t lowmsg;

    printfUART("p1: %p p2: %p\n", msg_payload, call Packet.getPayload(msg, 0));
    // set up the ip message structaddFragment
    lowmsg.data = msg_payload;
    lowmsg.len  = len;
    lowmsg.src  = call Ieee154Packet.source(msg);
    lowmsg.dst  = call Ieee154Packet.destination(msg);

    printfUART("receive(): %i\n", len);

    BLIP_STATS_INCR(stats.rx_total);

    call IPRouting.reportReception(call Ieee154Packet.source(msg),
                                   call ReadLqi.read(msg));

    lowmsg.headers = getHeaderBitmap(&lowmsg);
    if (lowmsg.headers == LOWPAN_NALP_PATTERN) {
      goto fail;
    }

    // consume it
    if (!hasFragNHeader(&(lowmsg))) {
      // in this case, we need to unpack the addressing information
      // and either dispatch the packet locally or forward it.
      msg = handle1stFrag(msg, &lowmsg);
      goto done;
    } else {
      // otherwise, it's a fragN packet, and we just need to copy it
      // into a buffer or forward it.
      forward_entry_t *fwd;
      reconstruct_t *recon;
      uint8_t offset_cmpr;
      uint16_t offset, amount_here, tag;
      uint8_t *payload;

      if (getFragDgramTag(&lowmsg, &tag)) goto fail;
      if (getFragDgramOffset(&lowmsg, &offset_cmpr)) goto fail;

      forward_lookup_tag = tag;
      forward_lookup_src = call Ieee154Packet.source(msg);

      fwd = table_search(&forward_cache, forward_lookup);
      payload = getLowpanPayload(&lowmsg);
      
      recon = get_reconstruct(lowmsg.src, tag);
      if (recon != NULL && recon->timeout > T_UNUSED && recon->buf != NULL) {
        // for packets we are reconstructing.
        
        offset =  (offset_cmpr * 8); 
        amount_here = lowmsg.len - (payload - lowmsg.data);
        
        if (offset + amount_here > recon->size) goto fail;
        ip_memcpy(recon->buf + offset, payload, amount_here);
        
        recon->bytes_rcvd += amount_here;
        
        printfUART("sz: %i rcv: %i\n", recon->size, recon->bytes_rcvd);
        if (recon->size == recon->bytes_rcvd) { 
          // signal and free the recon.
          signalDone(recon);
        }
      } else if (fwd != NULL && fwd->timeout > T_UNUSED) {
        // this only catches if we've forwarded all the past framents
        // successfully.