From 4ef0264916153e26245350ab212fbfb2c05c4ddb Mon Sep 17 00:00:00 2001
From: Martin Duquesnoy <xorg62@gmail.com>
Date: Wed, 22 Jun 2011 19:29:25 +0200
Subject: [PATCH] Add payload displaying

---
 hftsniff.c | 21 ++++++++++++++++++++-
 1 file changed, 20 insertions(+), 1 deletion(-)

diff --git a/hftsniff.c b/hftsniff.c
index ae86324..9bc52f7 100644
--- a/hftsniff.c
+++ b/hftsniff.c
@@ -9,6 +9,7 @@
 #include <unistd.h>
 #include <string.h>
 #include <limits.h>
+#include <ctype.h>
 
 #include <pcap.h>
 #include <sys/socket.h>
@@ -31,6 +32,14 @@
           printf("( \e[1m%s\e[0m ) ", s); \
      } while(/* CONSTCOND */ 0);
 
+#define DISP_PAYLOAD(p, l)               \
+     do {                                \
+          int i = 0;                     \
+          for(; i < (l) && (p)++; ++i)   \
+               if(isprint(*(p)))         \
+                    putchar((char)*(p)); \
+     } while(/* CONSTCOND */ 0);
+
 #define BYE(s, er)                           \
      do {                                    \
           fprintf(stderr,  s": (%s)\n", er); \
@@ -165,7 +174,8 @@ pkt_handle(unsigned char *args, const struct pcap_pkthdr *header, const unsigned
 {
      struct ethernet_header *eth;
      struct ip_header       *ip;
-     int                     len, plen, i = 0;
+     const unsigned char    *payload;
+     int                     len, plen, paylen, i = 0;
      bool                    pfound = false;
 
      /* Translate ethernet pkt */
@@ -198,8 +208,17 @@ pkt_handle(unsigned char *args, const struct pcap_pkthdr *header, const unsigned
           IP_SRCDEST(inet_ntoa(ip->ip_src), -1,  /* src  */
                      inet_ntoa(ip->ip_dst), -1); /* dest */
 
+
+          puts("\n");
+          return;
      }
 
+     /* Get / Display payload */
+     payload = (unsigned char*)(packet + ETHERNET_SIZE + len + plen);
+
+     if((paylen = ntohs(ip->ip_len) - (len + plen)) > 0)
+          DISP_PAYLOAD(payload, paylen);
+
      puts("\n");
 
      return;