Merge pull request #83 from luxonis/fix-discovery-shared-memory

Fix discovery shared memory

include/XLink/XLink.h

@@ -313,7 +313,7 @@ XLinkError_t XLinkWriteData_(streamId_t streamId, const uint8_t* buffer, int siz
  */
 XLinkError_t XLinkWriteFd(streamId_t const streamId, const long fd);
 XLinkError_t XLinkWriteFd_(streamId_t streamId, const long fd, XLinkTimespec* outTSend);
-XLinkError_t XLinkWriteFdData(streamId_t streamId, const long fd, int fdSize, const uint8_t* dataBuffer, int dataSize);
+XLinkError_t XLinkWriteFdData(streamId_t streamId, const long fd, const uint8_t* dataBuffer, int dataSize);
 
 
 /**

include/XLink/XLinkPlatform.h

@@ -35,6 +35,7 @@ typedef enum {
     X_LINK_PLATFORM_USB_DRIVER_NOT_LOADED = X_LINK_PLATFORM_DRIVER_NOT_LOADED+X_LINK_USB_VSC,
     X_LINK_PLATFORM_TCP_IP_DRIVER_NOT_LOADED = X_LINK_PLATFORM_DRIVER_NOT_LOADED+X_LINK_TCP_IP,
     X_LINK_PLATFORM_LOCAL_SHDMEM_DRIVER_NOT_LOADED = X_LINK_PLATFORM_DRIVER_NOT_LOADED+X_LINK_LOCAL_SHDMEM,
+    X_LINK_PLATFORM_TCP_IP_OR_LOCAL_SHDMEM_DRIVER_NOT_LOADED = X_LINK_PLATFORM_DRIVER_NOT_LOADED+X_LINK_TCP_IP_OR_LOCAL_SHDMEM,
     X_LINK_PLATFORM_PCIE_DRIVER_NOT_LOADED = X_LINK_PLATFORM_DRIVER_NOT_LOADED+X_LINK_PCIE,
 } xLinkPlatformErrorCode_t;
 

include/XLink/XLinkPublicDefines.h

@@ -55,6 +55,7 @@ typedef enum{
     X_LINK_INIT_USB_ERROR,
     X_LINK_INIT_TCP_IP_ERROR,
     X_LINK_INIT_LOCAL_SHDMEM_ERROR,
+    X_LINK_INIT_TCP_IP_OR_LOCAL_SHDMEM_ERROR,
     X_LINK_INIT_PCIE_ERROR,
 } XLinkError_t;
 

src/pc/PlatformData.c

@@ -99,6 +99,8 @@ int XLinkPlatformWrite(xLinkDeviceHandle_t *deviceHandle, void *data, int size)
 	case X_LINK_LOCAL_SHDMEM:
 	    return shdmemPlatformWrite(deviceHandle->xLinkFD, data, size);
 #endif
+	case X_LINK_TCP_IP_OR_LOCAL_SHDMEM:
+	    mvLog(MVLOG_ERROR, "Failed to write with TCP_IP_OR_LOCAL_SHDMEM\n");
 
         default:
             return X_LINK_PLATFORM_INVALID_PARAMETERS;
@@ -161,6 +163,8 @@ int XLinkPlatformWriteFd(xLinkDeviceHandle_t *deviceHandle, const long fd, void
 		return result;
 	    }
 #endif
+	case X_LINK_TCP_IP_OR_LOCAL_SHDMEM:
+	    mvLog(MVLOG_ERROR, "Failed to write FD with TCP_IP_OR_LOCAL_SHDMEM\n");
         default:
             return X_LINK_PLATFORM_INVALID_PARAMETERS;
     }
@@ -187,7 +191,8 @@ int XLinkPlatformRead(xLinkDeviceHandle_t *deviceHandle, void *data, int size, l
 	case X_LINK_LOCAL_SHDMEM:
 	    return shdmemPlatformRead(deviceHandle->xLinkFD, data, size, fd);
 #endif
-
+	case X_LINK_TCP_IP_OR_LOCAL_SHDMEM:
+	    mvLog(MVLOG_ERROR, "Failed to read with TCP_IP_OR_LOCAL_SHDMEM\n");
         default:
             return X_LINK_PLATFORM_INVALID_PARAMETERS;
     }

src/pc/PlatformDeviceSearch.c

@@ -9,6 +9,7 @@
 #include "usb_host.h"
 #include "pcie_host.h"
 #include "tcpip_host.h"
+#include "local_memshd.h"
 #include "XLinkStringUtils.h"
 
 
@@ -33,6 +34,11 @@ static xLinkPlatformErrorCode_t getTcpIpDevices(const deviceDesc_t in_deviceRequ
                                                     deviceDesc_t* out_foundDevices, int sizeFoundDevices,
                                                     unsigned int *out_amountOfFoundDevices);
 
+#if defined(__unix__)
+static xLinkPlatformErrorCode_t getLocalShdmemDevices(const deviceDesc_t in_deviceRequirements,
+                                                    deviceDesc_t* out_foundDevices, int sizeFoundDevices,
+                                                    unsigned int *out_amountOfFoundDevices);
+#endif
 
 // ------------------------------------
 // Helpers declaration. End.
@@ -50,6 +56,7 @@ xLinkPlatformErrorCode_t XLinkPlatformFindDevices(const deviceDesc_t in_deviceRe
     xLinkPlatformErrorCode_t USB_rc;
     xLinkPlatformErrorCode_t PCIe_rc;
     xLinkPlatformErrorCode_t TCPIP_rc;
+    xLinkPlatformErrorCode_t SHDMEM_rc;
     unsigned numFoundDevices = 0;
     *out_amountOfFoundDevices = 0;
 
@@ -66,15 +73,21 @@ xLinkPlatformErrorCode_t XLinkPlatformFindDevices(const deviceDesc_t in_deviceRe
         case X_LINK_PCIE:
             return getPCIeDeviceName(0, state, in_deviceRequirements, out_foundDevice);
         */
-
         case X_LINK_TCP_IP:
             if(!XLinkIsProtocolInitialized(in_deviceRequirements.protocol)) {
                 return X_LINK_PLATFORM_DRIVER_NOT_LOADED+in_deviceRequirements.protocol;
             }
             return getTcpIpDevices(in_deviceRequirements, out_foundDevices, sizeFoundDevices, out_amountOfFoundDevices);
 
-        case X_LINK_ANY_PROTOCOL:
+#if defined(__unix__)
+	case X_LINK_LOCAL_SHDMEM:
+	    if(!XLinkIsProtocolInitialized(in_deviceRequirements.protocol)) {
+                return X_LINK_PLATFORM_DRIVER_NOT_LOADED+in_deviceRequirements.protocol;
+	    }
+            return getLocalShdmemDevices(in_deviceRequirements, out_foundDevices, sizeFoundDevices, out_amountOfFoundDevices);
+#endif
 
+        case X_LINK_ANY_PROTOCOL:
             // If USB protocol is initialized
             if(XLinkIsProtocolInitialized(X_LINK_USB_VSC)) {
                 // Find first correct USB Device
@@ -108,6 +121,22 @@ xLinkPlatformErrorCode_t XLinkPlatformFindDevices(const deviceDesc_t in_deviceRe
             }
             */
 
+	case X_LINK_TCP_IP_OR_LOCAL_SHDMEM:
+#if defined(__unix__)
+	    if(XLinkIsProtocolInitialized(X_LINK_LOCAL_SHDMEM)) {
+                numFoundDevices = 0;
+                SHDMEM_rc = getLocalShdmemDevices(in_deviceRequirements, out_foundDevices, sizeFoundDevices, &numFoundDevices);
+                *out_amountOfFoundDevices += numFoundDevices;
+                out_foundDevices += numFoundDevices;
+                // Found enough devices, return
+                if (numFoundDevices >= sizeFoundDevices) {
+                    return X_LINK_PLATFORM_SUCCESS;
+                } else {
+                    sizeFoundDevices -= numFoundDevices;
+                }
+            }
+#endif
+
             // Try find TCPIP device
             if(XLinkIsProtocolInitialized(X_LINK_TCP_IP)) {
                 numFoundDevices = 0;
@@ -150,6 +179,7 @@ char* XLinkPlatformErrorToStr(const xLinkPlatformErrorCode_t errorCode) {
         case X_LINK_PLATFORM_USB_DRIVER_NOT_LOADED: return "X_LINK_PLATFORM_USB_DRIVER_NOT_LOADED";
         case X_LINK_PLATFORM_TCP_IP_DRIVER_NOT_LOADED: return "X_LINK_PLATFORM_TCP_IP_DRIVER_NOT_LOADED";
 	case X_LINK_PLATFORM_LOCAL_SHDMEM_DRIVER_NOT_LOADED: return "X_LINK_PLATFORM_LOCAL_SHDMEM_DRIVER_NOT_LOADED";
+	case X_LINK_PLATFORM_TCP_IP_OR_LOCAL_SHDMEM_DRIVER_NOT_LOADED: return "X_LINK_PLATFORM_LOCAL_SHDMEM_DRIVER_NOT_LOADED";
         case X_LINK_PLATFORM_PCIE_DRIVER_NOT_LOADED: return "X_LINK_PLATFORM_PCIE_DRIVER_NOT_LOADED";
         case X_LINK_PLATFORM_INVALID_PARAMETERS: return "X_LINK_PLATFORM_INVALID_PARAMETERS";
         default: return "";
@@ -294,6 +324,33 @@ xLinkPlatformErrorCode_t getTcpIpDevices(const deviceDesc_t in_deviceRequirement
     return tcpip_get_devices(in_deviceRequirements, out_foundDevices, sizeFoundDevices, out_amountOfFoundDevices);
 }
 
+
+#if defined(__unix__)
+xLinkPlatformErrorCode_t getLocalShdmemDevices(const deviceDesc_t in_deviceRequirements,
+                                                    deviceDesc_t* out_foundDevices, int sizeFoundDevices,
+                                                    unsigned int *out_amountOfFoundDevices)
+{
+    ASSERT_XLINK_PLATFORM(out_foundDevices);
+    ASSERT_XLINK_PLATFORM(out_amountOfFoundDevices);
+    if (in_deviceRequirements.platform == X_LINK_MYRIAD_2) {
+        /**
+         * No case with TCP IP devices on TCP_IP protocol
+         */
+        return X_LINK_PLATFORM_ERROR;
+    }
+
+    if(in_deviceRequirements.state == X_LINK_UNBOOTED) {
+        /**
+         * There is no condition where unbooted
+         * state device to be found using tcp/ip.
+        */
+        return X_LINK_PLATFORM_DEVICE_NOT_FOUND;
+    }
+
+    return shdmemGetDevices(in_deviceRequirements, out_foundDevices, sizeFoundDevices, out_amountOfFoundDevices);
+}
+#endif
+
 // ------------------------------------
 // Helpers implementation. End.
 // ------------------------------------

src/pc/PlatformDeviceSearchDynamic.cpp

@@ -9,6 +9,7 @@
 #include "usb_host.h"
 #include "pcie_host.h"
 #include "tcpip_host.h"
+#include "local_memshd.h"
 #include "XLinkStringUtils.h"
 #include <thread>
 #include <chrono>
@@ -39,6 +40,7 @@ xLinkPlatformErrorCode_t XLinkPlatformFindDevicesDynamic(const deviceDesc_t in_d
     void* tcpip_ctx;
     bool usb_vsc_available = false;
     bool tcpip_available = false;
+    bool shdmem_available = false;
 
     if(XLinkIsProtocolInitialized(X_LINK_USB_VSC)) {
         usb_vsc_available = true;
@@ -47,6 +49,10 @@ xLinkPlatformErrorCode_t XLinkPlatformFindDevicesDynamic(const deviceDesc_t in_d
         tcpip_available = true;
     }
 
+    if(XLinkIsProtocolInitialized(X_LINK_LOCAL_SHDMEM)) {
+	shdmem_available = true;
+    }
+
     xLinkPlatformErrorCode_t status = X_LINK_PLATFORM_TIMEOUT;
     do {
 
@@ -89,8 +95,6 @@ xLinkPlatformErrorCode_t XLinkPlatformFindDevicesDynamic(const deviceDesc_t in_d
                     }
                 }
 
-
-
                 /* TODO(themarpe) - reenable PCIe
                 if(XLinkIsProtocolInitialized(X_LINK_PCIE)) {
                     numFoundDevices = 0;
@@ -106,6 +110,7 @@ xLinkPlatformErrorCode_t XLinkPlatformFindDevicesDynamic(const deviceDesc_t in_d
                 }
                 */
 
+	    case X_LINK_TCP_IP_OR_LOCAL_SHDMEM:
                 // Try find TCPIP device
                 if(tcpip_available) {
                     numFoundDevices = 0;

src/pc/protocols/local_memshd.cpp

@@ -215,4 +215,30 @@ int shdmemSetProtocol(XLinkProtocol_t *protocol, const char* devPathRead, const
     return X_LINK_SUCCESS;
 }
 
+
+xLinkPlatformErrorCode_t shdmemGetDevices(const deviceDesc_t in_deviceRequirements, deviceDesc_t* out_foundDevices, int sizeFoundDevices, unsigned int *out_amountOfFoundDevices) {
+    if (access(SHDMEM_DEFAULT_SOCKET, F_OK) != 0) {
+	return X_LINK_PLATFORM_ERROR;
+    }
+
+    // Status
+    out_foundDevices[0].status = X_LINK_SUCCESS;
+    // IP
+    memset(out_foundDevices[0].name, 0, sizeof(out_foundDevices[0].name));
+    strncpy(out_foundDevices[0].name, SHDMEM_DEFAULT_SOCKET, sizeof(out_foundDevices[0].name));
+    // MXID
+    memset(out_foundDevices[0].mxid, 0, sizeof(out_foundDevices[0].mxid));
+    strncpy(out_foundDevices[0].mxid, in_deviceRequirements.mxid, sizeof(out_foundDevices[0].mxid));
+    // Platform
+    out_foundDevices[0].platform = X_LINK_MYRIAD_X;
+    // Protocol
+    out_foundDevices[0].protocol = X_LINK_LOCAL_SHDMEM;
+    // State
+    out_foundDevices[0].state = X_LINK_BOOTED;
+
+    *out_amountOfFoundDevices = 1;
+
+    return X_LINK_PLATFORM_SUCCESS;
+}
+
 #endif /* !defined(__unix__) */

src/pc/protocols/local_memshd.h

@@ -34,6 +34,8 @@ int shdmemPlatformWrite(void *desc, void *data, int size);
 int shdmemPlatformWriteFd(void *desc, const long fd, void *data2, int size2);
 
 int shdmemSetProtocol(XLinkProtocol_t *protocol, const char* devPathRead, const char* devPathWrite);
+
+xLinkPlatformErrorCode_t shdmemGetDevices(const deviceDesc_t in_deviceRequirements, deviceDesc_t* out_foundDevices, int sizeFoundDevices, unsigned int *out_amountOfFoundDevices);
 
 #ifdef __cplusplus
 }

src/pc/protocols/tcpip_memshd.cpp

@@ -2,6 +2,9 @@
 #include "local_memshd.h"
 #include "tcpip_memshd.h"
 
+#define MVLOG_UNIT_NAME tcpip_memshd
+#include "XLinkLog.h"
+
 #include <signal.h>
 
 #include <atomic>
@@ -24,6 +27,21 @@ int tcpipOrLocalShdmemPlatformServer(XLinkProtocol_t *protocol, const char *devP
     void *fdTcpIp = nullptr, *fdShdmem = nullptr;
     long tcpIpSockFd = -1, shdmemSockFd = -1;
 
+    auto threadShdmem = std::thread([&connectionMutex,
+		                     &cv,
+				     &isShdmemThreadFinished,
+				     &retShdmem,
+				     &fdShdmem,
+				     &shdmemSockFd](){
+        auto ret = shdmemPlatformServer(SHDMEM_DEFAULT_SOCKET, SHDMEM_DEFAULT_SOCKET, &fdShdmem, &shdmemSockFd);
+        {
+            std::unique_lock<std::mutex> l(connectionMutex);
+            retShdmem = ret;
+            isShdmemThreadFinished = true;
+        }
+        cv.notify_one();
+    });
+
     auto threadTcpip = std::thread([&connectionMutex,
 		                    &cv,
 				    &isTcpIpThreadFinished,
@@ -39,21 +57,6 @@ int tcpipOrLocalShdmemPlatformServer(XLinkProtocol_t *protocol, const char *devP
             isTcpIpThreadFinished = true;
         }
         cv.notify_one();
-    });  
-
-    auto threadShdmem = std::thread([&connectionMutex,
-		                     &cv,
-				     &isShdmemThreadFinished,
-				     &retShdmem,
-				     &fdShdmem,
-				     &shdmemSockFd](){
-        auto ret = shdmemPlatformServer(SHDMEM_DEFAULT_SOCKET, SHDMEM_DEFAULT_SOCKET, &fdShdmem, &shdmemSockFd);
-        {
-            std::unique_lock<std::mutex> l(connectionMutex);
-            retShdmem = ret;
-            isShdmemThreadFinished = true;
-        }
-        cv.notify_one();
     });
 
     {
@@ -64,27 +67,30 @@ int tcpipOrLocalShdmemPlatformServer(XLinkProtocol_t *protocol, const char *devP
 
     // As soon as either one finishes, the other can be cleaned
     // Use signals, as "accept" cannot be unblocked by "close"ing the underlying socket
-    if(!isShdmemThreadFinished) {
-	if(shdmemSockFd >= 0) {
-	    shutdown(shdmemSockFd, SHUT_RDWR);
+    if(!isTcpIpThreadFinished) {
+	if(tcpIpSockFd >= 0) {
+	    shutdown(tcpIpSockFd, SHUT_RDWR);
             #if defined(SO_LINGER)
         	const int set = 0;
-		setsockopt(shdmemSockFd, SOL_SOCKET, SO_LINGER, (const char*)&set, sizeof(set));
+		setsockopt(tcpIpSockFd, SOL_SOCKET, SO_LINGER, (const char*)&set, sizeof(set));
             #endif
-	    close(shdmemSockFd);
+	    close(tcpIpSockFd);
 	}
-
+
+	mvLog(MVLOG_ERROR, "Failed to start server with TCP/IP");
     }
 
-    if(!isTcpIpThreadFinished) {
-	if(tcpIpSockFd >= 0) {
-	    shutdown(tcpIpSockFd, SHUT_RDWR);
+    if(!isShdmemThreadFinished) {
+	if(shdmemSockFd >= 0) {
+	    shutdown(shdmemSockFd, SHUT_RDWR);
             #if defined(SO_LINGER)
         	const int set = 0;
-		setsockopt(tcpIpSockFd, SOL_SOCKET, SO_LINGER, (const char*)&set, sizeof(set));
+		setsockopt(shdmemSockFd, SOL_SOCKET, SO_LINGER, (const char*)&set, sizeof(set));
             #endif
-	    close(tcpIpSockFd);
+	    close(shdmemSockFd);
 	}
+
+	mvLog(MVLOG_ERROR, "Failed to start server with SHDMEM");
     }
 
     // Wait for both threads to wrap up
@@ -112,11 +118,17 @@ int tcpipOrLocalShdmemPlatformServer(XLinkProtocol_t *protocol, const char *devP
 
 int tcpipOrLocalShdmemPlatformConnect(XLinkProtocol_t *protocol, const char *devPathRead, const char *devPathWrite, void **fd) {
     if(shdmemPlatformConnect(SHDMEM_DEFAULT_SOCKET, SHDMEM_DEFAULT_SOCKET, fd) == X_LINK_SUCCESS) {
+	mvLog(MVLOG_ERROR, "Failed to connect with SHDMEM");
 	return shdmemSetProtocol(protocol, devPathRead, devPathWrite);
     }
-
-    *protocol = X_LINK_TCP_IP;
-    return tcpipPlatformConnect(devPathRead, devPathWrite, fd);
+
+    if (tcpipPlatformConnect(devPathRead, devPathWrite, fd) == X_LINK_SUCCESS) {
+	mvLog(MVLOG_ERROR, "Failed to connect with TCP/IP");
+	*protocol = X_LINK_TCP_IP;
+	return X_LINK_SUCCESS;
+    }
+
+    return X_LINK_ERROR;
 }
 
 #else 

src/shared/XLinkData.c

@@ -170,7 +170,7 @@ XLinkError_t XLinkWriteFd_(streamId_t streamId, const long fd, XLinkTimespec* ou
     return X_LINK_SUCCESS;
 }
 
-XLinkError_t XLinkWriteFdData(streamId_t streamId, const long fd, int fdSize, const uint8_t* dataBuffer, int dataSize)
+XLinkError_t XLinkWriteFdData(streamId_t streamId, const long fd, const uint8_t* dataBuffer, int dataSize)
 {
     ASSERT_XLINK(dataBuffer);
 

src/shared/XLinkDevice.c

@@ -650,6 +650,7 @@ XLinkError_t parsePlatformError(xLinkPlatformErrorCode_t rc) {
 	    return X_LINK_INIT_LOCAL_SHDMEM_ERROR;
         case X_LINK_PLATFORM_PCIE_DRIVER_NOT_LOADED:
             return X_LINK_INIT_PCIE_ERROR;
+	case X_LINK_PLATFORM_TCP_IP_OR_LOCAL_SHDMEM_DRIVER_NOT_LOADED:
         case X_LINK_PLATFORM_ERROR:
         case X_LINK_PLATFORM_INVALID_PARAMETERS:
         default:
@@ -678,6 +679,7 @@ const char* XLinkErrorToStr(XLinkError_t val) {
         case X_LINK_INIT_USB_ERROR: return "X_LINK_INIT_USB_ERROR";
         case X_LINK_INIT_TCP_IP_ERROR: return "X_LINK_INIT_TCP_IP_ERROR";
         case X_LINK_INIT_LOCAL_SHDMEM_ERROR: return "X_LINK_INIT_LOCAL_SHDMEM_ERROR";
+        case X_LINK_INIT_TCP_IP_OR_LOCAL_SHDMEM_ERROR: return "X_LINK_INIT_TCP_IP_OR_LOCAL_SHDMEM_ERROR";
         case X_LINK_INIT_PCIE_ERROR: return "X_LINK_INIT_PCIE_ERROR";
         default:
             return "INVALID_ENUM_VALUE";