sniffer.h

00001 #ifndef SNIFFER_H
00002 #define SNIFFER_H
00003 
00004 #include "mbed.h"
00005 
00006 #include "main.h"
00007 #include "util/types.h"
00008 #include "net/net.h"
00009 
00010 #include <cstdio>
00011 #include <cstring>
00012 #include <functional>
00013 
00014 /**
00015   \file sniffer.h
00016   \brief Ethernet packet Sniffer
00017   
00018   This file is the bread and butter of the NetTool; it processes and constructs ethernet frames
00019   on a bitwise level.
00020 */
00021 
00022 template <class Arg1, class Arg2, class Result>
00023 class handler
00024 {
00025 public:
00026   virtual inline Result operator() (Arg1 x, Arg2 y) const {};
00027 };
00028 
00029 template <class Arg1, class Arg2, class Result>
00030 class function_handler
00031 : public handler <Arg1,Arg2,Result>
00032 {
00033 protected:
00034   Result (*pfunc)(Arg1,Arg2);
00035 public:
00036   explicit inline function_handler ( Result (*f)(Arg1,Arg2) ) : pfunc (f) {}
00037   virtual inline Result operator() (Arg1 x, Arg2 y) const { return pfunc(x,y); }
00038 };
00039 
00040 template <class Type, class Arg1, class Arg2, class Result>
00041 class member_handler
00042 : public handler <Arg1,Arg2,Result>
00043 {
00044 protected:
00045   Type *inst;
00046   Result (Type::*pfunc)(Arg1,Arg2);
00047 public:
00048   explicit inline member_handler ( Type *i, Result (Type::*f)(Arg1,Arg2) ) : inst(i), pfunc (f) {}
00049   virtual inline Result operator() (Arg1 x, Arg2 y) const { return (inst->*pfunc)(x,y); }
00050 };
00051 
00052 /// Demo - Ethernet Packet Sniffer
00053 class Sniffer {
00054 public:
00055   Ethernet_MAC mac;
00056   
00057 private:
00058   // Ethernet interface
00059   Ethernet     eth;
00060   IP_Address   addr;
00061   
00062   // Status LEDs
00063   DigitalOut linked;
00064   DigitalOut received;
00065 
00066   // Frame data (big enough for largest ethernet frame)
00067   int  frame_size;
00068   char frame[0x600];
00069   
00070   // Outgoing frames
00071   char outframe[0x600];
00072   Ethernet_FrameHeader *outframe_header;
00073 
00074 public:  
00075   /// Ethernet Frame Header (incoming)
00076   Ethernet_FrameHeader *frame_header;
00077   
00078   /// IP Packet Header (incoming)
00079   IP_PacketHeader *ip_packet;
00080   
00081   /// ARP Packet (incoming)
00082   ARP_Packet *arp_packet;
00083   
00084   /// TCP Packet (incoming)
00085   TCP_SegmentHeader *tcp_packet;
00086   
00087   /// UDP Packet (incoming)
00088   UDP_Packet *udp_packet;
00089   
00090   /// ICMP Packet (incoming)
00091   ICMP_Packet *icmp_packet;
00092   
00093   /// Generic - total data bytes
00094   unsigned int data_bytes;
00095 
00096 public:
00097   /// Constructor
00098   inline Sniffer()
00099   : linked(LED1), received(LED2)
00100   {
00101     eth.set_link(Ethernet::AutoNegotiate);
00102     eth.address((char *)mac.octet);
00103   }
00104   
00105   /// Inject the raw ethernet frame
00106   inline bool inject(void *data, unsigned int bytes)
00107   {
00108     // Send the packet
00109     eth.write((char*)data, bytes);
00110     int send_status = eth.send();
00111 
00112     //decode_ethernet(data);
00113     
00114     return send_status;
00115   }
00116   
00117   /// Inject the raw payload into an ethernet frame with the given destination and ethertype
00118   inline bool inject(Ethernet_MAC dest, u16 ethertype, void *packet, unsigned int bytes)
00119   {
00120     memset(outframe, 0x00, bytes);
00121     
00122     outframe_header = (Ethernet_FrameHeader*)outframe;
00123     
00124     // Set the ethernet frame source
00125     memcpy(&outframe_header->source, mac.octet, 6);
00126     
00127     // Set the ethernet frame destination
00128     outframe_header->destination = dest;
00129     
00130     // Set the ethernet ethertype
00131     outframe_header->ethertype = ethertype;
00132     
00133     // Make sure the payload won't be too large
00134     if (sizeof(Ethernet_FrameHeader) + bytes > sizeof(outframe))
00135     {
00136       main_log.printf("ERROR: Attempt to inject packet failed; Payload size of %d is too large", bytes);
00137       return false;
00138     }
00139     
00140     // Set the payload
00141     memcpy(outframe_header->payload, packet, bytes);
00142     fix_endian_ethernet(outframe_header);
00143     
00144     // Send the packet
00145     eth.write(outframe, sizeof(Ethernet_FrameHeader) + bytes);
00146     int send_status = eth.send();
00147 
00148     //decode_ethernet(outframe);
00149 
00150     return send_status;
00151   }
00152   
00153   /// Wait until there is more data to receive
00154   inline void wait_for_data()
00155   { 
00156     while (true)
00157     {
00158       wait(0.0001);
00159       
00160       if (!(linked = eth.link()))
00161         continue;
00162       
00163       received = (frame_size = eth.receive());
00164       if (!frame_size)
00165         continue;
00166       
00167       eth.read(frame, frame_size);
00168       break;
00169     }
00170   }
00171 
00172   /// Wait for an ethernet frame (will be stored in appropriate class member pointers)
00173   inline void next()
00174   {
00175     wait_for_data();
00176     
00177     // Zero out all of the packet pointers
00178     frame_header = NULL;
00179     arp_packet = NULL;
00180     icmp_packet = NULL;
00181     tcp_packet = NULL;
00182     udp_packet = NULL;
00183     data_bytes = 0;
00184     
00185     decode_ethernet(frame);
00186   }
00187   
00188   /// Decode the given ethernet frame
00189   inline void decode_ethernet(void *frame)
00190   {
00191     Ethernet_FrameHeader *header = frame_header = (Ethernet_FrameHeader*)frame;
00192     fix_endian_ethernet(header);
00193     
00194     switch (header->ethertype)
00195     {
00196       case ETHERTYPE_IPV4:
00197       case ETHERTYPE_IPV6:
00198         decode_ip((IP_PacketHeader*)header->payload);
00199         break;
00200       case ETHERTYPE_ARP:
00201         decode_arp((ARP_Packet*)header->payload);
00202         break;
00203       default:
00204         break; // Unknown ethertype
00205     }
00206   }
00207   
00208   /// Decode the given ARP packet
00209   inline void decode_arp(ARP_Packet *packet)
00210   {
00211     fix_endian_arp(packet);
00212     if (packet->hardware_type != 0x0001 || packet->protocol_type != 0x0800) return;
00213     arp_packet = packet;
00214   }
00215   
00216   /// Decode the given IPv4 packet
00217   inline void decode_ip(IP_PacketHeader *packet)
00218   {
00219     u16 chk = checksum(packet, sizeof(IP_PacketHeader), &packet->header_checksum, 2);
00220     fix_endian_ip(packet);
00221     ip_packet = packet;
00222   
00223     if (packet->version != 4) return;
00224     
00225     data_bytes = packet->packet_bytes;
00226     data_bytes -= sizeof(IP_PacketHeader);
00227     
00228     if (packet->protocol == IPPROTO_UDP)
00229     {
00230       UDP_Packet *segment = udp_packet = (UDP_Packet*)packet->data;
00231       fix_endian_udp(segment);
00232       data_bytes -= sizeof(UDP_Packet);
00233     }
00234     else if (packet->protocol == IPPROTO_ICMP)
00235     {
00236       ICMP_Packet *segment = icmp_packet = (ICMP_Packet *)packet->data;
00237       fix_endian_icmp(segment);
00238       data_bytes -= sizeof(ICMP_Packet);
00239     }
00240     else if (packet->protocol == IPPROTO_TCP)
00241     {
00242       TCP_SegmentHeader *segment = tcp_packet = (TCP_SegmentHeader*)packet->data;
00243       fix_endian_tcp(segment);
00244       data_bytes -= sizeof(TCP_SegmentHeader);
00245       dispatch_tcp(segment,data_bytes);
00246     }
00247   }
00248   
00249   handler<TCP_SegmentHeader*,u32,void> *tcp_handler;
00250   inline void dispatch_tcp(TCP_SegmentHeader *tcp_packet, u32 data_bytes)
00251   {
00252     if (tcp_handler) (*tcp_handler)(tcp_packet,data_bytes);
00253   }
00254   
00255   /// Attach a member function to be called on all TCP packets
00256   template <class T>
00257   inline void attach_tcp(T *inst, void (T::*func)(TCP_SegmentHeader *tcp_packet, u32 data_bytes))
00258   {
00259     tcp_handler = new member_handler<T,TCP_SegmentHeader*,u32,void>(inst, func);
00260   }
00261   
00262   /// Attach a non-member function to be called on all TCP packets
00263   inline void attach_tcp(void (*func)(TCP_SegmentHeader *tcp_packet, u32 data_bytes))
00264   {
00265     tcp_handler = new function_handler<TCP_SegmentHeader*,u32,void>(func);
00266   }
00267 };
00268 
00269 #endif // SNIFFER_H