source: flair-src/trunk/lib/FlairCore/src/Socket_impl.cpp @ 2

Last change on this file since 2 was 2, checked in by Sanahuja Guillaume, 6 years ago

flaircore

File size: 10.9 KB
Line 
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
28using std::string;
29using namespace flair::core;
30
31Socket_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
40Socket_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
56void 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
162Socket_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
175void 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
209void 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
236ssize_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__
309void* 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
Note: See TracBrowser for help on using the repository browser.