sniffer.h

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