From b1e32737c2da8be334f592879257ea2a66a91ffb Mon Sep 17 00:00:00 2001
From: John Safranek <john@wolfssl.com>
Date: Tue, 16 Dec 2014 19:09:40 -0800
Subject: [PATCH] Framing in the service request messages

---
 src/internal.c     | 64 ++++++++++++++++++++++++++++++++++++++++++++++
 wolfssh/internal.h |  1 +
 2 files changed, 65 insertions(+)

diff --git a/src/internal.c b/src/internal.c
index 1e87e24..b645fd6 100644
--- a/src/internal.c
+++ b/src/internal.c
@@ -1137,6 +1137,33 @@ static int DoDisconnect(WOLFSSH* ssh, uint8_t* buf, uint32_t len, uint32_t* idx)
 }
 
 
+static const char serviceNameUserAuth[] = "ssh-userauth";
+/*static const char serviceNameConnection[] = "ssh-connection";*/
+
+
+static int DoServiceRequest(WOLFSSH* ssh,
+                            uint8_t* buf, uint32_t len, uint32_t* idx)
+{
+    uint32_t    begin = *idx;
+    uint32_t    nameSz;
+    char foo[32];
+    (void)ssh;
+    (void)buf;
+    (void)len;
+
+    ato32(buf + begin, &nameSz);
+    begin += LENGTH_SZ;
+
+    XMEMCPY(foo, buf + begin, nameSz);
+    foo[nameSz] = 0;
+
+    printf("Requesting service: %s\n", foo);
+    SendServiceAccept(ssh, serviceNameUserAuth);
+
+    return WS_SUCCESS;
+}
+
+
 static int DoPacket(WOLFSSH* ssh)
 {
     uint8_t* buf = (uint8_t*)ssh->inputBuffer.buffer;
@@ -1204,6 +1231,12 @@ static int DoPacket(WOLFSSH* ssh)
             DoKexDhInit(ssh, buf, payloadSz - 1, &idx);
             break;
 
+        case MSGID_SERVICE_REQUEST:
+            WLOG(WS_LOG_DEBUG, "Decoding MSGID_SERVICE_REQUEST (len = %d)",
+                 payloadSz - 1);
+            DoServiceRequest(ssh, buf, payloadSz - 1, &idx);
+            break;
+
         default:
             WLOG(WS_LOG_DEBUG, "Unimplemented message ID (%d)", msg);
             DumpOctetString(buf + idx, payloadSz - 1);
@@ -1529,6 +1562,7 @@ static int BundlePacket(WOLFSSH* ssh)
     paddingSz = ssh->paddingSz;
 
     /* Add the padding */
+    WLOG(WS_LOG_DEBUG, "paddingSz = %u", paddingSz);
     if (ssh->encryptId == ID_NONE)
         WMEMSET(output + idx, 0, paddingSz);
     else
@@ -2018,6 +2052,36 @@ int SendDebug(WOLFSSH* ssh, byte alwaysDisplay, const char* msg)
 }
 
 
+int SendServiceAccept(WOLFSSH* ssh, const char* name)
+{
+    uint32_t nameSz;
+    uint8_t* output;
+    uint32_t idx;
+
+    if (ssh == NULL || name == NULL)
+        return WS_BAD_ARGUMENT;
+
+    nameSz = (uint32_t)WSTRLEN(name);
+    PreparePacket(ssh, MSG_ID_SZ + LENGTH_SZ + nameSz);
+
+    output = ssh->outputBuffer.buffer;
+    idx = ssh->outputBuffer.length;
+
+    output[idx++] = MSGID_SERVICE_ACCEPT;
+    c32toa(nameSz, output + idx);
+    idx += LENGTH_SZ;
+    WMEMCPY(output + idx, name, nameSz);
+    idx += nameSz;
+
+    ssh->outputBuffer.length = idx;
+
+    BundlePacket(ssh);
+    SendBuffered(ssh);
+
+    return WS_SUCCESS;
+}
+
+
 #define LINE_WIDTH 16
 void DumpOctetString(const uint8_t* input, uint32_t inputSz)
 {
diff --git a/wolfssh/internal.h b/wolfssh/internal.h
index a10ce88..6698c4b 100644
--- a/wolfssh/internal.h
+++ b/wolfssh/internal.h
@@ -239,6 +239,7 @@ WOLFSSH_LOCAL int SendUnimplemented(WOLFSSH*);
 WOLFSSH_LOCAL int SendDisconnect(WOLFSSH*, uint32_t);
 WOLFSSH_LOCAL int SendIgnore(WOLFSSH*, const unsigned char*, uint32_t);
 WOLFSSH_LOCAL int SendDebug(WOLFSSH*, byte, const char*);
+WOLFSSH_LOCAL int SendServiceAccept(WOLFSSH*, const char*);
 
 
 enum AcceptStates {