James Sayer / Mbed 2 deprecated smart2

test icmp

Dependencies:   mbed

Fork of ethspam by Rolf Meyer

sniffer.h@1:feaa107f56b3, 2015-04-24 (annotated)

Committer:
jamessayer
Date:
Fri Apr 24 03:11:02 2015 +0000
Revision:
1:feaa107f56b3
first version;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jamessayer 1:feaa107f56b3 1 #ifndef SNIFFER_H
jamessayer 1:feaa107f56b3 2 #define SNIFFER_H
jamessayer 1:feaa107f56b3 3
jamessayer 1:feaa107f56b3 4 #include "mbed.h"
jamessayer 1:feaa107f56b3 5
jamessayer 1:feaa107f56b3 6 #include "util/types.h"
jamessayer 1:feaa107f56b3 7 #include "net/net.h"
jamessayer 1:feaa107f56b3 8
jamessayer 1:feaa107f56b3 9 #include <cstdio>
jamessayer 1:feaa107f56b3 10 #include <cstring>
jamessayer 1:feaa107f56b3 11 #include <functional>
jamessayer 1:feaa107f56b3 12
jamessayer 1:feaa107f56b3 13 /**
jamessayer 1:feaa107f56b3 14 \file sniffer.h
jamessayer 1:feaa107f56b3 15 \brief Ethernet packet Sniffer
jamessayer 1:feaa107f56b3 16
jamessayer 1:feaa107f56b3 17 This file is the bread and butter of the NetTool; it processes and constructs ethernet frames
jamessayer 1:feaa107f56b3 18 on a bitwise level.
jamessayer 1:feaa107f56b3 19 */
jamessayer 1:feaa107f56b3 20
jamessayer 1:feaa107f56b3 21 template <class Arg1, class Arg2, class Result>
jamessayer 1:feaa107f56b3 22 class handler
jamessayer 1:feaa107f56b3 23 {
jamessayer 1:feaa107f56b3 24 public:
jamessayer 1:feaa107f56b3 25 virtual inline Result operator() (Arg1 x, Arg2 y) const {};
jamessayer 1:feaa107f56b3 26 };
jamessayer 1:feaa107f56b3 27
jamessayer 1:feaa107f56b3 28 template <class Arg1, class Arg2, class Result>
jamessayer 1:feaa107f56b3 29 class function_handler
jamessayer 1:feaa107f56b3 30 : public handler <Arg1,Arg2,Result>
jamessayer 1:feaa107f56b3 31 {
jamessayer 1:feaa107f56b3 32 protected:
jamessayer 1:feaa107f56b3 33 Result (*pfunc)(Arg1,Arg2);
jamessayer 1:feaa107f56b3 34 public:
jamessayer 1:feaa107f56b3 35 explicit inline function_handler ( Result (*f)(Arg1,Arg2) ) : pfunc (f) {}
jamessayer 1:feaa107f56b3 36 virtual inline Result operator() (Arg1 x, Arg2 y) const { return pfunc(x,y); }
jamessayer 1:feaa107f56b3 37 };
jamessayer 1:feaa107f56b3 38
jamessayer 1:feaa107f56b3 39 template <class Type, class Arg1, class Arg2, class Result>
jamessayer 1:feaa107f56b3 40 class member_handler
jamessayer 1:feaa107f56b3 41 : public handler <Arg1,Arg2,Result>
jamessayer 1:feaa107f56b3 42 {
jamessayer 1:feaa107f56b3 43 protected:
jamessayer 1:feaa107f56b3 44 Type *inst;
jamessayer 1:feaa107f56b3 45 Result (Type::*pfunc)(Arg1,Arg2);
jamessayer 1:feaa107f56b3 46 public:
jamessayer 1:feaa107f56b3 47 explicit inline member_handler ( Type *i, Result (Type::*f)(Arg1,Arg2) ) : inst(i), pfunc (f) {}
jamessayer 1:feaa107f56b3 48 virtual inline Result operator() (Arg1 x, Arg2 y) const { return (inst->*pfunc)(x,y); }
jamessayer 1:feaa107f56b3 49 };
jamessayer 1:feaa107f56b3 50
jamessayer 1:feaa107f56b3 51 /// Demo - Ethernet Packet Sniffer
jamessayer 1:feaa107f56b3 52 class Sniffer {
jamessayer 1:feaa107f56b3 53 public:
jamessayer 1:feaa107f56b3 54 Ethernet_MAC mac;
jamessayer 1:feaa107f56b3 55
jamessayer 1:feaa107f56b3 56 private:
jamessayer 1:feaa107f56b3 57 // Ethernet interface
jamessayer 1:feaa107f56b3 58 Ethernet eth;
jamessayer 1:feaa107f56b3 59 IP_Address addr;
jamessayer 1:feaa107f56b3 60
jamessayer 1:feaa107f56b3 61 // Status LEDs
jamessayer 1:feaa107f56b3 62 DigitalOut linked;
jamessayer 1:feaa107f56b3 63 DigitalOut received;
jamessayer 1:feaa107f56b3 64
jamessayer 1:feaa107f56b3 65 // Frame data (big enough for largest ethernet frame)
jamessayer 1:feaa107f56b3 66 int frame_size;
jamessayer 1:feaa107f56b3 67 char frame[0x600];
jamessayer 1:feaa107f56b3 68
jamessayer 1:feaa107f56b3 69 // Outgoing frames
jamessayer 1:feaa107f56b3 70 char outframe[0x600];
jamessayer 1:feaa107f56b3 71 Ethernet_FrameHeader *outframe_header;
jamessayer 1:feaa107f56b3 72
jamessayer 1:feaa107f56b3 73 public:
jamessayer 1:feaa107f56b3 74 /// Ethernet Frame Header (incoming)
jamessayer 1:feaa107f56b3 75 Ethernet_FrameHeader *frame_header;
jamessayer 1:feaa107f56b3 76
jamessayer 1:feaa107f56b3 77 /// IP Packet Header (incoming)
jamessayer 1:feaa107f56b3 78 IP_PacketHeader *ip_packet;
jamessayer 1:feaa107f56b3 79
jamessayer 1:feaa107f56b3 80 /// ARP Packet (incoming)
jamessayer 1:feaa107f56b3 81 ARP_Packet *arp_packet;
jamessayer 1:feaa107f56b3 82
jamessayer 1:feaa107f56b3 83 /// TCP Packet (incoming)
jamessayer 1:feaa107f56b3 84 TCP_SegmentHeader *tcp_packet;
jamessayer 1:feaa107f56b3 85
jamessayer 1:feaa107f56b3 86 /// UDP Packet (incoming)
jamessayer 1:feaa107f56b3 87 UDP_Packet *udp_packet;
jamessayer 1:feaa107f56b3 88
jamessayer 1:feaa107f56b3 89 /// ICMP Packet (incoming)
jamessayer 1:feaa107f56b3 90 ICMP_Packet *icmp_packet;
jamessayer 1:feaa107f56b3 91
jamessayer 1:feaa107f56b3 92 /// Generic - total data bytes
jamessayer 1:feaa107f56b3 93 unsigned int data_bytes;
jamessayer 1:feaa107f56b3 94
jamessayer 1:feaa107f56b3 95 public:
jamessayer 1:feaa107f56b3 96 /// Constructor
jamessayer 1:feaa107f56b3 97 inline Sniffer()
jamessayer 1:feaa107f56b3 98 : linked(LED1), received(LED2)
jamessayer 1:feaa107f56b3 99 {
jamessayer 1:feaa107f56b3 100 eth.set_link(Ethernet::AutoNegotiate);
jamessayer 1:feaa107f56b3 101 eth.address((char *)mac.octet);
jamessayer 1:feaa107f56b3 102 }
jamessayer 1:feaa107f56b3 103
jamessayer 1:feaa107f56b3 104 /// Inject the raw ethernet frame
jamessayer 1:feaa107f56b3 105 inline bool inject(void *data, unsigned int bytes)
jamessayer 1:feaa107f56b3 106 {
jamessayer 1:feaa107f56b3 107 // Send the packet
jamessayer 1:feaa107f56b3 108 eth.write((char*)data, bytes);
jamessayer 1:feaa107f56b3 109 int send_status = eth.send();
jamessayer 1:feaa107f56b3 110
jamessayer 1:feaa107f56b3 111 //decode_ethernet(data);
jamessayer 1:feaa107f56b3 112
jamessayer 1:feaa107f56b3 113 return send_status;
jamessayer 1:feaa107f56b3 114 }
jamessayer 1:feaa107f56b3 115
jamessayer 1:feaa107f56b3 116 /// Inject the raw payload into an ethernet frame with the given destination and ethertype
jamessayer 1:feaa107f56b3 117 inline bool inject(Ethernet_MAC dest, u16 ethertype, void *packet, unsigned int bytes)
jamessayer 1:feaa107f56b3 118 {
jamessayer 1:feaa107f56b3 119 memset(outframe, 0x00, bytes);
jamessayer 1:feaa107f56b3 120
jamessayer 1:feaa107f56b3 121 outframe_header = (Ethernet_FrameHeader*)outframe;
jamessayer 1:feaa107f56b3 122
jamessayer 1:feaa107f56b3 123 // Set the ethernet frame source
jamessayer 1:feaa107f56b3 124 memcpy(&outframe_header->source, mac.octet, 6);
jamessayer 1:feaa107f56b3 125
jamessayer 1:feaa107f56b3 126 // Set the ethernet frame destination
jamessayer 1:feaa107f56b3 127 outframe_header->destination = dest;
jamessayer 1:feaa107f56b3 128
jamessayer 1:feaa107f56b3 129 // Set the ethernet ethertype
jamessayer 1:feaa107f56b3 130 outframe_header->ethertype = ethertype;
jamessayer 1:feaa107f56b3 131
jamessayer 1:feaa107f56b3 132 // Make sure the payload won't be too large
jamessayer 1:feaa107f56b3 133 if (sizeof(Ethernet_FrameHeader) + bytes > sizeof(outframe))
jamessayer 1:feaa107f56b3 134 {
jamessayer 1:feaa107f56b3 135 printf("ERROR: Attempt to inject packet failed; Payload size of %d is too large /n", bytes);
jamessayer 1:feaa107f56b3 136 return false;
jamessayer 1:feaa107f56b3 137 }
jamessayer 1:feaa107f56b3 138
jamessayer 1:feaa107f56b3 139 // Set the payload
jamessayer 1:feaa107f56b3 140 memcpy(outframe_header->payload, packet, bytes);
jamessayer 1:feaa107f56b3 141 fix_endian_ethernet(outframe_header);
jamessayer 1:feaa107f56b3 142
jamessayer 1:feaa107f56b3 143 // Send the packet
jamessayer 1:feaa107f56b3 144 eth.write(outframe, sizeof(Ethernet_FrameHeader) + bytes);
jamessayer 1:feaa107f56b3 145 int send_status = eth.send();
jamessayer 1:feaa107f56b3 146
jamessayer 1:feaa107f56b3 147 //decode_ethernet(outframe);
jamessayer 1:feaa107f56b3 148
jamessayer 1:feaa107f56b3 149 return send_status;
jamessayer 1:feaa107f56b3 150 }
jamessayer 1:feaa107f56b3 151
jamessayer 1:feaa107f56b3 152 /// Wait until there is more data to receive
jamessayer 1:feaa107f56b3 153 inline void wait_for_data()
jamessayer 1:feaa107f56b3 154 {
jamessayer 1:feaa107f56b3 155 while (true)
jamessayer 1:feaa107f56b3 156 {
jamessayer 1:feaa107f56b3 157 wait(0.0001);
jamessayer 1:feaa107f56b3 158
jamessayer 1:feaa107f56b3 159 if (!(linked = eth.link()))
jamessayer 1:feaa107f56b3 160 continue;
jamessayer 1:feaa107f56b3 161
jamessayer 1:feaa107f56b3 162 received = (frame_size = eth.receive());
jamessayer 1:feaa107f56b3 163 if (!frame_size)
jamessayer 1:feaa107f56b3 164 continue;
jamessayer 1:feaa107f56b3 165
jamessayer 1:feaa107f56b3 166 eth.read(frame, frame_size);
jamessayer 1:feaa107f56b3 167 break;
jamessayer 1:feaa107f56b3 168 }
jamessayer 1:feaa107f56b3 169 }
jamessayer 1:feaa107f56b3 170
jamessayer 1:feaa107f56b3 171 /// Wait for an ethernet frame (will be stored in appropriate class member pointers)
jamessayer 1:feaa107f56b3 172 inline void next()
jamessayer 1:feaa107f56b3 173 {
jamessayer 1:feaa107f56b3 174 wait_for_data();
jamessayer 1:feaa107f56b3 175
jamessayer 1:feaa107f56b3 176 // Zero out all of the packet pointers
jamessayer 1:feaa107f56b3 177 frame_header = NULL;
jamessayer 1:feaa107f56b3 178 arp_packet = NULL;
jamessayer 1:feaa107f56b3 179 icmp_packet = NULL;
jamessayer 1:feaa107f56b3 180 tcp_packet = NULL;
jamessayer 1:feaa107f56b3 181 udp_packet = NULL;
jamessayer 1:feaa107f56b3 182 data_bytes = 0;
jamessayer 1:feaa107f56b3 183
jamessayer 1:feaa107f56b3 184 decode_ethernet(frame);
jamessayer 1:feaa107f56b3 185 }
jamessayer 1:feaa107f56b3 186
jamessayer 1:feaa107f56b3 187 /// Decode the given ethernet frame
jamessayer 1:feaa107f56b3 188 inline void decode_ethernet(void *frame)
jamessayer 1:feaa107f56b3 189 {
jamessayer 1:feaa107f56b3 190 Ethernet_FrameHeader *header = frame_header = (Ethernet_FrameHeader*)frame;
jamessayer 1:feaa107f56b3 191 fix_endian_ethernet(header);
jamessayer 1:feaa107f56b3 192
jamessayer 1:feaa107f56b3 193 switch (header->ethertype)
jamessayer 1:feaa107f56b3 194 {
jamessayer 1:feaa107f56b3 195 case ETHERTYPE_IPV4:
jamessayer 1:feaa107f56b3 196 case ETHERTYPE_IPV6:
jamessayer 1:feaa107f56b3 197 decode_ip((IP_PacketHeader*)header->payload);
jamessayer 1:feaa107f56b3 198 break;
jamessayer 1:feaa107f56b3 199 case ETHERTYPE_ARP:
jamessayer 1:feaa107f56b3 200 decode_arp((ARP_Packet*)header->payload);
jamessayer 1:feaa107f56b3 201 break;
jamessayer 1:feaa107f56b3 202 default:
jamessayer 1:feaa107f56b3 203 break; // Unknown ethertype
jamessayer 1:feaa107f56b3 204 }
jamessayer 1:feaa107f56b3 205 }
jamessayer 1:feaa107f56b3 206
jamessayer 1:feaa107f56b3 207 /// Decode the given ARP packet
jamessayer 1:feaa107f56b3 208 inline void decode_arp(ARP_Packet *packet)
jamessayer 1:feaa107f56b3 209 {
jamessayer 1:feaa107f56b3 210 fix_endian_arp(packet);
jamessayer 1:feaa107f56b3 211 if (packet->hardware_type != 0x0001 || packet->protocol_type != 0x0800) return;
jamessayer 1:feaa107f56b3 212 arp_packet = packet;
jamessayer 1:feaa107f56b3 213 }
jamessayer 1:feaa107f56b3 214
jamessayer 1:feaa107f56b3 215 /// Decode the given IPv4 packet
jamessayer 1:feaa107f56b3 216 inline void decode_ip(IP_PacketHeader *packet)
jamessayer 1:feaa107f56b3 217 {
jamessayer 1:feaa107f56b3 218 u16 chk = checksum(packet, sizeof(IP_PacketHeader), &packet->header_checksum, 2);
jamessayer 1:feaa107f56b3 219 fix_endian_ip(packet);
jamessayer 1:feaa107f56b3 220 ip_packet = packet;
jamessayer 1:feaa107f56b3 221
jamessayer 1:feaa107f56b3 222 if (packet->version != 4) return;
jamessayer 1:feaa107f56b3 223
jamessayer 1:feaa107f56b3 224 data_bytes = packet->packet_bytes;
jamessayer 1:feaa107f56b3 225 data_bytes -= sizeof(IP_PacketHeader);
jamessayer 1:feaa107f56b3 226
jamessayer 1:feaa107f56b3 227 if (packet->protocol == IPPROTO_UDP)
jamessayer 1:feaa107f56b3 228 {
jamessayer 1:feaa107f56b3 229 UDP_Packet *segment = udp_packet = (UDP_Packet*)packet->data;
jamessayer 1:feaa107f56b3 230 fix_endian_udp(segment);
jamessayer 1:feaa107f56b3 231 data_bytes -= sizeof(UDP_Packet);
jamessayer 1:feaa107f56b3 232 }
jamessayer 1:feaa107f56b3 233 else if (packet->protocol == IPPROTO_ICMP)
jamessayer 1:feaa107f56b3 234 {
jamessayer 1:feaa107f56b3 235 ICMP_Packet *segment = icmp_packet = (ICMP_Packet *)packet->data;
jamessayer 1:feaa107f56b3 236 fix_endian_icmp(segment);
jamessayer 1:feaa107f56b3 237 data_bytes -= sizeof(ICMP_Packet);
jamessayer 1:feaa107f56b3 238 }
jamessayer 1:feaa107f56b3 239 else if (packet->protocol == IPPROTO_TCP)
jamessayer 1:feaa107f56b3 240 {
jamessayer 1:feaa107f56b3 241 TCP_SegmentHeader *segment = tcp_packet = (TCP_SegmentHeader*)packet->data;
jamessayer 1:feaa107f56b3 242 fix_endian_tcp(segment);
jamessayer 1:feaa107f56b3 243 data_bytes -= sizeof(TCP_SegmentHeader);
jamessayer 1:feaa107f56b3 244 dispatch_tcp(segment,data_bytes);
jamessayer 1:feaa107f56b3 245 }
jamessayer 1:feaa107f56b3 246 }
jamessayer 1:feaa107f56b3 247
jamessayer 1:feaa107f56b3 248 handler<TCP_SegmentHeader*,u32,void> *tcp_handler;
jamessayer 1:feaa107f56b3 249 inline void dispatch_tcp(TCP_SegmentHeader *tcp_packet, u32 data_bytes)
jamessayer 1:feaa107f56b3 250 {
jamessayer 1:feaa107f56b3 251 if (tcp_handler) (*tcp_handler)(tcp_packet,data_bytes);
jamessayer 1:feaa107f56b3 252 }
jamessayer 1:feaa107f56b3 253
jamessayer 1:feaa107f56b3 254 /// Attach a member function to be called on all TCP packets
jamessayer 1:feaa107f56b3 255 template <class T>
jamessayer 1:feaa107f56b3 256 inline void attach_tcp(T *inst, void (T::*func)(TCP_SegmentHeader *tcp_packet, u32 data_bytes))
jamessayer 1:feaa107f56b3 257 {
jamessayer 1:feaa107f56b3 258 tcp_handler = new member_handler<T,TCP_SegmentHeader*,u32,void>(inst, func);
jamessayer 1:feaa107f56b3 259 }
jamessayer 1:feaa107f56b3 260
jamessayer 1:feaa107f56b3 261 /// Attach a non-member function to be called on all TCP packets
jamessayer 1:feaa107f56b3 262 inline void attach_tcp(void (*func)(TCP_SegmentHeader *tcp_packet, u32 data_bytes))
jamessayer 1:feaa107f56b3 263 {
jamessayer 1:feaa107f56b3 264 tcp_handler = new function_handler<TCP_SegmentHeader*,u32,void>(func);
jamessayer 1:feaa107f56b3 265 }
jamessayer 1:feaa107f56b3 266 };
jamessayer 1:feaa107f56b3 267
jamessayer 1:feaa107f56b3 268 #endif // SNIFFER_H