From da480bcaf07104dd7b802e5f231296bbae223493 Mon Sep 17 00:00:00 2001
From: Atlante45 <clement.brisset@gmail.com>
Date: Wed, 29 Jul 2015 20:05:15 -0700
Subject: [PATCH] Fix ControlPackets read/write header

---
 .../networking/src/udt/ControlPacket.cpp      | 20 +++++--------------
 libraries/networking/src/udt/Packet.cpp       | 19 +++++++-----------
 libraries/networking/src/udt/SequenceNumber.h |  1 +
 3 files changed, 13 insertions(+), 27 deletions(-)

diff --git a/libraries/networking/src/udt/ControlPacket.cpp b/libraries/networking/src/udt/ControlPacket.cpp
index bd9e851a79..cee2436165 100644
--- a/libraries/networking/src/udt/ControlPacket.cpp
+++ b/libraries/networking/src/udt/ControlPacket.cpp
@@ -61,7 +61,7 @@ ControlPacket::ControlPacket(Type type) :
     
     open(QIODevice::ReadWrite);
     
-    writeControlBitAndType();
+    writeType();
 }
 
 ControlPacket::ControlPacket(Type type, qint64 size) :
@@ -72,7 +72,7 @@ ControlPacket::ControlPacket(Type type, qint64 size) :
     
     open(QIODevice::ReadWrite);
     
-    writeControlBitAndType();
+    writeType();
 }
 
 ControlPacket::ControlPacket(std::unique_ptr<char> data, qint64 size, const HifiSockAddr& senderSockAddr) :
@@ -106,20 +106,11 @@ void ControlPacket::setType(udt::ControlPacket::Type type) {
     writeType();
 }
 
-void ControlPacket::writeControlBitAndType() {
-    ControlBitAndType* bitAndType = reinterpret_cast<ControlBitAndType*>(_packet.get());
-
-    // write the control bit by OR'ing the current value with the CONTROL_BIT_MASK
-    *bitAndType = (*bitAndType | CONTROL_BIT_MASK);
-    
-    writeType();
-}
-
 void ControlPacket::writeType() {
     ControlBitAndType* bitAndType = reinterpret_cast<ControlBitAndType*>(_packet.get());
     
-    // write the type by OR'ing the new type with the current value & CONTROL_BIT_MASK
-    *bitAndType = (*bitAndType & CONTROL_BIT_MASK) | (_type << (sizeof(ControlPacket::Type) * 8 - 1));
+    // We override the control bit here by writing the type but it's okay, it'll always be 1
+    *bitAndType = CONTROL_BIT_MASK | (ControlBitAndType(_type) << (8 * sizeof(Type)));
 }
 
 void ControlPacket::readType() {
@@ -128,6 +119,5 @@ void ControlPacket::readType() {
     Q_ASSERT_X(bitAndType & CONTROL_BIT_MASK, "ControlPacket::readHeader()", "This should be a control packet");
     
     // read the type
-    uint32_t oversizeType = (uint32_t) (bitAndType & ~CONTROL_BIT_MASK);
-    _type = (Type) oversizeType;
+    _type = (Type) ((bitAndType & ~CONTROL_BIT_MASK) >> (8 * sizeof(Type)));
 }
diff --git a/libraries/networking/src/udt/Packet.cpp b/libraries/networking/src/udt/Packet.cpp
index cbe9c986bd..673013a4a7 100644
--- a/libraries/networking/src/udt/Packet.cpp
+++ b/libraries/networking/src/udt/Packet.cpp
@@ -135,21 +135,16 @@ void Packet::writeHeader() const {
     // grab pointer to current SequenceNumberAndBitField
     SequenceNumberAndBitField* seqNumBitField = reinterpret_cast<SequenceNumberAndBitField*>(_packet.get());
     
-    // 0 for data packets
-    *seqNumBitField &= ~CONTROL_BIT_MASK;
-    
-    if (_isPartOfMessage) {
-        *seqNumBitField |= MESSAGE_BIT_MASK;
-    } else {
-        *seqNumBitField &= ~MESSAGE_BIT_MASK;
-    }
+    // Write sequence number and reset bit field
+    Q_ASSERT_X(!((SequenceNumber::Type)_sequenceNumber & BIT_FIELD_MASK),
+               "Packet::writeHeader()", "Sequence number is overflowing into bit field");
+    *seqNumBitField = ((SequenceNumber::Type)_sequenceNumber);
     
     if (_isReliable) {
         *seqNumBitField |= RELIABILITY_BIT_MASK;
-    } else {
-        *seqNumBitField &= ~RELIABILITY_BIT_MASK;
     }
     
-    // write new value by ORing (old value & BIT_FIELD_MASK) with new seqNum
-    *seqNumBitField = (*seqNumBitField & BIT_FIELD_MASK) | (SequenceNumber::Type)_sequenceNumber;
+    if (_isPartOfMessage) {
+        *seqNumBitField |= MESSAGE_BIT_MASK;
+    }
 }
diff --git a/libraries/networking/src/udt/SequenceNumber.h b/libraries/networking/src/udt/SequenceNumber.h
index 6215d23494..d5b61c0a95 100644
--- a/libraries/networking/src/udt/SequenceNumber.h
+++ b/libraries/networking/src/udt/SequenceNumber.h
@@ -92,6 +92,7 @@ private:
     
     friend struct std::hash<SequenceNumber>;
 };
+static_assert(sizeof(SequenceNumber) == sizeof(uint32_t), "SequenceNumber invalid size");
     
     
 inline bool operator<(const SequenceNumber&  a, const SequenceNumber& b) {