icmp.c

   1 // L2TPNS: icmp
   2
   3 char const *cvs_id_icmp = "$Id: icmp.c,v 1.6 2004/12/16 08:49:53 bodea Exp $";
   4
   5 #include <arpa/inet.h>
   6 #include <netdb.h>
   7 #include <netinet/in.h>
   8 #include <asm/types.h>
   9 #include <linux/ip.h>
  10 #include <linux/icmp.h>
  11 #include <stdio.h>
  12 #include <sys/socket.h>
  13 #include <unistd.h>
  14 #include <sys/types.h>
  15 #include <sys/wait.h>
  16 #include <memory.h>
  17
  18 #include "l2tpns.h"
  19
  20 static uint16_t _checksum(unsigned char *addr, int count);
  21
  22 void host_unreachable(in_addr_t destination, uint16_t id, in_addr_t source, char *packet, int packet_len)
  23 {
  24         char buf[128] = {0};
  25         struct iphdr *iph;
  26         struct icmphdr *icmp;
  27         char *data;
  28         int len = 0, on = 1, icmp_socket;
  29         struct sockaddr_in whereto = {0};
  30
  31         if ((icmp_socket = socket(AF_INET, SOCK_RAW, IPPROTO_RAW)) < 0)
  32                 return;
  33
  34         setsockopt(icmp_socket, IPPROTO_IP, IP_HDRINCL, (char *)&on, sizeof(on));
  35
  36         whereto.sin_addr.s_addr = destination;
  37         whereto.sin_family = AF_INET;
  38
  39         iph = (struct iphdr *)(buf);
  40         len = sizeof(struct iphdr);
  41         icmp = (struct icmphdr *)(buf + len);
  42         len += sizeof(struct icmphdr);
  43         data = (char *)(buf + len);
  44         len += (packet_len < 64) ? packet_len : 64;
  45         memcpy(data, packet, (packet_len < 64) ? packet_len : 64);
  46
  47         iph->tos = 0;
  48         iph->id = id;
  49         iph->frag_off = 0;
  50         iph->ttl = 30;
  51         iph->check = 0;
  52         iph->version = 4;
  53         iph->ihl = 5;
  54         iph->protocol = 1;
  55         iph->check = 0;
  56         iph->daddr = destination;
  57         iph->saddr = source;
  58
  59         iph->tot_len = ntohs(len);
  60
  61         icmp->type = ICMP_DEST_UNREACH;
  62         icmp->code = ICMP_HOST_UNREACH;
  63         icmp->checksum = _checksum((char *) icmp, sizeof(struct icmphdr) + ((packet_len < 64) ? packet_len : 64));
  64
  65         iph->check = _checksum((char *) iph, sizeof(struct iphdr));
  66
  67         sendto(icmp_socket, (char *)buf, len, 0, (struct sockaddr *)&whereto, sizeof(struct sockaddr));
  68         close(icmp_socket);
  69 }
  70
  71 static uint16_t _checksum(unsigned char *addr, int count)
  72 {
  73         register long sum = 0;
  74
  75         for (; count > 1; count -= 2)
  76         {
  77                 sum += ntohs(*(uint32_t *) addr);
  78                 addr += 2;
  79         }
  80
  81         if (count > 1) sum += *(unsigned char *)addr;
  82
  83         // take only 16 bits out of the 32 bit sum and add up the carries
  84         while (sum >> 16)
  85                 sum = (sum & 0xFFFF) + (sum >> 16);
  86
  87         // one's complement the result
  88         sum = ~sum;
  89
  90         return htons((uint16_t) sum);
  91 }