S Morita / mbed-mros2

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependents:   mbed-os-example-mros2 example-mbed-mros2-sub-pose example-mbed-mros2-pub-twist example-mbed-mros2-mturtle-teleop

embeddedRTPS/src/communication/UdpDriver.cpp@0:580aba13d1a1, 2021-12-30 (annotated)

Committer:
smoritaemb
Date:
Thu Dec 30 21:06:29 2021 +0900
Revision:
0:580aba13d1a1
Updated to catch up to mros2 v2.3

Who changed what in which revision?

UserRevisionLine numberNew contents of line
smoritaemb 0:580aba13d1a1 1 /*
smoritaemb 0:580aba13d1a1 2 The MIT License
smoritaemb 0:580aba13d1a1 3 Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University
smoritaemb 0:580aba13d1a1 4 Permission is hereby granted, free of charge, to any person obtaining a copy
smoritaemb 0:580aba13d1a1 5 of this software and associated documentation files (the "Software"), to deal
smoritaemb 0:580aba13d1a1 6 in the Software without restriction, including without limitation the rights
smoritaemb 0:580aba13d1a1 7 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
smoritaemb 0:580aba13d1a1 8 copies of the Software, and to permit persons to whom the Software is
smoritaemb 0:580aba13d1a1 9 furnished to do so, subject to the following conditions:
smoritaemb 0:580aba13d1a1 10 The above copyright notice and this permission notice shall be included in
smoritaemb 0:580aba13d1a1 11 all copies or substantial portions of the Software.
smoritaemb 0:580aba13d1a1 12 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
smoritaemb 0:580aba13d1a1 13 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
smoritaemb 0:580aba13d1a1 14 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
smoritaemb 0:580aba13d1a1 15 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
smoritaemb 0:580aba13d1a1 16 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
smoritaemb 0:580aba13d1a1 17 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
smoritaemb 0:580aba13d1a1 18 THE SOFTWARE
smoritaemb 0:580aba13d1a1 19
smoritaemb 0:580aba13d1a1 20 This file is part of embeddedRTPS.
smoritaemb 0:580aba13d1a1 21
smoritaemb 0:580aba13d1a1 22 Author: i11 - Embedded Software, RWTH Aachen University
smoritaemb 0:580aba13d1a1 23 */
smoritaemb 0:580aba13d1a1 24
smoritaemb 0:580aba13d1a1 25 #include "rtps/communication/UdpDriver.h"
smoritaemb 0:580aba13d1a1 26 #include "rtps/communication/TcpipCoreLock.h"
smoritaemb 0:580aba13d1a1 27
smoritaemb 0:580aba13d1a1 28 #include <lwip/igmp.h>
smoritaemb 0:580aba13d1a1 29 #include <lwip/tcpip.h>
smoritaemb 0:580aba13d1a1 30
smoritaemb 0:580aba13d1a1 31 using rtps::UdpDriver;
smoritaemb 0:580aba13d1a1 32
smoritaemb 0:580aba13d1a1 33 #define UDP_DRIVER_VERBOSE 0
smoritaemb 0:580aba13d1a1 34
smoritaemb 0:580aba13d1a1 35 UdpDriver::UdpDriver(rtps::UdpDriver::udpRxFunc_fp callback, void *args)
smoritaemb 0:580aba13d1a1 36 : m_rxCallback(callback), m_callbackArgs(args) {}
smoritaemb 0:580aba13d1a1 37
smoritaemb 0:580aba13d1a1 38 const rtps::UdpConnection *
smoritaemb 0:580aba13d1a1 39 UdpDriver::createUdpConnection(Ip4Port_t receivePort) {
smoritaemb 0:580aba13d1a1 40 for (uint8_t i = 0; i < m_numConns; ++i) {
smoritaemb 0:580aba13d1a1 41 if (m_conns[i].port == receivePort) {
smoritaemb 0:580aba13d1a1 42 return &m_conns[i];
smoritaemb 0:580aba13d1a1 43 }
smoritaemb 0:580aba13d1a1 44 }
smoritaemb 0:580aba13d1a1 45
smoritaemb 0:580aba13d1a1 46 if (m_numConns == m_conns.size()) {
smoritaemb 0:580aba13d1a1 47 return nullptr;
smoritaemb 0:580aba13d1a1 48 }
smoritaemb 0:580aba13d1a1 49
smoritaemb 0:580aba13d1a1 50 UdpConnection udp_conn(receivePort);
smoritaemb 0:580aba13d1a1 51
smoritaemb 0:580aba13d1a1 52 {
smoritaemb 0:580aba13d1a1 53 TcpipCoreLock lock;
smoritaemb 0:580aba13d1a1 54 err_t err = udp_bind(udp_conn.pcb, IP_ADDR_ANY,
smoritaemb 0:580aba13d1a1 55 receivePort); // to receive multicast
smoritaemb 0:580aba13d1a1 56
smoritaemb 0:580aba13d1a1 57 if (err != ERR_OK && err != ERR_USE) {
smoritaemb 0:580aba13d1a1 58 return nullptr;
smoritaemb 0:580aba13d1a1 59 }
smoritaemb 0:580aba13d1a1 60
smoritaemb 0:580aba13d1a1 61 udp_recv(udp_conn.pcb, m_rxCallback, m_callbackArgs);
smoritaemb 0:580aba13d1a1 62 }
smoritaemb 0:580aba13d1a1 63
smoritaemb 0:580aba13d1a1 64 m_conns[m_numConns] = std::move(udp_conn);
smoritaemb 0:580aba13d1a1 65 m_numConns++;
smoritaemb 0:580aba13d1a1 66 #if UDP_DRIVER_VERBOSE
smoritaemb 0:580aba13d1a1 67 printf("Successfully created UDP connection on port %u \n", receivePort);
smoritaemb 0:580aba13d1a1 68 #endif
smoritaemb 0:580aba13d1a1 69 return &m_conns[m_numConns - 1];
smoritaemb 0:580aba13d1a1 70 }
smoritaemb 0:580aba13d1a1 71
smoritaemb 0:580aba13d1a1 72 bool UdpDriver::isSameSubnet(ip4_addr_t addr) {
smoritaemb 0:580aba13d1a1 73 return (ip4_addr_netcmp(&addr, &(netif_default->ip_addr),
smoritaemb 0:580aba13d1a1 74 &(netif_default->netmask)) != 0);
smoritaemb 0:580aba13d1a1 75 }
smoritaemb 0:580aba13d1a1 76
smoritaemb 0:580aba13d1a1 77 bool UdpDriver::joinMultiCastGroup(ip4_addr_t addr) const {
smoritaemb 0:580aba13d1a1 78 err_t iret;
smoritaemb 0:580aba13d1a1 79
smoritaemb 0:580aba13d1a1 80 {
smoritaemb 0:580aba13d1a1 81 TcpipCoreLock lock;
smoritaemb 0:580aba13d1a1 82 iret = igmp_joingroup(IP_ADDR_ANY, (&addr));
smoritaemb 0:580aba13d1a1 83 }
smoritaemb 0:580aba13d1a1 84
smoritaemb 0:580aba13d1a1 85 if (iret != ERR_OK) {
smoritaemb 0:580aba13d1a1 86 #if UDP_DRIVER_VERBOSE
smoritaemb 0:580aba13d1a1 87 printf("Failed to join IGMP multicast group %s\n", ipaddr_ntoa(&addr));
smoritaemb 0:580aba13d1a1 88 #endif
smoritaemb 0:580aba13d1a1 89 return false;
smoritaemb 0:580aba13d1a1 90 } else {
smoritaemb 0:580aba13d1a1 91 #if UDP_DRIVER_VERBOSE
smoritaemb 0:580aba13d1a1 92 printf("Succesfully joined IGMP multicast group %s\n", ipaddr_ntoa(&addr));
smoritaemb 0:580aba13d1a1 93 #endif
smoritaemb 0:580aba13d1a1 94 }
smoritaemb 0:580aba13d1a1 95 return true;
smoritaemb 0:580aba13d1a1 96 }
smoritaemb 0:580aba13d1a1 97
smoritaemb 0:580aba13d1a1 98 bool UdpDriver::sendPacket(const UdpConnection &conn, ip4_addr_t &destAddr,
smoritaemb 0:580aba13d1a1 99 Ip4Port_t destPort, pbuf &buffer) {
smoritaemb 0:580aba13d1a1 100 err_t err;
smoritaemb 0:580aba13d1a1 101 {
smoritaemb 0:580aba13d1a1 102 TcpipCoreLock lock;
smoritaemb 0:580aba13d1a1 103 err = udp_sendto(conn.pcb, &buffer, &destAddr, destPort);
smoritaemb 0:580aba13d1a1 104 }
smoritaemb 0:580aba13d1a1 105
smoritaemb 0:580aba13d1a1 106 if (err != ERR_OK) {
smoritaemb 0:580aba13d1a1 107 ;
smoritaemb 0:580aba13d1a1 108 #if UDP_DRIVER_VERBOSE
smoritaemb 0:580aba13d1a1 109 printf("UDP TRANSMIT NOT SUCCESSFUL %s:%u size: %u err: %i\n",
smoritaemb 0:580aba13d1a1 110 ipaddr_ntoa(&destAddr), destPort, buffer.tot_len, err);
smoritaemb 0:580aba13d1a1 111 #endif
smoritaemb 0:580aba13d1a1 112 return false;
smoritaemb 0:580aba13d1a1 113 }
smoritaemb 0:580aba13d1a1 114 return true;
smoritaemb 0:580aba13d1a1 115 }
smoritaemb 0:580aba13d1a1 116
smoritaemb 0:580aba13d1a1 117 void UdpDriver::sendPacket(PacketInfo &packet) {
smoritaemb 0:580aba13d1a1 118 auto p_conn = createUdpConnection(packet.srcPort);
smoritaemb 0:580aba13d1a1 119 if (p_conn == nullptr) {
smoritaemb 0:580aba13d1a1 120 ;
smoritaemb 0:580aba13d1a1 121 #if UDP_DRIVER_VERBOSE
smoritaemb 0:580aba13d1a1 122 printf("Failed to create connection on port %u \n", packet.srcPort);
smoritaemb 0:580aba13d1a1 123 #endif
smoritaemb 0:580aba13d1a1 124 return;
smoritaemb 0:580aba13d1a1 125 }
smoritaemb 0:580aba13d1a1 126
smoritaemb 0:580aba13d1a1 127 sendPacket(*p_conn, packet.destAddr, packet.destPort,
smoritaemb 0:580aba13d1a1 128 *packet.buffer.firstElement);
smoritaemb 0:580aba13d1a1 129 }