diff --git a/simulation/halsim_ds_socket/src/dev/native/cpp/main.cpp b/simulation/halsim_ds_socket/src/dev/native/cpp/main.cpp new file mode 100644 index 0000000000..83c4568502 --- /dev/null +++ b/simulation/halsim_ds_socket/src/dev/native/cpp/main.cpp @@ -0,0 +1,18 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include + +#include +#include + +extern "C" int HALSIM_InitExtension(void); + +int main() { + HAL_Initialize(500, 0); + HALSIM_InitExtension(); +} diff --git a/simulation/halsim_ds_socket/src/main/native/cpp/DSCommPacket.cpp b/simulation/halsim_ds_socket/src/main/native/cpp/DSCommPacket.cpp index 0ea245f065..f90bfeb054 100644 --- a/simulation/halsim_ds_socket/src/main/native/cpp/DSCommPacket.cpp +++ b/simulation/halsim_ds_socket/src/main/native/cpp/DSCommPacket.cpp @@ -17,6 +17,8 @@ #include #include +using namespace halsim; + /*---------------------------------------------------------------------------- ** The following methods help parse and hold information about the ** driver station and it's joysticks. diff --git a/simulation/halsim_ds_socket/src/main/native/cpp/main.cpp b/simulation/halsim_ds_socket/src/main/native/cpp/main.cpp index e5115383cd..1503055f16 100644 --- a/simulation/halsim_ds_socket/src/main/native/cpp/main.cpp +++ b/simulation/halsim_ds_socket/src/main/native/cpp/main.cpp @@ -19,146 +19,87 @@ #include #include -#if defined(Win32) || defined(_WIN32) -#include -#pragma comment(lib, "Ws2_32.lib") -#else -#include -#include -#include -#endif - #include +#include +#include +#include +#include +#include +#include +#include -/*---------------------------------------------------------------------------- -** Open a socket and listen for connections -** Returns socket handle on success, -1 on error -**--------------------------------------------------------------------------*/ -static int OpenListenSocket(int port, bool tcp) { -#if defined(WIN32) || defined(_WIN32) - SOCKET s; -#else - int s; +#if defined(Win32) || defined(_WIN32) +#pragma comment(lib, "Ws2_32.lib") #endif - if (tcp) - s = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP); - else - s = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP); +using namespace wpi::uv; - if (!s) { - std::perror("socket"); - return -1; - } +static std::unique_ptr singleByte; - int reuse = 1; -#if defined(WIN32) || defined(_WIN32) - setsockopt(s, SOL_SOCKET, SO_REUSEADDR, reinterpret_cast(&reuse), - sizeof(reuse)); -#else - setsockopt(s, SOL_SOCKET, SO_REUSEADDR, static_cast(&reuse), - sizeof(reuse)); -#endif +namespace { +struct DataStore { + wpi::SmallVector m_frame; + size_t m_frameSize = std::numeric_limits::max(); + halsim::DSCommPacket* dsPacket; +}; +} // namespace - struct sockaddr_in addr; - std::memset(&addr, 0, sizeof(addr)); - addr.sin_family = AF_INET; - addr.sin_port = htons(port); - addr.sin_addr.s_addr = INADDR_ANY; - - if (bind(s, (struct sockaddr*)&addr, sizeof(addr))) { - std::perror("bind"); -#if defined(WIN32) || defined(_WIN32) - closesocket(s); -#else - close(s); -#endif - return -1; - } - - if (tcp) { - if (listen(s, 10)) { -#if defined(WIN32) || defined(_WIN32) - closesocket(s); -#else - close(s); -#endif - return -1; +static void HandleTcpDataStream(Buffer& buf, size_t size, DataStore& store) { + wpi::StringRef data{buf.base, size}; + while (!data.empty()) { + if (store.m_frameSize != std::numeric_limits::max()) { + if (store.m_frame.size() < 2u) { + size_t toCopy = std::min(2u - store.m_frame.size(), data.size()); + store.m_frame.append(data.bytes_begin(), data.bytes_begin() + toCopy); + data = data.drop_front(toCopy); + if (store.m_frame.size() < 2u) return; // need more data + } + store.m_frameSize = (static_cast(store.m_frame[0]) << 8) | + static_cast(store.m_frame[1]); + } + if (store.m_frameSize != 0) { + size_t need = store.m_frameSize - (store.m_frame.size() - 2); + size_t toCopy = std::min(need, data.size()); + store.m_frame.append(data.bytes_begin(), data.bytes_begin() + toCopy); + data = data.drop_front(toCopy); + need -= toCopy; + if (need == 0) { + auto ds = store.dsPacket; + ds->DecodeTCP(reinterpret_cast(store.m_frame.data()), + store.m_frame.size()); + ds->SendTCPToHALSim(); + store.m_frame.clear(); + store.m_frameSize = std::numeric_limits::max(); + } } } - - return s; } -/*---------------------------------------------------------------------------- -** TCP thread -** This thread only dies at program shut down; it opens a socket -** and listens for incoming connections, and then processes data -** sent on the socket. -**--------------------------------------------------------------------------*/ -static void TCPThreadFunc(DSCommPacket* ds) { - static const int kTCPPort = 1740; - while (true) { - int socket = OpenListenSocket(kTCPPort, true); - if (socket < 0) { -#if defined(WIN32) || defined(_WIN32) - Sleep(1000); -#else - sleep(1); -#endif - continue; - } +static void SetupTcp(wpi::uv::Loop& loop) { + auto tcp = Tcp::Create(loop); + auto tcpWaitTimer = Timer::Create(loop); - while (true) { - int client = accept(socket, NULL, 0); - if (client < 0) { -#if defined(WIN32) || defined(_WIN32) - closesocket(socket); -#else - close(socket); -#endif - break; - } + auto recStore = std::make_shared(); - uint8_t buf[8192]; - int len = 0; - do { - int rc = recv(client, reinterpret_cast(buf + len), - sizeof(buf) - len, 0); - if (rc <= 0) break; - len += rc; + tcp->SetData(recStore); - do { - int deduct = ds->DecodeTCP(buf, len); - if (deduct <= 0) break; - std::memmove(buf, buf + deduct, len - deduct); - len -= deduct; - if (deduct > 2) ds->SendTCPToHALSim(); - } while (true); - } while (true); + tcp->Bind("0.0.0.0", 1740); -#if defined(WIN32) || defined(_WIN32) - closesocket(client); - Sleep(1000); -#else - close(client); - sleep(1); -#endif - } - } + tcp->Listen([t = tcp.get()] { + auto client = t->Accept(); + t->data.connect([t](Buffer& buf, size_t len) { + HandleTcpDataStream(buf, len, *t->GetData()); + }); + }); } /*---------------------------------------------------------------------------- ** Send a reply packet back to the DS **--------------------------------------------------------------------------*/ -static void SendReplyPacket(int socket, struct sockaddr* addr, int addrlen, - int reply_port, DSCommPacket* ds) { +static void SetupReplyPacket(halsim::DSCommPacket* ds) { static const uint8_t kTagGeneral = 0x01; - struct sockaddr_in* in4 = reinterpret_cast(addr); - in4->sin_port = htons(reply_port); - uint8_t data[8]; - std::memset(data, 0, sizeof(data)); + uint8_t* data = reinterpret_cast(ds->GetSendBuffer().base); ds->GetIndex(data[0], data[1]); @@ -169,58 +110,61 @@ static void SendReplyPacket(int socket, struct sockaddr* addr, int addrlen, data[5] = 12; // Voltage upper data[6] = 0; // Voltage lower data[7] = 0; // Request - -#if defined(WIN32) || defined(_WIN32) - sendto(socket, reinterpret_cast(data), sizeof(data), 0, addr, - addrlen); -#else - sendto(socket, data, sizeof(data), 0, addr, addrlen); -#endif } -/*---------------------------------------------------------------------------- -** UDP thread -** This thread only dies at program shut down; it opens a socket -** and listens for incoming connections, and then processes data -** sent on the socket. -**--------------------------------------------------------------------------*/ -static void UDPThreadFunc(DSCommPacket* ds) { - static const int kUDPListenPort = 1110; - static const int kUDPReplyPort = 1150; - while (true) { - int socket = OpenListenSocket(kUDPListenPort, false); - if (socket < 0) { -#if defined(WIN32) || defined(_WIN32) - Sleep(1000); -#else - sleep(1); -#endif - continue; - } +static void SetupUdp(wpi::uv::Loop& loop) { + auto udp = wpi::uv::Udp::Create(loop); + udp->Bind("0.0.0.0", 1110); - do { - uint8_t buf[1024]; - struct sockaddr addr; -#if defined(Win32) || defined(_WIN32) - int addrlen = sizeof(addr); - int rc = recvfrom(socket, reinterpret_cast(buf), sizeof(buf), 0, - &addr, &addrlen); -#else - socklen_t addrlen = sizeof(addr); - ssize_t rc = recvfrom(socket, buf, sizeof(buf), 0, &addr, &addrlen); -#endif + // Simulation mode packet + auto simLoopTimer = Timer::Create(loop); + struct sockaddr_in simAddr; + NameToAddr("127.0.0.1", 1135, &simAddr); + simLoopTimer->timeout.connect([ udpLocal = udp.get(), simAddr ] { + udpLocal->Send(simAddr, wpi::ArrayRef{singleByte.get(), 1}, + [](auto buf, Error err) { + if (err) { + wpi::errs() << err.str() << "\n"; + wpi::errs().flush(); + } + }); + }); + simLoopTimer->Start(Timer::Time{100}, Timer::Time{100}); - if (rc > 0) { - ds->DecodeUDP(buf, rc); - SendReplyPacket(socket, &addr, addrlen, kUDPReplyPort, ds); - ds->SendUDPToHALSim(); - } else { - break; - } - } while (true); - } + // UDP Receive then send + udp->received.connect([udpLocal = udp.get()]( + Buffer & buf, size_t len, const sockaddr& recSock, unsigned int port) { + auto ds = udpLocal->GetLoop()->GetData(); + ds->DecodeUDP(reinterpret_cast(buf.base), len); + SetupReplyPacket(ds.get()); + + struct sockaddr_in outAddr; + std::memcpy(&outAddr, &recSock, sizeof(sockaddr_in)); + outAddr.sin_family = PF_INET; + outAddr.sin_port = htons(1150); + + udpLocal->Send(outAddr, wpi::ArrayRef{&ds->GetSendBuffer(), 1}, + [](auto buf, Error err) { + if (err) { + wpi::errs() << err.str() << "\n"; + wpi::errs().flush(); + } + }); + ds->SendUDPToHALSim(); + }); + + udp->StartRecv(); } +static void SetupEventLoop(wpi::uv::Loop& loop) { + auto loopData = std::make_shared(); + loop.SetData(loopData); + SetupUdp(loop); + SetupTcp(loop); +} + +static std::unique_ptr eventLoopRunner; + /*---------------------------------------------------------------------------- ** Main entry point. We will start listen threads going, processing ** against our driver station packet @@ -230,9 +174,6 @@ extern "C" { __declspec(dllexport) #endif int HALSIM_InitExtension(void) { - static DSCommPacket ds; - static std::thread udp_thread; - static std::thread tcp_thread; static bool once = false; if (once) { @@ -244,11 +185,11 @@ __declspec(dllexport) std::cout << "DriverStationSocket Initializing." << std::endl; - udp_thread = std::thread(UDPThreadFunc, &ds); - udp_thread.detach(); + singleByte = std::make_unique("0"); - tcp_thread = std::thread(TCPThreadFunc, &ds); - tcp_thread.detach(); + eventLoopRunner = std::make_unique(); + + eventLoopRunner->ExecAsync(SetupEventLoop); std::cout << "DriverStationSocket Initialized!" << std::endl; return 0; diff --git a/simulation/halsim_ds_socket/src/main/native/include/DSCommJoystickPacket.h b/simulation/halsim_ds_socket/src/main/native/include/DSCommJoystickPacket.h index fd8aafd42f..410c807d14 100644 --- a/simulation/halsim_ds_socket/src/main/native/include/DSCommJoystickPacket.h +++ b/simulation/halsim_ds_socket/src/main/native/include/DSCommJoystickPacket.h @@ -8,9 +8,13 @@ #pragma once #include +namespace halsim { + typedef struct { std::vector axes; uint8_t button_count; uint32_t buttons; std::vector povs; } DSCommJoystickPacket; + +} // namespace halsim diff --git a/simulation/halsim_ds_socket/src/main/native/include/DSCommPacket.h b/simulation/halsim_ds_socket/src/main/native/include/DSCommPacket.h index 0992a1dd8a..a61a4d83ad 100644 --- a/simulation/halsim_ds_socket/src/main/native/include/DSCommPacket.h +++ b/simulation/halsim_ds_socket/src/main/native/include/DSCommPacket.h @@ -16,10 +16,16 @@ #include #include #include +#include + +namespace halsim { class DSCommPacket { public: - DSCommPacket(void) { std::fill_n(m_joystick_types, kMaxJoysticks, -1); } + DSCommPacket(void) { + std::fill_n(m_joystick_types, kMaxJoysticks, -1); + sendDataBuffer = wpi::uv::Buffer::Allocate(8); + } void Lock() { m_mutex.lock(); } void Unlock() { m_mutex.unlock(); } void SetIndex(uint8_t hi, uint8_t lo); @@ -36,6 +42,7 @@ class DSCommPacket { void SendTCPToHALSim(void); void SendUDPToHALSim(void); void SendJoysticks(void); + wpi::uv::Buffer& GetSendBuffer(void) { return sendDataBuffer; } /* TCP (FMS) types */ static const uint8_t kGameDataType = 0x0e; @@ -76,4 +83,7 @@ class DSCommPacket { int m_udp_packets; std::chrono::high_resolution_clock::time_point m_packet_time; double m_match_time; + wpi::uv::Buffer sendDataBuffer; }; + +} // namespace halsim diff --git a/simulation/halsim_ds_socket/src/main/native/include/FRCComm.h b/simulation/halsim_ds_socket/src/main/native/include/FRCComm.h index 83737e090b..538aa3edd3 100644 --- a/simulation/halsim_ds_socket/src/main/native/include/FRCComm.h +++ b/simulation/halsim_ds_socket/src/main/native/include/FRCComm.h @@ -15,13 +15,7 @@ #ifndef WPILIB_SIMULATION_HALSIM_DS_SOCKET_SRC_MAIN_NATIVE_INCLUDE_FRCCOMM_H_ #define WPILIB_SIMULATION_HALSIM_DS_SOCKET_SRC_MAIN_NATIVE_INCLUDE_FRCCOMM_H_ -#ifdef _WIN32 -#include -#elif defined(__vxworks) -#include -#elif defined(__linux) #include -#endif #define ERR_FRCSystem_NetCommNotResponding -44049 #define ERR_FRCSystem_NoDSConnection -44018