LNetworkUdp.cpp

00001 /**************************************************************************************
00002  * Copyright (c) 2016, Tomoaki Yamaguchi
00003  *
00004  * All rights reserved. This program and the accompanying materials
00005  * are made available under the terms of the Eclipse Public License v1.0
00006  * and Eclipse Distribution License v1.0 which accompany this distribution.
00007  *
00008  * The Eclipse Public License is available at
00009  *    http://www.eclipse.org/legal/epl-v10.html
00010  * and the Eclipse Distribution License is available at
00011  *   http://www.eclipse.org/org/documents/edl-v10.php.
00012  *
00013  * Contributors:
00014  *    Tomoaki Yamaguchi - initial API and implementation and/or initial documentation
00015  **************************************************************************************/
00016 
00017 #include <stdio.h>
00018 #include <sys/time.h>
00019 #include <sys/types.h>
00020 #include <sys/socket.h>
00021 #include <sys/stat.h>
00022 #include <unistd.h>
00023 #include <stdlib.h>
00024 #include <string.h>
00025 #include <fcntl.h>
00026 #include <termios.h>
00027 
00028 #include "LNetworkUdp.h"
00029 #include "LTimer.h"
00030 #include "LScreen.h"
00031 
00032 #include "LMqttsnClientApp.h"
00033 
00034 using namespace std;
00035 using namespace linuxAsyncClient;
00036 
00037 extern uint16_t getUint16(const uint8_t* pos);
00038 extern uint32_t getUint32(const uint8_t* pos);
00039 extern LScreen* theScreen;
00040 extern bool theClientMode;
00041 /*=========================================
00042        Class LNetwork
00043  =========================================*/
00044 LNetwork::LNetwork(){
00045     _sleepflg = false;
00046     resetGwAddress();
00047 }
00048 
00049 LNetwork::~LNetwork(){
00050 
00051 }
00052 
00053 int LNetwork::broadcast(const uint8_t* xmitData, uint16_t dataLen){
00054     return LUdpPort::multicast(xmitData, (uint32_t)dataLen);
00055 }
00056 
00057 int  LNetwork::unicast(const uint8_t* xmitData, uint16_t dataLen){
00058     return LUdpPort::unicast(xmitData, dataLen, _gwIpAddress, _gwPortNo);
00059 }
00060 
00061 
00062 uint8_t*  LNetwork::getMessage(int* len){
00063     *len = 0;
00064     if (checkRecvBuf()){
00065         uint16_t recvLen = LUdpPort::recv(_rxDataBuf, MQTTSN_MAX_PACKET_SIZE, false, &_ipAddress, &_portNo);
00066         if(_gwIpAddress && isUnicast() && (_ipAddress != _gwIpAddress) && (_portNo != _gwPortNo)){
00067             return 0;
00068         }
00069 
00070         if(recvLen < 0){
00071             *len = recvLen;
00072             return 0;
00073         }else{
00074             if(_rxDataBuf[0] == 0x01){
00075                 *len = getUint16(_rxDataBuf + 1 );
00076             }else{
00077                 *len = _rxDataBuf[0];
00078             }
00079             //if(recvLen != *len){
00080             //  *len = 0;
00081             //  return 0;
00082             //}else{
00083                 return _rxDataBuf;
00084             //}
00085         }
00086     }
00087     return 0;
00088 }
00089 
00090 void LNetwork::setGwAddress(void){
00091     _gwPortNo = _portNo;
00092     _gwIpAddress = _ipAddress;
00093 }
00094 
00095 void LNetwork::setFixedGwAddress(void){
00096     _gwPortNo = LUdpPort::_gPortNo;
00097     _gwIpAddress = LUdpPort::_gIpAddr;
00098 }
00099 
00100 void LNetwork::resetGwAddress(void){
00101     _gwIpAddress = 0;
00102     _gwPortNo = 0;
00103 }
00104 
00105 
00106 bool LNetwork::initialize(LUdpConfig  config){
00107     return LUdpPort::open(config);
00108 }
00109 
00110 void LNetwork::setSleep(){
00111     _sleepflg = true;
00112 }
00113 
00114 /*=========================================
00115        Class udpStack
00116  =========================================*/
00117 LUdpPort::LUdpPort(){
00118     _disconReq = false;
00119     _sockfdUcast = -1;
00120     _sockfdMcast = -1;
00121     _castStat = 0;
00122 }
00123 
00124 LUdpPort::~LUdpPort(){
00125     close();
00126 }
00127 
00128 
00129 void LUdpPort::close(){
00130     if(_sockfdMcast > 0){
00131         ::close( _sockfdMcast);
00132         _sockfdMcast = -1;
00133     if(_sockfdUcast > 0){
00134         ::close( _sockfdUcast);
00135             _sockfdUcast = -1;
00136         }
00137     }
00138 }
00139 
00140 bool LUdpPort::open(LUdpConfig config){
00141     const int reuse = 1;
00142     char loopch = 1;
00143 
00144     uint8_t sav = config.ipAddress[3];
00145     config.ipAddress[3] = config.ipAddress[0];
00146     config.ipAddress[0] = sav;
00147     sav = config.ipAddress[2];
00148     config.ipAddress[2] = config.ipAddress[1];
00149     config.ipAddress[1] = sav;
00150 
00151     _gPortNo = htons(config.gPortNo);
00152     _gIpAddr = getUint32((const uint8_t*)config.ipAddress);
00153     _uPortNo = htons(config.uPortNo);
00154 
00155     if( _gPortNo == 0 || _gIpAddr == 0 || _uPortNo == 0){
00156         return false;
00157     }
00158 
00159     _sockfdUcast = socket(AF_INET, SOCK_DGRAM, 0);
00160     if (_sockfdUcast < 0){
00161         return false;
00162     }
00163 
00164     setsockopt(_sockfdUcast, SOL_SOCKET, SO_REUSEADDR, &reuse, sizeof(reuse));
00165 
00166     struct sockaddr_in addr;
00167     addr.sin_family = AF_INET;
00168     addr.sin_port = _uPortNo;
00169     addr.sin_addr.s_addr = INADDR_ANY;
00170 
00171     if( ::bind ( _sockfdUcast, (struct sockaddr*)&addr,  sizeof(addr)) <0){
00172         return false;
00173     }
00174 
00175     _sockfdMcast = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
00176     if (_sockfdMcast < 0){
00177         return false;
00178     }
00179 
00180     struct sockaddr_in addrm;
00181     addrm.sin_family = AF_INET;
00182     addrm.sin_port = _gPortNo;
00183     addrm.sin_addr.s_addr = htonl(INADDR_ANY);
00184 
00185     setsockopt(_sockfdMcast, SOL_SOCKET, SO_REUSEADDR, &reuse, sizeof(reuse));
00186 
00187     if( ::bind ( _sockfdMcast, (struct sockaddr*)&addrm,  sizeof(addrm)) <0){
00188         return false;
00189     }
00190 
00191     if(setsockopt(_sockfdMcast, IPPROTO_IP, IP_MULTICAST_LOOP,(char*)&loopch, sizeof(loopch)) <0 ){
00192         D_NWLOG("\033[0m\033[0;31merror IP_MULTICAST_LOOP in UdpPPort::open\033[0m\033[0;37m\n");
00193         DISPLAY("\033[0m\033[0;31m\nerror IP_MULTICAST_LOOP in UdpPPort::open\033[0m\033[0;37m\n");
00194         close();
00195         return false;
00196     }
00197 
00198     ip_mreq mreq;
00199     mreq.imr_interface.s_addr = INADDR_ANY;
00200     mreq.imr_multiaddr.s_addr = _gIpAddr;
00201 
00202     if( setsockopt(_sockfdMcast, IPPROTO_IP, IP_ADD_MEMBERSHIP, &mreq, sizeof(mreq) )< 0){
00203         D_NWLOG("\033[0m\033[0;31merror IP_ADD_MEMBERSHIP in UdpPort::open\033[0m\033[0;37m\n");
00204         DISPLAY("\033[0m\033[0;31m\nerror IP_ADD_MEMBERSHIP in UdpPort::open\033[0m\033[0;37m\n");
00205         close();
00206         return false;
00207     }
00208     return true;
00209 }
00210 
00211 bool LUdpPort::isUnicast(){
00212     return ( _castStat == STAT_UNICAST);
00213 }
00214 
00215 
00216 int LUdpPort::unicast(const uint8_t* buf, uint32_t length, uint32_t ipAddress, uint16_t port  ){
00217     struct sockaddr_in dest;
00218     dest.sin_family = AF_INET;
00219     dest.sin_port = port;
00220     dest.sin_addr.s_addr = ipAddress;
00221 
00222     int status = ::sendto( _sockfdUcast, buf, length, 0, (const sockaddr*)&dest, sizeof(dest) );
00223     if( status < 0){
00224         D_NWLOG("errno == %d in UdpPort::unicast\n", errno);
00225         DISPLAY("errno == %d in UdpPort::unicast\n", errno);
00226     }else{
00227         D_NWLOG("sendto %-15s:%-6u",inet_ntoa(dest.sin_addr),htons(port));
00228         for(uint16_t i = 0; i < length ; i++){
00229             D_NWLOG(" %02x", *(buf + i));
00230         }
00231         D_NWLOG("\n");
00232 
00233         if ( !theClientMode )
00234         {
00235             char sbuf[SCREEN_BUFF_SIZE];
00236             int pos = 0;
00237             sprintf(sbuf,"\033[0;34msendto %-15s:%-6u",inet_ntoa(dest.sin_addr),htons(port));
00238             pos = strlen(sbuf);
00239             for(uint16_t i = 0; i < length ; i++){
00240                 sprintf(sbuf + pos, " %02x", *(buf + i));
00241                 if (strlen(sbuf) > SCREEN_BUFF_SIZE - 20 )  // -20 for Escape sequence
00242                 {
00243                     break;
00244                 }
00245                 pos += 3;
00246             }
00247             sprintf(sbuf + strlen(sbuf), "\033[0;37m\n");
00248             theScreen->display(sbuf);
00249         }
00250     }
00251     return status;
00252 }
00253 
00254 
00255 int LUdpPort::multicast( const uint8_t* buf, uint32_t length ){
00256     struct sockaddr_in dest;
00257     dest.sin_family = AF_INET;
00258     dest.sin_port = _gPortNo;
00259     dest.sin_addr.s_addr = _gIpAddr;
00260 
00261     int status = ::sendto( _sockfdMcast, buf, length, 0, (const sockaddr*)&dest, sizeof(dest) );
00262     if( status < 0){
00263         D_NWLOG("\033[0m\033[0;31merrno == %d in UdpPort::multicast\033[0m\033[0;37m\n", errno);
00264         DISPLAY("\033[0m\033[0;31merrno == %d in UdpPort::multicast\033[0m\033[0;37m\n", errno);
00265         return errno;
00266     }else{
00267         D_NWLOG("sendto %-15s:%-6u",inet_ntoa(dest.sin_addr),htons(_gPortNo));
00268 
00269         for(uint16_t i = 0; i < length ; i++){
00270             D_NWLOG(" %02x", *(buf + i));
00271             DISPLAY(" %02x", *(buf + i));
00272         }
00273         D_NWLOG("\n");
00274 
00275         if ( !theClientMode )
00276         {
00277             char sbuf[SCREEN_BUFF_SIZE];
00278             int pos = 0;
00279             sprintf(sbuf,"\033[0;34msendto %-15s:%-6u",inet_ntoa(dest.sin_addr),htons(_gPortNo));
00280             pos = strlen(sbuf);
00281             for(uint16_t i = 0; i < length ; i++){
00282                 sprintf(sbuf + pos, " %02x", *(buf + i));
00283                 if (strlen(sbuf) > SCREEN_BUFF_SIZE - 20 )
00284                 {
00285                     break;
00286                 }
00287                 pos += 3;
00288             }
00289             sprintf(sbuf + strlen(sbuf), "\033[0;37m\n");
00290             theScreen->display(sbuf);
00291         }
00292         return status;
00293     }
00294 
00295 }
00296 
00297 bool LUdpPort::checkRecvBuf(){
00298     struct timeval timeout;
00299     timeout.tv_sec = 0;
00300     timeout.tv_usec = 50000;    // 50 msec
00301 
00302     uint8_t buf[2];
00303     fd_set recvfds;
00304     int maxSock = 0;
00305 
00306     FD_ZERO(&recvfds);
00307     FD_SET(_sockfdUcast, &recvfds);
00308     FD_SET(_sockfdMcast, &recvfds);
00309 
00310     if(_sockfdMcast > _sockfdUcast){
00311         maxSock = _sockfdMcast;
00312     }else{
00313         maxSock = _sockfdUcast;
00314     }
00315 
00316     select(maxSock + 1, &recvfds, 0, 0, &timeout);
00317 
00318     if(FD_ISSET(_sockfdUcast, &recvfds)){
00319         if( ::recv(_sockfdUcast, buf, 1,  MSG_DONTWAIT | MSG_PEEK) > 0){
00320             _castStat = STAT_UNICAST;
00321             return true;
00322         }
00323     }else if(FD_ISSET(_sockfdMcast, &recvfds)){
00324         if( ::recv(_sockfdMcast, buf, 1,  MSG_DONTWAIT | MSG_PEEK) > 0){
00325             _castStat = STAT_MULTICAST;
00326             return true;
00327         }
00328     }
00329     _castStat = 0;
00330     return false;
00331 }
00332 
00333 int LUdpPort::recv(uint8_t* buf, uint16_t len, bool flg, uint32_t* ipAddressPtr, uint16_t* portPtr){
00334     int flags = flg ? MSG_DONTWAIT : 0;
00335     return recvfrom (buf, len, flags, ipAddressPtr, portPtr );
00336 }
00337 
00338 int LUdpPort::recvfrom (uint8_t* buf, uint16_t length, int flags, uint32_t* ipAddressPtr, uint16_t* portPtr ){
00339     struct sockaddr_in sender;
00340     int status;
00341     socklen_t addrlen = sizeof(sender);
00342     memset(&sender, 0, addrlen);
00343 
00344     if(isUnicast()){
00345         status = ::recvfrom( _sockfdUcast, buf, length, flags, (struct sockaddr*)&sender, &addrlen );
00346     }else if(_castStat == STAT_MULTICAST){
00347         status = ::recvfrom( _sockfdMcast, buf, length, flags, (struct sockaddr*)&sender, &addrlen );
00348     }else{
00349         return 0;
00350     }
00351 
00352     if (status < 0 && errno != EAGAIN)  {
00353         D_NWLOG("\033[0m\033[0;31merrno == %d in UdpPort::recvfrom \033[0m\033[0;37m\n", errno);
00354         DISPLAY("\033[0m\033[0;31merrno == %d in UdpPort::recvfrom \033[0m\033[0;37m\n", errno);
00355     }else if(status > 0){
00356         *ipAddressPtr = sender.sin_addr.s_addr;
00357         *portPtr = sender.sin_port;
00358         D_NWLOG("\nrecved %-15s:%-6u",inet_ntoa(sender.sin_addr), htons(*portPtr));
00359         for(uint16_t i = 0; i < status ; i++){
00360             D_NWLOG(" %02x", *(buf + i));
00361         }
00362         D_NWLOG("\n");
00363 
00364         if ( !theClientMode )
00365         {
00366             char sbuf[SCREEN_BUFF_SIZE];
00367             int pos = 0;
00368             sprintf(sbuf, "\033[0;34mrecved %-15s:%-6u",inet_ntoa(sender.sin_addr), htons(*portPtr));
00369             pos = strlen(sbuf);
00370             for(uint16_t i = 0; i < status ; i++){
00371                 sprintf(sbuf + pos, " %02x", *(buf + i));
00372                 if (strlen(sbuf) > SCREEN_BUFF_SIZE - 20 )
00373                 {
00374                     break;
00375                 }
00376                 pos += 3;
00377             }
00378             sprintf(sbuf + strlen(sbuf), "\033[0;37m\n");
00379             theScreen->display(sbuf);
00380         }
00381         return status;
00382     }else{
00383         return 0;
00384     }
00385     return status;
00386 }
00387 
00388 
00389