From 41638b437bb0f77904eb6e3b530c5c5a0835c644 Mon Sep 17 00:00:00 2001
From: John Safranek <john@wolfssl.com>
Date: Tue, 20 Dec 2016 16:16:50 -0800
Subject: [PATCH] DTLS Multicast 1. Add configured group ID to outbound DTLS
 datagrams. 2. Parse the group ID from inbound DTLS datagrams.

---
 src/internal.c     | 45 ++++++++++++++++++++++++++++++++++++++-------
 wolfssl/internal.h |  4 +++-
 2 files changed, 41 insertions(+), 8 deletions(-)

diff --git a/src/internal.c b/src/internal.c
index 3608df49c..249867bf6 100755
--- a/src/internal.c
+++ b/src/internal.c
@@ -4906,18 +4906,42 @@ static INLINE void DtlsGetSEQ(WOLFSSL* ssl, int order, word32 seq[2])
 {
     if (order == PREV_ORDER) {
         /* Previous epoch case */
-        seq[0] = ((ssl->keys.dtls_epoch - 1) << 16) |
-                 (ssl->keys.dtls_prev_sequence_number_hi & 0xFFFF);
+        if (ssl->options.haveMcast) {
+        #ifdef WOLFSSL_MULTICAST
+            seq[0] = ((ssl->keys.dtls_epoch - 1) << 16) |
+                     (ssl->options.mcastID << 8) |
+                     (ssl->keys.dtls_prev_sequence_number_hi & 0xFF);
+        #endif
+        }
+        else
+            seq[0] = ((ssl->keys.dtls_epoch - 1) << 16) |
+                     (ssl->keys.dtls_prev_sequence_number_hi & 0xFFFF);
         seq[1] = ssl->keys.dtls_prev_sequence_number_lo;
     }
     else if (order == PEER_ORDER) {
-        seq[0] = (ssl->keys.curEpoch << 16) |
-                 (ssl->keys.curSeq_hi & 0xFFFF);
+        if (ssl->options.haveMcast) {
+        #ifdef WOLFSSL_MULTICAST
+            seq[0] = (ssl->keys.curEpoch << 16) |
+                     (ssl->keys.curPeerId << 8) |
+                     (ssl->keys.curSeq_hi & 0xFF);
+        #endif
+        }
+        else
+            seq[0] = (ssl->keys.curEpoch << 16) |
+                     (ssl->keys.curSeq_hi & 0xFFFF);
         seq[1] = ssl->keys.curSeq_lo; /* explicit from peer */
     }
     else {
-        seq[0] = (ssl->keys.dtls_epoch << 16) |
-                 (ssl->keys.dtls_sequence_number_hi & 0xFFFF);
+        if (ssl->options.haveMcast) {
+        #ifdef WOLFSSL_MULTICAST
+            seq[0] = (ssl->keys.dtls_epoch << 16) |
+                     (ssl->options.mcastID << 8) |
+                     (ssl->keys.dtls_sequence_number_hi & 0xFF);
+        #endif
+        }
+        else
+            seq[0] = (ssl->keys.dtls_epoch << 16) |
+                     (ssl->keys.dtls_sequence_number_hi & 0xFFFF);
         seq[1] = ssl->keys.dtls_sequence_number_lo;
     }
 }
@@ -6217,7 +6241,14 @@ static int GetRecordHeader(WOLFSSL* ssl, const byte* input, word32* inOutIdx,
         *inOutIdx += ENUM_LEN + VERSION_SZ;
         ato16(input + *inOutIdx, &ssl->keys.curEpoch);
         *inOutIdx += OPAQUE16_LEN;
-        ato16(input + *inOutIdx, &ssl->keys.curSeq_hi);
+        if (ssl->options.haveMcast) {
+        #ifdef WOLFSSL_MULTICAST
+            ssl->keys.curPeerId = input[*inOutIdx];
+            ssl->keys.curSeq_hi = input[*inOutIdx+1];
+        #endif
+        }
+        else
+            ato16(input + *inOutIdx, &ssl->keys.curSeq_hi);
         *inOutIdx += OPAQUE16_LEN;
         ato32(input + *inOutIdx, &ssl->keys.curSeq_lo);
         *inOutIdx += OPAQUE32_LEN;  /* advance past rest of seq */
diff --git a/wolfssl/internal.h b/wolfssl/internal.h
index 5482ad724..3805883dd 100755
--- a/wolfssl/internal.h
+++ b/wolfssl/internal.h
@@ -1756,6 +1756,9 @@ typedef struct Keys {
     word16 curEpoch;    /* Received epoch in current record    */
     word16 curSeq_hi;   /* Received sequence in current record */
     word32 curSeq_lo;
+#ifdef WOLFSSL_MULTICAST
+    byte   curPeerId;   /* Received peer group ID in current record */
+#endif
 
     word32 prevWindow[WOLFSSL_DTLS_WINDOW_WORDS];
                         /* Sliding window for old epoch        */
@@ -2846,7 +2849,6 @@ typedef struct Options {
     word16            dtlsSctp:1;         /* DTLS-over-SCTP mode */
 #endif
 #endif
-#ifdef WOLFSSL_MULTICAST
     word16            haveMcast:1;        /* using multicast ? */
 #endif
     word16            haveEMS:1;          /* using extended master secret */