ilkay KOZAK / RadioHead-148

V148

Fork of RadioHead-148 by David Rimer

RH_TCP.cpp@1:b7641da2b203, 2017-10-25 (annotated)

Committer:
ilkaykozak
Date:
Wed Oct 25 05:14:09 2017 +0000
Revision:
1:b7641da2b203
Parent:
0:ab4e012489ef 
V148

Who changed what in which revision?

UserRevisionLine numberNew contents of line
davidr99 0:ab4e012489ef 1 // RH_TCP.cpp
davidr99 0:ab4e012489ef 2 //
davidr99 0:ab4e012489ef 3 // Copyright (C) 2014 Mike McCauley
davidr99 0:ab4e012489ef 4 // $Id: RH_TCP.cpp,v 1.5 2015/08/13 02:45:47 mikem Exp $
davidr99 0:ab4e012489ef 5
davidr99 0:ab4e012489ef 6 #include <RadioHead.h>
davidr99 0:ab4e012489ef 7
davidr99 0:ab4e012489ef 8 // This can only build on Linux and compatible systems
davidr99 0:ab4e012489ef 9 #if (RH_PLATFORM == RH_PLATFORM_UNIX)
davidr99 0:ab4e012489ef 10
davidr99 0:ab4e012489ef 11 #include <RH_TCP.h>
davidr99 0:ab4e012489ef 12 #include <sys/types.h>
davidr99 0:ab4e012489ef 13 #include <errno.h>
davidr99 0:ab4e012489ef 14 #include <sys/socket.h>
davidr99 0:ab4e012489ef 15 #include <netinet/in.h>
davidr99 0:ab4e012489ef 16 #include <arpa/inet.h>
davidr99 0:ab4e012489ef 17 #include <unistd.h>
davidr99 0:ab4e012489ef 18 #include <sys/ioctl.h>
davidr99 0:ab4e012489ef 19 #include <netdb.h>
davidr99 0:ab4e012489ef 20 #include <string>
davidr99 0:ab4e012489ef 21
davidr99 0:ab4e012489ef 22 RH_TCP::RH_TCP(const char* server)
davidr99 0:ab4e012489ef 23 : _server(server),
davidr99 0:ab4e012489ef 24 _rxBufLen(0),
davidr99 0:ab4e012489ef 25 _rxBufValid(false),
davidr99 0:ab4e012489ef 26 _socket(-1)
davidr99 0:ab4e012489ef 27 {
davidr99 0:ab4e012489ef 28 }
davidr99 0:ab4e012489ef 29
davidr99 0:ab4e012489ef 30 bool RH_TCP::init()
davidr99 0:ab4e012489ef 31 {
davidr99 0:ab4e012489ef 32 if (!connectToServer())
davidr99 0:ab4e012489ef 33 return false;
davidr99 0:ab4e012489ef 34 return sendThisAddress(_thisAddress);
davidr99 0:ab4e012489ef 35 }
davidr99 0:ab4e012489ef 36
davidr99 0:ab4e012489ef 37 bool RH_TCP::connectToServer()
davidr99 0:ab4e012489ef 38 {
davidr99 0:ab4e012489ef 39 struct addrinfo hints;
davidr99 0:ab4e012489ef 40 struct addrinfo *result, *rp;
davidr99 0:ab4e012489ef 41 int sfd, s;
davidr99 0:ab4e012489ef 42 struct sockaddr_storage peer_addr;
davidr99 0:ab4e012489ef 43 socklen_t peer_addr_len;
davidr99 0:ab4e012489ef 44
davidr99 0:ab4e012489ef 45 memset(&hints, 0, sizeof(struct addrinfo));
davidr99 0:ab4e012489ef 46 hints.ai_family = AF_UNSPEC; // Allow IPv4 or IPv6
davidr99 0:ab4e012489ef 47 hints.ai_socktype = SOCK_STREAM; // Stream socket
davidr99 0:ab4e012489ef 48 hints.ai_flags = AI_PASSIVE; // For wildcard IP address
davidr99 0:ab4e012489ef 49 hints.ai_protocol = 0; // Any protocol
davidr99 0:ab4e012489ef 50 hints.ai_canonname = NULL;
davidr99 0:ab4e012489ef 51 hints.ai_addr = NULL;
davidr99 0:ab4e012489ef 52 hints.ai_next = NULL;
davidr99 0:ab4e012489ef 53
davidr99 0:ab4e012489ef 54 std::string server(_server);
davidr99 0:ab4e012489ef 55 std::string port("4000");
davidr99 0:ab4e012489ef 56 size_t indexOfSeparator = server.find_first_of(':');
davidr99 0:ab4e012489ef 57 if (indexOfSeparator != std::string::npos)
davidr99 0:ab4e012489ef 58 {
davidr99 0:ab4e012489ef 59 port = server.substr(indexOfSeparator+1);
davidr99 0:ab4e012489ef 60 server.erase(indexOfSeparator);
davidr99 0:ab4e012489ef 61 }
davidr99 0:ab4e012489ef 62
davidr99 0:ab4e012489ef 63 s = getaddrinfo(server.c_str(), port.c_str(), &hints, &result);
davidr99 0:ab4e012489ef 64 if (s != 0)
davidr99 0:ab4e012489ef 65 {
davidr99 0:ab4e012489ef 66 fprintf(stderr, "RH_TCP::connect getaddrinfo failed: %s\n", gai_strerror(s));
davidr99 0:ab4e012489ef 67 return false;
davidr99 0:ab4e012489ef 68 }
davidr99 0:ab4e012489ef 69
davidr99 0:ab4e012489ef 70 // getaddrinfo() returns a list of address structures.
davidr99 0:ab4e012489ef 71 // Try each address until we successfully connect(2).
davidr99 0:ab4e012489ef 72 // If socket(2) (or connect(2)) fails, we (close the socket
davidr99 0:ab4e012489ef 73 // and) try the next address. */
davidr99 0:ab4e012489ef 74
davidr99 0:ab4e012489ef 75 for (rp = result; rp != NULL; rp = rp->ai_next)
davidr99 0:ab4e012489ef 76 {
davidr99 0:ab4e012489ef 77 _socket = socket(rp->ai_family, rp->ai_socktype, rp->ai_protocol);
davidr99 0:ab4e012489ef 78 if (_socket == -1)
davidr99 0:ab4e012489ef 79 continue;
davidr99 0:ab4e012489ef 80
davidr99 0:ab4e012489ef 81 if (connect(_socket, rp->ai_addr, rp->ai_addrlen) == 0)
davidr99 0:ab4e012489ef 82 break; /* Success */
davidr99 0:ab4e012489ef 83
davidr99 0:ab4e012489ef 84 close(_socket);
davidr99 0:ab4e012489ef 85 }
davidr99 0:ab4e012489ef 86
davidr99 0:ab4e012489ef 87 if (rp == NULL)
davidr99 0:ab4e012489ef 88 { /* No address succeeded */
davidr99 0:ab4e012489ef 89 fprintf(stderr, "RH_TCP::connect could not connect to %s\n", _server);
davidr99 0:ab4e012489ef 90 return false;
davidr99 0:ab4e012489ef 91 }
davidr99 0:ab4e012489ef 92
davidr99 0:ab4e012489ef 93 freeaddrinfo(result); /* No longer needed */
davidr99 0:ab4e012489ef 94
davidr99 0:ab4e012489ef 95 // Now make the socket non-blocking
davidr99 0:ab4e012489ef 96 int on = 1;
davidr99 0:ab4e012489ef 97 int rc = ioctl(_socket, FIONBIO, (char *)&on);
davidr99 0:ab4e012489ef 98 if (rc < 0)
davidr99 0:ab4e012489ef 99 {
davidr99 0:ab4e012489ef 100 fprintf(stderr,"RH_TCP::init failed to set socket non-blocking: %s\n", strerror(errno));
davidr99 0:ab4e012489ef 101 close(_socket);
davidr99 0:ab4e012489ef 102 _socket = -1;
davidr99 0:ab4e012489ef 103 return false;
davidr99 0:ab4e012489ef 104 }
davidr99 0:ab4e012489ef 105 return true;
davidr99 0:ab4e012489ef 106 }
davidr99 0:ab4e012489ef 107
davidr99 0:ab4e012489ef 108 void RH_TCP::clearRxBuf()
davidr99 0:ab4e012489ef 109 {
davidr99 0:ab4e012489ef 110 _rxBufValid = false;
davidr99 0:ab4e012489ef 111 _rxBufLen = 0;
davidr99 0:ab4e012489ef 112 }
davidr99 0:ab4e012489ef 113
davidr99 0:ab4e012489ef 114 void RH_TCP::checkForEvents()
davidr99 0:ab4e012489ef 115 {
davidr99 0:ab4e012489ef 116 #define RH_TCP_SOCKETBUF_LEN 500
davidr99 0:ab4e012489ef 117 static uint8_t socketBuf[RH_TCP_SOCKETBUF_LEN]; // Room for several messages
davidr99 0:ab4e012489ef 118 static uint16_t socketBufLen = 0;
davidr99 0:ab4e012489ef 119
davidr99 0:ab4e012489ef 120 // Read at most the amount of space we have left in the buffer
davidr99 0:ab4e012489ef 121 ssize_t count = read(_socket, socketBuf + socketBufLen, sizeof(socketBuf) - socketBufLen);
davidr99 0:ab4e012489ef 122 if (count < 0)
davidr99 0:ab4e012489ef 123 {
davidr99 0:ab4e012489ef 124 if (errno != EAGAIN)
davidr99 0:ab4e012489ef 125 {
davidr99 0:ab4e012489ef 126 fprintf(stderr,"RH_TCP::checkForEvents read error: %s\n", strerror(errno));
davidr99 0:ab4e012489ef 127 exit(1);
davidr99 0:ab4e012489ef 128 }
davidr99 0:ab4e012489ef 129 }
davidr99 0:ab4e012489ef 130 else if (count == 0)
davidr99 0:ab4e012489ef 131 {
davidr99 0:ab4e012489ef 132 // End of file
davidr99 0:ab4e012489ef 133 fprintf(stderr,"RH_TCP::checkForEvents unexpected end of file on read\n");
davidr99 0:ab4e012489ef 134 exit(1);
davidr99 0:ab4e012489ef 135 }
davidr99 0:ab4e012489ef 136 else
davidr99 0:ab4e012489ef 137 {
davidr99 0:ab4e012489ef 138 socketBufLen += count;
davidr99 0:ab4e012489ef 139 while (socketBufLen >= 5)
davidr99 0:ab4e012489ef 140 {
davidr99 0:ab4e012489ef 141 RHTcpTypeMessage* message = ((RHTcpTypeMessage*)socketBuf);
davidr99 0:ab4e012489ef 142 uint32_t len = ntohl(message->length);
davidr99 0:ab4e012489ef 143 uint32_t messageLen = len + sizeof(message->length);
davidr99 0:ab4e012489ef 144 if (len > sizeof(socketBuf) - sizeof(message->length))
davidr99 0:ab4e012489ef 145 {
davidr99 0:ab4e012489ef 146 // Bogus length
davidr99 0:ab4e012489ef 147 fprintf(stderr, "RH_TCP::checkForEvents read ridiculous length: %d. Corrupt message stream? Aborting\n", len);
davidr99 0:ab4e012489ef 148 exit(1);
davidr99 0:ab4e012489ef 149 }
davidr99 0:ab4e012489ef 150 if (socketBufLen >= len + sizeof(message->length))
davidr99 0:ab4e012489ef 151 {
davidr99 0:ab4e012489ef 152 // Got at least all of this message
davidr99 0:ab4e012489ef 153 if (message->type == RH_TCP_MESSAGE_TYPE_PACKET && len >= 5)
davidr99 0:ab4e012489ef 154 {
davidr99 0:ab4e012489ef 155 // REVISIT: need to check if we are actually receiving?
davidr99 0:ab4e012489ef 156 // Its a new packet, extract the headers and payload
davidr99 0:ab4e012489ef 157 RHTcpPacket* packet = ((RHTcpPacket*)socketBuf);
davidr99 0:ab4e012489ef 158 _rxHeaderTo = packet->to;
davidr99 0:ab4e012489ef 159 _rxHeaderFrom = packet->from;
davidr99 0:ab4e012489ef 160 _rxHeaderId = packet->id;
davidr99 0:ab4e012489ef 161 _rxHeaderFlags = packet->flags;
davidr99 0:ab4e012489ef 162 uint32_t payloadLen = len - 5;
davidr99 0:ab4e012489ef 163 if (payloadLen <= sizeof(_rxBuf))
davidr99 0:ab4e012489ef 164 {
davidr99 0:ab4e012489ef 165 // Enough room in our receiver buffer
davidr99 0:ab4e012489ef 166 memcpy(_rxBuf, packet->payload, payloadLen);
davidr99 0:ab4e012489ef 167 _rxBufLen = payloadLen;
davidr99 0:ab4e012489ef 168 _rxBufFull = true;
davidr99 0:ab4e012489ef 169 }
davidr99 0:ab4e012489ef 170 }
davidr99 0:ab4e012489ef 171 // check for other message types here
davidr99 0:ab4e012489ef 172 // Now remove the used message by copying the trailing bytes (maybe start of a new message?)
davidr99 0:ab4e012489ef 173 // to the top of the buffer
davidr99 0:ab4e012489ef 174 memcpy(socketBuf, socketBuf + messageLen, sizeof(socketBuf) - messageLen);
davidr99 0:ab4e012489ef 175 socketBufLen -= messageLen;
davidr99 0:ab4e012489ef 176 }
davidr99 0:ab4e012489ef 177 }
davidr99 0:ab4e012489ef 178 }
davidr99 0:ab4e012489ef 179 }
davidr99 0:ab4e012489ef 180
davidr99 0:ab4e012489ef 181 void RH_TCP::validateRxBuf()
davidr99 0:ab4e012489ef 182 {
davidr99 0:ab4e012489ef 183 // The headers have already been extracted
davidr99 0:ab4e012489ef 184 if (_promiscuous ||
davidr99 0:ab4e012489ef 185 _rxHeaderTo == _thisAddress ||
davidr99 0:ab4e012489ef 186 _rxHeaderTo == RH_BROADCAST_ADDRESS)
davidr99 0:ab4e012489ef 187 {
davidr99 0:ab4e012489ef 188 _rxGood++;
davidr99 0:ab4e012489ef 189 _rxBufValid = true;
davidr99 0:ab4e012489ef 190 }
davidr99 0:ab4e012489ef 191 }
davidr99 0:ab4e012489ef 192
davidr99 0:ab4e012489ef 193 bool RH_TCP::available()
davidr99 0:ab4e012489ef 194 {
davidr99 0:ab4e012489ef 195 if (_socket < 0)
davidr99 0:ab4e012489ef 196 return false;
davidr99 0:ab4e012489ef 197 checkForEvents();
davidr99 0:ab4e012489ef 198 if (_rxBufFull)
davidr99 0:ab4e012489ef 199 {
davidr99 0:ab4e012489ef 200 validateRxBuf();
davidr99 0:ab4e012489ef 201 _rxBufFull= false;
davidr99 0:ab4e012489ef 202 }
davidr99 0:ab4e012489ef 203 return _rxBufValid;
davidr99 0:ab4e012489ef 204 }
davidr99 0:ab4e012489ef 205
davidr99 0:ab4e012489ef 206 // Block until something is available
davidr99 0:ab4e012489ef 207 void RH_TCP::waitAvailable()
davidr99 0:ab4e012489ef 208 {
davidr99 0:ab4e012489ef 209 waitAvailableTimeout(0); // 0 = Wait forever
davidr99 0:ab4e012489ef 210 }
davidr99 0:ab4e012489ef 211
davidr99 0:ab4e012489ef 212 // Block until something is available or timeout expires
davidr99 0:ab4e012489ef 213 bool RH_TCP::waitAvailableTimeout(uint16_t timeout)
davidr99 0:ab4e012489ef 214 {
davidr99 0:ab4e012489ef 215 int max_fd;
davidr99 0:ab4e012489ef 216 fd_set input;
davidr99 0:ab4e012489ef 217 int result;
davidr99 0:ab4e012489ef 218
davidr99 0:ab4e012489ef 219 FD_ZERO(&input);
davidr99 0:ab4e012489ef 220 FD_SET(_socket, &input);
davidr99 0:ab4e012489ef 221 max_fd = _socket + 1;
davidr99 0:ab4e012489ef 222
davidr99 0:ab4e012489ef 223 if (timeout)
davidr99 0:ab4e012489ef 224 {
davidr99 0:ab4e012489ef 225 struct timeval timer;
davidr99 0:ab4e012489ef 226 // Timeout is in milliseconds
davidr99 0:ab4e012489ef 227 timer.tv_sec = timeout / 1000;
davidr99 0:ab4e012489ef 228 timer.tv_usec = (timeout % 1000) * 1000;
davidr99 0:ab4e012489ef 229 result = select(max_fd, &input, NULL, NULL, &timer);
davidr99 0:ab4e012489ef 230 }
davidr99 0:ab4e012489ef 231 else
davidr99 0:ab4e012489ef 232 {
davidr99 0:ab4e012489ef 233 result = select(max_fd, &input, NULL, NULL, NULL);
davidr99 0:ab4e012489ef 234 }
davidr99 0:ab4e012489ef 235 if (result < 0)
davidr99 0:ab4e012489ef 236 fprintf(stderr, "RH_TCP::waitAvailableTimeout: select failed %s\n", strerror(errno));
davidr99 0:ab4e012489ef 237 return result > 0;
davidr99 0:ab4e012489ef 238 }
davidr99 0:ab4e012489ef 239
davidr99 0:ab4e012489ef 240 bool RH_TCP::recv(uint8_t* buf, uint8_t* len)
davidr99 0:ab4e012489ef 241 {
davidr99 0:ab4e012489ef 242 if (!available())
davidr99 0:ab4e012489ef 243 return false;
davidr99 0:ab4e012489ef 244
davidr99 0:ab4e012489ef 245 if (buf && len)
davidr99 0:ab4e012489ef 246 {
davidr99 0:ab4e012489ef 247 if (*len > _rxBufLen)
davidr99 0:ab4e012489ef 248 *len = _rxBufLen;
davidr99 0:ab4e012489ef 249 memcpy(buf, _rxBuf, *len);
davidr99 0:ab4e012489ef 250 }
davidr99 0:ab4e012489ef 251 clearRxBuf();
davidr99 0:ab4e012489ef 252 return true;
davidr99 0:ab4e012489ef 253 }
davidr99 0:ab4e012489ef 254
davidr99 0:ab4e012489ef 255 bool RH_TCP::send(const uint8_t* data, uint8_t len)
davidr99 0:ab4e012489ef 256 {
davidr99 0:ab4e012489ef 257 bool ret = sendPacket(data, len);
davidr99 0:ab4e012489ef 258 delay(10); // Wait for transmit to succeed. REVISIT: depends on length and speed
davidr99 0:ab4e012489ef 259 return ret;
davidr99 0:ab4e012489ef 260 }
davidr99 0:ab4e012489ef 261
davidr99 0:ab4e012489ef 262 uint8_t RH_TCP::maxMessageLength()
davidr99 0:ab4e012489ef 263 {
davidr99 0:ab4e012489ef 264 return RH_TCP_MAX_MESSAGE_LEN;
davidr99 0:ab4e012489ef 265 }
davidr99 0:ab4e012489ef 266
davidr99 0:ab4e012489ef 267 void RH_TCP::setThisAddress(uint8_t address)
davidr99 0:ab4e012489ef 268 {
davidr99 0:ab4e012489ef 269 RHGenericDriver::setThisAddress(address);
davidr99 0:ab4e012489ef 270 sendThisAddress(_thisAddress);
davidr99 0:ab4e012489ef 271 }
davidr99 0:ab4e012489ef 272
davidr99 0:ab4e012489ef 273 bool RH_TCP::sendThisAddress(uint8_t thisAddress)
davidr99 0:ab4e012489ef 274 {
davidr99 0:ab4e012489ef 275 if (_socket < 0)
davidr99 0:ab4e012489ef 276 return false;
davidr99 0:ab4e012489ef 277 RHTcpThisAddress m;
davidr99 0:ab4e012489ef 278 m.length = htonl(2);
davidr99 0:ab4e012489ef 279 m.type = RH_TCP_MESSAGE_TYPE_THISADDRESS;
davidr99 0:ab4e012489ef 280 m.thisAddress = thisAddress;
davidr99 0:ab4e012489ef 281 ssize_t sent = write(_socket, &m, sizeof(m));
davidr99 0:ab4e012489ef 282 return sent > 0;
davidr99 0:ab4e012489ef 283 }
davidr99 0:ab4e012489ef 284
davidr99 0:ab4e012489ef 285 bool RH_TCP::sendPacket(const uint8_t* data, uint8_t len)
davidr99 0:ab4e012489ef 286 {
davidr99 0:ab4e012489ef 287 if (_socket < 0)
davidr99 0:ab4e012489ef 288 return false;
davidr99 0:ab4e012489ef 289 RHTcpPacket m;
davidr99 0:ab4e012489ef 290 m.length = htonl(len + 4);
davidr99 0:ab4e012489ef 291 m.type = RH_TCP_MESSAGE_TYPE_PACKET;
davidr99 0:ab4e012489ef 292 m.to = _txHeaderTo;
davidr99 0:ab4e012489ef 293 m.from = _txHeaderFrom;
davidr99 0:ab4e012489ef 294 m.id = _txHeaderId;
davidr99 0:ab4e012489ef 295 m.flags = _txHeaderFlags;
davidr99 0:ab4e012489ef 296 memcpy(m.payload, data, len);
davidr99 0:ab4e012489ef 297 ssize_t sent = write(_socket, &m, len + 8);
davidr99 0:ab4e012489ef 298 return sent > 0;
davidr99 0:ab4e012489ef 299 }
davidr99 0:ab4e012489ef 300
davidr99 0:ab4e012489ef 301 #endif