Do not use input header for error responses

When parsing of the input header fails, it could be for a variety of
issues, including an overflow of the language tag length. We should not
utilize any variable aspects of the header in our response and
instead just send back a default of all 0's with the static fields set
along with error the code.

Utilize std::bitset to get the function ID properly written to
cout/cerr.

Tested:
- Verified when sending in an invalid header, it no longer can cause
  unexpected error paths in parseHeader function

Change-Id: I17a8a013bcd90e083b1156f794f059fc1b830a63
Signed-off-by: Andrew Geissler <geissonator@yahoo.com>
diff --git a/meson.build b/meson.build
index 4c28048..5f90ea5 100644
--- a/meson.build
+++ b/meson.build
@@ -39,3 +39,17 @@
           include_directories: '../'
       )
   )
+
+  test(
+      'test_slp_message_handler',
+      executable('test_slp_message_handler',
+          './test/slp_message_handler_test.cpp',
+          'slp_parser.cpp',
+          'slp_message_handler.cpp',
+          dependencies: [
+              gtest,
+          ],
+          implicit_include_directories: true,
+          include_directories: '../'
+      )
+  )
diff --git a/slp_message_handler.cpp b/slp_message_handler.cpp
index b0e0cdd..9441545 100644
--- a/slp_message_handler.cpp
+++ b/slp_message_handler.cpp
@@ -9,6 +9,7 @@
 #include <string.h>
 
 #include <algorithm>
+#include <bitset>
 
 namespace slp
 {
@@ -363,7 +364,8 @@
 {
     int rc = slp::SUCCESS;
     buffer resp(0);
-    std::cout << "SLP Processing Request=" << msg.header.functionID << "\n";
+    std::cout << "SLP Processing Request="
+              << std::bitset<8>(msg.header.functionID) << "\n";
 
     switch (msg.header.functionID)
     {
@@ -382,14 +384,62 @@
 
 buffer processError(const Message& req, uint8_t err)
 {
-    buffer buff;
-    buff = slp::handler::internal::prepareHeader(req);
+    if (req.header.functionID != 0)
+    {
+        std::cout << "Processing Error for function: "
+                  << std::bitset<8>(req.header.functionID) << std::endl;
+    }
+
+    /*  0                   1                   2                   3
+        0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
+        +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+        |        Service Location header (function = SrvRply = 2)       |
+        +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+        |        Error Code             |        URL Entry count        |
+        +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+        |       <URL Entry 1>          ...       <URL Entry N>          \
+        +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+*/
+
+    // There has been some sort of issue processing the request from
+    // the client. Can not assume the input request buffer is valid
+    // so just create an empty buffer with the non-variable size
+    // fields set and the error code
+    uint8_t length = slp::header::MIN_LEN +     /* 14 bytes for header     */
+                     slp::response::SIZE_ERROR; /*  2 bytes for error code */
+
+    buffer buff(length, 0);
 
     static_assert(sizeof(err) == 1, "Errors should be 1 byte.");
 
+    buff[slp::header::OFFSET_VERSION] = req.header.version;
+
+    // will increment the function id from 1 as reply
+    buff[slp::header::OFFSET_FUNCTION] = req.header.functionID + 1;
+
+    std::copy_n(&length, slp::header::SIZE_LENGTH,
+                buff.data() + slp::header::OFFSET_LENGTH);
+
+    auto flags = endian::to_network(req.header.flags);
+
+    std::copy_n((uint8_t*)&flags, slp::header::SIZE_FLAGS,
+                buff.data() + slp::header::OFFSET_FLAGS);
+
+    std::copy_n(req.header.extOffset.data(), slp::header::SIZE_EXT,
+                buff.data() + slp::header::OFFSET_EXT);
+
+    auto xid = endian::to_network(req.header.xid);
+
+    std::copy_n((uint8_t*)&xid, slp::header::SIZE_XID,
+                buff.data() + slp::header::OFFSET_XID);
+
+    // This is an invalid header from user so just fill in 0 for langtag
+    uint16_t langtagLen = 0;
+    std::copy_n((uint8_t*)&langtagLen, slp::header::SIZE_LANG,
+                buff.data() + slp::header::OFFSET_LANG_LEN);
+
     // Since this is network order, the err should go in the 2nd byte of the
-    // error field.  This is immediately after the langtag.
-    buff[slp::header::MIN_LEN + req.header.langtag.length() + 1] = err;
+    // error field.
+    buff[slp::header::MIN_LEN + 1] = err;
 
     return buff;
 }
diff --git a/test/slp_message_handler_test.cpp b/test/slp_message_handler_test.cpp
new file mode 100644
index 0000000..a756bd2
--- /dev/null
+++ b/test/slp_message_handler_test.cpp
@@ -0,0 +1,101 @@
+#include "slp.hpp"
+#include "slp_meta.hpp"
+
+#include <gtest/gtest.h>
+
+// Header
+/*  0                   1                   2                   3
+    0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
+   +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+   |    Version    |  Function-ID  |            Length             |
+   +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+   | Length, contd.|O|F|R|       reserved          |Next Ext Offset|
+   +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+   |  Next Extension Offset, contd.|              XID              |
+   +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+   |      Language Tag Length      |         Language Tag          \
+   +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ */
+
+// Error response
+/*  0                   1                   2                   3
+    0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
+    +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+    |        Service Location header (function = SrvRply = 2)       |
+    +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+    |        Error Code             |        URL Entry count        |
+    +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+    |       <URL Entry 1>          ...       <URL Entry N>          \
+    +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+*/
+
+TEST(processError, BasicGoodPath)
+{
+    // Basic buffer with valid Function-ID
+    slp::buffer testData{0x02, 0x01, 0x00, 0x00, 0x0E, 0x00, 0x00,
+                         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
+
+    slp::Message req;
+    int rc = slp::SUCCESS;
+    std::tie(rc, req) = slp::parser::internal::parseHeader(testData);
+
+    EXPECT_EQ(rc, 0);
+
+    // Verify all expected fields show up in response buffer
+    std::vector<uint8_t> resp = slp::handler::processError(
+        req, static_cast<uint8_t>(slp::Error::MSG_NOT_SUPPORTED));
+
+    EXPECT_EQ(resp.size(), slp::header::MIN_LEN + slp::response::SIZE_ERROR);
+    EXPECT_EQ(resp[slp::header::OFFSET_VERSION], 2);
+    EXPECT_EQ(resp[slp::header::OFFSET_FUNCTION], 2);
+    EXPECT_EQ(resp[slp::header::MIN_LEN + 1],
+              static_cast<uint8_t>(slp::Error::MSG_NOT_SUPPORTED));
+}
+
+TEST(processError, InvalidLangTag)
+{
+    // Basic buffer with valid Function-ID
+    slp::buffer testData{0x02, 0x01, 0x00, 0x00, 0x0E, 0x00, 0x00,
+                         0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x10,
+                         0x00, 0x01, 0x02, 0x03, 0x04};
+
+    slp::Message req;
+    int rc = slp::SUCCESS;
+    std::tie(rc, req) = slp::parser::internal::parseHeader(testData);
+
+    EXPECT_NE(rc, 0);
+
+    // Verify all expected fields show up in response buffer even
+    // with an inavlid langugage tag size in the header
+    std::vector<uint8_t> resp = slp::handler::processError(
+        req, static_cast<uint8_t>(slp::Error::MSG_NOT_SUPPORTED));
+
+    EXPECT_EQ(resp.size(), slp::header::MIN_LEN + slp::response::SIZE_ERROR);
+    EXPECT_EQ(resp[slp::header::OFFSET_VERSION], 2);
+    EXPECT_EQ(resp[slp::header::OFFSET_FUNCTION], 2);
+    EXPECT_EQ(resp[slp::header::MIN_LEN + 1],
+              static_cast<uint8_t>(slp::Error::MSG_NOT_SUPPORTED));
+}
+
+TEST(processError, InvalidEverything)
+{
+    // Basic buffer with valid Function-ID
+    slp::buffer testData{0x03, 0x99, 0x99, 0x99, 0xA0, 0xA0, 0xA1,
+                         0xA2, 0xB9, 0x55, 0x44, 0x33, 0x21, 0x90,
+                         0x78, 0x1,  0x02, 0x03, 0x04};
+
+    slp::Message req;
+    int rc = slp::SUCCESS;
+    std::tie(rc, req) = slp::parser::internal::parseHeader(testData);
+
+    EXPECT_NE(rc, 0);
+
+    // Verify all expected fields show up in response buffer even
+    // with an inavlid langugage tag size in the header
+    std::vector<uint8_t> resp = slp::handler::processError(
+        req, static_cast<uint8_t>(slp::Error::MSG_NOT_SUPPORTED));
+
+    EXPECT_EQ(resp.size(), slp::header::MIN_LEN + slp::response::SIZE_ERROR);
+    EXPECT_EQ(resp[slp::header::OFFSET_VERSION], 3);
+    EXPECT_EQ(resp[slp::header::OFFSET_FUNCTION], 0x9A);
+    EXPECT_EQ(resp[slp::header::MIN_LEN + 1],
+              static_cast<uint8_t>(slp::Error::MSG_NOT_SUPPORTED));
+}