udpUtils.h

00001 /*
00002 The MIT License
00003 Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University
00004 Permission is hereby granted, free of charge, to any person obtaining a copy
00005 of this software and associated documentation files (the "Software"), to deal
00006 in the Software without restriction, including without limitation the rights
00007 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
00008 copies of the Software, and to permit persons to whom the Software is
00009 furnished to do so, subject to the following conditions:
00010 The above copyright notice and this permission notice shall be included in
00011 all copies or substantial portions of the Software.
00012 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
00013 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
00014 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
00015 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
00016 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00017 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
00018 THE SOFTWARE
00019 
00020 This file is part of embeddedRTPS.
00021 
00022 Author: i11 - Embedded Software, RWTH Aachen University
00023 */
00024 
00025 #ifndef RTPS_UDP_UTILS_H
00026 #define RTPS_UDP_UTILS_H
00027 
00028 #include "rtps/config.h"
00029 
00030 namespace rtps {
00031 namespace {
00032 const uint16_t PB = 7400; // Port Base Number
00033 const uint16_t DG = 250;  // DomainId Gain
00034 const uint16_t PG = 2;    // ParticipantId Gain
00035 // Additional Offsets
00036 const uint16_t D0 = 0;  // Builtin multicast
00037 const uint16_t D1 = 10; // Builtin unicast
00038 const uint16_t D2 = 1;  // User multicast
00039 const uint16_t D3 = 11; // User unicast
00040 } // namespace
00041 
00042 constexpr ip4_addr transformIP4ToU32(uint8_t MSB, uint8_t p2, uint8_t p1,
00043                                      uint8_t LSB) {
00044   return {((uint32_t)(LSB << 24)) | ((uint32_t)(p1 << 16)) |
00045           ((uint32_t)(p2 << 8)) | MSB};
00046 }
00047 
00048 constexpr Ip4Port_t getBuiltInUnicastPort(ParticipantId_t participantId) {
00049   return PB + DG * Config::DOMAIN_ID + D1 + PG * participantId;
00050 }
00051 
00052 constexpr Ip4Port_t getBuiltInMulticastPort() {
00053   return PB + DG * Config::DOMAIN_ID + D0;
00054 }
00055 
00056 constexpr Ip4Port_t getUserUnicastPort(ParticipantId_t participantId) {
00057   return PB + DG * Config::DOMAIN_ID + D3 + PG * participantId;
00058 }
00059 
00060 constexpr Ip4Port_t getUserMulticastPort() {
00061   return PB + DG * Config::DOMAIN_ID + D2;
00062 }
00063 
00064 constexpr bool isUserPort(Ip4Port_t port) { return (port & 1) == 1; }
00065 
00066 inline bool isMultiCastPort(Ip4Port_t port) {
00067   const auto idWithoutBase = port - PB - DG * Config::DOMAIN_ID;
00068   return idWithoutBase == D0 || idWithoutBase == D2;
00069 }
00070 
00071 inline ParticipantId_t getParticipantIdFromUnicastPort(Ip4Port_t port,
00072                                                        bool isUserPort) {
00073   // if(isMultiCastPort(port)){ // TODO remove?
00074   //    return PARTICIPANT_ID_INVALID;
00075   //}
00076 
00077   const auto basePart = PB + DG * Config::DOMAIN_ID;
00078   ParticipantId_t participantPart = port - basePart;
00079 
00080   uint16_t offset = 0;
00081   if (isUserPort) {
00082     offset = D3;
00083   } else {
00084     offset = D1;
00085   }
00086 
00087   participantPart -= offset;
00088 
00089   auto id = static_cast<ParticipantId_t>(participantPart / PG);
00090   if (id * PG + basePart + offset == port) {
00091     return id;
00092   } else {
00093     return PARTICIPANT_ID_INVALID;
00094   }
00095 }
00096 
00097 } // namespace rtps
00098 
00099 #endif // RTPS_UDP_H