Example #1
0
void handle_not_repsponding_to_arp(byte *payload, unsigned len) {
    debug_println("Not responding to arp:\n");
    
    
    packet_info_t *pi = malloc_or_die(sizeof(packet_info_t));                   //Free'd (below).
    pi->packet = malloc_or_die((IPV4_HEADER_OFFSET+len)*sizeof(uint8_t));       //Free'd (below).
    pi->len = len;
    
    memcpy(pi->packet+IPV4_HEADER_OFFSET, payload, len);
    
    struct eth_hdr *ethhdr = (void *)pi->packet;
    ETH_DEST_SET(ethhdr, make_mac_addr(0, 0, 0, 0, 0, 0));
    ETH_SRC_SET(ethhdr, make_mac_addr(0, 0, 0, 0, 0, 0));
    ETH_TYPE_SET(ethhdr, 0);
    
    //Reverse soruce and destination again.
    struct ip_hdr *iphdr;/* = (void *)pi->packet+IPV4_HEADER_OFFSET;
    swap_bytes(&IPH_SRC(iphdr), &IPH_DEST(iphdr), 4);*/
    
    unsigned i;
    for (i = 0; i < pi->len; i += 2)
        printf("%02X%02X ", *(pi->packet+i),*(pi->packet+i+1));
    printf("\n");
    
    if (generate_response_ICMP_packet(pi, 3, 1)) return;
    iphdr = (void *)pi->packet+IPV4_HEADER_OFFSET; //pi->packet have moved in memory, so re-define.
    
    IPH_CHKSUM_SET(iphdr, 0);
    IPH_CHKSUM_SET(iphdr, htons(calc_checksum(pi->packet+IPV4_HEADER_OFFSET, IPV4_HEADER_LENGTH)));
    
    for (i = 0; i < pi->len; i += 2)
        printf("%02X%02X ", *(pi->packet+i),*(pi->packet+i+1));
    printf("\n");
    
    addr_ip_t target_ip = sr_integ_findnextip(IPH_DEST(iphdr));
    char target_ip_str[STRLEN_IP];
    ip_to_string(target_ip_str, target_ip);
    debug_println("target_ip=%s", target_ip_str);
    
    //send_packet(pi->packet+IPV4_HEADER_OFFSET, IPH_SRC(iphdr), target_ip, pi->len-IPV4_HEADER_OFFSET, FALSE, FALSE);

    free(pi->packet);
    free(pi);
}
Example #2
0
File: udp.c Project: ORCOS/ORCOS
/**
 * Process an incoming UDP datagram.
 *
 * Given an incoming UDP datagram (as a chain of pbufs) this function
 * finds a corresponding UDP PCB and hands over the pbuf to the pcbs
 * recv function. If no pcb is found or the datagram is incorrect, the
 * pbuf is freed.
 *
 * @param p pbuf to be demultiplexed to a UDP PCB.
 * @param inp network interface on which the datagram was received.
 *
 */
void
udp_input(struct pbuf *p, struct netif *inp)
{
    struct udp_hdr *udphdr;
    struct udp_pcb *pcb, *prev;
    struct udp_pcb *uncon_pcb;
    struct ip_hdr *iphdr;
    u16_t src, dest;
    u8_t local_match;
    u8_t broadcast;
    struct ip_addr dest_ipaddr;
    struct ip_addr src_ipaddr;

    PERF_START;

    UDP_STATS_INC(udp.recv);

    iphdr = p->payload;

    /* Check minimum length (IP header + UDP header)
     * and move payload pointer to UDP header */
    if (p->tot_len < (IPH_HL(iphdr) + UDP_HLEN) || pbuf_header(p, -(s16_t) (IPH_HL(iphdr)))) {
        /* drop short packets */
        LWIP_DEBUGF(UDP_DEBUG, ("udp_input: short UDP datagram (%"U16_F" bytes) discarded"NEWLINE, p->tot_len));
        UDP_STATS_INC(udp.lenerr);
        UDP_STATS_INC(udp.drop);
        snmp_inc_udpinerrors();
        pbuf_free(p);
        goto end;
    }

    udphdr = (struct udp_hdr *) p->payload;

    IPH_DEST(iphdr, &dest_ipaddr);
    IPH_SRC(iphdr, &src_ipaddr);

    /* is broadcast packet ? */
    broadcast = ip_addr_isbroadcast(&dest_ipaddr, inp);

    LWIP_DEBUGF(UDP_DEBUG, ("udp_input: received datagram of length %"U16_F""NEWLINE, p->tot_len));

    /* convert src and dest ports to host byte order */
    src = ntohs(udphdr->src);
    dest = ntohs(udphdr->dest);

    udp_debug_print(udphdr);

    /* print the UDP source and destination */
    /* LWIP_DEBUGF(UDP_DEBUG,
     ("udp (%"U16_F".%"U16_F".%"U16_F".%"U16_F", %"U16_F") <-- "
     "(%"U16_F".%"U16_F".%"U16_F".%"U16_F", %"U16_F")"NEWLINE,
     ip4_addr1(&iphdr->dest), ip4_addr2(&iphdr->dest),
     ip4_addr3(&iphdr->dest), ip4_addr4(&iphdr->dest), ntohs(udphdr->dest),
     ip4_addr1(&iphdr->src), ip4_addr2(&iphdr->src),
     ip4_addr3(&iphdr->src), ip4_addr4(&iphdr->src), ntohs(udphdr->src)));*/

#if LWIP_DHCP
    pcb = NULL;
    /* when LWIP_DHCP is active, packets to DHCP_CLIENT_PORT may only be processed by
     the dhcp module, no other UDP pcb may use the local UDP port DHCP_CLIENT_PORT */
  if (dest == DHCP_CLIENT_PORT) {
        /* all packets for DHCP_CLIENT_PORT not coming from DHCP_SERVER_PORT are dropped! */
    if (src == DHCP_SERVER_PORT) {
      if ((inp->dhcp != NULL) && (inp->dhcp->pcb != NULL)) {
                /* accept the packe if
                 (- broadcast or directed to us) -> DHCP is link-layer-addressed, local ip is always ANY!
                 - inp->dhcp->pcb->remote == ANY or iphdr->src */
                if ((ip_addr_isany(&inp->dhcp->pcb->remote_ip) ||
                                ip_addr_cmp(&(inp->dhcp->pcb->remote_ip), &src_ipaddr)))
                {
                    pcb = inp->dhcp->pcb;
                }
            }
        }
  } else
#endif /* LWIP_DHCP */
    {
        prev = NULL;
        local_match = 0;
        uncon_pcb = NULL;
        /* Iterate through the UDP pcb list for a matching pcb.
         * 'Perfect match' pcbs (connected to the remote port & ip address) are
         * preferred. If no perfect match is found, the first unconnected pcb that
         * matches the local port and ip address gets the datagram. */
        for (pcb = udp_pcbs; pcb != NULL; pcb = pcb->next) {
            local_match = 0;
            /* print the PCB local and remote address */
            /*    LWIP_DEBUGF(UDP_DEBUG,
             ("pcb (%"U16_F".%"U16_F".%"U16_F".%"U16_F", %"U16_F") --- "
             "(%"U16_F".%"U16_F".%"U16_F".%"U16_F", %"U16_F")"NEWLINE,
             ip4_addr1(&pcb->local_ip), ip4_addr2(&pcb->local_ip),
             ip4_addr3(&pcb->local_ip), ip4_addr4(&pcb->local_ip), pcb->local_port,
             ip4_addr1(&pcb->remote_ip), ip4_addr2(&pcb->remote_ip),
             ip4_addr3(&pcb->remote_ip), ip4_addr4(&pcb->remote_ip), pcb->remote_port)); */

            /* compare PCB local addr+port to UDP destination addr+port */
            if ((pcb->local_port == dest) && ((!broadcast && ip_addr_isany(&pcb->local_ip)) || ip_addr_cmp(&(pcb->local_ip), &(dest_ipaddr)) ||
#if LWIP_IGMP
                    ip_addr_ismulticast(&(iphdr->dest)) ||
#endif /* LWIP_IGMP */
#if IP_SOF_BROADCAST_RECV
                    (broadcast && (pcb->so_options & SOF_BROADCAST))))
                    {
#else  /* IP_SOF_BROADCAST_RECV */
                    (broadcast)))
            {
#endif /* IP_SOF_BROADCAST_RECV */
                local_match = 1;
                if ((uncon_pcb == NULL) && ((pcb->flags & UDP_FLAGS_CONNECTED) == 0)) {
                    /* the first unconnected matching PCB */
                    uncon_pcb = pcb;
                }
            }

            /* compare PCB remote addr+port to UDP source addr+port */
            if ((local_match != 0) && (pcb->remote_port == src) && (ip_addr_isany(&pcb->remote_ip) || ip_addr_cmp(&(pcb->remote_ip), &(src_ipaddr)))) {
                /* the first fully matching PCB */
                if (prev != NULL) {
                    /* move the pcb to the front of udp_pcbs so that is
                     found faster next time */
                    prev->next = pcb->next;
                    pcb->next = udp_pcbs;
                    udp_pcbs = pcb;
                } else {
                    UDP_STATS_INC(udp.cachehit);
                }
                break;
            }
            prev = pcb;
        }
        /* no fully matching pcb found? then look for an unconnected pcb */
        if (pcb == NULL) {
            pcb = uncon_pcb;
        }
    }

    u8_t cmp_addr;

    if (iphdr->v == IPV4) {
        cmp_addr = ip4_addr_cmp(&dest_ipaddr.addr.ip4addr, &inp->ip4_addr);
    } else {
        cmp_addr = ip6_addr_cmp(&dest_ipaddr.addr.ip6addr, &inp->ip6_addr);
    }

    /* Check checksum if this is a match or if it was directed at us. */
    if (pcb != NULL || cmp_addr) {
        LWIP_DEBUGF(UDP_DEBUG | LWIP_DBG_TRACE, ("udp_input: calculating checksum"NEWLINE));
#if LWIP_UDPLITE
        if (IPH_PROTO(iphdr) == IP_PROTO_UDPLITE) {
            /* Do the UDP Lite checksum */
#if CHECKSUM_CHECK_UDP
            u16_t chklen = ntohs(udphdr->len);
            if (chklen < sizeof(struct udp_hdr)) {
                if (chklen == 0) {
                    /* For UDP-Lite, checksum length of 0 means checksum
                     over the complete packet (See RFC 3828 chap. 3.1) */
                    chklen = p->tot_len;
        } else {
                    /* At least the UDP-Lite header must be covered by the
                     checksum! (Again, see RFC 3828 chap. 3.1) */
                    UDP_STATS_INC(udp.chkerr);
                    UDP_STATS_INC(udp.drop);
                    snmp_inc_udpinerrors();
                    pbuf_free(p);
                    goto end;
                }
            }
            if (inet_chksum_pseudo_partial(p,
                                           (struct ip_addr *)&(iphdr->src),
                                           (struct ip_addr *)&(iphdr->dest),
                                           IP_PROTO_UDPLITE,
                                           p->tot_len, chklen) != 0) {
                LWIP_DEBUGF(UDP_DEBUG | LWIP_DBG_LEVEL_SERIOUS,
                        ("udp_input: UDP Lite datagram discarded due to failing checksum"NEWLINE));
                UDP_STATS_INC(udp.chkerr);
                UDP_STATS_INC(udp.drop);
                snmp_inc_udpinerrors();
                pbuf_free(p);
                goto end;
            }
#endif /* CHECKSUM_CHECK_UDP */
    } else
#endif /* LWIP_UDPLITE */
        {
#if CHECKSUM_CHECK_UDP
            if (udphdr->chksum != 0) {
                if (inet_chksum_pseudo(p, &src_ipaddr.addr, &dest_ipaddr.addr, src_ipaddr.version, IP4_PROTO_UDP, p->tot_len) != 0) {
                    LWIP_DEBUGF(UDP_DEBUG | LWIP_DBG_LEVEL_SERIOUS, ("udp_input: UDP datagram discarded due to failing checksum"NEWLINE));
                    UDP_STATS_INC(udp.chkerr);
                    UDP_STATS_INC(udp.drop);
                    snmp_inc_udpinerrors();
                    pbuf_free(p);
                    goto end;
                }
            }
#endif /* CHECKSUM_CHECK_UDP */
        }
        if (pbuf_header(p, -UDP_HLEN)) {
            /* Can we cope with this failing? Just assert for now */
            LWIP_ASSERT("pbuf_header failed"NEWLINE, 0);UDP_STATS_INC(udp.drop);snmp_inc_udpinerrors();
            pbuf_free(p);
            goto end;
        }
        if (pcb != NULL) {
            snmp_inc_udpindatagrams();
#if SO_REUSE && SO_REUSE_RXTOALL
      if ((broadcast || ip_addr_ismulticast(&current_iphdr_dest)) &&
          ip_get_option(pcb, SOF_REUSEADDR)) {
        /* pass broadcast- or multicast packets to all multicast pcbs
           if SOF_REUSEADDR is set on the first match */
        struct udp_pcb *mpcb;
        u8_t p_header_changed = 0;
        for (mpcb = udp_pcbs; mpcb != NULL; mpcb = mpcb->next) {
          if (mpcb != pcb) {
            /* compare PCB local addr+port to UDP destination addr+port */
            if ((mpcb->local_port == dest) &&
                ((!broadcast && ip_addr_isany(&mpcb->local_ip)) ||
                 ip_addr_cmp(&(mpcb->local_ip), &current_iphdr_dest) ||
#if LWIP_IGMP
                 ip_addr_ismulticast(&current_iphdr_dest) ||
#endif /* LWIP_IGMP */
#if IP_SOF_BROADCAST_RECV
                 (broadcast && ip_get_option(mpcb, SOF_BROADCAST)))) {
#else  /* IP_SOF_BROADCAST_RECV */
                 (broadcast))) {
#endif /* IP_SOF_BROADCAST_RECV */
              /* pass a copy of the packet to all local matches */
              if (mpcb->recv != NULL) {
                struct pbuf *q;
                /* for that, move payload to IP header again */
                if (p_header_changed == 0) {
                  pbuf_header(p, (s16_t)((IPH_HL(iphdr) * 4) + UDP_HLEN));
                  p_header_changed = 1;
                }
                q = pbuf_alloc(PBUF_RAW, p->tot_len, PBUF_RAM);
                if (q != NULL) {
                  err_t err = pbuf_copy(q, p);
                  if (err == ERR_OK) {
                    /* move payload to UDP data */
                    pbuf_header(q, -(s16_t)((IPH_HL(iphdr) * 4) + UDP_HLEN));
                    mpcb->recv(mpcb->recv_arg, mpcb, q, ip_current_src_addr(), src);
                  }
                }
              }
            }
          }
        }
        if (p_header_changed) {
          /* and move payload to UDP data again */
          pbuf_header(p, -(s16_t)((IPH_HL(iphdr) * 4) + UDP_HLEN));
        }
      }
#endif /* SO_REUSE && SO_REUSE_RXTOALL */
            /* callback */
            if (pcb->recv != NULL) {
                /* now the recv function is responsible for freeing p */
                pcb->recv(pcb->recv_arg, pcb, p, &src_ipaddr, src);
            } else {
                /* no recv function registered? then we have to free the pbuf! */
                pbuf_free(p);
                goto end;
            }
        } else {
Example #3
0
bool generate_response_ICMP_packet(packet_info_t *pi, int type, int code) {
    byte *old_packet = malloc_or_die(pi->len-IPV4_HEADER_OFFSET); //Free'd (below).
    memcpy(old_packet, pi->packet+IPV4_HEADER_OFFSET, pi->len-IPV4_HEADER_OFFSET);
    
    struct ip_hdr *old_iphdr = (void *)old_packet;
    
    free(pi->packet);
    pi->len = IPV4_HEADER_OFFSET+IPV4_HEADER_LENGTH+36;//14 for Ethernet header, 20 for IPv4 header and 36 for ICMP time exceeded packet.
    pi->packet = malloc_or_die((pi->len)*sizeof(byte)); //Free'd (before).
    
    struct eth_hdr *ethhdr = (void *)pi->packet;
    ETH_DEST_SET(ethhdr, make_mac_addr(0, 0, 0, 0, 0, 0));
    ETH_SRC_SET(ethhdr, make_mac_addr(0, 0, 0, 0, 0, 0));
    ETH_TYPE_SET(ethhdr, 0);
    
    struct ip_hdr *iphdr = (void *)pi->packet+IPV4_HEADER_OFFSET;
    
    IPH_VHLTOS_SET(iphdr, 4, 5, 0); //Set version and IHL.
    IPH_LEN_SET(iphdr, htons(pi->len-IPV4_HEADER_OFFSET));
    IPH_ID_SET(iphdr, 0);
    IPH_OFFSET_SET(iphdr, 0);
    IPH_TTL_SET(iphdr, 32);
    IPH_PROTO_SET(iphdr, 1);
    
    addr_ip_t dest = IPH_SRC(old_iphdr);
    addr_ip_t target = sr_integ_findnextip(dest);
    interface_t *source_intf = sr_integ_findsrcintf(target);
    
    if (source_intf == NULL) {
        char ip_str[STRLEN_IP], target_str[STRLEN_IP];
        ip_to_string(ip_str, dest);
        ip_to_string(target_str, target);
        debug_println("No route to source %s with target %s, dropping packet!", ip_str, target_str);
        free(old_packet);
        return 1;
    }
    addr_ip_t source = source_intf->ip;
    
    char dest_str[15];
    ip_to_string(dest_str, dest);
    char source_str[15];
    ip_to_string(source_str, source);
    
    debug_println("dest=%s, source=%s", dest_str, source_str);
    
    memcpy(&IPH_SRC(iphdr), &source, 6);
    memcpy(&IPH_DEST(iphdr), &dest, 6);
    
    byte *icmp_packet = pi->packet+IPV4_HEADER_OFFSET+IPV4_HEADER_LENGTH;
    struct icmp_dur_hdr *icmp_hdr = (void *)pi->packet+IPV4_HEADER_OFFSET+IPV4_HEADER_LENGTH;
    ICMPH_TYPE_SET(icmp_hdr, type);
    ICMPH_CODE_SET(icmp_hdr, code);
    ICMPH_UNUSED_SET(icmp_hdr);
    
    memcpy(icmp_packet+ICMP_TIME_EX_IP_HEADER_OFFSET, old_packet, IPV4_HEADER_LENGTH + 8);
    ICMPH_CHKSUM_SET(icmp_hdr, 0);
    ICMPH_CHKSUM_SET(icmp_hdr, htons(calc_checksum(icmp_packet, pi->len-IPV4_HEADER_OFFSET-IPV4_HEADER_LENGTH)));
    
    free(old_packet);
    return 0;
}