| 1 | // %flair:license{
|
---|
| 2 | // This file is part of the Flair framework distributed under the
|
---|
| 3 | // CECILL-C License, Version 1.0.
|
---|
| 4 | // %flair:license}
|
---|
| 5 | // created: 2013/11/17
|
---|
| 6 | // filename: Socket_impl.cpp
|
---|
| 7 | //
|
---|
| 8 | // author: Guillaume Sanahuja
|
---|
| 9 | // Copyright Heudiasyc UMR UTC/CNRS 7253
|
---|
| 10 | //
|
---|
| 11 | // version: $Id: $
|
---|
| 12 | //
|
---|
| 13 | // purpose: classe pour une Socket_impl
|
---|
| 14 | //
|
---|
| 15 | //
|
---|
| 16 | /*********************************************************************/
|
---|
| 17 | #include "Socket_impl.h"
|
---|
| 18 | #include "Socket.h"
|
---|
| 19 | #include "FrameworkManager.h"
|
---|
| 20 | #include <fcntl.h>
|
---|
| 21 | #include <cstdlib>
|
---|
| 22 | #include <unistd.h>
|
---|
| 23 | #include <string.h>
|
---|
| 24 | #ifdef __XENO__
|
---|
| 25 | #include "config.h"
|
---|
| 26 | #endif
|
---|
| 27 |
|
---|
| 28 | using std::string;
|
---|
| 29 | using namespace flair::core;
|
---|
| 30 |
|
---|
| 31 | Socket_impl::Socket_impl(const Socket* self,string name,uint16_t port)
|
---|
| 32 | {
|
---|
| 33 | this->self=self;
|
---|
| 34 | this->port=port;
|
---|
| 35 | this->address="";
|
---|
| 36 | this->broadcast=false;
|
---|
| 37 | Init();
|
---|
| 38 | }
|
---|
| 39 |
|
---|
| 40 | Socket_impl::Socket_impl(const Socket* self,string name,string address,bool broadcast)
|
---|
| 41 | {
|
---|
| 42 | this->self=self;
|
---|
| 43 | int pos = address.find(":");
|
---|
| 44 | this->address=address.substr (0,pos);
|
---|
| 45 | port=atoi(address.substr(pos+1).c_str());
|
---|
| 46 | this->broadcast=broadcast;
|
---|
| 47 |
|
---|
| 48 | if(pos==0 || address=="")
|
---|
| 49 | {
|
---|
| 50 | self->Err("address %s is not correct\n",address.c_str());
|
---|
| 51 | }
|
---|
| 52 | Init();
|
---|
| 53 |
|
---|
| 54 | }
|
---|
| 55 |
|
---|
| 56 | void Socket_impl::Init(void)
|
---|
| 57 | {
|
---|
| 58 | int yes=1;
|
---|
| 59 |
|
---|
| 60 | fd = socket(AF_INET, SOCK_DGRAM, 0); //UDP
|
---|
| 61 |
|
---|
| 62 | if(broadcast==true)
|
---|
| 63 | {
|
---|
| 64 | if(setsockopt(fd, SOL_SOCKET, SO_BROADCAST, &yes, sizeof(int) )!=0)
|
---|
| 65 | self->Err("Setsockopt error\n");
|
---|
| 66 | }
|
---|
| 67 |
|
---|
| 68 | if(setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, &yes, sizeof(int) )!=0)
|
---|
| 69 | self->Err("Setsockopt error\n");
|
---|
| 70 |
|
---|
| 71 | if(address=="" || broadcast==true)
|
---|
| 72 | {
|
---|
| 73 | sockaddr_in sin = { 0 };
|
---|
| 74 |
|
---|
| 75 | if(broadcast==true)
|
---|
| 76 | {
|
---|
| 77 | struct hostent *hostinfo;
|
---|
| 78 |
|
---|
| 79 | hostinfo = gethostbyname(this->address.c_str());
|
---|
| 80 | if (hostinfo == NULL)
|
---|
| 81 | {
|
---|
| 82 | self->Err("hostinfo error\n");
|
---|
| 83 | }
|
---|
| 84 | sin.sin_addr = *(in_addr *) hostinfo->h_addr;
|
---|
| 85 | }
|
---|
| 86 | else
|
---|
| 87 | {
|
---|
| 88 | sin.sin_addr.s_addr=INADDR_ANY;
|
---|
| 89 | }
|
---|
| 90 |
|
---|
| 91 | sin.sin_port = htons(port);
|
---|
| 92 | sin.sin_family = AF_INET;
|
---|
| 93 | if(bind(fd,(sockaddr *) &sin, sizeof sin) == -1)
|
---|
| 94 | {
|
---|
| 95 | self->Err("bind error\n");
|
---|
| 96 | }
|
---|
| 97 | }
|
---|
| 98 |
|
---|
| 99 |
|
---|
| 100 | #ifdef __XENO__
|
---|
| 101 | string tmp_name;
|
---|
| 102 | int status;
|
---|
| 103 | is_running=true;
|
---|
| 104 |
|
---|
| 105 | //pipe
|
---|
| 106 | tmp_name= getFrameworkManager()->ObjectName() + "-" + self->ObjectName()+ "-pipe";
|
---|
| 107 | //xenomai limitation
|
---|
| 108 | if(tmp_name.size()>31) self->Err("rt_pipe_create error (%s is too long)\n",tmp_name.c_str());
|
---|
| 109 | #ifdef RT_PIPE_SIZE
|
---|
| 110 | status=rt_pipe_create(&pipe, tmp_name.c_str(), P_MINOR_AUTO,RT_PIPE_SIZE);
|
---|
| 111 | #else
|
---|
| 112 | status=rt_pipe_create(&pipe, tmp_name.c_str(), P_MINOR_AUTO,0);
|
---|
| 113 | #endif
|
---|
| 114 | if(status!=0)
|
---|
| 115 | {
|
---|
| 116 | self->Err("rt_pipe_create error (%s)\n",strerror(-status));
|
---|
| 117 | }
|
---|
| 118 |
|
---|
| 119 | //start user side thread
|
---|
| 120 | #ifdef NRT_STACK_SIZE
|
---|
| 121 | // Initialize thread creation attributes
|
---|
| 122 | pthread_attr_t attr;
|
---|
| 123 | if(pthread_attr_init(&attr) != 0)
|
---|
| 124 | {
|
---|
| 125 | self->Err("pthread_attr_init error\n");
|
---|
| 126 | }
|
---|
| 127 | if(pthread_attr_setstacksize(&attr, NRT_STACK_SIZE) != 0)
|
---|
| 128 | {
|
---|
| 129 | self->Err("pthread_attr_setstacksize error\n");
|
---|
| 130 | }
|
---|
| 131 | if(pthread_create(&user_thread, &attr, user, (void*)this) < 0)
|
---|
| 132 | {
|
---|
| 133 | self->Err("pthread_create error\n");
|
---|
| 134 | }
|
---|
| 135 | if(pthread_attr_destroy(&attr) != 0)
|
---|
| 136 | {
|
---|
| 137 | self->Err("pthread_attr_destroy error\n");
|
---|
| 138 | }
|
---|
| 139 | #else //NRT_STACK_SIZE
|
---|
| 140 | if(pthread_create(&user_thread, NULL, user, (void*)this) < 0)
|
---|
| 141 | {
|
---|
| 142 | self->Err("pthread_create error\n");
|
---|
| 143 | }
|
---|
| 144 | #endif //NRT_STACK_SIZE
|
---|
| 145 | #endif //__XENO__
|
---|
| 146 |
|
---|
| 147 | if(address!="")
|
---|
| 148 | {
|
---|
| 149 | struct hostent *hostinfo;
|
---|
| 150 | hostinfo = gethostbyname(address.c_str());
|
---|
| 151 | if (hostinfo == NULL)
|
---|
| 152 | {
|
---|
| 153 | self->Err("gethostbyname\n");
|
---|
| 154 | }
|
---|
| 155 |
|
---|
| 156 | sock_in.sin_addr=*(in_addr *) hostinfo->h_addr;
|
---|
| 157 | sock_in.sin_port = htons(port);
|
---|
| 158 | sock_in.sin_family = AF_INET;
|
---|
| 159 | }
|
---|
| 160 | }
|
---|
| 161 |
|
---|
| 162 | Socket_impl::~Socket_impl()
|
---|
| 163 | {
|
---|
| 164 | #ifdef __XENO__
|
---|
| 165 | is_running=false;
|
---|
| 166 |
|
---|
| 167 | pthread_join(user_thread,NULL);
|
---|
| 168 | int status=rt_pipe_delete(&pipe);
|
---|
| 169 | if(status!=0) self->Err("rt_pipe_delete error (%s)\n",strerror(-status));
|
---|
| 170 |
|
---|
| 171 | #endif
|
---|
| 172 | close(fd);
|
---|
| 173 | }
|
---|
| 174 |
|
---|
| 175 | void Socket_impl::SendMessage(const char* src,size_t src_len)
|
---|
| 176 | {
|
---|
| 177 | ssize_t written;
|
---|
| 178 | string to_send;
|
---|
| 179 |
|
---|
| 180 | if(broadcast==true)
|
---|
| 181 | {
|
---|
| 182 | to_send=getFrameworkManager()->ObjectName()+ ":" + string(src, src_len);
|
---|
| 183 | src_len=to_send.size();
|
---|
| 184 | src=(char*)to_send.c_str();
|
---|
| 185 | }
|
---|
| 186 |
|
---|
| 187 | #ifdef __XENO__
|
---|
| 188 | //Printf("send pipe %s\n",src);
|
---|
| 189 | written=rt_pipe_write(&pipe,src,src_len,P_NORMAL);
|
---|
| 190 |
|
---|
| 191 | if(written<0)
|
---|
| 192 | {
|
---|
| 193 | self->Err("rt_pipe_write error (%s)\n",strerror(-written));
|
---|
| 194 | }
|
---|
| 195 | else if (written != (ssize_t)src_len)
|
---|
| 196 | {
|
---|
| 197 | self->Err("rt_pipe_write error %i/%i\n",written,to_send.size());
|
---|
| 198 | }
|
---|
| 199 | #else
|
---|
| 200 | written=sendto(fd,src,src_len, 0, (struct sockaddr *) &sock_in, sizeof(sock_in));
|
---|
| 201 |
|
---|
| 202 | if(written!=(ssize_t)src_len)
|
---|
| 203 | {
|
---|
| 204 | self->Err("sendto error\n");
|
---|
| 205 | }
|
---|
| 206 | #endif
|
---|
| 207 | }
|
---|
| 208 |
|
---|
| 209 | void Socket_impl::SendMessage(string message)
|
---|
| 210 | {
|
---|
| 211 | ssize_t written;
|
---|
| 212 |
|
---|
| 213 | if(broadcast==true) message=self->Parent()->ObjectName()+ ":" + message;
|
---|
| 214 | //Printf("SendMessage %s\n",message.c_str());
|
---|
| 215 | #ifdef __XENO__
|
---|
| 216 | written=rt_pipe_write(&pipe,message.c_str(),message.size(),P_NORMAL);
|
---|
| 217 |
|
---|
| 218 | if(written<0)
|
---|
| 219 | {
|
---|
| 220 | self->Err("rt_pipe_write error (%s)\n",strerror(-written));
|
---|
| 221 | }
|
---|
| 222 | else if (written != (ssize_t)message.size())
|
---|
| 223 | {
|
---|
| 224 | self->Err("rt_pipe_write error %i/%i\n",written,message.size());
|
---|
| 225 | }
|
---|
| 226 | #else
|
---|
| 227 | written=sendto(fd,message.c_str(),message.size(), 0, (struct sockaddr *) &sock_in, sizeof(sock_in));
|
---|
| 228 | if(written!=(ssize_t)message.size())
|
---|
| 229 | {
|
---|
| 230 | self->Err("sendto error\n");
|
---|
| 231 | }
|
---|
| 232 |
|
---|
| 233 | #endif
|
---|
| 234 | }
|
---|
| 235 |
|
---|
| 236 | ssize_t Socket_impl::RecvMessage(char* msg,size_t msg_len,Time timeout,char* src,size_t* src_len)
|
---|
| 237 | {
|
---|
| 238 | ssize_t nb_read;
|
---|
| 239 | char buffer[128];
|
---|
| 240 | #ifdef __XENO__
|
---|
| 241 | nb_read=rt_pipe_read(&pipe,&buffer,sizeof(buffer),timeout);
|
---|
| 242 | #else
|
---|
| 243 | socklen_t sinsize = sizeof(sock_in);
|
---|
| 244 | struct timeval tv;
|
---|
| 245 |
|
---|
| 246 | if(timeout!=TIME_NONBLOCK) {
|
---|
| 247 | int attr=fcntl(fd, F_GETFL,0);
|
---|
| 248 | fcntl(fd, F_SETFL, attr & (~O_NONBLOCK));
|
---|
| 249 |
|
---|
| 250 | tv.tv_sec = timeout/1000000000;
|
---|
| 251 | timeout=timeout-(timeout/1000000000)*1000000000;
|
---|
| 252 | tv.tv_usec = timeout/1000;
|
---|
| 253 | if (setsockopt(fd, SOL_SOCKET, SO_RCVTIMEO,&tv,sizeof(tv)) < 0) {
|
---|
| 254 | self->Err("setsockopt SO_RCVTIMEO failed\n");
|
---|
| 255 | }
|
---|
| 256 | } else {
|
---|
| 257 | fcntl(fd, F_SETFL, O_NONBLOCK);
|
---|
| 258 | }
|
---|
| 259 |
|
---|
| 260 | if(broadcast==false) {
|
---|
| 261 | nb_read = recvfrom(fd, buffer, sizeof(buffer), 0, (sockaddr *) &sock_in, &sinsize);
|
---|
| 262 | } else {
|
---|
| 263 | nb_read = recvfrom(fd, buffer, sizeof(buffer), 0, NULL,NULL);
|
---|
| 264 | }
|
---|
| 265 | #endif
|
---|
| 266 | if(nb_read<=0) {
|
---|
| 267 | return nb_read;
|
---|
| 268 | } else {
|
---|
| 269 | //printf("%s\n",buffer);
|
---|
| 270 | if(broadcast==true) {
|
---|
| 271 | int index=-1;
|
---|
| 272 | for(int i=0;i<nb_read;i++) {
|
---|
| 273 | if(buffer[i]==':') {
|
---|
| 274 | index=i;
|
---|
| 275 | break;
|
---|
| 276 | }
|
---|
| 277 | }
|
---|
| 278 | if(index<0) {
|
---|
| 279 | self->Warn("Malformed message\n");
|
---|
| 280 | return -1;
|
---|
| 281 | } else if(src_len!=NULL && src!=NULL) {
|
---|
| 282 | if(index+1>(int)(*src_len) && src!=NULL) { //+1 pour inserer un 0)
|
---|
| 283 | self->Warn("insufficent src size\n");
|
---|
| 284 | return -1;
|
---|
| 285 | }
|
---|
| 286 | } else if(nb_read-index-1+1>(int)msg_len) { //+1 pour inserer un 0
|
---|
| 287 | self->Warn("insufficent msg size (%i/%i)\n",nb_read-index-1+1,msg_len);
|
---|
| 288 | return -1;
|
---|
| 289 | }
|
---|
| 290 | if(src!=NULL) {
|
---|
| 291 | memcpy(src,buffer,index);
|
---|
| 292 | src[index]=0;
|
---|
| 293 | }
|
---|
| 294 | memcpy(msg,&buffer[index+1],nb_read-index-1);
|
---|
| 295 | msg[nb_read-index-1]=0;
|
---|
| 296 | return nb_read-index;
|
---|
| 297 | } else {
|
---|
| 298 | if(nb_read>(int)msg_len) {
|
---|
| 299 | self->Warn("insufficent msg size (%i/%i)\n",nb_read,msg_len);
|
---|
| 300 | return -1;
|
---|
| 301 | }
|
---|
| 302 | memcpy(msg,buffer,nb_read);
|
---|
| 303 | return nb_read;
|
---|
| 304 | }
|
---|
| 305 | }
|
---|
| 306 | }
|
---|
| 307 |
|
---|
| 308 | #ifdef __XENO__
|
---|
| 309 | void* Socket_impl::user(void * arg)
|
---|
| 310 | {
|
---|
| 311 | Socket_impl *caller = (Socket_impl*)arg;
|
---|
| 312 | int pipe_fd=-1;
|
---|
| 313 | string devname;
|
---|
| 314 |
|
---|
| 315 | devname= NRT_PIPE_PATH + getFrameworkManager()->ObjectName() + "-" + caller->self->ObjectName()+ "-pipe";
|
---|
| 316 | while(pipe_fd<0)
|
---|
| 317 | {
|
---|
| 318 | pipe_fd = open(devname.c_str(), O_RDWR);
|
---|
| 319 | if (pipe_fd < 0 && errno!=ENOENT) caller->self->Err("open pipe_fd error (%s)\n",strerror(-errno));
|
---|
| 320 | usleep(1000);
|
---|
| 321 | }
|
---|
| 322 |
|
---|
| 323 | while(caller->is_running==true)
|
---|
| 324 | {
|
---|
| 325 | fd_set set;
|
---|
| 326 | struct timeval timeout;
|
---|
| 327 | int rv;
|
---|
| 328 |
|
---|
| 329 | FD_ZERO(&set); // clear the set
|
---|
| 330 | FD_SET(caller->fd, &set); // add our file descriptor to the set
|
---|
| 331 | FD_SET(pipe_fd, &set); // add our file descriptor to the set
|
---|
| 332 |
|
---|
| 333 | timeout.tv_sec = 0;
|
---|
| 334 | timeout.tv_usec = SELECT_TIMEOUT_MS*1000;
|
---|
| 335 | rv = select(FD_SETSIZE, &set, NULL, NULL, &timeout);
|
---|
| 336 |
|
---|
| 337 | if(rv == -1)
|
---|
| 338 | {
|
---|
| 339 | caller->self->Err("select error\n"); // an error occured
|
---|
| 340 | }
|
---|
| 341 | else if(rv == 0)
|
---|
| 342 | {
|
---|
| 343 | //printf("timeout\n");
|
---|
| 344 | }
|
---|
| 345 | else
|
---|
| 346 | {
|
---|
| 347 | ssize_t nb_read,nb_write;
|
---|
| 348 | char buffer[1024];
|
---|
| 349 |
|
---|
| 350 | if(FD_ISSET(caller->fd, &set))
|
---|
| 351 | {
|
---|
| 352 | socklen_t sinsize = sizeof(caller->sock_in);
|
---|
| 353 | if (caller->broadcast==false)
|
---|
| 354 | {
|
---|
| 355 | nb_read = recvfrom(caller->fd, buffer, sizeof(buffer), 0, (sockaddr *) &(caller->sock_in), &sinsize);
|
---|
| 356 | }
|
---|
| 357 | else
|
---|
| 358 | {
|
---|
| 359 | nb_read = recvfrom(caller->fd, buffer, sizeof(buffer), 0, NULL,NULL);
|
---|
| 360 | }
|
---|
| 361 | if(nb_read < 0)
|
---|
| 362 | {
|
---|
| 363 | caller->self->Err("recvfrom error\n");
|
---|
| 364 | }
|
---|
| 365 | //printf("user %s\n",buffer);
|
---|
| 366 | //on ne garde que les messages venant pas de soi meme
|
---|
| 367 | if (caller->broadcast==false || (caller->broadcast==true && getFrameworkManager()->ObjectName().compare(0,getFrameworkManager()->ObjectName().size(),buffer,getFrameworkManager()->ObjectName().size()) != 0))
|
---|
| 368 | {
|
---|
| 369 | nb_write=write(pipe_fd,buffer,nb_read);
|
---|
| 370 | if(nb_write!=nb_read)
|
---|
| 371 | {
|
---|
| 372 | caller->self->Err("write error\n");
|
---|
| 373 | }
|
---|
| 374 | }
|
---|
| 375 | else
|
---|
| 376 | {
|
---|
| 377 | //printf("self %s\n",buffer);
|
---|
| 378 | }
|
---|
| 379 | }
|
---|
| 380 | if(FD_ISSET(pipe_fd, &set))
|
---|
| 381 | {
|
---|
| 382 | nb_read=read(pipe_fd,buffer,sizeof(buffer));
|
---|
| 383 | //printf("read pipe %i %s\n",nb_read,buffer);
|
---|
| 384 | if(nb_read>0)
|
---|
| 385 | {
|
---|
| 386 | //printf("send %s\n",buffer);
|
---|
| 387 | nb_write=sendto(caller->fd,buffer,nb_read, 0, (struct sockaddr *) &(caller->sock_in), sizeof(caller->sock_in));
|
---|
| 388 | if(nb_write!=nb_read)
|
---|
| 389 | {
|
---|
| 390 | caller->self->Err("sendto error\n");
|
---|
| 391 | }
|
---|
| 392 | }
|
---|
| 393 | }
|
---|
| 394 | }
|
---|
| 395 | }
|
---|
| 396 |
|
---|
| 397 | close(pipe_fd);
|
---|
| 398 | pthread_exit(0);
|
---|
| 399 | }
|
---|
| 400 | #endif
|
---|