Merge branch 'master' of github.com:falling-sky/mtu1280d

4 files changed, 140 insertions, 111 deletions

diff --git a/Makefile b/Makefile
index 33cd66f..c07924d 100644
--- a/Makefile
+++ b/Makefile
@@ -10,6 +10,9 @@ help:
 mtu1280d: mtu1280d.c
 	gcc -o mtu1280d mtu1280d.c -lnetfilter_queue  || ( echo "see README.md for prerequisites" && exit 1 )
 
+test: mtu1280d
+	sudo ./mtu1280d -g
+
 clean:
 	rm -f mtu1280d
 
diff --git a/README.md b/README.md
index 3791b51..42099ab 100644
--- a/README.md
+++ b/README.md
@@ -18,13 +18,14 @@ is recommended.
 Once up and running, configure ip6tables to route
 large packets destined to the desired IP to the netfilter queue.
 
-Example rule:
+Example rules:
 
 ```
-guest% sudo ip6tables-save | grep NFQ
--A INPUT -d 2001:470:1f04:d63::2/128 -m length --length 1281:65535 -j -NFQUEUE --queue-num 1280
+iptables -t mangle -A PREROUTING -d 2001:470:1f04:d63::2/128 -m length --length 1281:65535 -j -NFQUEUE --queue-num 1280
+iptables -A INPUT -m mark --mark 0x501 -m comment --comment "Drop packets marked 1281  (too big)" -j DROP
 ```
 
+
 REQUIREMENTS
 ------------
 
@@ -52,13 +53,26 @@ ip6tables-restore /etc/iptables/rules.v6
 
 /etc/iptables/rules.v6 (simplified version, only includes mtu1280d rule)
 ```
-# Generated by ip6tables-save v1.4.21 on Tue Feb 17 10:54:23 2015
+# Generated by ip6tables-save v1.4.21 on Wed Feb 18 10:14:54 2015
+*mangle
+:PREROUTING ACCEPT [0:0]
+:INPUT ACCEPT [0:0]
+:FORWARD ACCEPT [0:0]
+:OUTPUT ACCEPT [0:0]
+:POSTROUTING ACCEPT [0:0]
+-A PREROUTING -d 2001:470:1:18::1280/128 -m length --length 1:65535 -m comment --comment "Mark packets using mtu1280d as small enough (1280) or too big (1281)" -j NFQUEUE --queue-num 1280
+COMMIT
+# Completed on Wed Feb 18 10:14:54 2015
+# Generated by ip6tables-save v1.4.21 on Wed Feb 18 10:14:54 2015
 *filter
 :INPUT ACCEPT [0:0]
 :FORWARD ACCEPT [0:0]
 :OUTPUT ACCEPT [0:0]
--A INPUT -d 2001:470:1f04:d63::2/128 -m length --length 1281:65535 -j NFQUEUE --queue-num 1280
+:CHECK_ABUSE - [0:0]
+:ONLY-GIGO - [0:0]
+-A INPUT -m mark --mark 0x501 -m comment --comment "Drop packets marked 1281  (too big)" -j DROP
 COMMIT
+# Completed on Wed Feb 18 10:14:54 2015
 ```
 
 
diff --git a/mtu1280d.c b/mtu1280d.c
index 7948af9..00eaa2d 100644
--- a/mtu1280d.c
+++ b/mtu1280d.c
@@ -39,6 +39,12 @@
 #define ETHER_CRC_SIZE 4
 #define ETHER_TOTAL_SIZE (MTU + ETHER_SIZE)
 
+
+unsigned int queue = 1280;	// -q 
+unsigned int do_fork = 0;	// -d
+unsigned int do_debug = 0;	// -g 
+
+
 typedef struct fullframe
 {
   u_int8_t ether_frame[ETHER_SIZE];
@@ -78,7 +84,10 @@ macaddr_for_interface (int i)
 
       if (interface)
 	{
-	  printf ("Looked up %d, found %s ", i, interface);
+	  if (do_debug)
+	    {
+	      printf ("Looked up %d, found %s ", i, interface);
+	    }
 
 	  // Use ioctl() to look up interface name and get its MAC address.   
 	  memset (&ifr, 0, sizeof (ifr));
@@ -93,9 +102,12 @@ macaddr_for_interface (int i)
 	}
 
     }
-  printf ("interface %d mac %02x:%02x:%02x:%02x:%02x:%02x",
-	  i, buffer[0], buffer[1], buffer[2], buffer[3], buffer[4],
-	  buffer[5]);
+  if (do_debug)
+    {
+      printf ("interface %d mac %02x:%02x:%02x:%02x:%02x:%02x",
+	      i, buffer[0], buffer[1], buffer[2], buffer[3], buffer[4],
+	      buffer[5]);
+    }
 
   return buffer;
 }
@@ -105,6 +117,12 @@ void
 hexdump (char *s, uint8_t * p, int n)
 {
   int i;
+
+  if (!do_debug)
+    {
+      return;
+    }
+
   printf ("\nHEXDUMP: %s\n", s);
   for (i = 0; i < n; i++)
     {
@@ -165,7 +183,6 @@ static u_int32_t
 block_pkt (struct nfq_data *tb)
 {
   int id = 0;
-  struct nfqnl_msg_packet_hdr *ph;
   struct nfqnl_msg_packet_hw *hwph;
   u_int32_t mark, ifi;
   int ret;
@@ -173,6 +190,7 @@ block_pkt (struct nfq_data *tb)
   int copy_len;
   unsigned char *data;
   uint16_t c;
+  static u_int8_t previous_macaddr[6] = { 0, 0, 0, 0, 0, 0 };
 
   // Where to address the raw packets - fill this in 
   // as we go
@@ -194,24 +212,15 @@ block_pkt (struct nfq_data *tb)
   // Final ether CRC
 
 
-  // Get the packet ID.  NEeded for netfilter_queue response.
-  fprintf (stdout, "TRACE: %s %i\n", __FILE__, __LINE__);
-  ph = nfq_get_msg_packet_hdr (tb);
-  if (ph)
-    {
-      id = ntohl (ph->packet_id);
-      printf ("hw_protocol=0x%04x hook=%u id=%u ",
-	      ntohs (ph->hw_protocol), ph->hook, id);
-    }
-
   // Get the data payload from netfilter_queue
-  fprintf (stdout, "TRACE: %s %i\n", __FILE__, __LINE__);
   ret = nfq_get_payload (tb, &data);
-  if (ret >= 0)
-    printf ("payload_len=%d ", ret);
   data_len = ret;
   copy_len = (data_len > PAYLOAD_SIZE) ? PAYLOAD_SIZE : data_len;
-  printf ("copy_len=%d ", copy_len);
+
+  if (do_debug)
+    {
+      printf ("payload_len=%d copy_len=%d ", data_len, copy_len);
+    }
 
 
 
@@ -223,16 +232,41 @@ block_pkt (struct nfq_data *tb)
     {
       int i, hlen = ntohs (hwph->hw_addrlen);
 
-      printf ("hw_src_addr=");
-      for (i = 0; i < hlen - 1; i++)
-	printf ("%02x:", hwph->hw_addr[i]);
-      printf ("%02x ", hwph->hw_addr[hlen - 1]);
+      if (do_debug)
+	{
+	  printf ("hw_src_addr=");
+	  for (i = 0; i < hlen - 1; i++)
+	    printf ("%02x:", hwph->hw_addr[i]);
+	  printf ("%02x ", hwph->hw_addr[hlen - 1]);
+	}
 
       // Ethernet frame destination
       memcpy (&buffer.ether_frame[0], hwph->hw_addr, 6);
       memcpy (socket_address.sll_addr, hwph->hw_addr, 6);
+      memcpy (previous_macaddr, hwph->hw_addr, 6);
       socket_address.sll_halen = ETH_ALEN;
     }
+  else
+    {
+      if (do_debug)
+	{
+	  printf ("hw_src_addr=missing ");
+	}
+      memcpy (&buffer.ether_frame[0], previous_macaddr, 6);
+      memcpy (socket_address.sll_addr, previous_macaddr, 6);
+      socket_address.sll_halen = ETH_ALEN;
+    }
+
+  // Early-ish accept if the packets are small
+  if ((data_len > 0) && (data_len <= 1280))
+    {
+      if (do_debug)
+	{
+	  printf ("Accepting!\n");
+	}
+      return 1280;  // iptables mark to keep the packet
+    }
+
 
   // TODO: Ethernet frame source
 
@@ -311,17 +345,21 @@ block_pkt (struct nfq_data *tb)
 
 
   // Device ID that the packet came from
-  fprintf (stdout, "TRACE: %s %i\n", __FILE__, __LINE__);
   ifi = nfq_get_indev (tb);
   if (ifi)
     {
-      printf ("indev=%u ", ifi);
+      if (do_debug)
+	{
+	  printf ("indev=%u ", ifi);
+	}
       socket_address.sll_ifindex = ifi;
       memcpy (&buffer.ether_frame[6], macaddr_for_interface (ifi), 6);
     }
 
-  fputc ('\n', stdout);
-  fprintf (stdout, "TRACE: %s %i\n", __FILE__, __LINE__);
+  if (do_debug)
+    {
+      fputc ('\n', stdout);
+    }
 
   int tx_len = ETHER_SIZE + IPV6HDR_SIZE + ICMP6_SIZE + copy_len;
   if (sendto
@@ -330,81 +368,38 @@ block_pkt (struct nfq_data *tb)
     printf ("Send failed\n");
 
 
-
-
-  return id;
+  return 1281;  // iptables will drop this later as being too big
 }
 
 
-
-/* returns packet id */
-static u_int32_t
-print_pkt (struct nfq_data *tb)
+static int
+cb (struct nfq_q_handle *qh, struct nfgenmsg *nfmsg,
+    struct nfq_data *nfa, void *data)
 {
-  int id = 0;
   struct nfqnl_msg_packet_hdr *ph;
-  struct nfqnl_msg_packet_hw *hwph;
-  u_int32_t mark, ifi;
-  int ret;
-  unsigned char *data;
+  u_int32_t id = 0;
+  u_int32_t mark;
 
-  ph = nfq_get_msg_packet_hdr (tb);
-  if (ph)
+  if (do_debug)
     {
-      id = ntohl (ph->packet_id);
-      printf ("hw_protocol=0x%04x hook=%u id=%u ",
-	      ntohs (ph->hw_protocol), ph->hook, id);
+      printf ("do_debug=%d", do_debug);
+      printf ("entering callback\n");
     }
-
-  hwph = nfq_get_packet_hw (tb);
-  if (hwph)
+  ph = nfq_get_msg_packet_hdr (nfa);
+  if (ph)
     {
-      int i, hlen = ntohs (hwph->hw_addrlen);
-
-      printf ("hw_src_addr=");
-      for (i = 0; i < hlen - 1; i++)
-	printf ("%02x:", hwph->hw_addr[i]);
-      printf ("%02x ", hwph->hw_addr[hlen - 1]);
+      id = ntohl (ph->packet_id);
+      if (do_debug)
+	{
+	  printf ("hw_protocol=0x%04x hook=%u id=%u ",
+		  ntohs (ph->hw_protocol), ph->hook, id);
+	}
     }
-
-  mark = nfq_get_nfmark (tb);
-  if (mark)
-    printf ("mark=%u ", mark);
-
-  ifi = nfq_get_indev (tb);
-  if (ifi)
-    printf ("indev=%u ", ifi);
-
-  ifi = nfq_get_outdev (tb);
-  if (ifi)
-    printf ("outdev=%u ", ifi);
-  ifi = nfq_get_physindev (tb);
-  if (ifi)
-    printf ("physindev=%u ", ifi);
-
-  ifi = nfq_get_physoutdev (tb);
-  if (ifi)
-    printf ("physoutdev=%u ", ifi);
-
-  ret = nfq_get_payload (tb, &data);
-  if (ret >= 0)
-    printf ("payload_len=%d ", ret);
-
-  fputc ('\n', stdout);
-
-  return id;
-}
-
-
-static int
-cb (struct nfq_q_handle *qh, struct nfgenmsg *nfmsg,
-    struct nfq_data *nfa, void *data)
-{
-  static u_int32_t id;
-//        id = print_pkt(nfa);
-  printf ("entering callback\n");
-  id = block_pkt (nfa);
-  return nfq_set_verdict (qh, id, NF_DROP, 0, NULL);
+  mark = block_pkt (nfa);
+  if (do_debug) {
+    printf("\nnfq_set_verdict2(qh, id=%d, v=NF_ACCEPT, mark=%d, 0, NULL)\n",id,mark);
+  }
+  return nfq_set_verdict2 (qh, id, NF_ACCEPT, mark, 0, NULL);
 }
 
 
@@ -416,21 +411,20 @@ main (int argc, char **argv)
   struct nfnl_handle *nh;
   int fd;
   int rv;
-  unsigned int queue = 1280;	// -q 
-  unsigned int do_fork = 0;	// -d 
   char *interface;
   char buf[4096] __attribute__ ((aligned));
 
 // Getopt        
   int c;
   int opterr = 0;
-  while ((c = getopt (argc, argv, "dq:")) != -1)
+  while ((c = getopt (argc, argv, "dgq:")) != -1)
     switch (c)
       {
       case 'd':
-	fprintf (stderr, "setting\n");
 	do_fork = 1;
-	fprintf (stderr, "set!\n");
+	break;
+      case 'g':
+	do_debug = 1;
 	break;
       case 'q':
 	queue = strtol (optarg, NULL, 10);
@@ -450,15 +444,21 @@ main (int argc, char **argv)
 
 
 
-  printf ("opening library handle\n");
+
+  if (do_debug)
+    {
+      printf ("opening library handle\n");
+    }
   h = nfq_open ();
   if (!h)
     {
       fprintf (stdout, "error during nfq_open()\n");
       exit (1);
     }
-
-  printf ("unbinding existing nf_queue handler for AF_INET6 (if any)\n");
+  if (do_debug)
+    {
+      printf ("unbinding existing nf_queue handler for AF_INET6 (if any)\n");
+    }
   if (nfq_unbind_pf (h, AF_INET6) < 0)
     {
       fprintf (stdout, "error during nfq_unbind_pf()\n");
@@ -469,14 +469,20 @@ main (int argc, char **argv)
       exit (1);
     }
 
-  printf ("binding nfnetlink_queue as nf_queue handler for AF_INET6\n");
+  if (do_debug)
+    {
+      printf ("binding nfnetlink_queue as nf_queue handler for AF_INET6\n");
+    }
   if (nfq_bind_pf (h, AF_INET6) < 0)
     {
       fprintf (stdout, "error during nfq_bind_pf()\n");
       exit (1);
     }
 
-  printf ("binding this socket to queue '%u'\n", queue);
+  if (do_debug)
+    {
+      printf ("binding this socket to queue '%u'\n", queue);
+    }
   qh = nfq_create_queue (h, queue, &cb, NULL);
   if (!qh)
     {
@@ -485,7 +491,10 @@ main (int argc, char **argv)
     }
 
 
-  printf ("setting copy_packet mode\n");
+  if (do_debug)
+    {
+      printf ("setting copy_packet mode\n");
+    }
   if (nfq_set_mode (qh, NFQNL_COPY_PACKET, 0xffff) < 0)
     {
       fprintf (stdout, "can't set packet_copy mode\n");
@@ -504,10 +513,14 @@ main (int argc, char **argv)
 
   while ((rv = recv (fd, buf, sizeof (buf), 0)) && rv >= 0)
     {
-      printf ("pkt received\n");
+      if (do_debug)
+	{
+	  printf ("pkt received\n");
+	}
       nfq_handle_packet (h, buf, rv);
     }
 
+
   printf ("unbinding from queue 0\n");
   nfq_destroy_queue (qh);
 
diff --git a/upstart/mtu1280d.conf b/upstart/mtu1280d.conf
index ee996aa..86e5e0f 100755
--- a/upstart/mtu1280d.conf
+++ b/upstart/mtu1280d.conf
@@ -1,6 +1,5 @@
-# ssh - OpenBSD Secure Shell server
+# mtu1280d - Synthetic ICMPv6 Packet Too Big response
 #
-# The OpenSSH server provides secure shell access to the system.
 
 description	"Synthetic ICMPv6 Packet Too Big response"