icmp.c

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