1187 lines
38 KiB
C
1187 lines
38 KiB
C
/**
|
|
* @file GoDiscovery.c
|
|
*
|
|
* @internal
|
|
* Copyright (C) 2016-2021 by LMI Technologies Inc.
|
|
* Licensed under the MIT License.
|
|
* Redistributed files must retain the above copyright notice.
|
|
*/
|
|
#include <GoSdk/Internal/GoDiscovery.h>
|
|
#include <kApi/Utils/kUtils.h>
|
|
|
|
kBeginValueEx(Go, GoDiscoveryInfo)
|
|
kEndValueEx()
|
|
|
|
kBeginValueEx(Go, GoDiscoveryInterface)
|
|
kAddField(GoDiscoveryInterface, kIpAddress, address)
|
|
kAddField(GoDiscoveryInterface, kUdpClient, client)
|
|
kAddField(GoDiscoveryInterface, kSerializer, writer)
|
|
kAddField(GoDiscoveryInterface, kBool, enabled)
|
|
kEndValueEx()
|
|
|
|
/*
|
|
* GoDiscovery class
|
|
*/
|
|
|
|
kBeginClassEx(Go, GoDiscovery)
|
|
kAddVMethod(GoDiscovery, kObject, VRelease)
|
|
kEndClassEx()
|
|
|
|
// For debugging discovery protocol.
|
|
#ifdef DEBUG_GO_DISCOVERY
|
|
#include <stdio.h>
|
|
|
|
#define DEBUG_PRINT printf
|
|
#define DEBUG_PRINT_ENDPOINT PrintEndPoint
|
|
|
|
void PrintAddressPort(kIpAddress ipAddress, k32u port)
|
|
{
|
|
kText16 ipAddrStr;
|
|
|
|
kIpAddress_Format(ipAddress, ipAddrStr, kCountOf(ipAddrStr));
|
|
DEBUG_PRINT(" @@@ socket %s:%u\n", ipAddrStr, port);
|
|
}
|
|
|
|
void PrintEndPoint(kIpEndPoint* endPoint)
|
|
{
|
|
PrintAddressPort(endPoint->address, endPoint->port);
|
|
}
|
|
|
|
void PrintUdpClient(kUdpClient udpClient)
|
|
{
|
|
kSocket listenerSocket;
|
|
kIpEndPoint localEndPoint;
|
|
|
|
listenerSocket = kUdpClient_Socket(udpClient);
|
|
if (!kSuccess(kSocket_LocalEndPoint(listenerSocket, &localEndPoint)))
|
|
{
|
|
DEBUG_PRINT(" **** Error getting interface local endpoint for client %p\n", udpClient);
|
|
}
|
|
else
|
|
{
|
|
PrintEndPoint(&localEndPoint);
|
|
}
|
|
|
|
}
|
|
#else
|
|
#define DEBUG_PRINT
|
|
#define DEBUG_PRINT_ENDPOINT
|
|
#endif
|
|
|
|
// --------------- GoDiscoveryInfo Utility Functions --------------------
|
|
GoFx(kStatus) GoDiscoveryInfo_Init(GoDiscoveryInfo* info)
|
|
{
|
|
return (kMemSet(info, 0, sizeof(GoDiscoveryInfo)));
|
|
}
|
|
|
|
GoFx(k32u) GoDiscoveryInfo_Id(const GoDiscoveryInfo* info)
|
|
{
|
|
return info->id;
|
|
}
|
|
|
|
GoFx(GoAddressInfo) GoDiscoveryInfo_Address(const GoDiscoveryInfo* info)
|
|
{
|
|
return info->address;
|
|
}
|
|
|
|
GoFx(GoPortInfo) GoDiscoveryInfo_Ports(const GoDiscoveryInfo* info)
|
|
{
|
|
return info->ports;
|
|
}
|
|
|
|
GoFx(kVersion) GoDiscoveryInfo_Version(const GoDiscoveryInfo* info)
|
|
{
|
|
return info->version;
|
|
}
|
|
|
|
GoFx(GoDiscoveryOpMode) GoDiscoveryInfo_OpMode(const GoDiscoveryInfo* info)
|
|
{
|
|
return info->opMode;
|
|
}
|
|
|
|
GoFx(kIpAddress) GoDiscoveryInfo_AccelSensorIpAddress(const GoDiscoveryInfo* info)
|
|
{
|
|
return info->accelSensorIpAddress;
|
|
}
|
|
|
|
GoFx(kIpAddress) GoDiscoveryInfo_DiscoveryServerAddress(const GoDiscoveryInfo* info)
|
|
{
|
|
return info->discoveryServerAddress;
|
|
}
|
|
|
|
GoFx(kStatus) GoDiscoveryInfo_PartNumber(const GoDiscoveryInfo* info, kChar* partNumber, kSize capacity)
|
|
{
|
|
kCheck(kStrCopy(partNumber, capacity, info->partNumber));
|
|
|
|
return kOK;
|
|
}
|
|
|
|
GoFx(kStatus) GoDiscoveryInfo_ModelNumber(const GoDiscoveryInfo* info, kChar* modelNumber, kSize capacity)
|
|
{
|
|
kCheck(kStrCopy(modelNumber, capacity, info->modelNumber));
|
|
|
|
return kOK;
|
|
}
|
|
|
|
GoFx(kStatus) GoDiscoveryInfo_ModelDisplayName(const GoDiscoveryInfo* info, kChar* modelDisplayName, kSize capacity)
|
|
{
|
|
kCheck(kStrCopy(modelDisplayName, capacity, info->modelDisplayName));
|
|
|
|
return kOK;
|
|
}
|
|
|
|
GoFx(kStatus) GoDiscoveryInfo_Family(const GoDiscoveryInfo* info, kChar* family, kSize capacity)
|
|
{
|
|
kCheck(kStrCopy(family, capacity, info->family));
|
|
|
|
return kOK;
|
|
}
|
|
|
|
// ----------- End GoDiscoveryInfo Getter Functions -------------
|
|
|
|
GoFx(kStatus) GoDiscovery_Construct(GoDiscovery* discovery, kBool enableAutoDiscovery, kAlloc allocator)
|
|
{
|
|
kAlloc alloc = kAlloc_Fallback(allocator);
|
|
kStatus status;
|
|
|
|
kCheck(kAlloc_GetObject(alloc, kTypeOf(GoDiscovery), discovery));
|
|
|
|
if (!kSuccess(status = GoDiscovery_Init(*discovery, kTypeOf(GoDiscovery), enableAutoDiscovery, alloc)))
|
|
{
|
|
kAlloc_FreeRef(alloc, discovery);
|
|
}
|
|
|
|
return status;
|
|
}
|
|
|
|
GoFx(kStatus) GoDiscovery_Init(GoDiscovery discovery, kType type, kBool enableAutoDiscovery, kAlloc alloc)
|
|
{
|
|
kObjR(GoDiscovery, discovery);
|
|
kArrayList addresses = kNULL;
|
|
kStatus status;
|
|
kIpEndPoint localEndPoint;
|
|
kIpEntry localIpEntry;
|
|
kSize i;
|
|
|
|
kCheck(kObject_Init(discovery, type, alloc));
|
|
kAtomic32s_Init(&obj->compatEnabled, 0);
|
|
obj->localPort = 0;
|
|
obj->enumPeriod = 0;
|
|
obj->onEnumerate.function = kNULL;
|
|
obj->onEnumerate.receiver = kNULL;
|
|
kZero(obj->infoList);
|
|
kZero(obj->interfaces);
|
|
kZero(obj->receiver);
|
|
kZero(obj->readerBuffer);
|
|
kZero(obj->reader);
|
|
kZero(obj->eventTimer);
|
|
kZero(obj->stopwatch);
|
|
obj->enumPending = kFALSE;
|
|
kZero(obj->enumLock);
|
|
kZero(obj->sensorInfoMap);
|
|
|
|
kTry
|
|
{
|
|
kAtomic32s_Exchange(&obj->compatEnabled, 0);
|
|
|
|
kTest(kMap_Construct(&obj->sensorInfoMap, kTypeOf(k32u), kTypeOf(GoDiscoveryInfo), 0, alloc));
|
|
kTest(kArrayList_Construct(&obj->infoList, kTypeOf(GoDiscoveryInfo), 0, alloc));
|
|
kTest(kArrayList_Construct(&obj->interfaces, kTypeOf(GoDiscoveryInterface), 0, alloc));
|
|
|
|
kTest(kUdpClient_Construct(&obj->receiver, kIP_VERSION_4, alloc));
|
|
kTest(kUdpClient_EnableReuseAddress(obj->receiver, kTRUE));
|
|
|
|
// Keep the QNX buffer size the same, in case the exact choice is important.
|
|
// If the increase in discovery traffic from compat mode is a problem, user
|
|
// can disable it with GoSystem_EnableDiscoveryCompatibility.
|
|
#if defined(K_QNX)
|
|
kTest(kUdpClient_SetReadBuffers(obj->receiver, 65 * 1024, 2048));
|
|
#else
|
|
kTest(kUdpClient_SetReadBuffers(obj->receiver, GO_DISCOVERY_RECV_BUFFER_SIZE, 2048));
|
|
#endif
|
|
|
|
kTest(kUdpClient_Bind(obj->receiver, kIpAddress_AnyV4(), kIP_PORT_ANY));
|
|
|
|
kTest(kUdpClient_LocalEndPoint(obj->receiver, &localEndPoint));
|
|
obj->localPort = localEndPoint.port;
|
|
|
|
kTest(kMemory_Construct(&obj->readerBuffer, alloc));
|
|
kTest(kSerializer_Construct(&obj->reader, obj->readerBuffer, kNULL, alloc));
|
|
|
|
kTest(kPeriodic_Construct(&obj->eventTimer, alloc));
|
|
kTest(kTimer_Construct(&obj->stopwatch, alloc));
|
|
|
|
kTest(kLock_Construct(&obj->enumLock, alloc));
|
|
|
|
kTest(kArrayList_Construct(&addresses, kTypeOf(kIpEntry), 0, alloc));
|
|
kTest(kNetwork_LocalIpInterfaces(addresses));
|
|
|
|
#if !defined(K_QNX) && !defined(K_POSIX)
|
|
// Add localhost interfaces except for QNX & Linux systems.
|
|
kTest(kStrCopy(localIpEntry.name, kCountOf(localIpEntry.name), "localhost"));
|
|
localIpEntry.address = kIpAddress_LoopbackV4();
|
|
kTest(kArrayList_AddT(addresses, &localIpEntry));
|
|
#endif
|
|
|
|
for (i = 0; i < kArrayList_Count(addresses); ++i)
|
|
{
|
|
GoDiscoveryInterface iface;
|
|
kIpEntry entry;
|
|
|
|
entry = kArrayList_AsT(addresses, i, kIpEntry);
|
|
iface.address = entry.address;
|
|
iface.client = kNULL;
|
|
iface.writer = kNULL;
|
|
iface.enabled = enableAutoDiscovery;
|
|
|
|
kTest(kUdpClient_Construct(&iface.client, kIP_VERSION_4, alloc));
|
|
kTest(kUdpClient_EnableBroadcast(iface.client, kTRUE));
|
|
kTest(kUdpClient_EnableReuseAddress(iface.client, kTRUE));
|
|
|
|
if (kSuccess(kUdpClient_Bind(iface.client, iface.address, obj->localPort)))
|
|
{
|
|
kTest(kUdpClient_SetWriteBuffers(iface.client, -1, 2048));
|
|
kTest(kSerializer_Construct(&iface.writer, iface.client, kNULL, alloc));
|
|
kTest(kArrayList_AddT(obj->interfaces, &iface));
|
|
|
|
#if defined(K_QNX)
|
|
kTest(kSocket_BindToDevice(kUdpClient_Socket(iface.client), entry.name));
|
|
#endif
|
|
}
|
|
else
|
|
{
|
|
kDestroyRef(&iface.client);
|
|
}
|
|
}
|
|
}
|
|
kCatchEx(&status)
|
|
{
|
|
GoDiscovery_VRelease(discovery);
|
|
|
|
kEndCatchEx(status);
|
|
}
|
|
kFinallyEx
|
|
{
|
|
kDisposeRef(&addresses);
|
|
|
|
kEndFinallyEx();
|
|
}
|
|
|
|
return kOK;
|
|
}
|
|
|
|
GoFx(kStatus) GoDiscovery_VRelease(GoDiscovery discovery)
|
|
{
|
|
kObj(GoDiscovery, discovery);
|
|
kSize i;
|
|
|
|
if (!kIsNull(obj->eventTimer))
|
|
{
|
|
kCheck(GoDiscovery_StopEnum(discovery));
|
|
}
|
|
|
|
kCheck(kDisposeRef(&obj->infoList));
|
|
kCheck(kDisposeRef(&obj->sensorInfoMap));
|
|
|
|
if (!kIsNull(obj->interfaces))
|
|
{
|
|
for (i = 0; i < kArrayList_Count(obj->interfaces); ++i)
|
|
{
|
|
GoDiscoveryInterface* iface = kArrayList_AtT(obj->interfaces, i, GoDiscoveryInterface);
|
|
|
|
kCheck(kDestroyRef(&iface->writer));
|
|
kCheck(kDestroyRef(&iface->client));
|
|
}
|
|
|
|
kCheck(kDisposeRef(&obj->interfaces));
|
|
}
|
|
|
|
kCheck(kDestroyRef(&obj->enumLock));
|
|
|
|
kCheck(kDestroyRef(&obj->reader));
|
|
kCheck(kDestroyRef(&obj->readerBuffer));
|
|
kCheck(kDestroyRef(&obj->receiver));
|
|
|
|
kCheck(kDestroyRef(&obj->eventTimer));
|
|
kCheck(kDestroyRef(&obj->stopwatch));
|
|
|
|
return kObject_VRelease(discovery);
|
|
}
|
|
|
|
GoFx(kStatus) GoDiscovery_WriteIpAddress(kSerializer serializer, kIpAddress address)
|
|
{
|
|
kByte bytes[8];
|
|
|
|
bytes[0] = 0;
|
|
bytes[1] = 0;
|
|
bytes[2] = 0;
|
|
bytes[3] = 0;
|
|
bytes[4] = (address.address[0] & 0xFF);
|
|
bytes[5] = (address.address[1] & 0xFF);
|
|
bytes[6] = (address.address[2] & 0xFF);
|
|
bytes[7] = (address.address[3] & 0xFF);
|
|
|
|
kCheck(kSerializer_WriteByteArray(serializer, bytes, kCountOf(bytes)));
|
|
|
|
return kOK;
|
|
}
|
|
|
|
GoFx(kStatus) GoDiscovery_ReadIpAddress(kSerializer serializer, kIpAddress* address)
|
|
{
|
|
kByte bytes[8];
|
|
kSize i;
|
|
|
|
kCheck(kSerializer_ReadByteArray(serializer, bytes, kCountOf(bytes)));
|
|
|
|
address->address[0] = bytes[4];
|
|
address->address[1] = bytes[5];
|
|
address->address[2] = bytes[6];
|
|
address->address[3] = bytes[7];
|
|
|
|
for (i = 4; i < 16; i++)
|
|
{
|
|
address->address[i] = 0;
|
|
}
|
|
|
|
address->version = kIP_VERSION_4;
|
|
|
|
return kOK;
|
|
}
|
|
|
|
GoFx(kStatus) GoDiscovery_Enumerate(GoDiscovery discovery, kArrayList infoList)
|
|
{
|
|
kObj(GoDiscovery, discovery);
|
|
|
|
kCheck(kLock_Enter(obj->enumLock));
|
|
|
|
kTry
|
|
{
|
|
kTest(GoDiscovery_BeginEnum(discovery));
|
|
kTest(kThread_Sleep(GO_DISCOVERY_GET_ADDRESS_TIMEOUT));
|
|
kTest(GoDiscovery_EndEnum(discovery, infoList));
|
|
}
|
|
kFinally
|
|
{
|
|
kLock_Exit(obj->enumLock);
|
|
|
|
kEndFinally();
|
|
}
|
|
|
|
return kOK;
|
|
}
|
|
|
|
GoFx(kStatus) GoDiscovery_GetAddress(GoDiscovery discovery, k32u deviceId, GoAddressInfo* address)
|
|
{
|
|
kObj(GoDiscovery, discovery);
|
|
GoDiscoveryInfo info;
|
|
kIpEndPoint remoteEndPoint;
|
|
kSize i;
|
|
kStatus sendStatus = kERROR_NETWORK;
|
|
kStatus recvStatus = kERROR_NOT_FOUND;
|
|
|
|
kCheck(kLock_Enter(obj->enumLock));
|
|
kTry
|
|
{
|
|
for (i = 0; i < kArrayList_Count(obj->interfaces); ++i)
|
|
{
|
|
GoDiscoveryInterface* iface = kArrayList_AtT(obj->interfaces, i, GoDiscoveryInterface);
|
|
|
|
if (iface->enabled)
|
|
{
|
|
kTest(kSerializer_Write64s(iface->writer, GO_DISCOVERY_GET_ADDRESS_SIZE));
|
|
kTest(kSerializer_Write64s(iface->writer, GO_DISCOVERY_GET_ADDRESS));
|
|
kTest(kSerializer_Write64s(iface->writer, GO_DISOVERY_SIGNATURE));
|
|
kTest(kSerializer_Write64s(iface->writer, deviceId));
|
|
|
|
kTest(kSerializer_Flush(iface->writer));
|
|
|
|
if (kSuccess(kUdpClient_Send(iface->client, kIpAddress_BroadcastV4(), GO_DISCOVERY_PORT, GO_DISCOVERY_GET_ADDRESS_TIMEOUT, kTRUE)))
|
|
{
|
|
sendStatus = kOK;
|
|
}
|
|
}
|
|
}
|
|
|
|
kTest(sendStatus);
|
|
kTest(kTimer_Start(obj->stopwatch, GO_DISCOVERY_GET_ADDRESS_TIMEOUT));
|
|
|
|
do
|
|
{
|
|
if (kSuccess(GoDiscovery_ReceiveReply(discovery, &remoteEndPoint, kTimer_Remaining(obj->stopwatch)))
|
|
&& remoteEndPoint.port == GO_DISCOVERY_PORT
|
|
&& kSuccess(GoDiscovery_ParseGetAddressReply(discovery, &info, &remoteEndPoint))
|
|
&& info.id == deviceId)
|
|
{
|
|
*address = info.address;
|
|
recvStatus = kOK;
|
|
|
|
DEBUG_PRINT("%s: sensor %u remote end point\n", __FUNCTION__, deviceId);
|
|
DEBUG_PRINT_ENDPOINT(&remoteEndPoint);
|
|
|
|
break;
|
|
}
|
|
} while (!kTimer_IsExpired(obj->stopwatch));
|
|
}
|
|
kFinally
|
|
{
|
|
kLock_Exit(obj->enumLock);
|
|
|
|
kEndFinally();
|
|
}
|
|
|
|
return recvStatus;
|
|
}
|
|
|
|
GoFx(kStatus) GoDiscovery_SetAddress(GoDiscovery discovery, k32u deviceId, const GoAddressInfo* address)
|
|
{
|
|
kObj(GoDiscovery, discovery);
|
|
k32u responseDeviceId;
|
|
kIpEndPoint remoteEndPoint;
|
|
kIpAddress zero;
|
|
kSize i;
|
|
kStatus sendStatus = kERROR_NETWORK;
|
|
|
|
for (i = 0; i < 16; i++)
|
|
{
|
|
zero.address[i] = 0;
|
|
}
|
|
|
|
for (i = 0; i < kArrayList_Count(obj->interfaces); ++i)
|
|
{
|
|
GoDiscoveryInterface* iface = kArrayList_AtT(obj->interfaces, i, GoDiscoveryInterface);
|
|
|
|
if (iface->enabled)
|
|
{
|
|
kCheck(kSerializer_Write64s(iface->writer, GO_DISCOVERY_SET_ADDRESS_SIZE));
|
|
kCheck(kSerializer_Write64s(iface->writer, GO_DISCOVERY_SET_ADDRESS));
|
|
kCheck(kSerializer_Write64s(iface->writer, GO_DISOVERY_SIGNATURE));
|
|
kCheck(kSerializer_Write64s(iface->writer, deviceId));
|
|
kCheck(kSerializer_Write64s(iface->writer, address->useDhcp));
|
|
|
|
kCheck(GoDiscovery_WriteIpAddress(iface->writer, (address->useDhcp) ? kIpAddress_AnyV4() : address->address));
|
|
kCheck(GoDiscovery_WriteIpAddress(iface->writer, (address->useDhcp) ? kIpAddress_AnyV4() : address->mask));
|
|
kCheck(GoDiscovery_WriteIpAddress(iface->writer, (address->useDhcp) ? kIpAddress_AnyV4() : address->gateway));
|
|
kCheck(GoDiscovery_WriteIpAddress(iface->writer, (address->useDhcp) ? kIpAddress_AnyV4() : zero));
|
|
|
|
kCheck(kSerializer_Flush(iface->writer));
|
|
if (kSuccess(kUdpClient_Send(iface->client, kIpAddress_BroadcastV4(), GO_DISCOVERY_PORT, GO_DISCOVERY_SET_ADDRESS_TIMEOUT, kTRUE)))
|
|
{
|
|
sendStatus = kOK;
|
|
}
|
|
}
|
|
}
|
|
|
|
kCheck(sendStatus);
|
|
kCheck(kTimer_Start(obj->stopwatch, GO_DISCOVERY_SET_ADDRESS_TIMEOUT));
|
|
|
|
do
|
|
{
|
|
if (kSuccess(GoDiscovery_ReceiveReply(discovery, &remoteEndPoint, kTimer_Remaining(obj->stopwatch)))
|
|
&& remoteEndPoint.port == GO_DISCOVERY_PORT
|
|
&& kSuccess(GoDiscovery_ParseSetAddressReply(discovery, &responseDeviceId))
|
|
&& responseDeviceId == deviceId)
|
|
{
|
|
return kOK;
|
|
}
|
|
} while (!kTimer_IsExpired(obj->stopwatch));
|
|
|
|
return kERROR_NOT_FOUND;
|
|
}
|
|
|
|
GoFx(kStatus) GoDiscovery_SetAddressTest(GoDiscovery discovery, k32u deviceId, const GoAddressInfo* address)
|
|
{
|
|
kObj(GoDiscovery, discovery);
|
|
k32u responseDeviceId;
|
|
kIpEndPoint remoteEndPoint;
|
|
kIpAddress zero;
|
|
kSize i;
|
|
|
|
for (i = 0; i < 16; i++)
|
|
{
|
|
zero.address[i] = 0;
|
|
}
|
|
|
|
for (i = 0; i < kArrayList_Count(obj->interfaces); ++i)
|
|
{
|
|
GoDiscoveryInterface* iface = kArrayList_AtT(obj->interfaces, i, GoDiscoveryInterface);
|
|
|
|
if (iface->enabled)
|
|
{
|
|
kCheck(kSocket_Connect(kUdpClient_Socket(iface->client), kIpAddress_BroadcastV4(), GO_DISCOVERY_PORT, 1000000));
|
|
|
|
kCheck(kSerializer_Write64s(iface->writer, GO_DISCOVERY_SET_ADDRESS_SIZE));
|
|
kCheck(kSerializer_Write64s(iface->writer, GO_DISCOVERY_SET_ADDRESS_TEST));
|
|
kCheck(kSerializer_Write64s(iface->writer, GO_DISOVERY_SIGNATURE));
|
|
kCheck(kSerializer_Write64s(iface->writer, deviceId));
|
|
kCheck(kSerializer_Write64s(iface->writer, address->useDhcp));
|
|
|
|
kCheck(GoDiscovery_WriteIpAddress(iface->writer, (address->useDhcp) ? kIpAddress_AnyV4() : address->address));
|
|
kCheck(GoDiscovery_WriteIpAddress(iface->writer, (address->useDhcp) ? kIpAddress_AnyV4() : address->mask));
|
|
kCheck(GoDiscovery_WriteIpAddress(iface->writer, (address->useDhcp) ? kIpAddress_AnyV4() : address->gateway));
|
|
kCheck(GoDiscovery_WriteIpAddress(iface->writer, (address->useDhcp) ? kIpAddress_AnyV4() : zero));
|
|
|
|
kCheck(kSerializer_Flush(iface->writer));
|
|
kCheck(kUdpClient_Send(iface->client, kIpAddress_BroadcastV4(), GO_DISCOVERY_PORT, GO_DISCOVERY_SET_ADDRESS_TIMEOUT, kTRUE));
|
|
}
|
|
}
|
|
|
|
kCheck(kTimer_Start(obj->stopwatch, GO_DISCOVERY_SET_ADDRESS_TIMEOUT));
|
|
|
|
do
|
|
{
|
|
if (kSuccess(GoDiscovery_ReceiveReply(discovery, &remoteEndPoint, kTimer_Remaining(obj->stopwatch)))
|
|
&& remoteEndPoint.port == GO_DISCOVERY_PORT
|
|
&& kSuccess(GoDiscovery_ParseSetTestReply(discovery, &responseDeviceId))
|
|
&& responseDeviceId == deviceId)
|
|
{
|
|
return kOK;
|
|
}
|
|
} while (!kTimer_IsExpired(obj->stopwatch));
|
|
|
|
return kERROR_NOT_FOUND;
|
|
}
|
|
|
|
GoFx(kStatus) GoDiscovery_GetExtendedInfo(GoDiscovery discovery, k32u deviceId, GoDiscoveryExtInfo* info, kAlloc allocator)
|
|
{
|
|
kObj(GoDiscovery, discovery);
|
|
kIpEndPoint remoteEndPoint;
|
|
kSize i;
|
|
kStatus sendStatus = kERROR_NETWORK;
|
|
kStatus recvStatus = kERROR_NOT_FOUND;
|
|
|
|
allocator = kAlloc_Fallback(allocator);
|
|
|
|
kCheck(kLock_Enter(obj->enumLock));
|
|
|
|
kTry
|
|
{
|
|
|
|
for (i = 0; i < kArrayList_Count(obj->interfaces); ++i)
|
|
{
|
|
GoDiscoveryInterface* iface = kArrayList_AtT(obj->interfaces, i, GoDiscoveryInterface);
|
|
|
|
if (iface->enabled)
|
|
{
|
|
kTest(kSerializer_Write32u(iface->writer, GO_DISCOVERY_GET_INFO_SIZE));
|
|
kTest(kSerializer_Write32u(iface->writer, 0));
|
|
kTest(kSerializer_Write32u(iface->writer, GO_DISCOVERY_GET_INFO));
|
|
kTest(kSerializer_Write32u(iface->writer, 0));
|
|
kTest(kSerializer_Write64u(iface->writer, GO_DISOVERY_SIGNATURE));
|
|
kTest(kSerializer_Write32u(iface->writer, deviceId));
|
|
kTest(kSerializer_Write32u(iface->writer, 0));
|
|
|
|
kTest(kSerializer_Flush(iface->writer));
|
|
|
|
if (kSuccess(kUdpClient_Send(iface->client, kIpAddress_BroadcastV4(), GO_DISCOVERY_PORT, GO_DISCOVERY_GET_INFO_TIMEOUT, kTRUE)))
|
|
{
|
|
sendStatus = kOK;
|
|
}
|
|
}
|
|
}
|
|
|
|
kTest(sendStatus);
|
|
kTest(kTimer_Start(obj->stopwatch, GO_DISCOVERY_GET_INFO_TIMEOUT));
|
|
|
|
do
|
|
{
|
|
if (kSuccess(GoDiscovery_ReceiveReply(discovery, &remoteEndPoint, kTimer_Remaining(obj->stopwatch)))
|
|
&& remoteEndPoint.port == GO_DISCOVERY_PORT
|
|
&& kSuccess(GoDiscovery_ParseGetInfoReply(discovery, info, allocator))
|
|
&& GoDiscoveryExtInfo_Id(*info) == deviceId)
|
|
{
|
|
DEBUG_PRINT("%s: sensor %u remote end point\n", __FUNCTION__, deviceId);
|
|
DEBUG_PRINT_ENDPOINT(&remoteEndPoint);
|
|
|
|
recvStatus = kOK;
|
|
break;
|
|
}
|
|
else
|
|
{
|
|
if (!kIsNull(*info))
|
|
{
|
|
kDisposeRef(info);
|
|
}
|
|
}
|
|
} while (!kTimer_IsExpired(obj->stopwatch));
|
|
|
|
}
|
|
kFinally
|
|
{
|
|
kLock_Exit(obj->enumLock);
|
|
|
|
kEndFinally();
|
|
}
|
|
|
|
return recvStatus;
|
|
}
|
|
|
|
GoFx(kStatus) GoDiscovery_ReceiveReply(GoDiscovery discovery, kIpEndPoint* endPoint, k64u timeout)
|
|
{
|
|
kObj(GoDiscovery, discovery);
|
|
kSize readSize;
|
|
|
|
// Copy into a kStream whose pointer can be reset and the content read again.
|
|
kCheck(kUdpClient_Receive(obj->receiver, endPoint, &readSize, timeout));
|
|
|
|
kCheck(kStream_Seek(obj->readerBuffer, 0, kSEEK_ORIGIN_BEGIN));
|
|
kCheck(kStream_Copy(obj->readerBuffer, obj->receiver, readSize));
|
|
|
|
kCheck(kStream_Seek(obj->readerBuffer, 0, kSEEK_ORIGIN_BEGIN));
|
|
|
|
return kOK;
|
|
}
|
|
|
|
GoFx(kStatus) GoDiscovery_ParseGetAddressReply(GoDiscovery discovery, GoDiscoveryInfo* info, kIpEndPoint* remoteEndPoint)
|
|
{
|
|
kObj(GoDiscovery, discovery);
|
|
k64s responseId, signature, status, deviceId, useDhcp;
|
|
kIpAddress tempIp;
|
|
|
|
kCheck(kMemSet(info, 0, sizeof(info)));
|
|
|
|
// Read and store the message attributes data length ("attrSize")
|
|
// to determine how many attributes to read.
|
|
// The size field is included in the message size count stored in the size field.
|
|
//
|
|
// If this fails, the EndRead() could assert so skip the EndRead() if BeginRead() fails.
|
|
kCheck(kSerializer_BeginRead(obj->reader, kTypeOf(k64u), kTRUE));
|
|
|
|
kTry
|
|
{
|
|
kTest(kSerializer_Read64s(obj->reader, &responseId));
|
|
// The 5.1 and newer SDK sends both a GET ADDRESS and GET INFO
|
|
// discovery request. A 5.1 and newer sensor firmware version
|
|
// supports both requests and therefore sends back both replies.
|
|
// In the caller to this function, a reply is assumed to be a GET ADDRESS
|
|
// reply and if it is not, then the reply is parsed as a GET
|
|
// INFO reply.
|
|
// So it is valid that the response id is not an GET ADDRESS REPLY so
|
|
// it is not a real error but an expected scenario.
|
|
kTest(responseId == GO_DISCOVERY_GET_ADDRESS_REPLY);
|
|
|
|
kTest(kSerializer_Read64s(obj->reader, &status));
|
|
kTest(status);
|
|
|
|
kTest(kSerializer_Read64s(obj->reader, &signature));
|
|
kTest(signature == GO_DISOVERY_SIGNATURE);
|
|
|
|
kTest(kSerializer_Read64s(obj->reader, &deviceId));
|
|
info->id = (k32u) deviceId;
|
|
|
|
kTest(kSerializer_Read64s(obj->reader, &useDhcp));
|
|
info->address.useDhcp = (kBool) useDhcp;
|
|
|
|
kTest(GoDiscovery_ReadIpAddress(obj->reader, &info->address.address));
|
|
kTest(GoDiscovery_ReadIpAddress(obj->reader, &info->address.mask));
|
|
kTest(GoDiscovery_ReadIpAddress(obj->reader, &info->address.gateway));
|
|
kTest(GoDiscovery_ReadIpAddress(obj->reader, &tempIp));
|
|
|
|
// The ports must be set to a non-zero value to indicate they do not
|
|
// have a valid value. All the remaining fields in "info" can remain as zeroes.
|
|
info->ports.controlPort = k16U_NULL;
|
|
info->ports.dataPort = k16U_NULL;
|
|
info->ports.healthPort = k16U_NULL;
|
|
info->ports.upgradePort = k16U_NULL;
|
|
info->ports.webPort = k16U_NULL;
|
|
|
|
info->discoveryServerAddress = remoteEndPoint->address;
|
|
}
|
|
kFinally
|
|
{
|
|
// Automatically flush any unread data and free serializer read section.
|
|
// Ignore errors.
|
|
kSerializer_EndRead(obj->reader);
|
|
|
|
kEndFinally();
|
|
}
|
|
|
|
return kOK;
|
|
}
|
|
|
|
GoFx(kStatus) GoDiscovery_ParseSetAddressReplyCommon(GoDiscovery discovery, k64s replyMsgId, k32u* deviceId)
|
|
{
|
|
kObj(GoDiscovery, discovery);
|
|
k64s responseDeviceId;
|
|
k64s responseSignature;
|
|
k64s responseId;
|
|
k64s responseStatus;
|
|
|
|
// Read and store the message attributes data length ("attrSize")
|
|
// to determine how many attributes to read.
|
|
// The size field is included in the message size count stored in the size field.
|
|
//
|
|
// If this fails, the EndRead() could assert so skip the EndRead() if BeginRead() fails.
|
|
kCheck(kSerializer_BeginRead(obj->reader, kTypeOf(k64u), kTRUE));
|
|
|
|
kTry
|
|
{
|
|
kTest(kSerializer_Read64s(obj->reader, &responseId));
|
|
kTest(responseId == replyMsgId);
|
|
|
|
kTest(kSerializer_Read64s(obj->reader, &responseStatus));
|
|
kTest(responseStatus);
|
|
|
|
kTest(kSerializer_Read64s(obj->reader, &responseSignature));
|
|
kTest(responseSignature == GO_DISOVERY_SIGNATURE);
|
|
|
|
kTest(kSerializer_Read64s(obj->reader, &responseDeviceId));
|
|
|
|
*deviceId = (k32u) responseDeviceId;
|
|
}
|
|
kFinally
|
|
{
|
|
// Automatically flush any unread data and free serializer read section.
|
|
// Ignore errors.
|
|
kSerializer_EndRead(obj->reader);
|
|
|
|
kEndFinally();
|
|
}
|
|
|
|
return kOK;
|
|
}
|
|
|
|
GoFx(kStatus) GoDiscovery_ParseSetAddressReply(GoDiscovery discovery, k32u* deviceId)
|
|
{
|
|
return(GoDiscovery_ParseSetAddressReplyCommon(discovery, GO_DISCOVERY_SET_ADDRESS_REPLY, deviceId));
|
|
}
|
|
|
|
GoFx(kStatus) GoDiscovery_ParseSetTestReply(GoDiscovery discovery, k32u* deviceId)
|
|
{
|
|
return(GoDiscovery_ParseSetAddressReplyCommon(discovery, GO_DISCOVERY_SET_ADDRESS_TEST_REPLY, deviceId));
|
|
}
|
|
|
|
GoFx(kStatus) GoDiscovery_ParseGetInfoReply(GoDiscovery discovery, GoDiscoveryExtInfo* info, kAlloc allocator)
|
|
{
|
|
kObj(GoDiscovery, discovery);
|
|
|
|
kCheck(GoDiscoveryExtInfo_Construct(info, allocator));
|
|
kCheck(GoDiscoveryExtInfo_Read(*info, obj->reader, allocator));
|
|
|
|
return kOK;
|
|
}
|
|
|
|
GoFx(kStatus) GoDiscovery_SetEnumPeriod(GoDiscovery discovery, k64u period)
|
|
{
|
|
kObj(GoDiscovery, discovery);
|
|
|
|
obj->enumPeriod = period;
|
|
|
|
return kOK;
|
|
}
|
|
|
|
GoFx(kStatus) GoDiscovery_SetEnumHandler(GoDiscovery discovery, GoDiscoveryEnumFx function, kPointer receiver)
|
|
{
|
|
kObj(GoDiscovery, discovery);
|
|
|
|
obj->onEnumerate.function = (kCallbackFx) function;
|
|
obj->onEnumerate.receiver = receiver;
|
|
|
|
return kOK;
|
|
}
|
|
|
|
GoFx(kStatus) GoDiscovery_StartEnum(GoDiscovery discovery, kBool waitFirst)
|
|
{
|
|
kObj(GoDiscovery, discovery);
|
|
|
|
kCheckState(!kIsNull(obj->onEnumerate.function));
|
|
kCheckState(!kPeriodic_Enabled(obj->eventTimer));
|
|
|
|
if (waitFirst)
|
|
{
|
|
kCheck(GoDiscovery_Enumerate(discovery, obj->infoList));
|
|
kCheck(obj->onEnumerate.function(obj->onEnumerate.receiver, discovery, obj->infoList));
|
|
}
|
|
|
|
kCheck(kPeriodic_Start(obj->eventTimer, 0, GoDiscovery_OnEnumElapsed, discovery));
|
|
|
|
return kOK;
|
|
}
|
|
|
|
GoFx(kStatus) GoDiscovery_StopEnum(GoDiscovery discovery)
|
|
{
|
|
kObj(GoDiscovery, discovery);
|
|
|
|
kCheck(kPeriodic_Stop(obj->eventTimer));
|
|
|
|
return kOK;
|
|
}
|
|
|
|
GoFx(kStatus) GoDiscovery_BeginEnum(GoDiscovery discovery)
|
|
{
|
|
kObj(GoDiscovery, discovery);
|
|
kSize i;
|
|
|
|
for (i = 0; i < kArrayList_Count(obj->interfaces); ++i)
|
|
{
|
|
GoDiscoveryInterface* iface = kArrayList_AtT(obj->interfaces, i, GoDiscoveryInterface);
|
|
|
|
if (iface->enabled)
|
|
{
|
|
if (kAtomic32s_Get(&obj->compatEnabled) == 1)
|
|
{
|
|
kCheck(GoDiscovery_SendGetAddress(discovery, iface));
|
|
}
|
|
|
|
kCheck(GoDiscovery_SendGetExtendedInfo(discovery, iface));
|
|
}
|
|
}
|
|
|
|
return kOK;
|
|
}
|
|
|
|
// This function must pull every receive message out of the socket and
|
|
// process them all, even if one or more of them fail to parse correctly.
|
|
// Only when there are no more messages in the socket then this function
|
|
// should return.
|
|
// Do not return prematurely because of parsing related errors (GOC-13024)
|
|
// except for socket errors that prevent reading replies from the socket.
|
|
GoFx(kStatus) GoDiscovery_EndEnum(GoDiscovery discovery, kArrayList list)
|
|
{
|
|
kObj(GoDiscovery, discovery);
|
|
kMapItem mapItem;
|
|
|
|
// Clear map in anticipation of processing enumeration replies.
|
|
kCheck(kMap_Clear(obj->sensorInfoMap));
|
|
|
|
kTry
|
|
{
|
|
while (kSuccess(kSocket_Wait(kUdpClient_Socket(obj->receiver), 0)))
|
|
{
|
|
kIpEndPoint remoteEndPoint;
|
|
|
|
// If can't receive a pending reply, then it is a serious enough
|
|
// error to not bother trying to empty the socket. So bail out on error.
|
|
kTest(GoDiscovery_ReceiveReply(discovery, &remoteEndPoint, 1000000));
|
|
|
|
if (remoteEndPoint.port == GO_DISCOVERY_PORT)
|
|
{
|
|
// Don't stop processing replies on an error. Instead, keep
|
|
// processing replies until the socket is empty.
|
|
GoDiscovery_ParseReceivedEnumReply(discovery, &remoteEndPoint);
|
|
}
|
|
}
|
|
}
|
|
kFinally
|
|
{
|
|
kEndFinally();
|
|
}
|
|
|
|
// Do this only if the enumeration reply parsed successfully.
|
|
kCheck(kArrayList_Clear(list));
|
|
|
|
for (mapItem = kMap_First(obj->sensorInfoMap);
|
|
mapItem != kNULL;
|
|
mapItem = kMap_Next(obj->sensorInfoMap, mapItem))
|
|
{
|
|
kCheck(kArrayList_Add(list, kMap_Value(obj->sensorInfoMap, mapItem)));
|
|
}
|
|
|
|
return kOK;
|
|
}
|
|
|
|
GoFx(kStatus) GoDiscovery_ProcessGetAddressReply(GoDiscovery discovery, const GoDiscoveryInfo* info)
|
|
{
|
|
kObj(GoDiscovery, discovery);
|
|
|
|
if (!kSuccess(kMap_Find(obj->sensorInfoMap, &info->id, kNULL)))
|
|
{
|
|
kCheck(kMap_Add(obj->sensorInfoMap, &info->id, info));
|
|
}
|
|
|
|
return kOK;
|
|
}
|
|
|
|
GoFx(kStatus) GoDiscovery_ProcessGetExtendedInfoReply(GoDiscovery discovery, GoDiscoveryExtInfo extInfo, kIpEndPoint* remoteEndPoint)
|
|
{
|
|
kObj(GoDiscovery, discovery);
|
|
GoDiscoveryInfo info;
|
|
|
|
kCheck(kMemSet(&info, 0, sizeof(info)));
|
|
|
|
// Copy most of the extended discovery information so that the sensor
|
|
// object can be populated with the information later in the enumeration
|
|
// process.
|
|
info.id = GoDiscoveryExtInfo_Id(extInfo);
|
|
info.address = GoDiscoveryExtInfo_Address(extInfo);
|
|
info.ports = GoDiscoveryExtInfo_Ports(extInfo);
|
|
info.version = GoDiscoveryExtInfo_Version(extInfo);
|
|
info.opMode = GoDiscoveryExtInfo_OpMode(extInfo);
|
|
info.accelSensorIpAddress = GoDiscoveryExtInfo_AccelSensorIpAddress(extInfo);
|
|
info.discoveryServerAddress = remoteEndPoint->address;
|
|
|
|
// Call to GoDiscoveryExtInfo_CompleteModel() is deprecated; we more clearly provide
|
|
// PartNumber, ModelNumber, and ModelDisplayName instead.
|
|
// NOTE: the below functions may not fill in the fields if they are misssing from
|
|
// the discovery protocol.
|
|
kCheck(GoDiscoveryExtInfo_PartNumber(extInfo, info.partNumber, kCountOf(info.partNumber)));
|
|
kCheck(GoDiscoveryExtInfo_ModelNumber(extInfo, info.modelNumber, kCountOf(info.modelNumber)));
|
|
kCheck(GoDiscoveryExtInfo_ModelDisplayName(extInfo, info.modelDisplayName, kCountOf(info.modelDisplayName)));
|
|
kCheck(GoDiscoveryExtInfo_Family(extInfo, info.family, kCountOf(info.family)));
|
|
|
|
kCheck(kMap_Replace(obj->sensorInfoMap, &info.id, &info));
|
|
|
|
return kOK;
|
|
}
|
|
|
|
GoFx(kStatus) GoDiscovery_SendGetAddress(GoDiscovery discovery, GoDiscoveryInterface* iface)
|
|
{
|
|
kCheck(kSerializer_Write64s(iface->writer, GO_DISCOVERY_GET_ADDRESS_SIZE));
|
|
kCheck(kSerializer_Write64s(iface->writer, GO_DISCOVERY_GET_ADDRESS));
|
|
kCheck(kSerializer_Write64s(iface->writer, GO_DISOVERY_SIGNATURE));
|
|
kCheck(kSerializer_Write64s(iface->writer, 0));
|
|
|
|
kCheck(kSerializer_Flush(iface->writer));
|
|
|
|
// Don't care about errors; some interfaces may not allow sending.
|
|
(void)kUdpClient_Send(iface->client, kIpAddress_BroadcastV4(), GO_DISCOVERY_PORT, GO_DISCOVERY_GET_ADDRESS_TIMEOUT, kTRUE);
|
|
|
|
return kOK;
|
|
}
|
|
|
|
GoFx(kStatus) GoDiscovery_SendGetExtendedInfo(GoDiscovery discovery, GoDiscoveryInterface* iface)
|
|
{
|
|
kCheck(kSerializer_Write64s(iface->writer, GO_DISCOVERY_GET_INFO_SIZE));
|
|
kCheck(kSerializer_Write64s(iface->writer, GO_DISCOVERY_GET_INFO));
|
|
kCheck(kSerializer_Write64s(iface->writer, GO_DISOVERY_SIGNATURE));
|
|
kCheck(kSerializer_Write64s(iface->writer, 0));
|
|
|
|
kCheck(kSerializer_Flush(iface->writer));
|
|
|
|
// Don't care about errors; some interfaces may not allow sending.
|
|
(void)kUdpClient_Send(iface->client, kIpAddress_BroadcastV4(), GO_DISCOVERY_PORT, GO_DISCOVERY_GET_INFO_TIMEOUT, kTRUE);
|
|
|
|
return kOK;
|
|
}
|
|
|
|
GoFx(kStatus) GoDiscovery_OnEnumElapsed(GoDiscovery discovery, kPeriodic timer)
|
|
{
|
|
kObj(GoDiscovery, discovery);
|
|
|
|
if (!obj->enumPending)
|
|
{
|
|
kStatus status = kOK;
|
|
|
|
kCheck(kLock_Enter(obj->enumLock));
|
|
|
|
kTry
|
|
{
|
|
kTest(kTimer_Start(obj->stopwatch, obj->enumPeriod));
|
|
kTest(GoDiscovery_BeginEnum(discovery));
|
|
obj->enumPending = kTRUE;
|
|
|
|
kTest(kPeriodic_Start(obj->eventTimer, GO_DISCOVERY_GET_ADDRESS_TIMEOUT, GoDiscovery_OnEnumElapsed, discovery));
|
|
}
|
|
kCatch(&status)
|
|
{
|
|
kLock_Exit(obj->enumLock);
|
|
|
|
kEndCatch(status);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
kCheck(GoDiscovery_OnEnumElapsedRecv(discovery, timer));
|
|
}
|
|
|
|
return kOK;
|
|
}
|
|
|
|
GoFx(kStatus) GoDiscovery_OnEnumElapsedRecv(GoDiscovery discovery, kPeriodic timer)
|
|
{
|
|
kObj(GoDiscovery, discovery);
|
|
kStatus exception;
|
|
|
|
kTry
|
|
{
|
|
kTest(GoDiscovery_EndEnum(discovery, obj->infoList));
|
|
kTest(obj->onEnumerate.function(obj->onEnumerate.receiver, discovery, obj->infoList));
|
|
}
|
|
kCatchEx(&exception)
|
|
{
|
|
DEBUG_PRINT("%s: error %d processing enumeration end", __FUNCTION__, exception);
|
|
|
|
// The periodic enumeration cycle must never stop because of errors so
|
|
// ignore the error.
|
|
kEndCatchEx(kOK);
|
|
}
|
|
kFinallyEx
|
|
{
|
|
// Must always clear the enumPending flag and schedule enumeration
|
|
// to start again on the next enumeration interval.
|
|
// Don't check for return errors as there is no recovery action possible.
|
|
obj->enumPending = kFALSE;
|
|
kPeriodic_Start(obj->eventTimer, kTimer_Remaining(obj->stopwatch), GoDiscovery_OnEnumElapsed, discovery);
|
|
|
|
kLock_Exit(obj->enumLock);
|
|
|
|
kEndFinallyEx();
|
|
}
|
|
|
|
return kOK;
|
|
}
|
|
|
|
GoFx(kStatus) GoDiscovery_SetOneInterface(GoDiscovery discovery, kIpAddress* address, kBool enable)
|
|
{
|
|
kObj(GoDiscovery, discovery);
|
|
kSize i;
|
|
|
|
for (i = 0; i < kArrayList_Count(obj->interfaces); ++i)
|
|
{
|
|
GoDiscoveryInterface* iface = kArrayList_AtT(obj->interfaces, i, GoDiscoveryInterface);
|
|
|
|
if (kIpAddress_Equals(*address, iface->address))
|
|
{
|
|
iface->enabled = enable;
|
|
}
|
|
}
|
|
|
|
return kOK;
|
|
}
|
|
|
|
GoFx(kStatus) GoDiscovery_SetAllInterface(GoDiscovery discovery, kBool enable)
|
|
{
|
|
kObj(GoDiscovery, discovery);
|
|
kSize i;
|
|
|
|
for (i = 0; i < kArrayList_Count(obj->interfaces); ++i)
|
|
{
|
|
GoDiscoveryInterface* iface = kArrayList_AtT(obj->interfaces, i, GoDiscoveryInterface);
|
|
|
|
iface->enabled = enable;
|
|
}
|
|
|
|
return kOK;
|
|
}
|
|
|
|
GoFx(kStatus) GoDiscovery_EnableCompatMode(GoDiscovery discovery, kBool enable)
|
|
{
|
|
kObj(GoDiscovery, discovery);
|
|
|
|
kAtomic32s_Exchange(&obj->compatEnabled, enable ? 1 : 0);
|
|
|
|
return kOK;
|
|
}
|
|
|
|
GoFx(kBool) GoDiscovery_CompatModeEnabled(GoDiscovery discovery)
|
|
{
|
|
kObj(GoDiscovery, discovery);
|
|
|
|
return kAtomic32s_Get(&obj->compatEnabled) == 1;
|
|
}
|
|
|
|
// This function reads the response id from the received reply in the
|
|
// serializer input.
|
|
GoFx(kStatus) GoDiscovery_GetReceivedResponseId(GoDiscovery discovery, k64s* responseId)
|
|
{
|
|
kObj(GoDiscovery, discovery);
|
|
kStatus exception;
|
|
k64s localResponseId;
|
|
|
|
// Read and store the message attributes data length ("attrSize")
|
|
// to determine how many attributes to read.
|
|
// The size field is included in the message size count stored in the size field.
|
|
//
|
|
// If this fails, the EndRead() could assert so skip the EndRead() if BeginRead() fails.
|
|
kCheck(kSerializer_BeginRead(obj->reader, kTypeOf(k64u), kTRUE));
|
|
|
|
kTry
|
|
{
|
|
|
|
kTest(kSerializer_Read64s(obj->reader, &localResponseId));
|
|
|
|
*responseId = localResponseId;
|
|
}
|
|
kCatchEx(&exception)
|
|
{
|
|
kLogf("%s: error %d getting response id", __FUNCTION__, exception);
|
|
|
|
kEndCatchEx(exception);
|
|
}
|
|
kFinallyEx
|
|
{
|
|
// Automatically flush any unread data and free serializer read section.
|
|
// Ignore errors.
|
|
kSerializer_EndRead(obj->reader);
|
|
|
|
// Reset the received message buffer to the beginning after
|
|
// getting the response id so that functions to parse the contents
|
|
// can be reused.
|
|
kStream_Seek(obj->readerBuffer, 0, kSEEK_ORIGIN_BEGIN);
|
|
|
|
kEndFinallyEx();
|
|
}
|
|
|
|
return kOK;
|
|
}
|
|
|
|
GoFx(void) GoDiscovery_ParseReceivedEnumReply(GoDiscovery discovery, kIpEndPoint* remoteEndPoint)
|
|
{
|
|
kObj(GoDiscovery, discovery);
|
|
k64s responseId;
|
|
GoDiscoveryExtInfo extInfo = kNULL;
|
|
GoDiscoveryInfo info;
|
|
kStatus status;
|
|
|
|
if (kSuccess(GoDiscovery_GetReceivedResponseId(discovery, &responseId)))
|
|
{
|
|
// When compatibility mode is on, the reply could either be a GetAddress reply, or
|
|
// a GetExtendedInfo reply.
|
|
if (responseId == GO_DISCOVERY_GET_ADDRESS_REPLY)
|
|
{
|
|
if (!kSuccess(GoDiscoveryInfo_Init(&info)))
|
|
{
|
|
// Skip this message and go on to the next message.
|
|
return;
|
|
}
|
|
|
|
if (kSuccess(GoDiscovery_ParseGetAddressReply(discovery, &info, remoteEndPoint)))
|
|
{
|
|
status = GoDiscovery_ProcessGetAddressReply(discovery, &info);
|
|
if (kSuccess(status))
|
|
{
|
|
DEBUG_PRINT("%s: sensor %u GetAddressReply remote end point\n", __FUNCTION__, info.id);
|
|
DEBUG_PRINT_ENDPOINT(remoteEndPoint);
|
|
}
|
|
else
|
|
{
|
|
// Log error but keep processing all the received messages.
|
|
DEBUG_PRINT("%s: error %d processing get address reply for sensor %u\n", __FUNCTION__, status, info.id);
|
|
}
|
|
}
|
|
}
|
|
else if (responseId == GO_DISCOVERY_GET_INFO_REPLY)
|
|
{
|
|
if (kSuccess(GoDiscovery_ParseGetInfoReply(discovery, &extInfo, kObject_Alloc(discovery))))
|
|
{
|
|
status = GoDiscovery_ProcessGetExtendedInfoReply(discovery, extInfo, remoteEndPoint);
|
|
if (kSuccess(status))
|
|
{
|
|
DEBUG_PRINT("%s: sensor %u ExtInfoReply remote end point\n", __FUNCTION__, GoDiscoveryExtInfo_Id(extInfo));
|
|
DEBUG_PRINT_ENDPOINT(remoteEndPoint);
|
|
}
|
|
else
|
|
{
|
|
// Log error but keep processing all the received messages.
|
|
DEBUG_PRINT("%s: error %d processing extended info reply for sensor %u\n", __FUNCTION__, status, GoDiscoveryExtInfo_Id(extInfo));
|
|
}
|
|
}
|
|
|
|
// Ignore error and continue on to the next reply message in the socket.
|
|
(void) kDisposeRef(&extInfo);
|
|
}
|
|
else
|
|
{
|
|
// Probably got a response for a request sent by a different client thread.
|
|
// The enumeration code should be lenient on what it receives because
|
|
// the network socket is shared with other discovery request APIs
|
|
// (probably not a good idea to begin with. Is there a backwards
|
|
// compatibility reason for not changing that since the APIs are all
|
|
// internal APIs).
|
|
// Enumeration runs periodically so if one run doesn't get what it
|
|
// is expecting, it can try again next time.
|
|
DEBUG_PRINT("%s: received unexpected response id %#x for enumeration\n", __FUNCTION__, responseId);
|
|
}
|
|
}
|
|
}
|
|
|