icmp.c

   1 // L2TPNS: icmp
   2
   3 char const *cvs_id_icmp = "$Id: icmp.c,v 1.11 2006-04-27 09:53:49 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 <netinet/icmp6.h>
  12 #include <stdio.h>
  13 #include <sys/socket.h>
  14 #include <unistd.h>
  15 #include <sys/types.h>
  16 #include <sys/wait.h>
  17 #include <memory.h>
  18
  19 #include "l2tpns.h"
  20
  21 static uint16_t _checksum(uint8_t *addr, int count);
  22
  23 struct ipv6_pseudo_hdr {
  24         struct in6_addr src;
  25         struct in6_addr dest;
  26         uint32_t ulp_length;
  27         uint32_t zero    : 24;
  28         uint32_t nexthdr :  8;
  29 };
  30
  31 void host_unreachable(in_addr_t destination, uint16_t id, in_addr_t source, uint8_t *packet, int packet_len)
  32 {
  33         char buf[128] = {0};
  34         struct iphdr *iph;
  35         struct icmphdr *icmp;
  36         int len = 0, on = 1, icmp_socket;
  37         struct sockaddr_in whereto = {0};
  38
  39         if ((icmp_socket = socket(AF_INET, SOCK_RAW, IPPROTO_RAW)) < 0)
  40                 return;
  41
  42         setsockopt(icmp_socket, IPPROTO_IP, IP_HDRINCL, (char *)&on, sizeof(on));
  43
  44         whereto.sin_addr.s_addr = destination;
  45         whereto.sin_family = AF_INET;
  46
  47         iph = (struct iphdr *)(buf);
  48         len = sizeof(struct iphdr);
  49         icmp = (struct icmphdr *)(buf + len);
  50         len += sizeof(struct icmphdr);
  51
  52         /* ip header + first 8 bytes of payload */
  53         if (packet_len > (sizeof(struct iphdr) + 8))
  54                 packet_len = sizeof(struct iphdr) + 8;
  55
  56         memcpy(buf + len, packet, packet_len);
  57         len += packet_len;
  58
  59         iph->tos = 0;
  60         iph->id = id;
  61         iph->frag_off = 0;
  62         iph->ttl = 30;
  63         iph->check = 0;
  64         iph->version = 4;
  65         iph->ihl = 5;
  66         iph->protocol = 1;
  67         iph->check = 0;
  68         iph->daddr = destination;
  69         iph->saddr = source;
  70
  71         iph->tot_len = ntohs(len);
  72
  73         icmp->type = ICMP_DEST_UNREACH;
  74         icmp->code = ICMP_HOST_UNREACH;
  75         icmp->checksum = _checksum((uint8_t *) icmp, sizeof(struct icmphdr) + packet_len);
  76
  77         iph->check = _checksum((uint8_t *) iph, sizeof(struct iphdr));
  78
  79         sendto(icmp_socket, buf, len, 0, (struct sockaddr *)&whereto, sizeof(struct sockaddr));
  80         close(icmp_socket);
  81 }
  82
  83 static uint16_t _checksum(uint8_t *addr, int count)
  84 {
  85         register long sum = 0;
  86
  87         for (; count > 1; count -= 2)
  88         {
  89                 sum += ntohs(*(uint32_t *) addr);
  90                 addr += 2;
  91         }
  92
  93         if (count > 1) sum += *(unsigned char *)addr;
  94
  95         // take only 16 bits out of the 32 bit sum and add up the carries
  96         while (sum >> 16)
  97                 sum = (sum & 0xFFFF) + (sum >> 16);
  98
  99         // one's complement the result
 100         sum = ~sum;
 101
 102         return htons((uint16_t) sum);
 103 }
 104
 105 void send_ipv6_ra(sessionidt s, tunnelidt t, struct in6_addr *ip)
 106 {
 107         struct nd_opt_prefix_info *pinfo;
 108         struct ipv6_pseudo_hdr *phdr;
 109         uint8_t b[MAXETHER + 20];
 110         uint8_t c[MAXETHER + 20];
 111         int l;
 112         uint8_t *o;
 113
 114         LOG(3, s, t, "Sending IPv6 RA\n");
 115
 116         memset(b, 0, sizeof(b));
 117         o = makeppp(b, sizeof(b), 0, 0, s, t, PPPIPV6, 0, 0, 0);
 118
 119         if (!o)
 120         {
 121                 LOG(3, s, t, "failed to send IPv6 RA\n");
 122                 return;
 123         }
 124
 125         *o = 0x60;                      // IPv6
 126         *(o+1) = 0;
 127         *(o+5) = 48;                    // Length of payload (not header)
 128         *(o+6) = 58;                    // icmp6 is next
 129         *(o+7) = 255;                   // Hop limit
 130         memset(o+8, 0, 16);             // source = FE80::1
 131         *(o+8) = 0xFE;
 132         *(o+9) = 0x80;
 133         *(o+23) = 1;
 134         if (ip != NULL)
 135                 memcpy(o+24, ip, 16);   // dest = ip
 136         else
 137         {
 138                 // FF02::1 - all hosts
 139                 *(o+24) = 0xFF;
 140                 *(o+25) = 2;
 141                 *(o+39) = 1;
 142         }
 143         *(o+40) = 134;                  // RA message
 144         *(o+41) = 0;                    // Code
 145         *(o+42) = *(o+43) = 0;          // Checksum
 146         *(o+44) = 64;                   // Hop count
 147         *(o+45) = 0;                    // Flags
 148         *(o+46) = *(o+47) = 255;        // Lifetime
 149         *(uint32_t *)(o+48) = 0;        // Reachable time
 150         *(uint32_t *)(o+52) = 0;        // Retrans timer
 151         pinfo = (struct nd_opt_prefix_info *)(o+56);
 152         pinfo->nd_opt_pi_type           = ND_OPT_PREFIX_INFORMATION;
 153         pinfo->nd_opt_pi_len            = 4;
 154         pinfo->nd_opt_pi_prefix_len     = 64; // prefix length
 155         pinfo->nd_opt_pi_flags_reserved = ND_OPT_PI_FLAG_ONLINK;
 156         pinfo->nd_opt_pi_flags_reserved |= ND_OPT_PI_FLAG_AUTO;
 157         pinfo->nd_opt_pi_valid_time     = htonl(2592000);
 158         pinfo->nd_opt_pi_preferred_time = htonl(604800);
 159         pinfo->nd_opt_pi_reserved2      = 0;
 160         pinfo->nd_opt_pi_prefix         = config->ipv6_prefix;
 161         l = sizeof(*pinfo) + 56;
 162
 163         memset(c, 0, sizeof(c));
 164         phdr = (struct ipv6_pseudo_hdr *) c;
 165         memcpy(&phdr->src, o+8, 16);
 166         memcpy(&phdr->dest, o+24, 16);
 167         phdr->ulp_length = htonl(l - 40);
 168         phdr->nexthdr = IPPROTO_ICMPV6;
 169
 170         memcpy(c + sizeof(*phdr), o + 40, l - 40);
 171
 172         // Checksum is over the icmp6 payload plus the pseudo header
 173         *(uint16_t *)(o+42) = _checksum(c, l - 40 + sizeof(*phdr));
 174
 175         tunnelsend(b, l + (o-b), t); // send it...
 176         return;
 177 }