141 lines
3.5 KiB
C
141 lines
3.5 KiB
C
#include <stdio.h>
|
|
#include <stdint.h>
|
|
#include <stdbool.h>
|
|
#include <string.h>
|
|
#include <netinet/in.h>
|
|
|
|
struct ethernet_header {
|
|
uint8_t dest[6];
|
|
uint8_t src[6];
|
|
uint16_t ethertype;
|
|
} __attribute__((packed));
|
|
|
|
struct ipv6_header {
|
|
uint32_t vtf;
|
|
/*uint32_t version :4;
|
|
uint32_t traffic_class :8;
|
|
uint32_t flow_label :20;*/
|
|
uint16_t len;
|
|
uint8_t next_header;
|
|
uint8_t hop_limit;
|
|
|
|
uint8_t src[16];
|
|
uint8_t dest[16];
|
|
} __attribute__((packed));
|
|
|
|
struct icmpv6_header {
|
|
uint8_t type;
|
|
uint8_t code;
|
|
uint16_t checksum;
|
|
} __attribute__((packed));
|
|
|
|
struct ipv6_pseudo {
|
|
uint8_t src[16];
|
|
uint8_t dest[16];
|
|
uint32_t len;
|
|
uint32_t zeros :24;
|
|
uint32_t next_header :8;
|
|
} __attribute__((packed));
|
|
|
|
struct icmpv6_echo {
|
|
uint16_t identifier;
|
|
uint16_t sequence;
|
|
} __attribute__((packed));
|
|
|
|
struct packet {
|
|
struct ethernet_header eth;
|
|
struct ipv6_header ip6;
|
|
struct icmpv6_header icmp;
|
|
struct icmpv6_echo echo;
|
|
uint32_t ethernet_frame_check;
|
|
} __attribute__((packed));
|
|
|
|
uint16_t checksum (uint16_t *addr, size_t bytes) {
|
|
unsigned int i;
|
|
uint16_t *p = addr;
|
|
uint32_t sum = 0;
|
|
|
|
/* Sum */
|
|
for (i=bytes; i > 1; i -= 2)
|
|
sum += *p++;
|
|
|
|
/* If uneven length */
|
|
if (i > 0)
|
|
sum += (uint16_t) *((unsigned char *) (p));
|
|
|
|
/* Carry */
|
|
while ((sum & 0xFFFF0000) != 0)
|
|
sum = (sum >> 16) + (sum & 0xFFFF);
|
|
|
|
return ~((uint16_t) sum);
|
|
}
|
|
|
|
uint16_t calc_icmpv6_checksum(struct ipv6_header *ip6_header, void *data, size_t len) {
|
|
uint8_t buf[1500] = {};
|
|
struct ipv6_pseudo pseudo = {};
|
|
|
|
memcpy(pseudo.src, ip6_header->src, sizeof(ip6_header->src));
|
|
memcpy(pseudo.dest, ip6_header->dest, sizeof(ip6_header->dest));
|
|
pseudo.next_header = ip6_header->next_header;
|
|
pseudo.len = htonl(len);
|
|
|
|
memcpy(buf, &pseudo, sizeof(pseudo));
|
|
memcpy(buf + sizeof(pseudo), data, len);
|
|
|
|
return checksum(&buf, sizeof(pseudo) + len);
|
|
}
|
|
|
|
int main() {
|
|
struct packet pkt;
|
|
|
|
uint8_t eth_dest[] = {0xfc, 0xde, 0xad, 0xbe, 0xef, 0x05};
|
|
uint8_t eth_src[] = {0xfc, 0xde, 0xad, 0xbe, 0xef, 0x56};
|
|
memcpy(pkt.eth.dest, eth_dest, sizeof(pkt.eth.dest));
|
|
memcpy(pkt.eth.src, eth_src, sizeof(pkt.eth.src));
|
|
|
|
pkt.eth.ethertype = 0xdd86; // ipv6
|
|
|
|
pkt.ip6.vtf = htonl ((6 << 28) | (0 << 20) | 0);
|
|
/*pkt.ip6.version = 6;
|
|
pkt.ip6.traffic_class = 0;
|
|
pkt.ip6.flow_label = 0;*/
|
|
pkt.ip6.len = htons(sizeof(struct icmpv6_header) + sizeof(struct icmpv6_echo));
|
|
pkt.ip6.next_header = 0x3a; // ipv6-icmp
|
|
pkt.ip6.hop_limit = 2;
|
|
|
|
uint8_t ipv6_src[] = {
|
|
0xfe, 0x80, 0x00, 0x00,
|
|
0x00, 0x00, 0x00, 0x00,
|
|
0x00, 0x00, 0x00, 0x00,
|
|
0x00, 0x00, 0x00, 0x01
|
|
};
|
|
uint8_t ipv6_dest[] = {
|
|
0xfe, 0x80, 0x00, 0x00,
|
|
0x00, 0x00, 0x00, 0x00,
|
|
0x50, 0x5d, 0x18, 0xfb,
|
|
0x5e, 0x52, 0xec, 0xc0,
|
|
};
|
|
memcpy(pkt.ip6.src, ipv6_src, sizeof(pkt.ip6.src));
|
|
memcpy(pkt.ip6.dest, ipv6_dest, sizeof(pkt.ip6.dest));
|
|
|
|
pkt.icmp.type = 129; // icmp reply
|
|
pkt.icmp.checksum = 0;
|
|
|
|
pkt.echo.identifier = 0;
|
|
pkt.echo.sequence = 0;
|
|
|
|
pkt.icmp.checksum = calc_icmpv6_checksum(&pkt.ip6, &pkt.icmp, sizeof(struct icmpv6_header) + sizeof(struct icmpv6_echo));
|
|
|
|
pkt.ethernet_frame_check = 0xdeadbeef;
|
|
|
|
printf("sizeof(pkt) = %lu, pointer %p\n", sizeof(pkt), &pkt);
|
|
printf("checksum %#x\n", pkt.icmp.checksum);
|
|
|
|
int i;
|
|
for (i=0; i < sizeof(pkt); i++) {
|
|
printf("%02x ", *(((uint8_t *)&pkt) + i));
|
|
}
|
|
printf("\n%d", i);
|
|
return 0;
|
|
}
|