diff --git a/src/bbl.c b/src/bbl.c index 17ec0faf..12dce414 100644 --- a/src/bbl.c +++ b/src/bbl.c @@ -111,10 +111,7 @@ bbl_add_multicast_packets(bbl_ctx_s *ctx) } group = htobe32(group); /* Generate multicast destination MAC */ - *(uint32_t*)(&mac[2]) = group; - mac[0] = 0x01; - mac[2] = 0x5e; - mac[3] &= 0x7f; + ipv4_multicast_mac(group, mac); eth.src = interface->mac; eth.dst = mac; eth.vlan_outer = interface->vlan; diff --git a/src/bbl_interface.c b/src/bbl_interface.c index 05ff145a..baab3d22 100644 --- a/src/bbl_interface.c +++ b/src/bbl_interface.c @@ -266,6 +266,10 @@ bbl_add_network_interfaces(bbl_ctx_s *ctx) if(network_config->ip6.len && network_config->gateway6.len) { memcpy(&network_if->ip6, &network_config->ip6, sizeof(ipv6_prefix)); memcpy(&network_if->gateway6, &network_config->gateway6, sizeof(ipv6_prefix)); + memcpy(&network_if->gateway6_solicited_node_multicast, &ipv6_solicited_node_multicast, sizeof(ipv6addr_t)); + memcpy(((uint8_t*)&network_if->gateway6_solicited_node_multicast)+13, + ((uint8_t*)&network_if->gateway6.address)+13, 3); + /* Send initial ICMPv6 NS */ network_if->send_requests |= BBL_IF_SEND_ICMPV6_NS; } diff --git a/src/bbl_interface.h b/src/bbl_interface.h index 6bb18b0d..e76bc366 100644 --- a/src/bbl_interface.h +++ b/src/bbl_interface.h @@ -70,8 +70,9 @@ typedef struct bbl_interface_ ipv6_prefix ip6; ipv6_prefix gateway6; - bool icmpv6_nd_resolved; + ipv6addr_t gateway6_solicited_node_multicast; + bool icmpv6_nd_resolved; bool gateway_resolve_wait; uint8_t *mc_packets; diff --git a/src/bbl_protocols.h b/src/bbl_protocols.h index ab0d8024..4d31502b 100644 --- a/src/bbl_protocols.h +++ b/src/bbl_protocols.h @@ -235,12 +235,11 @@ static const ipv6addr_t ipv6_link_local_prefix = {0xFE, 0x80, 0x00, 0x00, 0x00, static const ipv6addr_t ipv6_multicast_all_nodes = {0xFF, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01}; static const ipv6addr_t ipv6_multicast_all_routers = {0xFF, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02}; static const ipv6addr_t ipv6_multicast_all_dhcp = {0xFF, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x02}; +static const ipv6addr_t ipv6_solicited_node_multicast = {0xFF, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0xff, 0x00, 0x00, 0x00}; /* MAC Addresses */ static const uint8_t broadcast_mac[ETH_ADDR_LEN] = { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff}; -static const uint8_t multicast_mac[ETH_ADDR_LEN] = { 0x01, 0x00, 0x5e, 0x00, 0x00, 0x00}; -static const uint8_t ipv6_multicast_mac[ETH_ADDR_LEN] = { 0x33, 0x33, 0xff, 0x00, 0x00, 0x10}; -static const uint8_t ipv6_multicast_mac_dhcp[ETH_ADDR_LEN] = { 0x33, 0x33, 0x00, 0x01, 0x00, 0x02}; + typedef enum protocol_error_ { PROTOCOL_SUCCESS = 0, diff --git a/src/bbl_stream.c b/src/bbl_stream.c index 56ef721b..89cb068d 100644 --- a/src/bbl_stream.c +++ b/src/bbl_stream.c @@ -522,10 +522,7 @@ bbl_stream_build_network_packet(bbl_stream *stream) { /* All IPv4 multicast addresses start with 1110 */ if((ipv4.dst & htobe32(0xf0000000)) == htobe32(0xe0000000)) { /* Generate multicast destination MAC */ - *(uint32_t*)(&mac[2]) = ipv4.dst; - mac[0] = 0x01; - mac[2] = 0x5e; - mac[3] &= 0x7f; + ipv4_multicast_mac(ipv4.dst, mac); eth.dst = mac; bbl.type = BBL_TYPE_MULTICAST; bbl.mc_source = ipv4.src; diff --git a/src/bbl_tx.c b/src/bbl_tx.c index bb694e6e..e4438b5a 100644 --- a/src/bbl_tx.c +++ b/src/bbl_tx.c @@ -538,12 +538,12 @@ bbl_encode_packet_icmpv6_rs (bbl_session_s *session) { bbl_pppoe_session_t pppoe = {0}; bbl_ipv6_t ipv6 = {0}; bbl_icmpv6_t icmpv6 = {0}; + uint8_t mac[ETH_ADDR_LEN]; interface = session->interface; ctx = interface->ctx; interface->stats.icmpv6_tx++; - eth.dst = session->server_mac; eth.src = session->client_mac; eth.qinq = session->access_config->qinq; eth.vlan_outer = session->vlan_key.outer_vlan_id; @@ -555,13 +555,17 @@ bbl_encode_packet_icmpv6_rs (bbl_session_s *session) { if(session->ip6cp_state != BBL_PPP_OPENED) { return WRONG_PROTOCOL_STATE; } + eth.dst = session->server_mac; eth.type = ETH_TYPE_PPPOE_SESSION; eth.next = &pppoe; + pppoe.session_id = session->pppoe_session_id; pppoe.protocol = PROTOCOL_IPV6; pppoe.next = &ipv6; } else { /* IPoE */ + ipv6_multicast_mac(ipv6_multicast_all_routers, mac); + eth.dst = mac; eth.type = ETH_TYPE_IPV6; eth.next = &ipv6; } @@ -616,6 +620,7 @@ bbl_encode_packet_dhcpv6_request (bbl_session_s *session) { bbl_udp_t udp = {0}; bbl_dhcpv6_t dhcpv6 = {0}; access_line_t access_line = {0}; + uint8_t mac[ETH_ADDR_LEN]; if(session->dhcpv6_state == BBL_DHCP_INIT || session->dhcpv6_state == BBL_DHCP_BOUND) { @@ -649,7 +654,8 @@ bbl_encode_packet_dhcpv6_request (bbl_session_s *session) { pppoe.next = &ipv6; } else { /* IPoE */ - eth.dst = (void*)ipv6_multicast_mac_dhcp; + ipv6_multicast_mac(ipv6_multicast_all_dhcp, mac); + eth.dst = mac; eth.vlan_outer_priority = ctx->config.dhcpv6_vlan_priority; eth.type = ETH_TYPE_IPV6; eth.next = &ipv6; @@ -1628,6 +1634,7 @@ bbl_encode_interface_packet (bbl_interface_s *interface, uint8_t *buf, uint16_t bbl_arp_t arp = {0}; bbl_ipv6_t ipv6 = {0}; bbl_icmpv6_t icmpv6 = {0}; + uint8_t mac[ETH_ADDR_LEN]; *len = 0; @@ -1649,15 +1656,12 @@ bbl_encode_interface_packet (bbl_interface_s *interface, uint8_t *buf, uint16_t result = encode_ethernet(buf, len, ð); } else if(interface->send_requests & BBL_IF_SEND_ICMPV6_NS) { interface->send_requests &= ~BBL_IF_SEND_ICMPV6_NS; - if(*(uint32_t*)interface->gateway_mac == 0) { - eth.dst = (uint8_t*)ipv6_multicast_mac; - } else { - eth.dst = interface->gateway_mac; - } + ipv6_multicast_mac(interface->gateway6_solicited_node_multicast, mac); + eth.dst = mac; eth.type = ETH_TYPE_IPV6; eth.next = &ipv6; ipv6.src = interface->ip6.address; - ipv6.dst = interface->gateway6.address; + ipv6.dst = interface->gateway6_solicited_node_multicast; ipv6.protocol = IPV6_NEXT_HEADER_ICMPV6; ipv6.next = &icmpv6; ipv6.ttl = 255; diff --git a/src/bbl_utils.c b/src/bbl_utils.c index 1584d2a3..bea68c8c 100644 --- a/src/bbl_utils.c +++ b/src/bbl_utils.c @@ -164,3 +164,31 @@ replace_substring (const char* source, *result_pos = '\0'; return result; } + +/** + * ipv4_multicast_mac + * + * @param ipv4 IPv4 multicast address + * @param mac target buffer to store multicast MAC + */ +void +ipv4_multicast_mac(const uint32_t ipv4, uint8_t* mac) { + *(uint32_t*)(&mac[2]) = ipv4; + mac[0] = 0x01; + mac[1] = 0x00; + mac[2] = 0x5e; + mac[3] &= 0x7f; +} + +/** + * ipv6_multicast_mac + * + * @param ipv6 IPv6 multicast address + * @param mac target buffer to store multicast MAC + */ +void +ipv6_multicast_mac(const uint8_t *ipv6, uint8_t* mac) { + *(uint32_t*)(&mac[2]) =*(uint32_t*)(&ipv6[12]); + mac[0] = 0x33; + mac[1] = 0x33; +} \ No newline at end of file diff --git a/src/bbl_utils.h b/src/bbl_utils.h index 7ebab36e..40651a68 100644 --- a/src/bbl_utils.h +++ b/src/bbl_utils.h @@ -21,7 +21,9 @@ char *format_mac_address(uint8_t *mac); char *format_ipv4_address(uint32_t *addr4); char *format_ipv6_address(ipv6addr_t *addr6); char *format_ipv6_prefix(ipv6_prefix *addr6); - +void ipv4_multicast_mac(const uint32_t ipv4, uint8_t* mac); +void ipv6_multicast_mac(const uint8_t *ipv6, uint8_t* mac); char *replace_substring (const char* s, const char* old, const char* new); const char *val2key (struct keyval_ *keyval, uint val); -#endif + +#endif \ No newline at end of file