ipv6.c (9344B)
1 // Copyright 2022, Brian Swetland <swetland@frotz.net> 2 // Licensed under the Apache License, Version 2.0. 3 4 #include <string.h> 5 #include <net/ipv6.h> 6 #include <hw/debug.h> 7 8 void hexdump(void *data, int len) { 9 unsigned off = 0; 10 while (len > 0) { 11 xprintf("%04x:", off); 12 for (int n = 0; n < 16; n++) { 13 if (n == len) break; 14 xprintf(" %02x", ((uint8_t*)data)[n]); 15 if (n == 7) xputc(' '); 16 } 17 xputc('\n'); 18 len -= 16; 19 data += 16; 20 off += 16; 21 } 22 } 23 24 // create IP6 Link Local Address from Ethernet MAC 25 void ip6lla_from_ethmac(ip6_addr_t* ip, const uint8_t *mac){ 26 ip->w[0] = 0; 27 ip->w[1] = 0; 28 ip->b[0] = 0xFE; 29 ip->b[1] = 0x80; 30 ip->b[8] = mac[0] ^ 2; 31 ip->b[9] = mac[1]; 32 ip->b[10] = mac[2]; 33 ip->b[11] = 0xFF; 34 ip->b[12] = 0xFE; 35 ip->b[13] = mac[3]; 36 ip->b[14] = mac[4]; 37 ip->b[15] = mac[5]; 38 } 39 40 // create IP6 Solicit Neighbor Multicast Address from Ethernet MAC 41 void ip6ns_from_ethmac(ip6_addr_t *ip, const uint8_t *mac) { 42 ip->w[0] = 0; 43 ip->w[1] = 0; 44 ip->w[2] = 0; 45 ip->b[0] = 0xFF; 46 ip->b[1] = 0x02; 47 ip->b[11] = 0x01; 48 ip->b[12] = 0xFF; 49 ip->b[13] = mac[3]; 50 ip->b[14] = mac[4]; 51 ip->b[15] = mac[5]; 52 } 53 54 // create Ethernet Multicast MAC from IP6 Multicast Address 55 void ethmac_from_ip6mc(uint8_t *mac, const ip6_addr_t* ip) { 56 mac[0] = 0x33; 57 mac[1] = 0x33; 58 mac[2] = ip->b[12]; 59 mac[3] = ip->b[13]; 60 mac[4] = ip->b[14]; 61 mac[5] = ip->b[15]; 62 } 63 64 #define NETBUF_MAX 1536 // 1.5KB 65 66 typedef struct netbuf { 67 struct netbuf* next; 68 uint32_t rsvd0; 69 uint32_t rsvd1; 70 uint32_t rsvd2; 71 uint8_t data[NETBUF_MAX]; 72 } netbuf_t; 73 74 // Interface Ethernet MAC 75 static uint8_t ifc_mac[ETH_ADDR_LEN] = { 0x72, 0x72, 0xaa, 0xbb, 0xcc, 0xdd }; 76 77 // Interface IP6 Link Local IP6 Address 78 static ip6_addr_t ifc_ll_ip6 = { .b = { 79 0xFE, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 80 0x70, 0x72, 0xaa, 0xff, 0xfe, 0xbb, 0xcc, 0xdd, } }; 81 82 // Multicast Neighbor Solicitation IP6 Address 83 static ip6_addr_t mcast_ns_ip6 = { .b = { 84 0xFF, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 85 0x00, 0x00, 0x00, 0x01, 0xff, 0xbb, 0xcc, 0xdd } }; 86 87 // Multicast Link Local All IP6 Address 88 static const ip6_addr_t mcast_ll_ip6 = { .b = { 89 0xFF, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 90 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, } }; 91 92 // fast mac matching via word comparison 93 typedef union { 94 uint32_t w[2]; 95 uint8_t b[8]; 96 } match_t; 97 98 // Fast Match for Interface, Link Local Multicast, 99 // and Neighbor Solicitation Multicast MACs 100 static match_t m_ifc_mac = { .b = { 0, 0, 0x72, 0x72, 0xaa, 0xbb, 0xcc, 0xdd } }; 101 static match_t m_ns_mac = { .b = { 0, 0, 0x33, 0x33, 0xff, 0xbb, 0xcc, 0xdd } }; 102 static const match_t m_ll_mac = { .b = { 0, 0, 0x33, 0x33, 0x00, 0x00, 0x00, 0x01 } }; 103 104 void net_init(const uint8_t *mac) { 105 // store our interface ether mac 106 memcpy(ifc_mac, mac, ETH_ADDR_LEN); 107 memcpy(m_ifc_mac.b + 2, mac, ETH_ADDR_LEN); 108 109 // generate an ip6 link layer address from it 110 ip6lla_from_ethmac(&ifc_ll_ip6, mac); 111 112 // generate ip6 NDP addresses from our ether mac 113 ip6ns_from_ethmac(&mcast_ns_ip6, mac); 114 ethmac_from_ip6mc(m_ns_mac.b + 2, &mcast_ns_ip6); 115 } 116 117 // returns 16bit checksum of provided byte data buffer 118 static uint32_t checksum(const void* _data, unsigned len, uint32_t sum) { 119 const uint16_t* data = _data; 120 while (len > 1) { 121 sum += *data++; 122 len -= 2; 123 } 124 if (len) { 125 sum += (*data & 0xFF); 126 } 127 while (sum > 0xFFFF) { 128 sum = (sum & 0xFFFF) + (sum >> 16); 129 } 130 return sum; 131 } 132 133 // Takes pointer to ip6 packet starting with ip6 header (filled out) 134 // and length of payload (exclusive of the ip6 header length) 135 // 136 // Returns 16bit ip6 checksum 137 static uint32_t checksum_ip6(ip6_hdr_t* hdr, unsigned len) { 138 // sum of length and protocol fields from pseudo-header 139 uint32_t sum = checksum(&hdr->length, 2, htons(hdr->next_header)); 140 // sum of src and dst fields from pseudo-header + payload 141 sum = checksum(&hdr->src, len + 32, sum); 142 // avoid return sum 0 143 return (sum == 0xffff) ? sum : ~sum; 144 } 145 146 void net_rx_udp6(void *data, unsigned dlen, int mcast) { 147 if (dlen < sizeof(udp_hdr_t)) { 148 xprintf("UPD6: runt\n"); 149 return; 150 } 151 udp_hdr_t *udp = data; 152 uint16_t port = ntohs(udp->dst_port); 153 uint16_t len = ntohs(udp->length); 154 if ((len < sizeof(udp_hdr_t)) || (len > dlen)) { 155 xprintf("UDP6: badlen %u (dlen %u)\n", len, dlen); 156 // bogus length or short packet 157 return; 158 } 159 pkt_rx_udp(data + sizeof(udp_hdr_t), len - sizeof(udp_hdr_t), port, mcast); 160 } 161 162 void net_rx_icmp6(void *data, unsigned dlen, int mcast) { 163 if (dlen < sizeof(icmp_hdr_t)) { 164 xprintf("ICMP6 runt\n"); 165 return; 166 } 167 icmp_hdr_t *icmp = data; 168 169 switch (icmp->type) { 170 case ICMP_NDP_N_SOLICIT: { 171 ndp_hdr_t *ndp = data; 172 if ((dlen < sizeof(ndp_hdr_t)) || (ndp->code != 0)) { 173 return; 174 } 175 if (memcmp(&ndp->target, &ifc_ll_ip6, sizeof(ip6_addr_t))) { 176 return; // not for us 177 } 178 179 struct { 180 ndp_hdr_t hdr; 181 uint8_t opt[8]; 182 } reply = { 183 .hdr.type = ICMP_NDP_N_ADVERTISE, 184 .hdr.code = 0, 185 .hdr.checksum = 0, 186 .hdr.flags = 0x60, // (S)olicited and (O)verride 187 .opt[0] = NDP_N_TGT_LL_ADDR, 188 .opt[1] = 1, 189 }; 190 memcpy(&reply.hdr.target, &ifc_ll_ip6, sizeof(ip6_addr_t)); 191 memcpy(&reply.opt[2], ifc_mac, ETH_ADDR_LEN); 192 193 net_tx_ip6_reply(data, &reply, sizeof(reply), IP_HDR_ICMP, offsetof(ndp_hdr_t, checksum)); 194 return; 195 } 196 case ICMP_ECHO_REQUEST: { 197 icmp_hdr_t *icmp = data; 198 if (dlen < sizeof(icmp_hdr_t)) { 199 return; 200 } 201 icmp->type = ICMP_ECHO_REPLY; 202 icmp->checksum = 0; 203 net_tx_ip6_reply(data, icmp, dlen, IP_HDR_ICMP, offsetof(icmp_hdr_t, checksum)); 204 return; 205 } 206 default: 207 xprintf("ICMP ? %u %u %s\n", icmp->type, icmp->code, mcast ? "M" : ""); 208 } 209 } 210 211 // len >= 44 212 void net_rx_ip6(void *data, unsigned len, int mcast) { 213 ip6_hdr_t *ip6 = data; 214 215 if (mcast) { 216 if (memcmp(&ip6->dst, &mcast_ll_ip6, sizeof(ip6_addr_t)) && 217 memcmp(&ip6->dst, &mcast_ns_ip6, sizeof(ip6_addr_t))) { 218 xprintf("IP6: MA ?\n"); 219 return; 220 } 221 } else { 222 if (memcmp(&ip6->dst, &ifc_ll_ip6, sizeof(ip6_addr_t))) { 223 xprintf("IP6: UA ?\n"); 224 return; 225 } 226 } 227 228 switch (ip6->next_header) { 229 case IP_HDR_ICMP: 230 net_rx_icmp6(data + sizeof(ip6_hdr_t), len - sizeof(ip6_hdr_t), mcast); 231 return; 232 case IP_HDR_UDP: 233 net_rx_udp6(data + sizeof(ip6_hdr_t), len - sizeof(ip6_hdr_t), mcast); 234 return; 235 default: 236 xprintf("IP6: HDR %u ?\n", ip6->next_header); 237 return; 238 } 239 } 240 241 void net_rx_eth(void *data, unsigned len) { 242 if (len < 60) { 243 xprintf("ETH runt\n"); 244 return; // runt packet 245 } 246 247 // discard non-ipv6 packets 248 eth_hdr_t *eth = data; 249 if (eth->type != htons(ETH_TYPE_IP6)) { 250 return; 251 } 252 253 //hexdump(data + 2, len - 2); 254 255 // accept packets which match our ethernet MAC or multicast address 256 uint32_t *w = data; 257 if ((w[0] == m_ifc_mac.w[0]) && (w[1] == m_ifc_mac.w[1])) { 258 net_rx_ip6(data + sizeof(eth_hdr_t), len - sizeof(eth_hdr_t), 0); 259 } else if ((w[0] == m_ns_mac.w[0]) && (w[1] == m_ns_mac.w[1])) { 260 net_rx_ip6(data + sizeof(eth_hdr_t), len - sizeof(eth_hdr_t), 1); 261 } else if ((w[0] == m_ll_mac.w[0]) && (w[1] == m_ll_mac.w[1])) { 262 net_rx_ip6(data + sizeof(eth_hdr_t), len - sizeof(eth_hdr_t), 1); 263 } else { 264 xprintf("ETH ? %02x %02x %02x %02x %02x %02x\n", 265 eth->dst[0], eth->dst[1], eth->dst[2], 266 eth->dst[3], eth->dst[4], eth->dst[5]); 267 } 268 269 } 270 271 typedef struct { 272 eth_hdr_t eth; 273 ip6_hdr_t ip6; 274 udp_hdr_t udp; 275 uint8_t data[]; 276 } udp6_pkt_t; 277 278 static_assert(sizeof(udp6_pkt_t) == (sizeof(eth_hdr_t) + sizeof(ip6_hdr_t) + sizeof(udp_hdr_t)), ""); 279 #define MAX_UDP_PAYLOAD (NETBUF_MAX - sizeof(udp6_pkt_t)) 280 281 void net_tx_udp_reply(void *replyto, void *data, unsigned len, unsigned port) { 282 uint8_t packet[NETBUF_MAX]; 283 udp6_pkt_t *tx = (void*) packet; 284 udp6_pkt_t *rx = replyto - sizeof(udp6_pkt_t); 285 286 if (len > MAX_UDP_PAYLOAD) { 287 return; 288 } 289 290 eth_addr_copy(tx->eth.dst, rx->eth.src); 291 eth_addr_copy(tx->eth.src, ifc_mac); 292 tx->eth.type = htons(ETH_TYPE_IP6); 293 294 tx->ip6.bits = 0x00000060; // ver=0, tc=0, flow=0 295 tx->ip6.length = htons(UDP_HDR_LEN + len); 296 tx->ip6.next_header = IP_HDR_UDP; 297 tx->ip6.hop_limit = 255; 298 ip6_addr_copy(&tx->ip6.src, &ifc_ll_ip6); 299 ip6_addr_copy(&tx->ip6.dst, &rx->ip6.src); 300 301 tx->udp.src_port = htons(port); 302 tx->udp.dst_port = rx->udp.src_port; 303 tx->udp.length = htons(len + UDP_HDR_LEN); 304 tx->udp.checksum = 0; 305 306 memcpy(tx->data, data, len); 307 308 tx->udp.checksum = checksum_ip6(&tx->ip6, len + UDP_HDR_LEN); 309 310 eth_tx(packet + 2, ETH_HDR_LEN + IP6_HDR_LEN + UDP_HDR_LEN + len); 311 } 312 313 typedef struct { 314 eth_hdr_t eth; 315 ip6_hdr_t ip6; 316 uint8_t data[]; 317 } ip6_pkt_t; 318 319 static_assert(sizeof(ip6_pkt_t) == (sizeof(eth_hdr_t) + sizeof(ip6_hdr_t)), ""); 320 #define MAX_IP6_PAYLOAD (NETBUF_MAX - sizeof(ip6_pkt_t)) 321 322 void net_tx_ip6_reply(void *replyto, void *data, unsigned len, 323 unsigned type, unsigned chk_off) { 324 uint8_t packet[NETBUF_MAX]; 325 ip6_pkt_t *tx = (void*) packet; 326 ip6_pkt_t *rx = replyto - sizeof(ip6_pkt_t); 327 328 if (len > MAX_IP6_PAYLOAD) { 329 return; 330 } 331 332 eth_addr_copy(tx->eth.dst, rx->eth.src); 333 eth_addr_copy(tx->eth.src, ifc_mac); 334 tx->eth.type = htons(ETH_TYPE_IP6); 335 336 tx->ip6.bits = 0x00000060; // ver=0, tc=0, flow=0 337 tx->ip6.length = htons(len); 338 tx->ip6.next_header = type; 339 tx->ip6.hop_limit = 255; 340 ip6_addr_copy(&tx->ip6.src, &ifc_ll_ip6); 341 ip6_addr_copy(&tx->ip6.dst, &rx->ip6.src); 342 343 memcpy(tx->data, data, len); 344 345 *((uint16_t*) (tx->data + chk_off)) = checksum_ip6(&tx->ip6, len); 346 347 //hexdump(packet + 2, ETH_HDR_LEN + IP6_HDR_LEN + len); 348 349 eth_tx(packet + 2, ETH_HDR_LEN + IP6_HDR_LEN + len); 350 } 351