Implement OVPN_IOCTL_MP_NEW_PEER

Implement adding peer in MP mode. Also add this to gui client. GitHub: OpenVPN#85 Signed-off-by: Lev Stipakov <[email protected]>
lstipakov · Oct 8, 2024 · d4ac048 · d4ac048
1 parent 3486c5b
commit d4ac048
Show file tree

Hide file tree

Showing 9 changed files with 313 additions and 16 deletions.
diff --git a/Driver.cpp b/Driver.cpp
@@ -317,6 +317,7 @@ OvpnDeviceCheckMode(OVPN_MODE mode, ULONG code)
         switch (code) {
         // those IOCTLs are for MP mode
         case OVPN_IOCTL_MP_START_VPN:
+        case OVPN_IOCTL_MP_NEW_PEER:
             return FALSE;
         }
     }
@@ -500,6 +501,10 @@ OvpnEvtIoDeviceControl(WDFQUEUE queue, WDFREQUEST request, size_t outputBufferLe
         status = OvpnMPStartVPN(device, request, &bytesReturned);
         break;
 
+    case OVPN_IOCTL_MP_NEW_PEER:
+        status = OvpnMPPeerNew(device, request);
+        break;
+
     default:
         LOG_WARN("Unknown <ioControlCode>", TraceLoggingValue(ioControlCode, "ioControlCode"));
         status = STATUS_INVALID_DEVICE_REQUEST;
@@ -705,8 +710,10 @@ OvpnEvtDeviceAdd(WDFDRIVER wdfDriver, PWDFDEVICE_INIT deviceInit) {
 
     GOTO_IF_NOT_NT_SUCCESS(done, status, OvpnCryptoInitAlgHandles(&device->AesAlgHandle, &device->ChachaAlgHandle));
 
-    // Initialize peers tree
+    // Initialize peers tables
     RtlInitializeGenericTable(&device->Peers, OvpnPeerCompareByPeerIdRoutine, OvpnPeerAllocateRoutine, OvpnPeerFreeRoutine, NULL);
+    RtlInitializeGenericTable(&device->PeersByVpn4, OvpnPeerCompareByVPN4Routine, OvpnPeerAllocateRoutine, OvpnPeerFreeRoutine, NULL);
+    RtlInitializeGenericTable(&device->PeersByVpn6, OvpnPeerCompareByVPN6Routine, OvpnPeerAllocateRoutine, OvpnPeerFreeRoutine, NULL);
 
     LOG_IF_NOT_NT_SUCCESS(status = OvpnAdapterCreate(device));
 
@@ -716,17 +723,17 @@ OvpnEvtDeviceAdd(WDFDRIVER wdfDriver, PWDFDEVICE_INIT deviceInit) {
     return status;
 }
 
-_Use_decl_annotations_
 NTSTATUS
-OvpnAddPeer(POVPN_DEVICE device, OvpnPeerContext* peer)
+OvpnAddPeerToTable(_In_ RTL_GENERIC_TABLE* table, _In_ OvpnPeerContext* peer)
 {
     NTSTATUS status;
     BOOLEAN newElem;
 
-    RtlInsertElementGenericTable(&device->Peers, (PVOID)&peer, sizeof(OvpnPeerContext*), &newElem);
+    RtlInsertElementGenericTable(table, (PVOID)&peer, sizeof(OvpnPeerContext*), &newElem);
 
     if (newElem) {
         status = STATUS_SUCCESS;
+        InterlockedIncrement(&peer->RefCounter);
     }
     else {
         LOG_ERROR("Unable to add new peer");
@@ -735,9 +742,33 @@ OvpnAddPeer(POVPN_DEVICE device, OvpnPeerContext* peer)
     return status;
 }
 
+
+_Use_decl_annotations_
+NTSTATUS
+OvpnAddPeer(POVPN_DEVICE device, OvpnPeerContext* peer)
+{
+    return OvpnAddPeerToTable(&device->Peers, peer);
+}
+
+_Use_decl_annotations_
+NTSTATUS
+OvpnAddPeerVpn4(POVPN_DEVICE device, OvpnPeerContext* peer)
+{
+    return OvpnAddPeerToTable(&device->PeersByVpn4, peer);
+}
+
+_Use_decl_annotations_
+NTSTATUS
+OvpnAddPeerVpn6(POVPN_DEVICE device, OvpnPeerContext* peer)
+{
+    return OvpnAddPeerToTable(&device->PeersByVpn6, peer);
+}
+
 _Use_decl_annotations_
 VOID
 OvpnFlushPeers(POVPN_DEVICE device) {
+    OvpnCleanupPeerTable(&device->PeersByVpn6);
+    OvpnCleanupPeerTable(&device->PeersByVpn4);
     OvpnCleanupPeerTable(&device->Peers);
 }
 
@@ -750,7 +781,9 @@ OvpnCleanupPeerTable(RTL_GENERIC_TABLE* peers)
         OvpnPeerContext* peer = *(OvpnPeerContext**)ptr;
         RtlDeleteElementGenericTable(peers, ptr);
 
-        OvpnPeerCtxFree(peer);
+        if (InterlockedDecrement(&peer->RefCounter) == 0) {
+            OvpnPeerCtxFree(peer);
+        }
     }
 }
 
@@ -761,3 +794,17 @@ OvpnGetFirstPeer(RTL_GENERIC_TABLE* peers)
     OvpnPeerContext** ptr = (OvpnPeerContext**)RtlGetElementGenericTable(peers, 0);
     return ptr ? (OvpnPeerContext*)*ptr : NULL;
 }
+
+_Use_decl_annotations_
+OvpnPeerContext*
+OvpnFindPeer(POVPN_DEVICE device, INT32 PeerId)
+{
+    if (device->Mode == OVPN_MODE_P2P) {
+        return OvpnGetFirstPeer(&device->Peers);
+    }
+
+    OvpnPeerContext p {};
+    p.PeerId = PeerId;
+    OvpnPeerContext** ptr = (OvpnPeerContext**)RtlLookupElementGenericTable(&device->Peers, &p);
+    return ptr ? (OvpnPeerContext*)*ptr : NULL;
+}
diff --git a/Driver.h b/Driver.h
@@ -94,6 +94,12 @@ struct OVPN_DEVICE {
     _Guarded_by_(SpinLock)
     RTL_GENERIC_TABLE Peers;
 
+    _Guarded_by_(SpinLock)
+    RTL_GENERIC_TABLE PeersByVpn4;
+
+    _Guarded_by_(SpinLock)
+    RTL_GENERIC_TABLE PeersByVpn6;
+
     OVPN_MODE Mode;
 };
 
@@ -114,6 +120,14 @@ _Must_inspect_result_
 NTSTATUS
 OvpnAddPeer(_In_ POVPN_DEVICE device, _In_ OvpnPeerContext* PeerCtx);
 
+_Must_inspect_result_
+NTSTATUS
+OvpnAddPeerVpn4(_In_ POVPN_DEVICE device, _In_ OvpnPeerContext* PeerCtx);
+
+_Must_inspect_result_
+NTSTATUS
+OvpnAddPeerVpn6(_In_ POVPN_DEVICE device, _In_ OvpnPeerContext* PeerCtx);
+
 VOID
 OvpnFlushPeers(_In_ POVPN_DEVICE device);
 
@@ -123,3 +137,7 @@ OvpnCleanupPeerTable(_In_ RTL_GENERIC_TABLE*);
 _Must_inspect_result_
 OvpnPeerContext*
 OvpnGetFirstPeer(_In_ RTL_GENERIC_TABLE*);
+
+_Must_inspect_result_
+OvpnPeerContext*
+OvpnFindPeer(_In_ POVPN_DEVICE device, INT32 PeerId);
diff --git a/crypto.h b/crypto.h
@@ -36,6 +36,7 @@
 #define OVPN_OP_DATA_V2 9
 #define OVPN_KEY_ID_MASK 0x07
 #define OVPN_OPCODE_SHIFT 3
+#define OVPN_PEER_ID_MASK 0x00FFFFFF
 
 struct OvpnCryptoKeySlot
 {

diff --git a/gui/gui.cpp b/gui/gui.cpp
@@ -20,7 +20,8 @@ LRESULT CALLBACK WindowProcedure(HWND, UINT, WPARAM, LPARAM);
 HWND hMPListenAddress, hMPListenPort,
     hP2PLocalAddress, hP2PLocalPort,
     hP2PRemoteAddress, hP2PRemotePort,
-    hCCMessage, hCCRemoteAddress, hCCRemotePort;
+    hCCMessage, hCCRemoteAddress, hCCRemotePort,
+    hMPNewPeerLocalIP, hMPNewPeerLocalPort, hMPNewPeerRemoteIP, hMPNewPeerRemotePort, hMPNewPeerVPNIP, hMPNewPeerPeerId;
 
 HWND hLogArea;
 std::unordered_map<DWORD, std::wstring> buttons = {
@@ -34,7 +35,8 @@ std::unordered_map<DWORD, std::wstring> buttons = {
     {OVPN_IOCTL_GET_VERSION, L"Get Version"},
     {OVPN_IOCTL_NEW_KEY_V2, L"New Key V2"},
     {OVPN_IOCTL_SET_MODE, L"Set Mode"},
-    {OVPN_IOCTL_MP_START_VPN, L"MP Start VPN"}
+    {OVPN_IOCTL_MP_START_VPN, L"MP Start VPN"},
+    {OVPN_IOCTL_MP_NEW_PEER, L"MP New Peer"}
 };
 
 #define MIN_FUNCTION_CODE 1
@@ -102,7 +104,7 @@ int WINAPI WinMain(HINSTANCE hInst, HINSTANCE hPrevInst, LPSTR args, int ncmdsho
 
     // Create the Window
     HWND hwnd = CreateWindowW(L"myWindowClass", L"ovpn-dco-win GUI", WS_OVERLAPPEDWINDOW | WS_VISIBLE,
-                              100, 100, 800, 600, NULL, NULL, NULL, NULL);
+                              100, 100, 900, 600, NULL, NULL, NULL, NULL);
 
     HANDLE hEvRead = CreateEventW(NULL, FALSE, FALSE, NULL);
     ovRead.hEvent = hEvRead;
@@ -324,6 +326,46 @@ void P2PStartVPN()
     }
 }
 
+void MPNewPeer()
+{
+    wchar_t localIP[16], localPort[6];
+    wchar_t remoteIP[16], remotePort[6], vpnIP[16];
+    wchar_t peerId[6];
+
+    GetWindowText(hMPNewPeerLocalIP, localIP, 16);
+    GetWindowText(hMPNewPeerLocalPort, localPort, 6);
+    GetWindowText(hMPNewPeerRemoteIP, remoteIP, 16);
+    GetWindowText(hMPNewPeerRemotePort, remotePort, 6);
+    GetWindowText(hMPNewPeerVPNIP, vpnIP, 16);
+
+    sockaddr_in localAddr  = {};
+    localAddr.sin_family = AF_INET;
+    InetPtonW(AF_INET, localIP, &(localAddr.sin_addr));
+    localAddr.sin_port = htons(_wtoi(localPort));
+
+    sockaddr_in remoteAddr = {};
+    remoteAddr.sin_family = AF_INET;
+    InetPtonW(AF_INET, remoteIP, &(remoteAddr.sin_addr));
+    remoteAddr.sin_port = htons(_wtoi(remotePort));
+
+    in_addr vpnAddress;
+    InetPtonW(AF_INET, vpnIP, &vpnAddress);
+
+    OVPN_MP_NEW_PEER newPeer = {};
+    newPeer.Local.Addr4 = localAddr;
+    newPeer.Remote.Addr4 = remoteAddr;
+    newPeer.VpnAddr4 = vpnAddress;
+    newPeer.PeerId = _wtoi(peerId);
+
+    DWORD bytesReturned;
+    if (!DeviceIoControl(hDev, OVPN_IOCTL_MP_NEW_PEER, &newPeer, sizeof(newPeer), NULL, 0, &bytesReturned, NULL)) {
+        Log("DeviceIoControl(OVPN_IOCTL_MP_NEW_PEER) failed with code ", GetLastError());
+    }
+    else {
+        Log("MP peer added");
+    }
+}
+
 void
 CreatePushButton(HWND hWnd, DWORD ioctl, int x, int y)
 {
@@ -412,6 +454,14 @@ LRESULT CALLBACK WindowProcedure(HWND hwnd, UINT msg, WPARAM wp, LPARAM lp)
         hCCRemoteAddress = CreateEditBox(hwnd, L"192.168.100.1", 290, 160, 120);
         hCCRemotePort = CreateEditBox(hwnd, L"1194", 430, 160, 60);
 
+        CreatePushButton(hwnd, OVPN_IOCTL_MP_NEW_PEER, 10, 210);
+        hMPNewPeerLocalIP = CreateEditBox(hwnd, L"192.168.100.2", 150, 210, 120);
+        hMPNewPeerLocalPort = CreateEditBox(hwnd, L"1194", 290, 210, 60);
+        hMPNewPeerRemoteIP = CreateEditBox(hwnd, L"192.168.100.1", 400, 210, 120);
+        hMPNewPeerRemotePort = CreateEditBox(hwnd, L"1194", 540, 210, 60);
+        hMPNewPeerVPNIP = CreateEditBox(hwnd, L"10.8.0.6", 650, 210, 120);
+        hMPNewPeerPeerId = CreateEditBox(hwnd, L"1", 790, 210, 60);
+
         SendMessage(hModes[0], BM_SETCHECK, BST_CHECKED, 0);
 
         // log area
@@ -450,6 +500,9 @@ LRESULT CALLBACK WindowProcedure(HWND hwnd, UINT msg, WPARAM wp, LPARAM lp)
             case OVPN_IOCTL_START_VPN:
                 P2PStartVPN();
                 break;
+
+            case OVPN_IOCTL_MP_NEW_PEER:
+                MPNewPeer();
             }
         }
         else if ((ULONG)wp == BTN_SEND_CC) {