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
|
---|