diff options
author | David Crocker <dcrocker@eschertech.com> | 2022-07-16 12:20:25 +0300 |
---|---|---|
committer | David Crocker <dcrocker@eschertech.com> | 2022-07-16 12:20:25 +0300 |
commit | 5aadf1d982b0cc9051debfa88c9e435644fe9439 (patch) | |
tree | c7f6db6e8430244a2a9351e45f06c02011fc0816 | |
parent | 35f8db40205f1743af81e6fc9c0960e778d02818 (diff) |
Implemented reboot from multicast message
-rw-r--r-- | src/Networking/MulticastDiscovery/MulticastResponder.cpp | 47 | ||||
-rw-r--r-- | src/Networking/MulticastDiscovery/MulticastResponder.h | 1 | ||||
-rw-r--r-- | src/Networking/MulticastDiscovery/fgmc_protocol.cpp | 540 |
3 files changed, 315 insertions, 273 deletions
diff --git a/src/Networking/MulticastDiscovery/MulticastResponder.cpp b/src/Networking/MulticastDiscovery/MulticastResponder.cpp index 96630a58..da3f582a 100644 --- a/src/Networking/MulticastDiscovery/MulticastResponder.cpp +++ b/src/Networking/MulticastDiscovery/MulticastResponder.cpp @@ -11,6 +11,8 @@ #include <Platform/RepRap.h> #include <Platform/Platform.h> +#include <Hardware/ExceptionHandlers.h> + #include "fgmc_header.h" #include "fgmc_protocol.h" @@ -29,6 +31,8 @@ static volatile uint16_t receivedPort; static uint16_t lastMessageReceivedPort; static unsigned int messagesProcessed = 0; static FGMCProtocol *fgmcHandler = nullptr; +static uint32_t ticksToReboot = 0; +static uint32_t whenRebootScheduled; static bool active = false; @@ -55,21 +59,33 @@ void MulticastResponder::Init() noexcept // Do some work, returning true if we did anything significant void MulticastResponder::Spin() noexcept { - pbuf *rxPbuf = receivedPbuf; - if (rxPbuf != nullptr) + if (ticksToReboot != 0) + { + if (millis() - whenRebootScheduled > ticksToReboot) + { + reprap.EmergencyStop(); // this disables heaters and drives - Duet WiFi pre-production boards need drives disabled here + SoftwareReset(SoftwareResetReason::user); // doesn't return + } + + } + else { - lastMessageReceivedPort = receivedPort; - debugPrintf("Rx UDP: addr %u.%u.%u.%u port %u data", - (unsigned int)(receivedIpAddr & 0xFF), (unsigned int)((receivedIpAddr >> 8) & 0xFF), (unsigned int)((receivedIpAddr >> 16) & 0xFF), (unsigned int)((receivedIpAddr >> 24) & 0xFF), lastMessageReceivedPort); - for (size_t i = 0; i < rxPbuf->len; ++i) + pbuf *rxPbuf = receivedPbuf; + if (rxPbuf != nullptr) { - debugPrintf(" %02x", ((const uint8_t*)(rxPbuf->payload))[i]); + lastMessageReceivedPort = receivedPort; + debugPrintf("Rx UDP: addr %u.%u.%u.%u port %u data", + (unsigned int)(receivedIpAddr & 0xFF), (unsigned int)((receivedIpAddr >> 8) & 0xFF), (unsigned int)((receivedIpAddr >> 16) & 0xFF), (unsigned int)((receivedIpAddr >> 24) & 0xFF), lastMessageReceivedPort); + for (size_t i = 0; i < rxPbuf->len; ++i) + { + debugPrintf(" %02x", ((const uint8_t*)(rxPbuf->payload))[i]); + } + debugPrintf("\n"); + receivedPbuf = nullptr; + fgmcHandler->handleStream(0, (uint8_t *)rxPbuf->payload, rxPbuf->len); + pbuf_free(rxPbuf); + ++messagesProcessed; } - debugPrintf("\n"); - receivedPbuf = nullptr; - fgmcHandler->handleStream(0, (uint8_t *)rxPbuf->payload, rxPbuf->len); - pbuf_free(rxPbuf); - ++messagesProcessed; } } @@ -134,6 +150,13 @@ void MulticastResponder::SendResponse(uint8_t *data, size_t length) noexcept //TODO actually send it } +// Schedule a reboot. We delay a little while to allow the response to be transmitted first. +void MulticastResponder::ScheduleReboot() noexcept +{ + whenRebootScheduled = millis(); + ticksToReboot = 1000; +} + #endif // End diff --git a/src/Networking/MulticastDiscovery/MulticastResponder.h b/src/Networking/MulticastDiscovery/MulticastResponder.h index 25b717d7..173a0087 100644 --- a/src/Networking/MulticastDiscovery/MulticastResponder.h +++ b/src/Networking/MulticastDiscovery/MulticastResponder.h @@ -20,6 +20,7 @@ namespace MulticastResponder void Start(TcpPort port) noexcept; void Stop() noexcept; void SendResponse(uint8_t *data, size_t length) noexcept; + void ScheduleReboot() noexcept; void Diagnostics(MessageType mtype) noexcept; } diff --git a/src/Networking/MulticastDiscovery/fgmc_protocol.cpp b/src/Networking/MulticastDiscovery/fgmc_protocol.cpp index cb936772..f1b99969 100644 --- a/src/Networking/MulticastDiscovery/fgmc_protocol.cpp +++ b/src/Networking/MulticastDiscovery/fgmc_protocol.cpp @@ -103,247 +103,262 @@ void FGMCProtocol::init() noexcept void FGMCProtocol::handleStream(unsigned int iFaceId, uint8_t* inputBufferAddress, uint32_t rxLength) noexcept { - // backup sciopta connection handle - // if a null pointer received => plattform should execute a exception - this->iface_id_ = iFaceId; + // backup sciopta connection handle + // if a null pointer received => plattform should execute a exception + this->iface_id_ = iFaceId; - if (rxLength >= static_cast<uint32_t>(sizeof(FGMC_GenericHeader))) { - FGMC_GenericHeader* pInGenericHeader = reinterpret_cast<FGMC_GenericHeader*>(inputBufferAddress); - - // read incoming packetid - uint32_t packetId = pInGenericHeader->fgmc_packet_id_; - - if (isPacketInBuffer(packetId)) { - return; // TODO: Filter out iFace 1, if iFace 0 is active (PACKET-ID) ? - } - insertPacketId(packetId); - - switch (pInGenericHeader->fgmc_command_) { - case FGMCCommand::MCD_COMMAND_UNETINF: { - if (pInGenericHeader->fgmc_destination_id_[0] == '\0') { - // fgmc command: upload network information - cmdUnetinf(packetId); - } - break; - } -#if 0 // not supported for now - case FGMCCommand::MCD_COMMAND_DNETINF: { - if (strncmp(pInGenericHeader->fgmc_destination_id_, unique_id_[iface_id_], SIZE_FGMC_DEST_ID) == 0) { - // fgmc command: download network information - FGMC_ReqDownloadNetInfoHeader* pInCmdPointer = reinterpret_cast<FGMC_ReqDownloadNetInfoHeader*>(reinterpret_cast<uint32_t>(inputBufferAddress) + - static_cast<uint32_t>(sizeof(FGMC_GenericHeader))); - cmdDnetinf(pInCmdPointer, packetId); - } - break; - } -#endif - case FGMCCommand::MCD_COMMAND_REBOOT: { - if (strncmp(pInGenericHeader->fgmc_destination_id_, unique_id_[iface_id_], SIZE_FGMC_DEST_ID) == 0) { - // fgmc command: reboot - cmdReboot(packetId); - } - break; - } - case FGMCCommand::MCD_COMMAND_IDENTIFY: { - if (strncmp(pInGenericHeader->fgmc_destination_id_, unique_id_[iface_id_], SIZE_FGMC_DEST_ID) == 0) { - // fgmc command: identify - FGMC_ReqIdentify* pInCmdPointer = - reinterpret_cast<FGMC_ReqIdentify*>(reinterpret_cast<uint32_t>(inputBufferAddress) + static_cast<uint32_t>(sizeof(FGMC_GenericHeader))); - cmdIdentify(pInCmdPointer, packetId); - } - break; - } - case FGMCCommand::MCD_COMMAND_GET_FIRMWARE_VERSION: { - if (strncmp(pInGenericHeader->fgmc_destination_id_, unique_id_[iface_id_], SIZE_FGMC_DEST_ID) == 0) { - // fgmc command: get firmware version - cmdGetFirmwareVersion(packetId); - } - break; - } - case FGMCCommand::MCD_COMMAND_GET_SUPPORTED_COMMANDS: { - if (strncmp(pInGenericHeader->fgmc_destination_id_, unique_id_[iface_id_], SIZE_FGMC_DEST_ID) == 0) { - // fgmc command: get supported commands - cmdGetSupportedCommands(packetId); - } - break; - } - default: { - // unknown command will be skipped - break; - } - } - } + if (rxLength >= static_cast<uint32_t>(sizeof(FGMC_GenericHeader))) + { + FGMC_GenericHeader* pInGenericHeader = reinterpret_cast<FGMC_GenericHeader*>(inputBufferAddress); + + // read incoming packetid + uint32_t packetId = pInGenericHeader->fgmc_packet_id_; + + if (isPacketInBuffer(packetId)) + { + return; // TODO: Filter out iFace 1, if iFace 0 is active (PACKET-ID) ? + } + + insertPacketId(packetId); + + switch (pInGenericHeader->fgmc_command_) + { + case FGMCCommand::MCD_COMMAND_UNETINF: + if (pInGenericHeader->fgmc_destination_id_[0] == '\0') + { + // fgmc command: upload network information + cmdUnetinf(packetId); + } + break; + + #if 0 // not supported for now + case FGMCCommand::MCD_COMMAND_DNETINF: + if (strncmp(pInGenericHeader->fgmc_destination_id_, unique_id_[iface_id_], SIZE_FGMC_DEST_ID) == 0) + { + // fgmc command: download network information + FGMC_ReqDownloadNetInfoHeader* const pInCmdPointer = + reinterpret_cast<FGMC_ReqDownloadNetInfoHeader*>(reinterpret_cast<uint32_t>(inputBufferAddress) + static_cast<uint32_t>(sizeof(FGMC_GenericHeader))); + cmdDnetinf(pInCmdPointer, packetId); + } + break; + #endif + + case FGMCCommand::MCD_COMMAND_REBOOT: + if (strncmp(pInGenericHeader->fgmc_destination_id_, unique_id_[iface_id_], SIZE_FGMC_DEST_ID) == 0) + { + // fgmc command: reboot + cmdReboot(packetId); + } + break; + + case FGMCCommand::MCD_COMMAND_IDENTIFY: + if (strncmp(pInGenericHeader->fgmc_destination_id_, unique_id_[iface_id_], SIZE_FGMC_DEST_ID) == 0) + { + // fgmc command: identify + FGMC_ReqIdentify* pInCmdPointer = + reinterpret_cast<FGMC_ReqIdentify*>(reinterpret_cast<uint32_t>(inputBufferAddress) + static_cast<uint32_t>(sizeof(FGMC_GenericHeader))); + cmdIdentify(pInCmdPointer, packetId); + } + break; + + case FGMCCommand::MCD_COMMAND_GET_FIRMWARE_VERSION: + if (strncmp(pInGenericHeader->fgmc_destination_id_, unique_id_[iface_id_], SIZE_FGMC_DEST_ID) == 0) + { + // fgmc command: get firmware version + cmdGetFirmwareVersion(packetId); + } + break; + + case FGMCCommand::MCD_COMMAND_GET_SUPPORTED_COMMANDS: + if (strncmp(pInGenericHeader->fgmc_destination_id_, unique_id_[iface_id_], SIZE_FGMC_DEST_ID) == 0) + { + // fgmc command: get supported commands + cmdGetSupportedCommands(packetId); + } + break; + + default: + // unknown command will be skipped + break; + } + } } void FGMCProtocol::sendGenericHeader(uint8_t* tx_netbuf, FGMCCommand cmd, uint32_t length, uint32_t packetId, uint32_t segmentIndex, uint32_t segmentCount) noexcept { - FGMC_GenericHeader* pOutGenericHeader = reinterpret_cast<FGMC_GenericHeader*>(tx_netbuf); - - //----------------------------------------------------------------------------------- - // Generic Multicast Header - //----------------------------------------------------------------------------------- - (void)memcpy(&pOutGenericHeader->fgmc_name_[0], &FGMC_NAME, SIZE_FGMC_NAME); - pOutGenericHeader->fgmc_length_ = length; - pOutGenericHeader->fgmc_hw_type_id_ = fgmc_device_id_; - // device unique fgmc destination id - (void)strncpy(&pOutGenericHeader->fgmc_destination_id_[0], unique_id_[iface_id_], SIZE_FGMC_DEST_ID); - pOutGenericHeader->fgmc_packet_id_ = packetId + 1; - pOutGenericHeader->fgmc_segment_index_ = segmentIndex; - pOutGenericHeader->fgmc_segment_count_ = segmentCount; - pOutGenericHeader->fgmc_command_ = cmd; - pOutGenericHeader->fgmc_command_version_ = FGMCCommandVersion::MCD_COMMAND_VERSION; - pOutGenericHeader->fgmc_error_code_ = FGMCErrorCode::FGMC_ERROR_CODE_NORMAL; - - // Send Message - insertPacketId(pOutGenericHeader->fgmc_packet_id_); - - MulticastResponder::SendResponse(tx_netbuf, length); + FGMC_GenericHeader* pOutGenericHeader = reinterpret_cast<FGMC_GenericHeader*>(tx_netbuf); + + //----------------------------------------------------------------------------------- + // Generic Multicast Header + //----------------------------------------------------------------------------------- + memcpy(pOutGenericHeader->fgmc_name_, FGMC_NAME, SIZE_FGMC_NAME); + pOutGenericHeader->fgmc_length_ = length; + pOutGenericHeader->fgmc_hw_type_id_ = fgmc_device_id_; + // device unique fgmc destination id + strncpy(&pOutGenericHeader->fgmc_destination_id_[0], unique_id_[iface_id_], SIZE_FGMC_DEST_ID); + pOutGenericHeader->fgmc_packet_id_ = packetId + 1; + pOutGenericHeader->fgmc_segment_index_ = segmentIndex; + pOutGenericHeader->fgmc_segment_count_ = segmentCount; + pOutGenericHeader->fgmc_command_ = cmd; + pOutGenericHeader->fgmc_command_version_ = FGMCCommandVersion::MCD_COMMAND_VERSION; + pOutGenericHeader->fgmc_error_code_ = FGMCErrorCode::FGMC_ERROR_CODE_NORMAL; + + // Send Message + insertPacketId(pOutGenericHeader->fgmc_packet_id_); + + MulticastResponder::SendResponse(tx_netbuf, length); } void FGMCProtocol::cmdUnetinf(uint32_t inPacketId) noexcept { - FGMC_ResUploadNetInfoHeader* pOutCmdHeader = reinterpret_cast<FGMC_ResUploadNetInfoHeader*>(tx_netbuf_); - (void)memset(pOutCmdHeader, 0x00, sizeof(FGMC_ResUploadNetInfoHeader)); + FGMC_ResUploadNetInfoHeader* pOutCmdHeader = reinterpret_cast<FGMC_ResUploadNetInfoHeader*>(tx_netbuf_); + (void)memset(pOutCmdHeader, 0x00, sizeof(FGMC_ResUploadNetInfoHeader)); - //----------------------------------------------------------------------------------- - // The Upload Netinformation Structure - //----------------------------------------------------------------------------------- + //----------------------------------------------------------------------------------- + // The Upload Netinformation Structure + //----------------------------------------------------------------------------------- - // ip address type - const bool dhcpEnable = reprap.GetNetwork().UsingDhcp(iface_id_); - if (dhcpEnable) - { - // dynamic - pOutCmdHeader->fgmc_ip_address_type_ = 1; - } - else - { - // static - pOutCmdHeader->fgmc_ip_address_type_ = 0; - } + // ip address type + const bool dhcpEnable = reprap.GetNetwork().UsingDhcp(iface_id_); + if (dhcpEnable) + { + // dynamic + pOutCmdHeader->fgmc_ip_address_type_ = 1; + } + else + { + // static + pOutCmdHeader->fgmc_ip_address_type_ = 0; + } - // mac address - const MacAddress& macAddress = reprap.GetNetwork().GetMacAddress(iface_id_); - memcpy(pOutCmdHeader->fgmc_mac_address_, macAddress.bytes, 6); + // mac address + const MacAddress& macAddress = reprap.GetNetwork().GetMacAddress(iface_id_); + memcpy(pOutCmdHeader->fgmc_mac_address_, macAddress.bytes, 6); - // IPv4 - const uint32_t ipaddress = reprap.GetNetwork().GetIPAddress(iface_id_).GetV4BigEndian(); - (void)memcpy(&pOutCmdHeader->fgmc_ip_v4_static_address_[0], &ipaddress, SIZE_IP_V4); + // IPv4 + const uint32_t ipaddress = reprap.GetNetwork().GetIPAddress(iface_id_).GetV4BigEndian(); + (void)memcpy(&pOutCmdHeader->fgmc_ip_v4_static_address_[0], &ipaddress, SIZE_IP_V4); - const uint32_t subnetmask = reprap.GetNetwork().GetNetmask(iface_id_).GetV4BigEndian(); - (void)memcpy(&pOutCmdHeader->fgmc_ip_v4_static_netmask_[0], &subnetmask, SIZE_IP_V4); + const uint32_t subnetmask = reprap.GetNetwork().GetNetmask(iface_id_).GetV4BigEndian(); + (void)memcpy(&pOutCmdHeader->fgmc_ip_v4_static_netmask_[0], &subnetmask, SIZE_IP_V4); - const uint32_t gateway = reprap.GetNetwork().GetGateway(iface_id_).GetV4BigEndian(); - (void)memcpy(&pOutCmdHeader->fgmc_ip_v4_static_gateway_[0], &gateway, SIZE_IP_V4); + const uint32_t gateway = reprap.GetNetwork().GetGateway(iface_id_).GetV4BigEndian(); + (void)memcpy(&pOutCmdHeader->fgmc_ip_v4_static_gateway_[0], &gateway, SIZE_IP_V4); - pOutCmdHeader->fgmc_ip_v4_static_dns_[0] = 0; - pOutCmdHeader->fgmc_ip_v4_static_dns_[1] = 0; - pOutCmdHeader->fgmc_ip_v4_static_dns_[2] = 0; - pOutCmdHeader->fgmc_ip_v4_static_dns_[3] = 0; + pOutCmdHeader->fgmc_ip_v4_static_dns_[0] = 0; + pOutCmdHeader->fgmc_ip_v4_static_dns_[1] = 0; + pOutCmdHeader->fgmc_ip_v4_static_dns_[2] = 0; + pOutCmdHeader->fgmc_ip_v4_static_dns_[3] = 0; - // IPv6 - (void)memset(&pOutCmdHeader->fgmc_ip_v6_static_6_addr_[0], 0x00, SIZE_IP_V6); - (void)memset(&pOutCmdHeader->fgmc_ip_v6_static_netmask_[0], 0x00, SIZE_IP_V6); - (void)memset(&pOutCmdHeader->fgmc_ip_v6_static_gateway_[0], 0x00, SIZE_IP_V6); - (void)memset(&pOutCmdHeader->fgmc_ip_v6_static_dns_[0], 0x00, SIZE_IP_V6); + // IPv6 + (void)memset(&pOutCmdHeader->fgmc_ip_v6_static_6_addr_[0], 0x00, SIZE_IP_V6); + (void)memset(&pOutCmdHeader->fgmc_ip_v6_static_netmask_[0], 0x00, SIZE_IP_V6); + (void)memset(&pOutCmdHeader->fgmc_ip_v6_static_gateway_[0], 0x00, SIZE_IP_V6); + (void)memset(&pOutCmdHeader->fgmc_ip_v6_static_dns_[0], 0x00, SIZE_IP_V6); - // IPv4 active - const uint32_t ipaddressActive = ipaddress; - (void)memcpy(&pOutCmdHeader->fgmc_ip_v4_address_[0], &ipaddressActive, SIZE_IP_V4); + // IPv4 active + const uint32_t ipaddressActive = ipaddress; + (void)memcpy(&pOutCmdHeader->fgmc_ip_v4_address_[0], &ipaddressActive, SIZE_IP_V4); - const uint32_t subnetmaskActive = subnetmask; - (void)memcpy(&pOutCmdHeader->fgmc_ip_v4_netmask_[0], &subnetmaskActive, SIZE_IP_V4); + const uint32_t subnetmaskActive = subnetmask; + (void)memcpy(&pOutCmdHeader->fgmc_ip_v4_netmask_[0], &subnetmaskActive, SIZE_IP_V4); - const uint32_t gatewayActive = gateway; - (void)memcpy(&pOutCmdHeader->fgmc_ip_v4_gateway_[0], &gatewayActive, SIZE_IP_V4); + const uint32_t gatewayActive = gateway; + (void)memcpy(&pOutCmdHeader->fgmc_ip_v4_gateway_[0], &gatewayActive, SIZE_IP_V4); - pOutCmdHeader->fgmc_ip_v4_dns_[0] = 0; - pOutCmdHeader->fgmc_ip_v4_dns_[1] = 0; - pOutCmdHeader->fgmc_ip_v4_dns_[2] = 0; - pOutCmdHeader->fgmc_ip_v4_dns_[3] = 0; + pOutCmdHeader->fgmc_ip_v4_dns_[0] = 0; + pOutCmdHeader->fgmc_ip_v4_dns_[1] = 0; + pOutCmdHeader->fgmc_ip_v4_dns_[2] = 0; + pOutCmdHeader->fgmc_ip_v4_dns_[3] = 0; - // NOC-CODE - (void)strncpy(&pOutCmdHeader->fgmc_noc_code_[0], &eeprom_noc_code_[0], SIZE_EEPROM_NOC_CODE); + // NOC-CODE + (void)strncpy(&pOutCmdHeader->fgmc_noc_code_[0], &eeprom_noc_code_[0], SIZE_EEPROM_NOC_CODE); - // iTTL - pOutCmdHeader->fgmc_ttl_ = 255; + // iTTL + pOutCmdHeader->fgmc_ttl_ = 255; - // Connection State - pOutCmdHeader->fgmc_connection_state_ = 0; + // Connection State + pOutCmdHeader->fgmc_connection_state_ = 0; - // Device Type - (void)strncpy(&pOutCmdHeader->fgmc_device_type_[0], &fgmc_device_type_[0], SIZE_DEVICE_TYPE); + // Device Type + (void)strncpy(&pOutCmdHeader->fgmc_device_type_[0], &fgmc_device_type_[0], SIZE_DEVICE_TYPE); - // Device Serial Number - (void)strncpy(&pOutCmdHeader->fgmc_serial_number_[0], &eeprom_product_key_[0], SIZE_SERIAL_NUMBER); + // Device Serial Number + (void)strncpy(&pOutCmdHeader->fgmc_serial_number_[0], &eeprom_product_key_[0], SIZE_SERIAL_NUMBER); - // Application Type - pOutCmdHeader->fgmc_application_type_ = fgmc_application_type_; + // Application Type + pOutCmdHeader->fgmc_application_type_ = fgmc_application_type_; - // Application Version - pOutCmdHeader->fgmc_application_version_ = 0; + // Application Version + pOutCmdHeader->fgmc_application_version_ = 0; - // Application Version Revision - pOutCmdHeader->fgmc_application_version_revision_ = 0; + // Application Version Revision + pOutCmdHeader->fgmc_application_version_revision_ = 0; - // Generic Info - (void)memset(&pOutCmdHeader->fgmc_generic_info_[0], 0x00, SIZE_GENERIC_INFO); + // Generic Info + (void)memset(&pOutCmdHeader->fgmc_generic_info_[0], 0x00, SIZE_GENERIC_INFO); - // Device Name - SafeStrncpy(pOutCmdHeader->fgmc_device_name_, BOARD_SHORT_NAME, ARRAY_SIZE(pOutCmdHeader->fgmc_device_name_)); + // Device Name + SafeStrncpy(pOutCmdHeader->fgmc_device_name_, BOARD_SHORT_NAME, ARRAY_SIZE(pOutCmdHeader->fgmc_device_name_)); - //----------------------------------------------------------------------------------- - // Generic Multicast Header - //----------------------------------------------------------------------------------- - sendGenericHeader(tx_netbuf_, FGMCCommand::MCD_COMMAND_UNETINF, sizeof(FGMC_ResUploadNetInfoHeader), inPacketId, 0, 1); + //----------------------------------------------------------------------------------- + // Generic Multicast Header + //----------------------------------------------------------------------------------- + sendGenericHeader(tx_netbuf_, FGMCCommand::MCD_COMMAND_UNETINF, sizeof(FGMC_ResUploadNetInfoHeader), inPacketId, 0, 1); } #if 0 // not supported for now -void FGMCProtocol::cmdDnetinf(FGMC_ReqDownloadNetInfoHeader* pInCmdHeader, uint32_t inPacketId) { - FGMC_ResDownloadNetInfoHeader* pOutCmdHeader = reinterpret_cast<FGMC_ResDownloadNetInfoHeader*>(tx_netbuf_); - (void)memset(pOutCmdHeader, 0x00, sizeof(FGMC_ResDownloadNetInfoHeader)); - - //----------------------------------------------------------------------------------- - // The Download Netinformation Structure - //----------------------------------------------------------------------------------- - - bool dhcpEnable = false; - uint32_t ipaddress = 0; - uint32_t subnetmask = 0; - uint32_t gateway = 0; - - // set new network settings - dhcpEnable = static_cast<bool>(pInCmdHeader->fgmc_ip_address_type_); - engp_router_[iface_id_]->SetDhcpenable(dhcpEnable); - (void)memcpy(&ipaddress, &pInCmdHeader->fgmc_ip_v4_static_address_[0], SIZE_IP_V4); - engp_router_[iface_id_]->SetIpaddress(ntohl(ipaddress)); - (void)memcpy(&subnetmask, &pInCmdHeader->fgmc_ip_v4_static_netmask_[0], SIZE_IP_V4); - engp_router_[iface_id_]->SetSubnetmask(ntohl(subnetmask)); - (void)memcpy(&gateway, &pInCmdHeader->fgmc_ip_v4_static_gateway_[0], SIZE_IP_V4); - engp_router_[iface_id_]->SetGateway(ntohl(gateway)); - - // set new device name - // filter out " (X19) / (X18) - for (uint32_t i = 0; i < SIZE_DEVICE_NAME; i++) { - if (pInCmdHeader->fgmc_device_name_[i] == '\0') { - if (i > 6) { - if ((pInCmdHeader->fgmc_device_name_[i - 6] == ' ') && (pInCmdHeader->fgmc_device_name_[i - 5] == '(') && - (pInCmdHeader->fgmc_device_name_[i - 4] == 'X') && (pInCmdHeader->fgmc_device_name_[i - 3] == '1') && - (pInCmdHeader->fgmc_device_name_[i - 1] == ')')) { - for (uint32_t j = 1; j <= 6; j++) { - pInCmdHeader->fgmc_device_name_[i - j] = '\0'; - } - } - } - break; - } - } - app_info_->setDeviceName(&pInCmdHeader->fgmc_device_name_[0], SIZE_DEVICE_NAME); +void FGMCProtocol::cmdDnetinf(FGMC_ReqDownloadNetInfoHeader* pInCmdHeader, uint32_t inPacketId) +{ + FGMC_ResDownloadNetInfoHeader* pOutCmdHeader = reinterpret_cast<FGMC_ResDownloadNetInfoHeader*>(tx_netbuf_); + (void)memset(pOutCmdHeader, 0x00, sizeof(FGMC_ResDownloadNetInfoHeader)); + + //----------------------------------------------------------------------------------- + // The Download Netinformation Structure + //----------------------------------------------------------------------------------- + + bool dhcpEnable = false; + uint32_t ipaddress = 0; + uint32_t subnetmask = 0; + uint32_t gateway = 0; + + // set new network settings + dhcpEnable = static_cast<bool>(pInCmdHeader->fgmc_ip_address_type_); + engp_router_[iface_id_]->SetDhcpenable(dhcpEnable); + (void)memcpy(&ipaddress, &pInCmdHeader->fgmc_ip_v4_static_address_[0], SIZE_IP_V4); + engp_router_[iface_id_]->SetIpaddress(ntohl(ipaddress)); + (void)memcpy(&subnetmask, &pInCmdHeader->fgmc_ip_v4_static_netmask_[0], SIZE_IP_V4); + engp_router_[iface_id_]->SetSubnetmask(ntohl(subnetmask)); + (void)memcpy(&gateway, &pInCmdHeader->fgmc_ip_v4_static_gateway_[0], SIZE_IP_V4); + engp_router_[iface_id_]->SetGateway(ntohl(gateway)); + + // set new device name + // filter out " (X19) / (X18) + for (uint32_t i = 0; i < SIZE_DEVICE_NAME; i++) + { + if (pInCmdHeader->fgmc_device_name_[i] == '\0') + { + if (i > 6) + { + if ((pInCmdHeader->fgmc_device_name_[i - 6] == ' ') && (pInCmdHeader->fgmc_device_name_[i - 5] == '(') && + (pInCmdHeader->fgmc_device_name_[i - 4] == 'X') && (pInCmdHeader->fgmc_device_name_[i - 3] == '1') && + (pInCmdHeader->fgmc_device_name_[i - 1] == ')')) + { + for (uint32_t j = 1; j <= 6; j++) + { + pInCmdHeader->fgmc_device_name_[i - j] = '\0'; + } + } + } + break; + } + } + app_info_->setDeviceName(&pInCmdHeader->fgmc_device_name_[0], SIZE_DEVICE_NAME); - //----------------------------------------------------------------------------------- - // Generic Multicast Header - //----------------------------------------------------------------------------------- - sendGenericHeader(tx_netbuf_, FGMCCommand::MCD_COMMAND_DNETINF, sizeof(FGMC_ResDownloadNetInfoHeader), inPacketId, 0, 1); + //----------------------------------------------------------------------------------- + // Generic Multicast Header + //----------------------------------------------------------------------------------- + sendGenericHeader(tx_netbuf_, FGMCCommand::MCD_COMMAND_DNETINF, sizeof(FGMC_ResDownloadNetInfoHeader), inPacketId, 0, 1); } #endif @@ -355,8 +370,7 @@ void FGMCProtocol::cmdReboot(uint32_t inPacketId) noexcept // Generic Multicast Header //----------------------------------------------------------------------------------- sendGenericHeader(tx_netbuf_, FGMCCommand::MCD_COMMAND_REBOOT, sizeof(FGMC_GenericHeader), inPacketId, 0, 1); - - //TODO schedule the reboot + MulticastResponder::ScheduleReboot(); } void FGMCProtocol::cmdIdentify(FGMC_ReqIdentify* pInCmdHeader, uint32_t inPacketId) noexcept @@ -388,78 +402,82 @@ void FGMCProtocol::cmdIdentify(FGMC_ReqIdentify* pInCmdHeader, uint32_t inPacket void FGMCProtocol::cmdGetFirmwareVersion(uint32_t inPacketId) noexcept { - FGMC_ResGetFwVersion* pOutCmdHeader = reinterpret_cast<FGMC_ResGetFwVersion*>(tx_netbuf_); - (void)memset(pOutCmdHeader, 0x00, sizeof(FGMC_ResGetFwVersion)); + FGMC_ResGetFwVersion* pOutCmdHeader = reinterpret_cast<FGMC_ResGetFwVersion*>(tx_netbuf_); + (void)memset(pOutCmdHeader, 0x00, sizeof(FGMC_ResGetFwVersion)); - //----------------------------------------------------------------------------------- - // Get Firmware Version - //----------------------------------------------------------------------------------- - (void)strncpy(&pOutCmdHeader->module_name_[0], &fgmc_device_type_[0], SIZE_DEVICE_TYPE); + //----------------------------------------------------------------------------------- + // Get Firmware Version + //----------------------------------------------------------------------------------- + (void)strncpy(&pOutCmdHeader->module_name_[0], &fgmc_device_type_[0], SIZE_DEVICE_TYPE); - SafeSnprintf(pOutCmdHeader->module_version_, SIZE_MODULE_VERSION, "%s version %s", FIRMWARE_NAME, VERSION); + SafeSnprintf(pOutCmdHeader->module_version_, SIZE_MODULE_VERSION, "%s version %s", FIRMWARE_NAME, VERSION); - //----------------------------------------------------------------------------------- - // Generic Multicast Header - //----------------------------------------------------------------------------------- - sendGenericHeader(tx_netbuf_, FGMCCommand::MCD_COMMAND_GET_FIRMWARE_VERSION, sizeof(FGMC_ResGetFwVersion), inPacketId, 0, 1); + //----------------------------------------------------------------------------------- + // Generic Multicast Header + //----------------------------------------------------------------------------------- + sendGenericHeader(tx_netbuf_, FGMCCommand::MCD_COMMAND_GET_FIRMWARE_VERSION, sizeof(FGMC_ResGetFwVersion), inPacketId, 0, 1); } void FGMCProtocol::cmdGetSupportedCommands(uint32_t inPacketId) noexcept { - uint32_t segmentIndex = 0; - uint32_t segmentCount = 0; + uint32_t segmentIndex = 0; + uint32_t segmentCount = 0; - FGMCCommand supportedCommands[] = - { - FGMCCommand::MCD_COMMAND_UNETINF, + FGMCCommand supportedCommands[] = + { + FGMCCommand::MCD_COMMAND_UNETINF, #if 0 // not supported for now - FGMCCommand::MCD_COMMAND_DNETINF, + FGMCCommand::MCD_COMMAND_DNETINF, #endif - FGMCCommand::MCD_COMMAND_IDENTIFY, - FGMCCommand::MCD_COMMAND_GET_FIRMWARE_VERSION, - FGMCCommand::MCD_COMMAND_GET_SUPPORTED_COMMANDS - }; - - for (uint8_t i = 0; i < (sizeof(supportedCommands) / sizeof(FGMCCommand)); i++) { - FGMC_ResGetSupportedCommands* pOutCmdHeader = reinterpret_cast<FGMC_ResGetSupportedCommands*>(tx_netbuf_); - (void)memset(pOutCmdHeader, 0x00, sizeof(FGMC_ResGetSupportedCommands)); - - //----------------------------------------------------------------------------------- - // Get Supported Commands - //----------------------------------------------------------------------------------- - pOutCmdHeader->cmd_ = static_cast<uint32_t>(supportedCommands[i]); - pOutCmdHeader->cmd_version_ = 0; - - //----------------------------------------------------------------------------------- - // Generic Multicast Header - //----------------------------------------------------------------------------------- - segmentIndex = static_cast<uint32_t>(i); - segmentCount = static_cast<uint32_t>(sizeof(supportedCommands)) / sizeof(FGMCCommand); - sendGenericHeader(tx_netbuf_, FGMCCommand::MCD_COMMAND_GET_SUPPORTED_COMMANDS, sizeof(FGMC_ResGetSupportedCommands), inPacketId, segmentIndex, - segmentCount); - } + FGMCCommand::MCD_COMMAND_IDENTIFY, + FGMCCommand::MCD_COMMAND_GET_FIRMWARE_VERSION, + FGMCCommand::MCD_COMMAND_GET_SUPPORTED_COMMANDS + }; + + for (uint8_t i = 0; i < (sizeof(supportedCommands) / sizeof(FGMCCommand)); i++) + { + FGMC_ResGetSupportedCommands* pOutCmdHeader = reinterpret_cast<FGMC_ResGetSupportedCommands*>(tx_netbuf_); + (void)memset(pOutCmdHeader, 0x00, sizeof(FGMC_ResGetSupportedCommands)); + + //----------------------------------------------------------------------------------- + // Get Supported Commands + //----------------------------------------------------------------------------------- + pOutCmdHeader->cmd_ = static_cast<uint32_t>(supportedCommands[i]); + pOutCmdHeader->cmd_version_ = 0; + + //----------------------------------------------------------------------------------- + // Generic Multicast Header + //----------------------------------------------------------------------------------- + segmentIndex = static_cast<uint32_t>(i); + segmentCount = static_cast<uint32_t>(sizeof(supportedCommands)) / sizeof(FGMCCommand); + sendGenericHeader(tx_netbuf_, FGMCCommand::MCD_COMMAND_GET_SUPPORTED_COMMANDS, sizeof(FGMC_ResGetSupportedCommands), inPacketId, segmentIndex, segmentCount); + } } bool FGMCProtocol::isPacketInBuffer(uint32_t packetId) noexcept { - for (uint32_t i = 0; i < ringBufferSize; i++) { - if (packetId == packetIdBuffer[iface_id_][i]) { - return true; - } - } - return false; + for (uint32_t i = 0; i < ringBufferSize; i++) + { + if (packetId == packetIdBuffer[iface_id_][i]) + { + return true; + } + } + return false; } void FGMCProtocol::insertPacketId(uint32_t packetId) noexcept { - if (isPacketInBuffer(packetId)) { - return; - } - packetIdBuffer[iface_id_][packetIdIndex[iface_id_]] = packetId; - packetIdIndex[iface_id_]++; - if (packetIdIndex[iface_id_] >= ringBufferSize) { - packetIdIndex[iface_id_] = 0; - } + if (isPacketInBuffer(packetId)) + { + return; + } + packetIdBuffer[iface_id_][packetIdIndex[iface_id_]] = packetId; + packetIdIndex[iface_id_]++; + if (packetIdIndex[iface_id_] >= ringBufferSize) + { + packetIdIndex[iface_id_] = 0; + } } #endif |