Diff: sniffer.h

Revision:

0:d494b853ce97

Child:

4:88fc7fa58931

diff -r 000000000000 -r d494b853ce97 sniffer.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/sniffer.h	Tue Oct 12 05:32:59 2010 +0000
@@ -0,0 +1,253 @@
+#ifndef SNIFFER_H
+#define SNIFFER_H
+
+#include "mbed.h"
+
+#include "main.h"
+#include "util/types.h"
+#include "net/net.h"
+
+#include <cstdio>
+#include <cstring>
+#include <functional>
+
+template <class Arg1, class Arg2, class Result>
+class handler
+{
+public:
+  virtual inline Result operator() (Arg1 x, Arg2 y) const {};
+};
+
+template <class Arg1, class Arg2, class Result>
+class function_handler
+: public handler <Arg1,Arg2,Result>
+{
+protected:
+  Result (*pfunc)(Arg1,Arg2);
+public:
+  explicit inline function_handler ( Result (*f)(Arg1,Arg2) ) : pfunc (f) {}
+  virtual inline Result operator() (Arg1 x, Arg2 y) const { return pfunc(x,y); }
+};
+
+template <class Type, class Arg1, class Arg2, class Result>
+class member_handler
+: public handler <Arg1,Arg2,Result>
+{
+protected:
+  Type *inst;
+  Result (Type::*pfunc)(Arg1,Arg2);
+public:
+  explicit inline member_handler ( Type *i, Result (Type::*f)(Arg1,Arg2) ) : inst(i), pfunc (f) {}
+  virtual inline Result operator() (Arg1 x, Arg2 y) const { return (inst->*pfunc)(x,y); }
+};
+
+
+class Sniffer {
+public:
+  Ethernet_MAC mac;
+  
+private:
+  // Ethernet interface
+  Ethernet     eth;
+  IP_Address   addr;
+  
+  // Status LEDs
+  DigitalOut linked;
+  DigitalOut received;
+
+  // Frame data (big enough for largest ethernet frame)
+  int  frame_size;
+  char frame[0x600];
+  
+  // Outgoing frames
+  char outframe[0x600];
+  Ethernet_FrameHeader *outframe_header;
+
+public:  
+  // Ethernet Frame Header
+  Ethernet_FrameHeader *frame_header;
+  
+  // IP Packet Header
+  IP_PacketHeader *ip_packet;
+  
+  // ARP Packet
+  ARP_Packet *arp_packet;
+  
+  // TCP Packet
+  TCP_SegmentHeader *tcp_packet;
+  
+  // UDP Packet
+  UDP_Packet *udp_packet;
+  
+  // ICMP Packet
+  ICMP_Packet *icmp_packet;
+  
+  // Generic
+  unsigned int data_bytes;
+
+public:
+  // Constructor
+  inline Sniffer()
+  : linked(LED1), received(LED2)
+  {
+    eth.set_link(Ethernet::AutoNegotiate);
+    eth.address((char *)mac.octet);
+  }
+  
+  inline bool inject(void *data, unsigned int bytes)
+  {
+    // Send the packet
+    eth.write((char*)data, bytes);
+    int send_status = eth.send();
+
+    //decode_ethernet(data);
+    
+    return send_status;
+  }
+  
+  inline bool inject(Ethernet_MAC dest, u16 ethertype, void *packet, unsigned int bytes)
+  {
+    memset(outframe, 0x00, bytes);
+    
+    outframe_header = (Ethernet_FrameHeader*)outframe;
+    
+    // Set the ethernet frame source
+    memcpy(&outframe_header->source, mac.octet, 6);
+    
+    // Set the ethernet frame destination
+    outframe_header->destination = dest;
+    
+    // Set the ethernet ethertype
+    outframe_header->ethertype = ethertype;
+    
+    // Make sure the payload won't be too large
+    if (sizeof(Ethernet_FrameHeader) + bytes > sizeof(outframe))
+    {
+      main_log.printf("ERROR: Attempt to inject packet failed; Payload size of %d is too large", bytes);
+      return false;
+    }
+    
+    // Set the payload
+    memcpy(outframe_header->payload, packet, bytes);
+    fix_endian_ethernet(outframe_header);
+    
+    // Send the packet
+    eth.write(outframe, sizeof(Ethernet_FrameHeader) + bytes);
+    int send_status = eth.send();
+
+    //decode_ethernet(outframe);
+
+    return send_status;
+  }
+  
+  inline void wait_for_data()
+  { 
+    while (true)
+    {
+      wait(0.0001);
+      
+      if (!(linked = eth.link()))
+        continue;
+      
+      received = (frame_size = eth.receive());
+      if (!frame_size)
+        continue;
+      
+      eth.read(frame, frame_size);
+      break;
+    }
+  }
+
+  // Wait for an ethernet frame
+  inline void next()
+  {
+    wait_for_data();
+    
+    // Zero out all of the packet pointers
+    frame_header = NULL;
+    arp_packet = NULL;
+    icmp_packet = NULL;
+    tcp_packet = NULL;
+    udp_packet = NULL;
+    data_bytes = 0;
+    
+    decode_ethernet(frame);
+  }
+  
+  inline void decode_ethernet(void *frame)
+  {
+    Ethernet_FrameHeader *header = frame_header = (Ethernet_FrameHeader*)frame;
+    fix_endian_ethernet(header);
+    
+    switch (header->ethertype)
+    {
+      case ETHERTYPE_IPV4:
+      case ETHERTYPE_IPV6:
+        decode_ip((IP_PacketHeader*)header->payload);
+        break;
+      case ETHERTYPE_ARP:
+        decode_arp((ARP_Packet*)header->payload);
+        break;
+      default:
+        break; // Unknown ethertype
+    }
+  }
+  
+  inline void decode_arp(ARP_Packet *packet)
+  {
+    fix_endian_arp(packet);
+    if (packet->hardware_type != 0x0001 || packet->protocol_type != 0x0800) return;
+    arp_packet = packet;
+  }
+  
+  inline void decode_ip(IP_PacketHeader *packet)
+  {
+    u16 chk = checksum(packet, sizeof(IP_PacketHeader), &packet->header_checksum, 2);
+    fix_endian_ip(packet);
+    ip_packet = packet;
+  
+    if (packet->version != 4) return;
+    
+    data_bytes = packet->packet_bytes;
+    data_bytes -= sizeof(IP_PacketHeader);
+    
+    if (packet->protocol == IPPROTO_UDP)
+    {
+      UDP_Packet *segment = udp_packet = (UDP_Packet*)packet->data;
+      fix_endian_udp(segment);
+      data_bytes -= sizeof(UDP_Packet);
+    }
+    else if (packet->protocol == IPPROTO_ICMP)
+    {
+      ICMP_Packet *segment = icmp_packet = (ICMP_Packet *)packet->data;
+      fix_endian_icmp(segment);
+      data_bytes -= sizeof(ICMP_Packet);
+    }
+    else if (packet->protocol == IPPROTO_TCP)
+    {
+      TCP_SegmentHeader *segment = tcp_packet = (TCP_SegmentHeader*)packet->data;
+      fix_endian_tcp(segment);
+      data_bytes -= sizeof(TCP_SegmentHeader);
+      dispatch_tcp(segment,data_bytes);
+    }
+  }
+  
+  handler<TCP_SegmentHeader*,u32,void> *tcp_handler;
+  inline void dispatch_tcp(TCP_SegmentHeader *tcp_packet, u32 data_bytes)
+  {
+    if (tcp_handler) (*tcp_handler)(tcp_packet,data_bytes);
+  }
+  
+  template <class T>
+  inline void attach_tcp(T *inst, void (T::*func)(TCP_SegmentHeader *tcp_packet, u32 data_bytes))
+  {
+    tcp_handler = new member_handler<T,TCP_SegmentHeader*,u32,void>(inst, func);
+  }
+  
+  inline void attach_tcp(void (*func)(TCP_SegmentHeader *tcp_packet, u32 data_bytes))
+  {
+    tcp_handler = new function_handler<TCP_SegmentHeader*,u32,void>(func);
+  }
+};
+
+#endif // SNIFFER_H
\ No newline at end of file