iCub-main
ethManager.cpp
Go to the documentation of this file.
1 // -*- Mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
2 
3 
4 /*
5  * Copyright (C) 2017 iCub Facility - Istituto Italiano di Tecnologia
6  * Author: Alberto cardellino, Marco Accame
7  * email: alberto.cardellino@iit.it, marco.accame@iit.it
8  * website: www.robotcub.org
9  * Permission is granted to copy, distribute, and/or modify this program
10  * under the terms of the GNU General Public License, version 2 or any
11  * later version published by the Free Software Foundation.
12  *
13  * A copy of the license can be found at
14  * http://www.robotcub.org/icub/license/gpl.txt
15  *
16  * This program is distributed in the hope that it will be useful, but
17  * WITHOUT ANY WARRANTY; without even the implied warranty of
18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
19  * Public License for more details
20 */
21 
22 
23 // --------------------------------------------------------------------------------------------------------------------
24 // - public interface
25 // --------------------------------------------------------------------------------------------------------------------
26 
27 #include <ethManager.h>
28 
29 
30 
31 // --------------------------------------------------------------------------------------------------------------------
32 // - external dependencies
33 // --------------------------------------------------------------------------------------------------------------------
34 
35 #include <string>
36 #include <ethResource.h>
37 #include <errno.h>
38 
39 #include "EOYtheSystem.h"
40 
41 #include "EOropframe.h"
42 
43 #include <stdexcept> // std::out_of_range
44 #include <yarp/os/Network.h>
45 #include <yarp/os/NetType.h>
46 #include <ace/Time_Value.h>
47 
48 #if defined(__unix__)
49 #include <pthread.h>
50 #include <unistd.h>
51 #endif
52 
53 using namespace yarp::os;
54 using namespace yarp::os::impl;
55 
56 
57 
58 #include <fakeEthResource.h>
59 #include <ethResource.h>
60 #include <ethParser.h>
61 
62 using namespace eth;
63 
64 
65 
66 // --------------------------------------------------------------------------------------------------------------------
67 // - pimpl: private implementation (see scott meyers: item 22 of effective modern c++, item 31 of effective c++
68 // --------------------------------------------------------------------------------------------------------------------
69 
70 
71 
72 // --------------------------------------------------------------------------------------------------------------------
73 // - the class
74 // --------------------------------------------------------------------------------------------------------------------
75 
76 
77 // - class eth::TheEthManager
78 
79 // explicit definition of static member variables: don't remove
80 
81 // marco.accame: std::mutex is in unlocked state after the constructor completes
82 // that is the same behaviour of the former yarp::os::Semaphore initted w/ value 1
83 std::mutex TheEthManager::managerSem {};
84 std::mutex TheEthManager::txSem {};
85 std::mutex TheEthManager::rxSem {};
86 
88 
89 TheEthManager::TheEthManager()
90 {
91  // it is a singleton. the constructor is private.
92  communicationIsInitted = false;
93  UDP_socket = NULL;
94 
95  // the container of ethernet boards: resources and attached interfaces
96  ethBoards = new(eth::EthBoards);
97 
98  // required by embobj system
99  TheEthManager::initEOYsystem();
100 
101  // default address
102  ipv4local.addr = eo_common_ipv4addr(10, 0, 1, 104);
103  ipv4local.port = 12345;
104 
105  // the time of creation according to yarp
106  startUpTime = yarp::os::Time::now();
107 }
108 
109 
110 
112 {
113  delete p;
114 }
115 
116 
117 TheEthManager::~TheEthManager()
118 {
119  yTrace();
120 
121  // Deinitialize feature interface
123 
124  // close communication (tx and rx threads, socket) BUT only if they were created and initted
125  if(isCommunicationInitted())
126  {
127  // for sure we should stop threads tx / rx.
128  // before stopping threads, flush all pkts not yet sent. we wait 250 ms.
129  yarp::os::Time::delay(0.250);
130  stopCommunicationThreads();
131 
132  // then we close and delete the socket
133  lock(true);
134  UDP_socket->close();
135  delete UDP_socket;
136  communicationIsInitted = false;
137 
138 
139  delete sender;
140  delete receiver;
141 
142  lock(false);
143  }
144 
145 
146  lock(true);
147 
148  // remove all ethresource ... we dont need to call lockTXRX() because we are not transmitting now
149  ethBoards->execute(delete_resources, NULL);
150  delete ethBoards;
151 
152  lock(false);
153 
154  handle = NULL;
155 }
156 
157 
158 
159 TheEthManager *TheEthManager::instance()
160 {
161  yTrace();
162  // marco.accame: in here we dont use this->lock() because if object does not already exists, the function does (?) not exist.
163  // much better using the static semaphore instead
164  std::lock_guard<std::mutex> lck(managerSem);
165  if (NULL == handle)
166  {
167  yTrace() << "Calling EthManager Constructor";
168  handle = new TheEthManager();
169  if (NULL == handle)
170  yError() << "While calling EthManager constructor";
171  else
172  feat_Initialise(static_cast<void*>(handle)); // we give the pointer to the feature-interface c module
173  }
174  return handle;
175 }
176 
177 
178 bool TheEthManager::killYourself()
179 {
180  yTrace();
181  delete handle;
182 
183  return true;
184 }
185 
186 
187 // this function is called by the embobj error manager
188 void embOBJerror(eOerrmanErrorType_t errtype, const char *info, eOerrmanCaller_t *caller, const eOerrmanDescriptor_t *des)
189 {
190  const char defobjstr[] = "EO?";
191  const char *eobjstr = (NULL == caller) ? (defobjstr) : (caller->eobjstr);
192 
193  yError() << "embOBJerror(): errtype = " << eo_errman_ErrorStringGet(eo_errman_GetHandle(), errtype) << "from EOobject = " << eobjstr << " w/ message = " << info;
194 
195  if(errtype == eo_errortype_fatal)
196  {
197  yError() << "embOBJerror(): FATAL ERROR: the calling thread shall now be stopped in a forever loop here inside";
198  for(;;);
199  }
200 
201 }
202 
203 void TheEthManager::initEOYsystem(void)
204 {
205  // marco.accame: in here we init the embOBJ system for YARP.
206  eOerrman_cfg_t errmanconfig = {0};
207  errmanconfig.extfn.usr_on_error = embOBJerror;
208  const eOysystem_cfg_t *syscfg = feat_getSYSconfig();
209  const eOmempool_cfg_t *mpoolcfg = NULL; // uses standard mode
210  eoy_sys_Initialise(syscfg, mpoolcfg, &errmanconfig);
211 }
212 
213 
214 
215 
217 {
218  if((NULL == r) || (NULL == p) || (r->isFake()))
219  {
220  return;
221  }
222 
223  TheEthManager *ethman = reinterpret_cast<TheEthManager*>(p);
224 
225 #if 0
226  uint16_t numofbytes = 0;
227  uint16_t numofrops = 0;
228  uint8_t* data2send = NULL;
229  bool transmitthepacket = r->getTXpacket(&data2send, numofbytes, numofrops);
230 
231  if(true == transmitthepacket)
232  {
233  eOipv4addressing_t ipv4addressing;
234  r->getIPv4addressing(ipv4addressing);
235  ethman->sendPacket(data2send, static_cast<size_t>(numofbytes), ipv4addressing);
236  }
237 #else
238 
239  eOipv4addressing_t ipv4addressing;
240  size_t numofbytes = 0;
241  uint16_t numofrops = 0;
242  const void * data2send = r->getUDPtransmit(ipv4addressing, numofbytes, numofrops);
243 
244  if(nullptr != data2send)
245  {
246  ethman->sendPacket(data2send, numofbytes, ipv4addressing);
247  }
248 
249 #endif
250 }
251 
252 
253 bool TheEthManager::Transmission(void)
254 {
255  lockTX(true);
256 
257  ethBoards->execute(ethEvalTXropframe, this);
258 
259  lockTX(false);
260 
261  return true;
262 }
263 
264 
266 {
267  if((NULL == r) || (NULL == p))
268  {
269  return;
270  }
271  r->Check();
272 }
273 
274 
275 bool TheEthManager::CheckPresence(void)
276 {
277  ethBoards->execute(ethEvalPresence, this);
278  return true;
279 }
280 
281 
282 
283 bool TheEthManager::verifyEthBoardInfo(yarp::os::Searchable &cfgtotal, eOipv4addr_t &boardipv4, string boardipv4string, string boardname)
284 {
286  if(false == eth::parser::read(cfgtotal, bdata))
287  {
288  yError() << "TheEthManager::verifyEthBoardInfo() fails";
289  return false;
290  }
291 
292 
293  boardipv4 = bdata.properties.ipv4addressing.addr;
294  boardipv4string = bdata.properties.ipv4string;
295  boardname = bdata.settings.name;
296 
297  return true;
298 }
299 
300 
301 
302 
303 bool TheEthManager::initCommunication(yarp::os::Searchable &cfgtotal)
304 {
305  eth::parser::pc104Data pc104data;
306  if(false == eth::parser::read(cfgtotal, pc104data))
307  {
308  yError() << "TheEthManager::initCommunication() fails";
309  eth::parser::print(pc104data);
310  return false;
311  }
312 
313  eth::parser::print(pc104data);
314 
315  int txrate = pc104data.txrate;
316  int rxrate = pc104data.rxrate;
317  eOipv4addressing_t tmpaddress = pc104data.localaddressing;
318  embBoardsConnected = pc104data.embBoardsConnected;
319 
320  // localaddress
321  if(false == createCommunicationObjects(tmpaddress, txrate, rxrate) )
322  {
323  yError () << "TheEthManager::initCommunication() cannot create communication objects";
324  return false;
325  }
326 
327  // save local address
328  ipv4local.addr = tmpaddress.addr;
329  ipv4local.port = tmpaddress.port;
330 
331  return true;
332 }
333 
334 
335 
336 eth::AbstractEthResource *TheEthManager::requestResource2(IethResource *interface, yarp::os::Searchable &cfgtotal)
337 {
338  if(false == communicationIsInitted)
339  {
340  yTrace() << "TheEthManager::requestResource2(): we need to init the communication";
341 
342  if(false == initCommunication(cfgtotal))
343  {
344  yError() << "TheEthManager::requestResource2(): cannot init the communication";
345  return NULL;
346  }
347  }
348 
350  if(false == eth::parser::read(cfgtotal, bdata))
351  {
352  yError() << "TheEthManager::requestResource2() fails";
353  return NULL;
354  }
355 
356  eOipv4addr_t ipv4addr = bdata.properties.ipv4addressing.addr;
357 
358  // i want to lock the use of resources managed by ethBoards to avoid that we attempt to use for TX a ethres not completely initted
359 
360  lockTXRX(true);
361 
362  // i do an attempt to get the resource.
363  eth::AbstractEthResource *rr = ethBoards->get_resource(ipv4addr);
364 
365  if(NULL == rr)
366  {
367  // i dont have the resource yet ...
368 
369  if(true == embBoardsConnected)
370  {
371  rr = new eth::EthResource();
372  }
373  else
374  {
375  rr = new eth::FakeEthResource();
376  }
377 
378  if(true == rr->open2(ipv4addr, cfgtotal))
379  {
380  ethBoards->add(rr);
381  }
382  else
383  {
384  yError() << "TheEthManager::requestResource2(): error creating a new ethResource for IP = " << bdata.properties.ipv4string;
385 
386  if(NULL != rr)
387  {
388  delete rr;
389  }
390 
391  rr = NULL;
392  return NULL;
393  }
394 
395  yDebug() << "TheEthManager::requestResource2(): has just succesfully created a new EthResource for board of type" << rr->getProperties().boardtypeString << "with IP = " << bdata.properties.ipv4string;
396  }
397 
398 
399  ethBoards->add(rr, interface);
400 
401 
402  lockTXRX(false);
403 
404  return(rr);
405 }
406 
407 
408 
409 int TheEthManager::releaseResource2(eth::AbstractEthResource* ethresource, IethResource* interface)
410 {
411  int ret = 1; // -1 means that the singleton is not needed anymore. 0 means error
412  if((NULL == ethresource) || (NULL == interface))
413  {
414  yError() << "TheEthManager::releaseResource2(): there is an attempt to release a NULL EthResource or IethResource";
415  return 0;
416  }
417 
418 
419  eth::AbstractEthResource* rr = ethresource;
420 
421  iethresType_t type = interface->type();
422 
423  // but you must change later on with eomn_serv_category_mc or ...
424  eOmn_serv_category_t category = eomn_serv_category_all;
425  switch(type)
426  {
427  case iethres_analogmais:
428  {
429  category = eomn_serv_category_mais;
430  } break;
431 
433  {
434  category = eomn_serv_category_strain;
435  } break;
436 
438  {
439  category = eomn_serv_category_mc;
440  } break;
441 
442  case iethres_skin:
443  {
444  category = eomn_serv_category_skin;
445  } break;
446 
448  {
449  category = eomn_serv_category_none;
450  } break;
451 
453  {
454  category = eomn_serv_category_inertials3;
455  } break;
456 
457  default:
458  {
459  category = eomn_serv_category_none;
460  } break;
461  }
462 
463  // marco.accame on 8 mar 2016: better to stop all services so that all regulars are removed and board immediately
464  // exits the control loop. later on i will remove this line .... when robotInterface stops crashing in exit.
465  // marco.accame on 10 mar 2016: now robotInterface does not crash in exit anymore. see today's commit.
466  // however, for now i keep on stopping all services in the board. i will change it later on if needed.
467 
468  category = eomn_serv_category_all;
469 
470  if(eomn_serv_category_none != category)
471  {
472  bool success = rr->serviceStop(category);
473  success = success;
474  }
475 
476  // at this time, if the serviceStop() is successful [hey, change its interfce to retrieve num of rops in category, in total and run mode and print them]
477  // the ropframe sent now do not contain any regular for the interface anymore, thus we can just removing the interface in list of those assciated
478  // to the resource, without any harm. only thing is: protect ethBoards with a mutex.
479 
480  // now we change internal data structure of ethBoards, thus .. must disable tx and rx
481  lockTXRX(true);
482 
483  // remove the interface
484  ethBoards->rem(rr, type);
485 
486  int remaining = ethBoards->number_of_interfaces(rr);
487  if(0 == remaining)
488  { // remove also the resource
489  rr->close();
490  ethBoards->rem(rr);
491  delete rr;
492  }
493 
494  if(0 == ethBoards->number_of_resources())
495  { // we dont have any more resources
496  ret = -1;
497  }
498 
499  lockTXRX(false);
500 
501 
502  return(ret);
503 }
504 
505 
506 
507 
508 const eOipv4addressing_t& TheEthManager::getLocalIPV4addressing(void)
509 {
510  return(ipv4local);
511 }
512 
513 
514 IethResource* TheEthManager::getInterface(eOipv4addr_t ipv4, eOprotID32_t id32)
515 {
516  IethResource *interfacePointer = ethBoards->get_interface(ipv4, id32);
517 
518  return interfacePointer;
519 }
520 
521 
522 IethResource* TheEthManager::getInterface(eOipv4addr_t ipv4, iethresType_t type)
523 {
524  IethResource *interfacePointer = ethBoards->get_interface(ipv4, type);
525 
526  return interfacePointer;
527 }
528 
529 double TheEthManager::getLifeTime(void)
530 {
531  return(yarp::os::Time::now() - startUpTime);
532 }
533 
534 
535 
536 bool TheEthManager::createCommunicationObjects(const eOipv4addressing_t &localaddress, int txrate, int rxrate)
537 {
538  lock(true);
539 
540  ACE_INET_Addr inetaddr = toaceinet(localaddress);
541 
542  if(!communicationIsInitted)
543  {
544  UDP_socket = new ACE_SOCK_Dgram();
545  if((embBoardsConnected) && (-1 == UDP_socket->open(inetaddr)))
546  {
547  char tmp[64] = {0};
548  inetaddr.addr_to_string(tmp, 64);
549  yError() << "\n/--------------------------------------------------------------------------------------------------------------\\"
550  << "\n| TheEthManager::createCommunicationObjects() is unable to bind to local IP address " << tmp
551  << "\n\\--------------------------------------------------------------------------------------------------------------/";
552  delete UDP_socket;
553  UDP_socket = NULL;
554  communicationIsInitted = false;
555  }
556  else
557  {
558  communicationIsInitted = true;
559  ipv4local = localaddress;
560 
561  if((txrate <= 0) || (txrate > EthSender::EthSenderMaxRate))
562  {
564  }
565  if((rxrate <= 0) | (rxrate > EthReceiver::EthReceiverMaxRate))
566  {
567  rxrate = EthReceiver::EthReceiverDefaultRate;
568  }
569  sender = new eth::EthSender(txrate);
570  receiver = new eth::EthReceiver(rxrate);
571 
572  sender->config(UDP_socket, this);
573  receiver->config(UDP_socket, this);
574 
575  /* Start the threads sending to and receiving messages from the boards.
576  * It will execute the threadInit and pass its return value to the following calls
577  * afterStart to check if they started correctly.
578  */
579  bool ret1, ret2;
580  ret1 = sender->start();
581  ret2 = receiver->start();
582 
583  if(!ret1 || !ret2)
584  {
585  yError() << "TheEthManager::createCommunicationObjects() fails in starting UDP communication threads ethSender / ethReceiver";
586 
587  // stop threads
588  stopCommunicationThreads();
589 
590  delete UDP_socket;
591  communicationIsInitted = false;
592  return false;
593  }
594  else
595  {
596  yTrace() << "TheEthManager::createCommunicationObjects(): both UDP communication threads ethSender / ethReceiver start correctly!";
597 
598  }
599  }
600  }
601 
602  lock(false);
603  return communicationIsInitted;
604 }
605 
606 
607 bool TheEthManager::isCommunicationInitted(void)
608 {
609  yTrace();
610  bool ret;
611  lock(true);
612  ret = communicationIsInitted;
613  lock(false);
614  return ret;
615 }
616 
617 
618 
619 bool TheEthManager::stopCommunicationThreads()
620 {
621  bool ret = true;
622  // Stop method also make a join waiting the thread to exit
623  if(sender->isRunning())
624  {
625  sender->stop();
626  }
627  if(receiver->isRunning())
628  {
629  receiver->stop();
630  }
631  return ret;
632 }
633 
634 
635 
636 int TheEthManager::sendPacket(const void *udpframe, size_t len, const eOipv4addressing_t &toaddressing)
637 {
638  ACE_INET_Addr inetaddr = toaceinet(toaddressing);
639  ssize_t ret = UDP_socket->send(udpframe, len, inetaddr);
640  return ret;
641 }
642 
643 
644 
645 eOipv4addr_t TheEthManager::toipv4addr(const ACE_INET_Addr &aceinetaddr)
646 {
647  ACE_UINT32 a32 = aceinetaddr.get_ip_address();
648  uint8_t ip4 = a32 & 0xff;
649  uint8_t ip3 = (a32 >> 8) & 0xff;
650  uint8_t ip2 = (a32 >> 16) & 0xff;
651  uint8_t ip1 = (a32 >> 24) & 0xff;
652  return eo_common_ipv4addr(ip1, ip2, ip3, ip4);
653 }
654 
655 ACE_INET_Addr TheEthManager::toaceinet(const eOipv4addressing_t &ipv4addressing)
656 {
657  uint8_t ip1, ip2, ip3, ip4;
658  eo_common_ipv4addr_to_decimal(ipv4addressing.addr, &ip1, &ip2, &ip3, &ip4);
659  ACE_UINT32 hostip = (ip1 << 24) | (ip2 << 16) | (ip3 << 8) | (ip4);
660  ACE_INET_Addr myIP((u_short)ipv4addressing.port, hostip);
661 
662  return myIP;
663 }
664 
665 
666 bool TheEthManager::Reception(eOipv4addr_t from, uint64_t* data, ssize_t size)
667 {
668  lockRX(true);
669 
670  eth::AbstractEthResource* r = ethBoards->get_resource(from);
671 
672  if((size >=0) && (NULL != r) && (!r->isFake()))
673  {
674  r->Tick();
675 
676  if(false == r->processRXpacket(data, size))
677  { // cannot give packet to ethresource
678  yError() << "TheEthManager::Reception() cannot give a received packet of size" << size << "to EthResource because EthResource::processRXpacket() returns false.";
679  }
680 
681  }
682  else
683  {
684  // adr.addr_to_string(address, sizeof(address));
685  // yError() << "TheEthManager::Reception cannot get a ethres associated to address" << address;
686  }
687 
688  lockRX(false);
689 
690 
691  return(true);
692 }
693 
694 
695 
696 int TheEthManager::getNumberOfResources(void)
697 {
698  return(ethBoards->number_of_resources());
699 }
700 
701 
702 const string & TheEthManager::getName(eOipv4addr_t ipv4)
703 {
704  return ethBoards->name(ipv4);
705 }
706 
707 
708 eth::AbstractEthResource* TheEthManager::getEthResource(eOipv4addr_t ipv4)
709 {
710  return(ethBoards->get_resource(ipv4));
711 }
712 
713 
714 bool TheEthManager::lock(bool on)
715 {
716  if(on)
717  {
718  managerSem.lock();
719  }
720  else
721  {
722  managerSem.unlock();
723  }
724 
725  return true;
726 }
727 
728 
729 bool TheEthManager::lockTX(bool on)
730 {
731  if(on)
732  {
733  txSem.lock();
734  }
735  else
736  {
737  txSem.unlock();
738  }
739 
740  return true;
741 }
742 
743 
744 bool TheEthManager::lockRX(bool on)
745 {
746  if(on)
747  {
748  rxSem.lock();
749  }
750  else
751  {
752  rxSem.unlock();
753  }
754 
755  return true;
756 }
757 
758 bool TheEthManager::lockTXRX(bool on)
759 {
760  if(on)
761  {
762  txSem.lock();
763  rxSem.lock();
764  }
765  else
766  {
767  rxSem.unlock();
768  txSem.unlock();
769  }
770 
771  return true;
772 }
773 
774 
775 // - end-of-file (leave a blank line after)----------------------------------------------------------------------------
776 
777 
778 
@ data
virtual bool isFake()=0
virtual const Properties & getProperties()=0
virtual const void * getUDPtransmit(eOipv4addressing_t &destination, size_t &sizeofpacket, uint16_t &numofrops)=0
virtual bool close()=0
virtual bool Tick()=0
virtual bool serviceStop(eOmn_serv_category_t category, double timeout=0.500)=0
virtual bool processRXpacket(const void *data, const size_t size)=0
virtual bool Check()=0
virtual bool open2(eOipv4addr_t remIP, yarp::os::Searchable &cfgtotal)=0
@ EthSenderDefaultRate
Definition: ethSender.h:62
virtual iethresType_t type()=0
int sendPacket(const void *udpframe, size_t len, const eOipv4addressing_t &toaddressing)
Definition: ethManager.cpp:636
void embOBJerror(eOerrmanErrorType_t errtype, const char *info, eOerrmanCaller_t *caller, const eOerrmanDescriptor_t *des)
Definition: ethManager.cpp:188
void ethEvalPresence(eth::AbstractEthResource *r, void *p)
Definition: ethManager.cpp:265
void delete_resources(eth::AbstractEthResource *p, void *par)
Definition: ethManager.cpp:111
void ethEvalTXropframe(eth::AbstractEthResource *r, void *p)
Definition: ethManager.cpp:216
const eOysystem_cfg_t * feat_getSYSconfig()
void feat_DeInitialise()
void feat_Initialise(void *handleOfTheEthManager)
bool read(yarp::os::Searchable &cfgtotal, pc104Data &pc104data)
Definition: ethParser.cpp:92
bool print(const pc104Data &pc104data)
Definition: ethParser.cpp:57
iethresType_t
Definition: IethResource.h:62
@ iethres_motioncontrol
Definition: IethResource.h:67
@ iethres_analogmais
Definition: IethResource.h:65
@ iethres_analoginertial3
Definition: IethResource.h:71
@ iethres_skin
Definition: IethResource.h:68
@ iethres_analogstrain
Definition: IethResource.h:66
@ iethres_analogvirtual
Definition: IethResource.h:69
static pcap_t * handle
grid on
Definition: show_eyes_axes.m:5
boardProperties properties
Definition: ethParser.h:87
boardSettings settings
Definition: ethParser.h:88
eOipv4addressing_t ipv4addressing
Definition: ethParser.h:39
eOipv4addressing_t localaddressing
Definition: ethParser.h:101
std::uint16_t rxrate
Definition: ethParser.h:103
std::uint16_t txrate
Definition: ethParser.h:102