diff options
-rw-r--r-- | trafgen/Makefile | 1 | ||||
-rw-r--r-- | trafgen_l3.c | 82 | ||||
-rw-r--r-- | trafgen_l3.h | 25 | ||||
-rw-r--r-- | trafgen_proto.c | 2 |
4 files changed, 110 insertions, 0 deletions
diff --git a/trafgen/Makefile b/trafgen/Makefile index 1c95118..4f342ca 100644 --- a/trafgen/Makefile +++ b/trafgen/Makefile @@ -21,6 +21,7 @@ trafgen-objs = xmalloc.o \ cpp.o \ trafgen_proto.o \ trafgen_l2.o \ + trafgen_l3.o \ trafgen_lexer.yy.o \ trafgen_parser.tab.o \ trafgen.o diff --git a/trafgen_l3.c b/trafgen_l3.c new file mode 100644 index 0000000..dd45395 --- /dev/null +++ b/trafgen_l3.c @@ -0,0 +1,82 @@ +/* + * netsniff-ng - the packet sniffing beast + * Subject to the GPL, version 2. + */ + +#include <linux/if_ether.h> + +#include "csum.h" +#include "built_in.h" +#include "trafgen_l2.h" +#include "trafgen_l3.h" +#include "trafgen_proto.h" +#include "trafgen_conf.h" + +static struct proto_field ipv4_fields[] = { + { .id = IP4_VER, .len = 1, .offset = 0, .shift = 4, .mask = 0xf0 }, + { .id = IP4_IHL, .len = 1, .offset = 0, .shift = 0, .mask = 0x0f }, + { .id = IP4_DSCP, .len = 1, .offset = 1, .shift = 2, .mask = 0xfc }, + { .id = IP4_ECN, .len = 1, .offset = 1, .shift = 0, .mask = 0x03 }, + { .id = IP4_TOS, .len = 1, .offset = 1 }, + { .id = IP4_LEN, .len = 2, .offset = 2 }, + { .id = IP4_ID, .len = 2, .offset = 4 }, + { .id = IP4_FLAGS, .len = 2, .offset = 6, .shift = 13, .mask = 0xe000 }, + { .id = IP4_MF, .len = 2, .offset = 6, .shift = 13, .mask = 0x2000 }, + { .id = IP4_DF, .len = 2, .offset = 6, .shift = 14, .mask = 0x4000 }, + { .id = IP4_FRAG_OFFS, .len = 2, .offset = 6, .shift = 0, .mask = 0x1fff }, + { .id = IP4_TTL, .len = 1, .offset = 8 }, + { .id = IP4_PROTO, .len = 1, .offset = 9 }, + { .id = IP4_CSUM, .len = 2, .offset = 10 }, + { .id = IP4_SADDR, .len = 4, .offset = 12 }, + { .id = IP4_DADDR, .len = 4, .offset = 16 }, +}; + +static void ipv4_header_init(struct proto_hdr *hdr) +{ + struct proto_hdr *lower; + + proto_lower_default_add(PROTO_ETH); + + lower = proto_current_header(); + + if (lower->id == PROTO_ETH) + proto_field_set_default_be16(lower, ETH_PROTO_ID, ETH_P_IP); + else if (lower->id == PROTO_IP4) + proto_field_set_default_u8(lower, IP4_PROTO, IPPROTO_IPIP); + + proto_header_fields_add(hdr, ipv4_fields, array_size(ipv4_fields)); + + proto_field_set_default_u8(hdr, IP4_VER, 4); + proto_field_set_default_u8(hdr, IP4_IHL, 5); + proto_field_set_default_dev_ipv4(hdr, IP4_SADDR); +} + +static void ipv4_packet_finish(struct proto_hdr *hdr) +{ + struct packet *pkt = current_packet(); + uint16_t total_len; + + total_len = pkt->len - hdr->pkt_offset; + proto_field_set_default_be16(hdr, IP4_LEN, total_len); + + if (!proto_field_is_set(hdr, IP4_CSUM)) { + uint16_t csum; + uint8_t ihl; + + ihl = proto_field_get_u8(hdr, IP4_IHL); + csum = htons(calc_csum(&pkt->payload[hdr->pkt_offset], ihl * 4)); + proto_field_set_u16(hdr, IP4_CSUM, bswap_16(csum)); + } +} + +static struct proto_hdr ipv4_hdr = { + .id = PROTO_IP4, + .layer = PROTO_L3, + .header_init = ipv4_header_init, + .packet_finish = ipv4_packet_finish, +}; + +void protos_l3_init(void) +{ + proto_header_register(&ipv4_hdr); +} diff --git a/trafgen_l3.h b/trafgen_l3.h new file mode 100644 index 0000000..9427378 --- /dev/null +++ b/trafgen_l3.h @@ -0,0 +1,25 @@ +#ifndef TRAFGEN_L3_H +#define TRAFGEN_L3_H + +enum ip4_field { + IP4_VER, + IP4_IHL, + IP4_DSCP, + IP4_ECN, + IP4_TOS, + IP4_LEN, + IP4_ID, + IP4_FLAGS, + IP4_FRAG_OFFS, + IP4_TTL, + IP4_PROTO, + IP4_CSUM, + IP4_SADDR, + IP4_DADDR, + IP4_DF, + IP4_MF, +}; + +extern void protos_l3_init(void); + +#endif /* TRAFGEN_L2_H */ diff --git a/trafgen_proto.c b/trafgen_proto.c index 2c090b2..75556f9 100644 --- a/trafgen_proto.c +++ b/trafgen_proto.c @@ -12,6 +12,7 @@ #include "xmalloc.h" #include "trafgen_conf.h" #include "trafgen_l2.h" +#include "trafgen_l3.h" #include "trafgen_proto.h" #define field_shift_and_mask(f, v) (((v) << (f)->shift) & \ @@ -360,6 +361,7 @@ void protos_init(const char *dev) ctx.dev = dev; protos_l2_init(); + protos_l3_init(); for (p = registered; p; p = p->next) p->ctx = &ctx; |