Implement Multicast listener discovery

src/EtherSia.cpp

@@ -113,6 +113,24 @@ uint16_t EtherSia::receivePacket()
 
     if (len) {
         IPv6Packet& packet = (IPv6Packet&)_ptr;
+
+        if (packet.protocol() == IP6_PROTO_HBH) {
+            // Packet with Hop-by-Hop Extension
+            IPv6HopByHopPacket& packet = (IPv6HopByHopPacket&)_ptr;
+            if (!packet.isValid() || !checkEthernetAddresses(packet)) {
+                _bufferContainsReceived = false;
+            } else {
+                if(packet.nextHeader() == IP6_PROTO_ICMP6) {
+                    ICMPv6Packet<IPv6HopByHopPacket>& packet = (ICMPv6Packet<IPv6HopByHopPacket>&)_ptr;
+                    if(packet.type == ICMP6_TYPE_MLQ && (packet.mld.target.isZero() || packet.mld.target == _globalAddress)) {
+                        // Multiast Listener Query, answer with listener Report for Solicited Node address
+                        icmp6SendMLR();
+                    }
+                }
+            }
+            return 0;
+        }
+
         if (!packet.isValid() || !checkEthernetAddresses(packet)) {
             _bufferContainsReceived = false;
             return 0;
@@ -127,6 +145,7 @@ uint16_t EtherSia::receivePacket()
                 return 0;
             }
         }
+
     } else {
         // We didn't receive anything
         _bufferContainsReceived = false;

src/EtherSia.h

@@ -14,6 +14,7 @@
 #include "MACAddress.h"
 #include "IPv6Address.h"
 #include "IPv6Packet.h"
+#include "IPv6HopByHopPacket.h"
 #include "Socket.h"
 #include "UDPSocket.h"
 
@@ -369,6 +370,11 @@ class EtherSia {
      */
     virtual uint16_t readFrame(uint8_t *buffer, uint16_t bufsize) = 0;
 
+    /**
+     * Send a Multicast Listener Report for the Solicited Node address
+     */
+    void icmp6SendMLR();
+
 protected:
     IPv6Address _linkLocalAddress;  /**< The IPv6 Link-local address of the Ethernet Interface */
     IPv6Address _globalAddress;     /**< The IPv6 Global address of the Ethernet Interface */
@@ -468,6 +474,13 @@ class EtherSia {
      * Ensures the protocol and checksum are set before sending.
      */
     void icmp6PacketSend();
+
+    /**
+     * Send an ICMPv6 packet stored in the EtherSia packet buffer
+     * Ensures the protocol and checksum are set before sending.
+     * @param hbh true if the packet to send contains a Hop-by-Hop option
+     */
+    void icmp6PacketSend(bool hbh);
 };
 
 #include "PingClient.h"

src/ICMPv6Packet.h

@@ -10,6 +10,8 @@
 #define ICMP6_TYPE_PARAM_PROB     4
 #define ICMP6_TYPE_ECHO           128
 #define ICMP6_TYPE_ECHO_REPLY     129
+#define ICMP6_TYPE_MLQ            130
+#define ICMP6_TYPE_MLR            131
 #define ICMP6_TYPE_RS             133
 #define ICMP6_TYPE_RA             134
 #define ICMP6_TYPE_NS             135
@@ -168,13 +170,28 @@ struct icmp6_na_header {
 /* Verify that compiler gets the structure size correct */
 static_assert(sizeof(struct icmp6_na_header) == ICMP6_NA_HEADER_LEN, "Size is not correct");
 
+/**
+ * Structure for accessing the fields of a ICMP6 Multicast Listener Discovery packet
+ * @private
+ */
+struct icmp6_mld_header {
+    uint16_t max_response_delay;
+    uint8_t reserved[2];
+    IPv6Address target;
+} __attribute__((__packed__));
+#define ICMP6_MLD_HEADER_LEN       (20)
+#define ICMP6_MLD_HEADER_OFFSET    (ICMP6_HEADER_OFFSET + ICMP6_HEADER_LEN)
+
+/* Verify that compiler gets the structure size correct */
+static_assert(sizeof(struct icmp6_mld_header) == ICMP6_MLD_HEADER_LEN, "Size is not correct");
+
 
 
 /**
  * Class for accessing the fields of a ICMP6 packet
  * @private
  */
-class ICMPv6Packet : public IPv6Packet {
+template<class T> class ICMPv6Packet : public T {
 
 public:
     uint8_t type;
@@ -188,6 +205,7 @@ class ICMPv6Packet : public IPv6Packet {
         struct icmp6_rs_header rs;
         struct icmp6_na_header na;
         struct icmp6_ns_header ns;
+        struct icmp6_mld_header mld;
     } __attribute__((__packed__));
 
 } __attribute__((__packed__));

src/IPv6HopByHopPacket.cpp

@@ -0,0 +1,77 @@
+#include <Arduino.h>
+
+#include "EtherSia.h"
+#include "IPv6HopByHopPacket.h"
+
+IPv6HopByHopPacket::IPv6HopByHopPacket()
+{
+    memset(this, 0, sizeof(IPv6HopByHopPacket));
+    init();
+}
+
+void IPv6HopByHopPacket::setNextHeader(uint8_t header) {
+    _next_header = header;
+}
+
+uint8_t IPv6HopByHopPacket::nextHeader() {
+    return _next_header;
+}
+
+void IPv6HopByHopPacket::setHopByHopLength(uint8_t length) {
+    _hbh_length = length;
+}
+
+uint8_t IPv6HopByHopPacket::hopByHopLength() {
+    return _hbh_length;
+}
+
+uint8_t IPv6HopByHopPacket::optionType() {
+    return hbh_option[0];
+}
+
+void IPv6HopByHopPacket::setMLDRouterAlert() {
+    _router_alert.option_type = 5;
+    _router_alert.length = 2;
+    _router_alert.alert_type = 0;
+    _router_alert.padding = 0;
+}
+
+uint8_t* IPv6HopByHopPacket::payload()
+{
+    return (uint8_t *)(this) + sizeof(IPv6HopByHopPacket);
+}
+
+uint16_t IPv6HopByHopPacket::calculateChecksum()
+{
+    /* First sum pseudoheader. */
+    /* IP protocol and length fields. This addition cannot carry. */
+    volatile uint16_t newsum = (payloadLength() - 8) + nextHeader();
+
+    /* Sum IP source and destination addresses. */
+    newsum = chksum(newsum, (uint8_t *)(source()), 16);
+    newsum = chksum(newsum, (uint8_t *)(destination()), 16);
+
+    /* Sum the payload header and data */
+    newsum = chksum(newsum, payload(), (payloadLength() - 8));
+
+    return ~newsum;
+}
+
+boolean IPv6HopByHopPacket::isValid()
+{
+    if (this->_etherType != ntohs(ETHER_TYPE_IPV6)) {
+        return false;
+    }
+
+    // Check the version header
+    if (this->version() != 6) {
+        return false;
+    }
+
+    // Verify the packet checksum (it should add up to 0)
+    if (calculateChecksum() != 0) {
+        return false;
+    }
+
+    return true;
+}

src/IPv6HopByHopPacket.h

@@ -0,0 +1,104 @@
+/**
+ * Header file for the ICMPv6Packet class
+ * @file IPv6HopByHopPacket.h
+ */
+#include "util.h"
+
+#ifndef IPV6_HOP_BY_HOP_PACKET_H
+#define IPV6_HOP_BY_HOP_PACKET_H
+
+/** Field-Type Value of a Router Alert option */
+#define IPV6_HBH_OPTION_ROUTER_ALERT 5
+
+/** Length of a Multicast Listener Discovery Hop-by-Hop option **/
+#define IPV6_HBH_MLD_OPTION_LENGTH 8
+
+/**
+ * Structure representing a Router Alert option
+ * @private
+ */
+struct router_alert {
+    uint8_t option_type;
+    uint8_t length;
+    uint16_t alert_type;
+    uint16_t padding;
+} __attribute__((__packed__));
+
+/**
+ * Class for getting and setting the fields of an IPv6 Hop-by-Hop Packet
+ * with Hop-by-Hop extension and its Ethernet header
+ */
+class IPv6HopByHopPacket : public IPv6Packet {
+    public:
+        /**
+         * Constructor for new empty IPv6 Hop-by-Hop packet
+         */
+         IPv6HopByHopPacket();
+
+        /**
+         * Set the type of the Hop by Hop packet payload
+         * @param header typecode of the payload type
+         */
+        void setNextHeader(uint8_t header);
+
+        /**
+         * Return the type of the Hop by Hop packet payload
+         * @return typecode of the payload type
+         */
+        uint8_t nextHeader();
+
+        /**
+         * Set the Hop by Hop option length
+         * @param length hop by hop option length
+         */
+        void setHopByHopLength(uint8_t length);
+
+        /**
+         * Return the Hop by Hop option length
+         * @return hop by hop option length
+         */
+        uint8_t hopByHopLength();
+
+        /**
+         * Return the Hop by Hop option type
+         * @return hop by hop option type
+         */
+        uint8_t optionType();
+
+        /**
+         * Set Hop-by-Hop option to Routar Alert with
+         * MLD alert-type
+         */
+        void setMLDRouterAlert();
+
+        /**
+         * Calculate the 16-bit checksum for the IPv6 packet
+         * @return the checksum of the packet
+         */
+        uint16_t calculateChecksum();
+
+        /**
+         * Get a pointer to the start of the IPv6 Hop-by-Hop packet payload
+         * May be used to get or change the contents of the packet
+         * @return a pointer to the memory containing the packet payload
+         */
+        uint8_t* payload();
+
+        /**
+         * Check if the Ethernet and IPv6 headers are valid
+         * Also verifies the checksum of the packet
+         *
+         * @return true if the packets fields are valid
+         */
+        boolean isValid();
+
+    protected:
+        uint8_t _next_header;   ///< The Type of the packet payload
+        uint8_t _hbh_length;    ///< The hop-by-hop option length
+        union {
+            uint8_t hbh_option[6];              ///< Raw hop-by-hop option content
+            struct router_alert _router_alert;   ///< RouterAlert hop-by-hop option
+        };
+} __attribute__((__packed__));
+
+#endif

src/IPv6Packet.h

@@ -20,6 +20,7 @@ enum ether_types {
 
 /** Enumeration of IP protocol numbers */
 enum ip_protocol {
+    IP6_PROTO_HBH = 0,      ///< IP protocol number for TCP
     IP6_PROTO_TCP = 6,      ///< IP protocol number for TCP
     IP6_PROTO_UDP = 17,     ///< IP protocol number for UDP
     IP6_PROTO_ICMP6 = 58    ///< IP protocol number for ICMP6

src/PingClient.cpp

@@ -18,7 +18,7 @@ boolean PingClient::setRemoteAddress(const char *remoteAddress)
 
 boolean PingClient::havePacket()
 {
-    ICMPv6Packet& packet = (ICMPv6Packet&)_ether.packet();
+    ICMPv6Packet<IPv6Packet>& packet = (ICMPv6Packet<IPv6Packet>&)_ether.packet();
 
     if (!_ether.bufferContainsReceived()) {
         return false;
@@ -75,7 +75,7 @@ void PingClient::send()
 
 void PingClient::sendInternal(uint16_t length, boolean /*isReply*/)
 {
-    ICMPv6Packet& packet = (ICMPv6Packet&)_ether.packet();
+    ICMPv6Packet<IPv6Packet>& packet = (ICMPv6Packet<IPv6Packet>&)_ether.packet();
     uint16_t totalLen = ICMP6_HEADER_LEN + ICMP6_ECHO_HEADER_LEN + length;
 
     packet.setProtocol(IP6_PROTO_ICMP6);
@@ -100,13 +100,13 @@ void PingClient::sendInternal(uint16_t length, boolean /*isReply*/)
 
 uint8_t* PingClient::payload()
 {
-    ICMPv6Packet& packet = (ICMPv6Packet&)_ether.packet();
+    ICMPv6Packet<IPv6Packet>& packet = (ICMPv6Packet<IPv6Packet>&)_ether.packet();
     return (uint8_t*)(packet.payload()) + ICMP6_HEADER_LEN + ICMP6_ECHO_HEADER_LEN;
 }
 
 uint16_t PingClient::payloadLength()
 {
-    ICMPv6Packet& packet = (ICMPv6Packet&)_ether.packet();
+    ICMPv6Packet<IPv6Packet>& packet = (ICMPv6Packet<IPv6Packet>&)_ether.packet();
     return packet.payloadLength() - ICMP6_HEADER_LEN - ICMP6_ECHO_HEADER_LEN;
 }