01 // vim: set ft=c:
02 
03 class CEthFrame {
04   U8  source_addr[6];
05   U8  padding[2];
06   U8  dest_addr[6];
07   U16 ethertype;
08 
09   U8* data;
10   I64 length;
11 };
12 
13 class CL3Protocol {
14   CL3Protocol* next;
15 
16   U16 ethertype;
17   U8  padding[6];
18 
19   I64 (*handler)(CEthFrame* frame);
20 };
21 
22 static CL3Protocol* l3_protocols = NULL;
23 
24 U8 eth_null[6] = {0, 0, 0, 0, 0, 0};
25 U8 eth_broadcast[6] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
26 
27 I64 EthernetFrameParse(CEthFrame* frame_out, U8* frame, U16 length) {
28     // FIXME: check length
29     // TODO: MemCpy has high overhead, get rid of it
30     MemCpy(frame_out->dest_addr, frame, 6);
31     MemCpy(frame_out->source_addr, frame + 6, 6);
32     frame_out->ethertype = frame[13] | (frame[12] << 8);
33 
34     /*"Rx dst: %02X:%02X:%02X:%02X:%02X:%02X\n",
35             frame_out->dest_addr[0], frame_out->dest_addr[1], frame_out->dest_addr[2], frame_out->dest_addr[3], frame_out->dest_addr[4], frame_out->dest_addr[5];
36 
37     "Rx src: %02X:%02X:%02X:%02X:%02X:%02X\n",
38             frame_out->source_addr[0], frame_out->source_addr[1], frame_out->source_addr[2], frame_out->source_addr[3], frame_out->source_addr[4], frame_out->source_addr[5];
39 
40     "Rx ethertype: %02X\n", frame_out->ethertype;*/
41 
42     frame_out->data = frame + 14;
43     frame_out->length = length - 14 - 4;      // ??
44     return 0;
45 }
46 
47 U0 RegisterL3Protocol(U16 ethertype, I64 (*handler)(CEthFrame* frame)) {
48   CL3Protocol* p = MAlloc(sizeof(CL3Protocol));
49 
50   p->next = l3_protocols;
51   p->ethertype = ethertype;
52   p->handler = handler;
53 
54   l3_protocols = p;
55 }