001 // vim: set ft=c: 002 003 #define IP_PROTO_ICMP 0x01 004 #define IP_PROTO_TCP 0x06 005 #define IP_PROTO_UDP 0x11 006 007 #define IPV4_EADDR_INVALID (-200001) 008 #define IPV4_EHOST_UNREACHABLE (-200002) 009 010 #define IPV4_TTL 64 011 012 class CIPv4Packet { 013 CEthFrame* l2_frame; 014 015 U32 source_ip; 016 U32 dest_ip; 017 U8 proto; 018 U8 padding[7]; 019 020 U8* data; 021 I64 length; 022 }; 023 024 class CIPv4Header { 025 U8 version_ihl; 026 U8 dscp_ecn; 027 U16 total_length; 028 U16 ident; 029 U16 flags_fragoff; 030 U8 ttl; 031 U8 proto; 032 U16 header_checksum; 033 U32 source_ip; 034 U32 dest_ip; 035 }; 036 037 class CL4Protocol { 038 CL4Protocol* next; 039 040 U8 proto; 041 U8 padding[7]; 042 043 U0 (*handler)(CIPv4Packet* packet); 044 }; 045 046 // *_n = stored in network order 047 static U32 my_ip = 0; 048 static U32 my_ip_n = 0; 049 050 static U32 ipv4_router_addr = 0; 051 static U32 ipv4_subnet_mask = 0; 052 053 static CL4Protocol* l4_protocols = NULL; 054 055 // http://stackoverflow.com/q/26774761/2524350 056 static U16 IPv4Checksum(U8* header, I64 length) { 057 I64 nleft = length; 058 U16* w = header; 059 I64 sum = 0; 060 061 while (nleft > 1) { 062 sum += *(w++); 063 nleft -= 2; 064 } 065 066 // mop up an odd byte, if necessary 067 if (nleft == 1) { 068 sum += ((*w) & 0x00ff); 069 } 070 071 // add back carry outs from top 16 bits to low 16 bits 072 sum = (sum >> 16) + (sum & 0xffff); // add hi 16 to low 16 073 sum += (sum >> 16); // add carry 074 return (~sum) & 0xffff; 075 } 076 077 static I64 GetEthernetAddressForIP(U32 ip, U8** mac_out) { 078 // invalid 079 if (ip == 0) { 080 return IPV4_EADDR_INVALID; 081 } 082 // broadcast 083 else if (ip == 0xffffffff) { 084 *mac_out = eth_broadcast; 085 return 0; 086 } 087 // outside this subnet; needs routing 088 else if ((ip & ipv4_subnet_mask) != (my_ip & ipv4_subnet_mask)) { 089 // FIXME: infinite loop if mis-configured 090 091 return GetEthernetAddressForIP(ipv4_router_addr, mac_out); 092 } 093 // local network 094 else { 095 // FIXME: this can stall NetHandlerTask, we might need a flag to bail early 096 097 CArpCacheEntry* e = ArpCacheFindByIP(ip); 098 099 if (e) { 100 *mac_out = e->mac; 101 return 0; 102 } 103 104 //"Not in cache, requesting\n"; 105 106 // Up to 4 retries, 500 ms each 107 I64 retries = 4; 108 109 while (retries) { 110 ArpSend(ARP_REQUEST, eth_broadcast, EthernetGetAddress(), my_ip_n, eth_null, htonl(ip)); 111 112 I64 try_ = 0; 113 114 for (try_ = 0; try_ < 50; try_++) { 115 Sleep(10); 116 117 e = ArpCacheFindByIP(ip); 118 if (e) break; 119 } 120 121 if (e) { 122 *mac_out = e->mac; 123 return 0; 124 } 125 126 retries--; 127 } 128 129 in_addr in; 130 in.s_addr = htonl(ip); 131 "IPv4: Failed to resolve address %s\n", inet_ntoa(in); 132 return IPV4_EHOST_UNREACHABLE; 133 } 134 } 135 136 I64 IPv4PacketAlloc(U8** frame_out, U8 proto, U32 source_ip, U32 dest_ip, I64 length) { 137 U8* frame; 138 U8* dest_mac; 139 140 I64 error = GetEthernetAddressForIP(dest_ip, &dest_mac); 141 142 if (error < 0) 143 return error; 144 145 I64 index = EthernetFrameAlloc(&frame, EthernetGetAddress(), dest_mac, 146 ETHERTYPE_IPV4, sizeof(CIPv4Header) + length, 0); 147 148 if (index < 0) 149 return index; 150 151 I64 internet_header_length = 5; 152 153 CIPv4Header* hdr = frame; 154 hdr->version_ihl = internet_header_length | (4 << 4); 155 hdr->dscp_ecn = 0; 156 hdr->total_length = htons(internet_header_length * 4 + length); 157 hdr->ident = 0; 158 hdr->flags_fragoff = 0; 159 hdr->ttl = IPV4_TTL; 160 hdr->proto = proto; 161 hdr->header_checksum = 0; 162 hdr->source_ip = htonl(source_ip); 163 hdr->dest_ip = htonl(dest_ip); 164 165 hdr->header_checksum = IPv4Checksum(hdr, internet_header_length * 4); 166 167 *frame_out = frame + sizeof(CIPv4Header); 168 return index; 169 } 170 171 I64 IPv4PacketFinish(I64 index) { 172 return EthernetFrameFinish(index); 173 } 174 175 U32 IPv4GetAddress() { 176 return my_ip; 177 } 178 179 U0 IPv4SetAddress(U32 addr) { 180 my_ip = addr; 181 my_ip_n = htonl(addr); 182 183 ArpSetIPv4Address(addr); 184 } 185 186 U0 IPv4SetSubnet(U32 router_addr, U32 subnet_mask) { 187 ipv4_router_addr = router_addr; 188 ipv4_subnet_mask = subnet_mask; 189 } 190 191 I64 IPv4ParsePacket(CIPv4Packet* packet_out, CEthFrame* eth_frame) { 192 if (eth_frame->ethertype != ETHERTYPE_IPV4) 193 return -1; 194 195 // FIXME: check eth_frame->length etc. 196 197 CIPv4Header* hdr = eth_frame->data; 198 I64 header_length = (hdr->version_ihl & 0x0f) * 4; 199 //"IPv4: hdr %d, proto %02X, source %08X, dest %08X, len %d\n", 200 // header_length, hdr->proto, ntohl(hdr->source_ip), ntohl(hdr->dest_ip), 201 // eth_frame->length - header_length; 202 203 U16 total_length = ntohs(hdr->total_length); 204 205 packet_out->l2_frame = eth_frame; 206 packet_out->source_ip = ntohl(hdr->source_ip); 207 packet_out->dest_ip = ntohl(hdr->dest_ip); 208 packet_out->proto = hdr->proto; 209 210 packet_out->data = eth_frame->data + header_length; 211 packet_out->length = total_length - header_length; 212 213 return 0; 214 } 215 216 U0 RegisterL4Protocol(U8 proto, I64 (*handler)(CIPv4Packet* frame)) { 217 CL4Protocol* p = MAlloc(sizeof(CL4Protocol)); 218 219 p->next = l4_protocols; 220 p->proto = proto; 221 p->handler = handler; 222 223 l4_protocols = p; 224 } 225 226 I64 IPv4Handler(CEthFrame* eth_frame) { 227 CIPv4Packet packet; 228 229 I64 error = IPv4ParsePacket(&packet, eth_frame); 230 231 if (error < 0) 232 return error; 233 234 // This seems necessary to receive connections under VBox NAT, 235 // but is also pretty slow, so should be optimized to use a better 236 // struct than linked list. 237 ArpCachePut(packet.source_ip, eth_frame->source_addr); 238 239 CL4Protocol* l4 = l4_protocols; 240 241 while (l4) { 242 if (l4->proto == packet.proto) { 243 l4->handler(&packet); 244 break; 245 } 246 l4 = l4->next; 247 } 248 249 return error; 250 } 251 252 RegisterL3Protocol(ETHERTYPE_IPV4, &IPv4Handler);