ethernet_frame.c

struct ethernet_header {
	uint8_t destination[6];
	uint8_t source[6];
	uint8_t type[2];
};

struct ip_header {
	uint8_t version;
	uint8_t services_field;
	uint16_t length;
	uint16_t identification;
	uint16_t flags_fragment_offset;
	uint8_t ttl;
	uint8_t protocol;
	uint16_t checksum;
	uint8_t destination[4];
	uint8_t source[4];
};

struct udp_header {
	uint16_t source;
	uint16_t destination;
	uint16_t length;
	uint16_t checksum;
};

struct tcp_header {
	uint16_t source;
	uint16_t destination;
	uint32_t seq;
	uint32_t ack;
	// rest ignored for now
};

#define ntohs(x) (((x) >> 8) | (((x) & 0xff) << 8))

void debug_packet(char *prefix, void *buf, int len)
{
	struct ethernet_header *ethernet = (struct ethernet_header *)buf;

	DEBUG("%s ethernet type: 0x%02X%02X %02X:%02X:%02X:%02X:%02X:%02X -> %02X:%02X:%02X:%02X:%02X:%02X (%u)",
		prefix,
		ethernet->source[0],
		ethernet->source[1],
		ethernet->source[2],
		ethernet->source[3],
		ethernet->source[4],
		ethernet->source[5],
		ethernet->destination[0],
		ethernet->destination[1],
		ethernet->destination[2],
		ethernet->destination[3],
		ethernet->destination[4],
		ethernet->destination[5],
		ethernet->type[0],
		ethernet->type[1],
		len
	);
	if (ethernet->type[0] == 0x08) {
		if (ethernet->type[1] == 0x06) { // ARP
		}
		if (ethernet->type[1] == 0x00) { // IP
			struct ip_header *ip = buf + sizeof(struct ethernet_header);
			DEBUG("%s ip type 0x%02X%02X protocol 0x%02X %u.%u.%u.%u -> %u.%u.%u.%u",
				prefix,
				ethernet->type[0],
				ethernet->type[1],
				ip->protocol,
				ip->source[0],
				ip->source[1],
				ip->source[2],
				ip->source[3],
				ip->destination[0],
				ip->destination[1],
				ip->destination[2],
				ip->destination[3]
			);
			if (ip->protocol == 0x11) { // UDP
				struct udp_header *udp = buf
					+ sizeof(struct ethernet_header)
					+ sizeof(struct ip_header);
				DEBUG("%s udp %u -> %u (%u)",
					prefix,
					ntohs(udp->source),
					ntohs(udp->destination),
					ntohs(udp->length)
				);
			}
			if (ip->protocol == 0x06) { // TCP
				struct tcp_header *tcp = buf
					+ sizeof(struct ethernet_header)
					+ sizeof(struct ip_header);
				DEBUG("%s tcp %u -> %u seq:%u ack:%u",
					prefix,
					ntohs(tcp->source),
					ntohs(tcp->destination),
					ntohs(tcp->seq),
					ntohs(tcp->ack)
				);
			}
		}
	}
}