![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
This is a low-level network debugging utility that utilizes raw packet i/o to construct and deconstruct tcp, udp, ipv4, arp, and icmp packets over ethernet.
Diff: sniffer.h
- Revision:
- 0:d494b853ce97
- Child:
- 4:88fc7fa58931
--- /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