Ignore:
Timestamp:
Apr 8, 2016, 3:40:57 PM (5 years ago)
Author:
Bayard Gildas
Message:

sources reformatted with flair-format-dir script

File:
1 edited

Legend:

Unmodified
Added
Removed
  • trunk/lib/FlairCore/src/FrameworkManager_impl.cpp

    r2 r15  
    4343using namespace flair::gui;
    4444
    45 ui_com* FrameworkManager_impl::com=NULL;
    46 FrameworkManager_impl* FrameworkManager_impl::_this=NULL;
     45ui_com *FrameworkManager_impl::com = NULL;
     46FrameworkManager_impl *FrameworkManager_impl::_this = NULL;
    4747
    4848namespace {
     
    5050
    5151#ifdef SIGDEBUG
    52     static const char *reason_str[] = {
    53         "undefined",
    54         "received signal",
    55         "invoked syscall",
    56         "triggered fault",
    57         "affected by priority inversion",
    58          "missing mlockall",
    59         "runaway thread",
    60     };
    61 
    62     void warn_upon_switch(int sig, siginfo_t *si, void *context)     {
    63         unsigned int reason = si->si_value.sival_int;
    64         void *bt[32];
    65         int nentries;
     52static const char *reason_str[] = {
     53    "undefined", "received signal", "invoked syscall", "triggered fault",
     54    "affected by priority inversion", "missing mlockall", "runaway thread",
     55};
     56
     57void warn_upon_switch(int sig, siginfo_t *si, void *context) {
     58  unsigned int reason = si->si_value.sival_int;
     59  void *bt[32];
     60  int nentries;
    6661#ifdef SIGDEBUG_WATCHDOG
    67         printf("\nSIGDEBUG received, reason %d: %s\n", reason,
    68            reason <= SIGDEBUG_WATCHDOG ? reason_str[reason] : "<unknown>");
    69 #endif
    70         // Dump a backtrace of the frame which caused the switch to secondary mode:
    71         nentries = backtrace(bt,sizeof(bt) / sizeof(bt[0]));
    72         backtrace_symbols_fd(bt,nentries,fileno(stdout));
    73     }
    74 #else //SIGDEBUG
    75     void warn_upon_switch(int sig __attribute__((unused))) {
    76         void *bt[32];
    77         int nentries;
    78 
    79         /* Dump a backtrace of the frame which caused the switch to
    80            secondary mode: */
    81         nentries = backtrace(bt,sizeof(bt) / sizeof(bt[0]));
    82         backtrace_symbols_fd(bt,nentries,fileno(stdout));
    83     }
    84 #endif //SIGDEBUG
     62  printf("\nSIGDEBUG received, reason %d: %s\n", reason,
     63         reason <= SIGDEBUG_WATCHDOG ? reason_str[reason] : "<unknown>");
     64#endif
     65  // Dump a backtrace of the frame which caused the switch to secondary mode:
     66  nentries = backtrace(bt, sizeof(bt) / sizeof(bt[0]));
     67  backtrace_symbols_fd(bt, nentries, fileno(stdout));
     68}
     69#else  // SIGDEBUG
     70void warn_upon_switch(int sig __attribute__((unused))) {
     71  void *bt[32];
     72  int nentries;
     73
     74  /* Dump a backtrace of the frame which caused the switch to
     75     secondary mode: */
     76  nentries = backtrace(bt, sizeof(bt) / sizeof(bt[0]));
     77  backtrace_symbols_fd(bt, nentries, fileno(stdout));
     78}
     79#endif // SIGDEBUG
    8580#endif //__XENO__
    86     void seg_fault(int sig __attribute__((unused))) {
    87         void *bt[32];
    88         int nentries;
    89 
    90         printf("Segmentation fault:\n");
    91         /* Dump a backtrace of the frame which caused the segfault: */
    92         nentries = backtrace(bt,sizeof(bt) / sizeof(bt[0]));
    93         backtrace_symbols_fd(bt,nentries,fileno(stdout));
    94 
    95         exit(1);
    96     }
    97 }
    98 
    99 FrameworkManager_impl::FrameworkManager_impl(FrameworkManager *self,string name): Thread(self,"FrameworkManager",FRAMEWORK_TASK_PRIORITY) {
    100     this->self=self;
    101     is_logging=false;
    102     logger_defined=false;
    103     disable_errors=false;
    104     ui_defined=false;
    105     rcv_buf=NULL;
    106     _this=this;
    107 
    108     // Avoids memory swapping for this program
    109     mlockall(MCL_CURRENT|MCL_FUTURE);
    110     struct sigaction sa;
    111 
    112     //catch segfault
    113     signal(SIGSEGV, seg_fault);
    114 
    115     //catch primary->secondary switch
     81void seg_fault(int sig __attribute__((unused))) {
     82  void *bt[32];
     83  int nentries;
     84
     85  printf("Segmentation fault:\n");
     86  /* Dump a backtrace of the frame which caused the segfault: */
     87  nentries = backtrace(bt, sizeof(bt) / sizeof(bt[0]));
     88  backtrace_symbols_fd(bt, nentries, fileno(stdout));
     89
     90  exit(1);
     91}
     92}
     93
     94FrameworkManager_impl::FrameworkManager_impl(FrameworkManager *self,
     95                                             string name)
     96    : Thread(self, "FrameworkManager", FRAMEWORK_TASK_PRIORITY) {
     97  this->self = self;
     98  is_logging = false;
     99  logger_defined = false;
     100  disable_errors = false;
     101  ui_defined = false;
     102  rcv_buf = NULL;
     103  _this = this;
     104
     105  // Avoids memory swapping for this program
     106  mlockall(MCL_CURRENT | MCL_FUTURE);
     107  struct sigaction sa;
     108
     109  // catch segfault
     110  signal(SIGSEGV, seg_fault);
     111
     112// catch primary->secondary switch
    116113#ifdef __XENO__
    117114#ifdef SIGDEBUG
    118     sigemptyset(&sa.sa_mask);
    119     sa.sa_sigaction = warn_upon_switch;
    120     sa.sa_flags = SA_SIGINFO;
    121     sigaction(SIGDEBUG, &sa, NULL);
    122 #else
    123     signal(SIGXCPU, warn_upon_switch);
    124 #endif
    125 
    126     string task_name="Framework_"+name;
    127     int status=rt_task_shadow(NULL,task_name.c_str(),10,0);
    128     if(status!=0) {
    129         self->Err("rt_task_shadow error (%s)\n",strerror(-status));
    130     }
     115  sigemptyset(&sa.sa_mask);
     116  sa.sa_sigaction = warn_upon_switch;
     117  sa.sa_flags = SA_SIGINFO;
     118  sigaction(SIGDEBUG, &sa, NULL);
     119#else
     120  signal(SIGXCPU, warn_upon_switch);
     121#endif
     122
     123  string task_name = "Framework_" + name;
     124  int status = rt_task_shadow(NULL, task_name.c_str(), 10, 0);
     125  if (status != 0) {
     126    self->Err("rt_task_shadow error (%s)\n", strerror(-status));
     127  }
    131128
    132129#endif //__XENO__
     
    134131
    135132void FrameworkManager_impl::ConnectionLost(void) {
    136     Err("connection lost\n");
    137     gcs_watchdog->SafeStop();
    138     connection_lost=true;
     133  Err("connection lost\n");
     134  gcs_watchdog->SafeStop();
     135  connection_lost = true;
    139136}
    140137
    141138FrameworkManager_impl::~FrameworkManager_impl() {
    142     //Printf("destruction FrameworkManager_impl\n");
    143     int status;
    144 
    145     SafeStop();
    146     Join();
    147 
    148     if(rcv_buf!=NULL) free(rcv_buf);
    149 
    150     if(logger_defined==true) {
    151         continuer=false;
    152         (void)pthread_join(log_th,NULL);
    153 
    154         status=DeletePipe(&cmd_pipe);
    155         if(status!=0) {
    156             Err("Error deleting pipe (%s)\n",strerror(-status));
     139  // Printf("destruction FrameworkManager_impl\n");
     140  int status;
     141
     142  SafeStop();
     143  Join();
     144
     145  if (rcv_buf != NULL)
     146    free(rcv_buf);
     147
     148  if (logger_defined == true) {
     149    continuer = false;
     150    (void)pthread_join(log_th, NULL);
     151
     152    status = DeletePipe(&cmd_pipe);
     153    if (status != 0) {
     154      Err("Error deleting pipe (%s)\n", strerror(-status));
     155    }
     156    status = DeletePipe(&data_pipe);
     157    if (status != 0) {
     158      Err("Error deleting pipe (%s)\n", strerror(-status));
     159    }
     160
     161#ifdef __XENO__
     162    status = rt_heap_delete(&log_heap);
     163    if (status != 0) {
     164      Err("rt_heap_delete error (%s)\n", strerror(-status));
     165    }
     166#endif
     167
     168    logs.clear();
     169  }
     170
     171  if (file_doc != NULL)
     172    xmlFreeDoc(file_doc);
     173
     174  if (ui_defined)
     175    delete top_layout;
     176
     177  if (com != NULL) {
     178    delete com;
     179    status = UDT::close(com_sock);
     180    if (status != 0)
     181      printf("Error udt::close %s", UDT::getlasterror().getErrorMessage());
     182
     183    status = UDT::close(file_sock);
     184    if (status != 0)
     185      printf("Error udt::close %s", UDT::getlasterror().getErrorMessage());
     186
     187    SleepMS(200); // a revoir, sinon UDT::cleanup bloque en RT
     188    UDT::cleanup();
     189  }
     190
     191  // Printf("destruction FrameworkManager_impl ok\n");
     192}
     193
     194void FrameworkManager_impl::SetupConnection(string address, uint16_t port,
     195                                            size_t rcv_buf_size) {
     196  UDT::startup();
     197  this->rcv_buf_size = rcv_buf_size;
     198
     199  // socket file_socket, doit être créé en premier, cf station sol
     200  Printf("Connecting to %s:%i\n", address.c_str(), port);
     201  file_sock = GetSocket(address, port);
     202  com_sock = GetSocket(address, port);
     203
     204  // receive buffer allocation
     205  rcv_buf = (char *)malloc(rcv_buf_size);
     206  if (rcv_buf == NULL) {
     207    Err("receive buffer malloc error\n");
     208  }
     209
     210  com = new ui_com(this, com_sock);
     211
     212  // file managment
     213  bool blocking = true;
     214  UDT::setsockopt(file_sock, 0, UDT_SNDSYN, &blocking, sizeof(bool));
     215  UDT::setsockopt(file_sock, 0, UDT_RCVSYN, &blocking, sizeof(bool));
     216
     217  // configure timeout of blocking receive, short timeout to have priority on
     218  // the other socket (see run method)
     219  int timeout = 1; // ms
     220  UDT::setsockopt(file_sock, 0, UDT_RCVTIMEO, &timeout, sizeof(int));
     221
     222  timeout = 100; // ms
     223  UDT::setsockopt(com_sock, 0, UDT_RCVTIMEO, &timeout, sizeof(int));
     224
     225  Start();
     226
     227  // watchdog for connection with ground station
     228  connection_lost = false;
     229  gcs_watchdog = new Watchdog(
     230      this, std::bind(&FrameworkManager_impl::ConnectionLost, this),
     231      (Time)1000000000);
     232  gcs_watchdog->Start();
     233}
     234
     235void FrameworkManager_impl::SetupUserInterface(string xml_file) {
     236  ui_defined = true;
     237  this->xml_file = xml_file;
     238
     239  // top_layout=new Layout(NULL,XML_ROOT_ELEMENT,XML_ROOT_TYPE);
     240  top_layout = new Layout(NULL, self->ObjectName(), XML_ROOT_TYPE);
     241
     242  // xml setup of the main widget
     243  if (xml_file != "") {
     244    xmlNodePtr *file_node = &(((Widget *)(top_layout))->pimpl_->file_node);
     245    file_doc = xmlParseFile(xml_file.c_str());
     246    if (file_doc == NULL) {
     247      self->Warn("XML document not parsed successfully. Creating a new one.\n");
     248      file_doc = xmlNewDoc((xmlChar *)"1.0");
     249      *file_node = xmlNewNode(NULL, (xmlChar *)XML_ROOT_TYPE);
     250      xmlSetProp(*file_node, (xmlChar *)"name",
     251                 (xmlChar *)ObjectName().c_str());
     252      xmlDocSetRootElement(file_doc, *file_node);
     253      // PrintXml();
     254    } else {
     255      *file_node = xmlDocGetRootElement(file_doc);
     256      if (xmlStrcmp((*file_node)->name, (xmlChar *)XML_ROOT_TYPE)) {
     257        self->Warn("%s, no match found in xml file\n", XML_ROOT_TYPE);
     258        *file_node = xmlNewNode(NULL, (xmlChar *)XML_ROOT_TYPE);
     259        xmlSetProp(*file_node, (xmlChar *)"name",
     260                   (xmlChar *)ObjectName().c_str());
     261        xmlDocSetRootElement(file_doc, *file_node);
     262      }
     263    }
     264  } else {
     265    self->Err("xml file not defined\n");
     266  }
     267
     268  // gui
     269  tabwidget =
     270      new TabWidget(top_layout->At(0, 0), XML_MAIN_TABWIDGET, TabWidget::North);
     271  save_button =
     272      new PushButton(top_layout->At(1, 0),
     273                     "save config on target (" + self->ObjectName() + ")");
     274  // load_button=new PushButton(top_layout->At(1,1),"load config on target (" +
     275  // self->ObjectName() + ")");
     276}
     277
     278// in case of RT, this thread switches to secondary mode when calling
     279// com->Receive
     280// it switches back to RT in ProcessXML when mutex are locked
     281void FrameworkManager_impl::Run(void) {
     282  while (!ToBeStopped()) {
     283    ssize_t bytesRead;
     284
     285    bytesRead = com->Receive(rcv_buf, rcv_buf_size);
     286
     287    if (bytesRead == (ssize_t)rcv_buf_size)
     288      Err("FrameworkManager max receive size, augmenter le buffer size!\n");
     289
     290    if (bytesRead > 0) {
     291      // printf("recu %ld, trame %x\n",bytesRead,(uint8_t)rcv_buf[0]);
     292      // rcv_buf[bytesRead-1]=0;//pour affichage
     293      // printf("%s\n",rcv_buf);
     294
     295      switch ((uint8_t)rcv_buf[0]) {
     296      case XML_HEADER: {
     297        xmlDoc *doc;
     298        rcv_buf[bytesRead] = 0;
     299
     300        doc = xmlReadMemory(rcv_buf, (int)bytesRead, "include.xml",
     301                            "ISO-8859-1", 0);
     302        xmlNode *cur_node = NULL;
     303
     304        for (cur_node = xmlDocGetRootElement(doc); cur_node;
     305             cur_node = cur_node->next) {
     306          if (cur_node->type == XML_ELEMENT_NODE) {
     307            if (!xmlStrcmp(cur_node->name, (xmlChar *)XML_ROOT_TYPE)) {
     308#ifdef __XENO__
     309              WarnUponSwitches(
     310                  true); // ProcessXML should not switch to secondary mode
     311#endif
     312              top_layout->Widget::pimpl_->ProcessXML(cur_node->children);
     313#ifdef __XENO__
     314              WarnUponSwitches(false); // other parts of this thread can switch
     315                                       // to secondary mode
     316#endif
     317              break;
     318            }
     319          }
    157320        }
    158         status=DeletePipe(&data_pipe);
    159         if(status!=0) {
    160             Err("Error deleting pipe (%s)\n",strerror(-status));
     321
     322        xmlFreeDoc(doc);
     323
     324        if (save_button->Clicked())
     325          SaveXml();
     326
     327        SaveXmlChange(rcv_buf);
     328
     329        break;
     330      }
     331      case WATCHDOG_HEADER: {
     332        gcs_watchdog->Touch();
     333        break;
     334      }
     335      default:
     336        Err("unknown id: %x\n", (uint8_t)rcv_buf[0]);
     337        break;
     338      }
     339    } else {
     340      if (com->ConnectionLost())
     341        SleepMS(10); // avoid infinite loop in this case
     342    }
     343  }
     344}
     345
     346void FrameworkManager_impl::SaveXml(void) {
     347  if (ui_defined)
     348    xmlSaveFormatFile(xml_file.c_str(), file_doc, 1);
     349}
     350
     351void FrameworkManager_impl::SaveXmlChange(char *buf) {
     352  if (is_logging == true) {
     353    FILE *xml_change;
     354    char filename[256];
     355    Time time = GetTime();
     356
     357    sprintf(filename, "%s/changes_at_%lld.xml", log_path.c_str(), time);
     358
     359    xml_change = fopen(filename, "a");
     360    fprintf(xml_change, "%s", buf);
     361    fclose(xml_change);
     362
     363    sprintf(filename, "changes_at_%lld.xml", time);
     364    xml_changes.push_back(filename);
     365  }
     366}
     367
     368void FrameworkManager_impl::SendFile(string path, string name) {
     369  char *buf, *more_buf;
     370  int size;
     371  filebuf *pbuf;
     372  ssize_t nb_write;
     373  string filename = path + "/" + name;
     374
     375  // open the file
     376  fstream ifs(filename.c_str(), ios::in | ios::binary);
     377  ifs.seekg(0, ios::end);
     378  size = ifs.tellg();
     379  ifs.seekg(0, ios::beg);
     380  pbuf = ifs.rdbuf();
     381
     382  if (size <= 0) {
     383    Err("error opening file %s\n", filename.c_str());
     384    return;
     385  }
     386
     387  buf = (char *)malloc(sizeof(uint8_t) + sizeof(int) + name.size());
     388  if (buf == NULL) {
     389    Err("malloc error, not sending file\n");
     390    return;
     391  }
     392
     393  if (IsBigEndian()) {
     394    buf[0] = FILE_INFO_BIG_ENDIAN;
     395  } else {
     396    buf[0] = FILE_INFO_LITTLE_ENDIAN;
     397  }
     398
     399  memcpy(buf + 1, &size, sizeof(int));
     400  memcpy(buf + 1 + sizeof(int), name.c_str(), name.size());
     401  Printf("sending %s, size: %i\n", filename.c_str(), size);
     402  // send file information
     403  UDT::sendmsg(file_sock, buf, sizeof(uint8_t) + sizeof(int) + name.size(), -1,
     404               true);
     405
     406  more_buf = (char *)realloc((void *)buf, size);
     407  if (more_buf == NULL) {
     408    Err("realloc error, not sending file\n");
     409    free(buf);
     410    return;
     411  } else {
     412    buf = more_buf;
     413  }
     414
     415  pbuf->sgetn(buf, size);
     416  // send the file
     417  nb_write = UDT::sendmsg(file_sock, buf, size, -1, true);
     418
     419  if (nb_write < 0) {
     420    Err("UDT::sendmsg error (%s)\n", UDT::getlasterror().getErrorMessage());
     421  } else if (nb_write != size) {
     422    Err("UDT::sendmsg error, sent %ld/%i\n", nb_write, size);
     423  }
     424
     425  ifs.close();
     426  free(buf);
     427}
     428
     429void FrameworkManager_impl::FinishSending() {
     430  char rm_cmd[256];
     431
     432  // send orignal xml
     433  SendFile(log_path, "setup.xml");
     434  sprintf(rm_cmd, "rm %s/setup.xml", log_path.c_str());
     435  system(rm_cmd);
     436
     437  // send xml changes
     438  for (size_t i = 0; i < xml_changes.size(); i++) {
     439    // Printf("%s\n",xml_changes.at(i).c_str());
     440    SendFile(log_path, xml_changes.at(i).c_str());
     441    sprintf(rm_cmd, "rm %s/%s", log_path.c_str(), xml_changes.at(i).c_str());
     442    system(rm_cmd);
     443  }
     444  xml_changes.clear();
     445
     446  // end notify
     447  char buf = END;
     448  int nb_write = UDT::sendmsg(file_sock, &buf, 1, -1, true);
     449
     450  if (nb_write < 0) {
     451    Err("UDT::sendmsg error (%s)\n", UDT::getlasterror().getErrorMessage());
     452  } else if (nb_write != 1) {
     453    Err("UDT::sendmsg error, sent %i/%i\n", nb_write, 1);
     454  }
     455}
     456
     457UDTSOCKET FrameworkManager_impl::GetSocket(string address, uint16_t port) {
     458  while (1) {
     459    UDTSOCKET new_fd;
     460    new_fd = UDT::socket(AF_INET, SOCK_DGRAM, 0);
     461    if (new_fd == UDT::INVALID_SOCK) {
     462      Err("socket error: %s\n", UDT::getlasterror().getErrorMessage());
     463      return 0;
     464    }
     465
     466    sockaddr_in serv_addr;
     467    serv_addr.sin_family = AF_INET;
     468    serv_addr.sin_port = htons(short(port));
     469
     470    if (inet_pton(AF_INET, address.c_str(), &serv_addr.sin_addr) <= 0) {
     471      Err("incorrect network address\n");
     472      return 0;
     473    }
     474
     475    memset(&(serv_addr.sin_zero), '\0', 8);
     476
     477    if (UDT::ERROR ==
     478        UDT::connect(new_fd, (sockaddr *)&serv_addr, sizeof(serv_addr))) {
     479      // Printf("connect error: %s
     480      // %i\n",UDT::getlasterror().getErrorMessage(),UDT::getlasterror().getErrorCode());
     481      UDT::close(new_fd);
     482      if (UDT::getlasterror().getErrorCode() != 1001 &&
     483          UDT::getlasterror().getErrorCode() != 1002) {
     484        Err("connect error: %s\n", UDT::getlasterror().getErrorMessage());
     485        return 0;
     486      }
     487    } else {
     488      // printf("connected to
     489      // %s:%i\n",inet_ntoa(serv_addr.sin_addr),serv_addr.sin_port);
     490      return new_fd;
     491    }
     492  }
     493}
     494
     495#ifdef __XENO__
     496int FrameworkManager_impl::CreatePipe(RT_PIPE *fd, string name) {
     497  name = self->ObjectName() + "-" + name;
     498  // xenomai limitation
     499  if (name.size() > 31)
     500    self->Err("rt_pipe_create error (%s is too long)\n", name.c_str());
     501// start log writter
     502#ifdef RT_PIPE_SIZE
     503  return rt_pipe_create(fd, name.c_str(), P_MINOR_AUTO, RT_PIPE_SIZE);
     504#else
     505  return rt_pipe_create(fd, name.c_str(), P_MINOR_AUTO, 0);
     506#endif
     507}
     508#else
     509int FrameworkManager_impl::CreatePipe(int (*fd)[2], string name) {
     510  // if(pipe2(fd[0],O_NONBLOCK) == -1)
     511  if (pipe(fd[0]) == -1) {
     512    return errno;
     513  } else {
     514    int attr = fcntl((*fd)[0], F_GETFL, 0);
     515    if (attr == -1) {
     516      return errno;
     517    }
     518    if (fcntl((*fd)[0], F_SETFL, attr | O_NONBLOCK) == -1) {
     519      return errno;
     520    }
     521    attr = fcntl((*fd)[1], F_GETFL, 0);
     522    if (attr == -1) {
     523      return errno;
     524    }
     525    if (fcntl((*fd)[1], F_SETFL, attr | O_NONBLOCK) == -1) {
     526      return errno;
     527    }
     528
     529    return 0;
     530  }
     531}
     532#endif
     533
     534#ifdef __XENO__
     535int FrameworkManager_impl::DeletePipe(RT_PIPE *fd) {
     536  return rt_pipe_delete(fd);
     537}
     538#else
     539int FrameworkManager_impl::DeletePipe(int (*fd)[2]) {
     540  int status1 = close((*fd)[0]);
     541  int status2 = close((*fd)[1]);
     542  if (status1 == 0 && status2 == 0)
     543    return 0;
     544  if (status1 != 0)
     545    return status1;
     546  if (status2 != 0)
     547    return status2;
     548}
     549#endif
     550
     551void FrameworkManager_impl::SetupLogger(string log_path) {
     552  if (logger_defined == true) {
     553    Warn("SetupLogger() was already called.\n");
     554    return;
     555  }
     556
     557  this->log_path = log_path;
     558
     559  int status = CreatePipe(&cmd_pipe, "log_cmd");
     560  if (status != 0) {
     561    Err("Error creating pipe (%s)\n", strerror(-status));
     562    return;
     563  }
     564
     565  status = CreatePipe(&data_pipe, "log_data");
     566  if (status != 0) {
     567    Err("Error creating pipe (%s)\n", strerror(-status));
     568    return;
     569  }
     570
     571#ifdef __XENO__
     572  string tmp_name;
     573  tmp_name = self->ObjectName() + "-log_heap";
     574  status = rt_heap_create(&log_heap, tmp_name.c_str(), LOG_HEAP, H_FIFO);
     575  if (status != 0) {
     576    Err("rt_heap_create error (%s)\n", strerror(-status));
     577    return;
     578  }
     579#endif //__XENO__
     580
     581  continuer = true;
     582
     583#ifdef NRT_STACK_SIZE
     584  // Initialize thread creation attributes
     585  pthread_attr_t attr;
     586  if (pthread_attr_init(&attr) != 0) {
     587    Err("pthread_attr_init error\n");
     588    return;
     589  }
     590
     591  if (pthread_attr_setstacksize(&attr, NRT_STACK_SIZE) != 0) {
     592    Err("pthread_attr_setstacksize error\n");
     593    return;
     594  }
     595
     596  if (pthread_create(&log_th, &attr, write_log_user, (void *)this) < 0)
     597#else
     598  if (pthread_create(&log_th, NULL, write_log_user, (void *)this) < 0)
     599#endif
     600  {
     601    Err("pthread_create error\n");
     602    return;
     603  }
     604#ifdef NRT_STACK_SIZE
     605  if (pthread_attr_destroy(&attr) != 0) {
     606    Err("pthread_attr_destroy error\n");
     607    return;
     608  }
     609#endif
     610
     611  logger_defined = true;
     612}
     613
     614void FrameworkManager_impl::AddDeviceToLog(IODevice *device) {
     615  if (logger_defined == false) {
     616    Err("SetupLogger() was not called, not starting log\n");
     617    return;
     618  }
     619
     620  if (is_logging == false) {
     621    if (device->pimpl_->SetToBeLogged()) {
     622      log_desc_t tmp;
     623      tmp.device = device;
     624      logs.push_back(tmp);
     625    } else {
     626      Warn("not adding it twice\n");
     627    }
     628  } else {
     629    Err("impossible while logging\n");
     630  }
     631}
     632
     633void FrameworkManager_impl::StartLog(void) {
     634  if (logger_defined == false) {
     635    Err("SetupLogger() was not called, not starting log\n");
     636    return;
     637  }
     638
     639  ssize_t written;
     640  size_t nb_err = 0;
     641
     642  if (logs.size() == 0) {
     643    Warn("Not starting log: nothing to log!\n");
     644    return;
     645  }
     646
     647  if (is_logging == false) {
     648    for (size_t i = 0; i < logs.size(); i++) {
     649
     650      logs.at(i).running = true;
     651      logs.at(i).dbtFile = NULL;
     652      logs.at(i).size = logs.at(i).device->pimpl_->LogSize();
     653#ifdef __XENO__
     654      written =
     655          rt_pipe_write(&cmd_pipe, &logs.at(i), sizeof(log_desc_t), P_NORMAL);
     656#else
     657      written = write(cmd_pipe[1], &logs.at(i), sizeof(log_desc_t));
     658#endif
     659
     660      if (written < 0) {
     661        Err("write pipe error (%s)\n", strerror(-written));
     662        nb_err++;
     663        logs.at(i).running = false;
     664      } else if (written != sizeof(log_desc_t)) {
     665        Err("write pipe error %ld/%ld\n", written, sizeof(log_desc_t));
     666        nb_err++;
     667        logs.at(i).running = false;
     668      }
     669    }
     670
     671    if (nb_err != logs.size())
     672      is_logging = true;
     673  } else {
     674    Warn("Already logging\n");
     675  }
     676}
     677
     678void FrameworkManager_impl::StopLog(void) {
     679  ssize_t written;
     680
     681  if (is_logging == true) {
     682    for (size_t i = 0; i < logs.size(); i++) {
     683      logs.at(i).running = false;
     684    }
     685// send only one running false condition, user thread will stop and send all
     686#ifdef __XENO__
     687    written =
     688        rt_pipe_write(&cmd_pipe, &logs.at(0), sizeof(log_desc_t), P_NORMAL);
     689#else
     690    written = write(cmd_pipe[1], &logs.at(0), sizeof(log_desc_t));
     691#endif
     692
     693    if (written < 0) {
     694      Err("write pipe error (%s)\n", strerror(-written));
     695      return;
     696    } else if (written != sizeof(log_desc_t)) {
     697      Err("write pipe error %ld/%ld\n", written, sizeof(log_desc_t));
     698      return;
     699    }
     700
     701    is_logging = false;
     702  } else {
     703    Warn("Not logging\n");
     704  }
     705}
     706
     707char *FrameworkManager_impl::GetBuffer(size_t sz) {
     708// Printf("alloc %i\n",sz);
     709#ifdef __XENO__
     710  void *ptr;
     711  int status = rt_heap_alloc(&log_heap, sz, TM_NONBLOCK, &ptr);
     712  if (status != 0) {
     713    Err("rt_heap_alloc error (%s)\n", strerror(-status));
     714    ptr = NULL;
     715  }
     716  return (char *)ptr;
     717#else
     718  return (char *)malloc(sz);
     719#endif
     720}
     721
     722void FrameworkManager_impl::ReleaseBuffer(char *buf) {
     723#ifdef __XENO__
     724  int status = rt_heap_free(&log_heap, buf);
     725
     726  if (status != 0) {
     727    Err("rt_heap_free error (%s)\n", strerror(-status));
     728  }
     729#else
     730  free(buf);
     731#endif
     732}
     733
     734void FrameworkManager_impl::WriteLog(const char *buf, size_t size) {
     735  ssize_t written;
     736
     737#ifdef __XENO__
     738  written = rt_pipe_write(&data_pipe, buf, size, P_NORMAL);
     739#else
     740  written = write(data_pipe[1], buf, size);
     741#endif
     742
     743  if (written < 0) {
     744    Err("erreur write pipe (%s)\n", strerror(-written));
     745  } else if (written != (ssize_t)size) {
     746    Err("erreur write pipe %ld/%ld\n", written, size);
     747  }
     748}
     749
     750void *FrameworkManager_impl::write_log_user(void *arg) {
     751  int cmd_pipe = -1;
     752  int data_pipe = -1;
     753  string filename;
     754  fd_set set;
     755  struct timeval timeout;
     756  FrameworkManager_impl *caller = (FrameworkManager_impl *)arg;
     757  int rv;
     758
     759  vector<log_desc_t> logs;
     760
     761#ifdef __XENO__
     762  filename = NRT_PIPE_PATH + caller->self->ObjectName() + "-log_cmd";
     763  while (cmd_pipe < 0) {
     764    cmd_pipe = open(filename.c_str(), O_RDWR);
     765    if (cmd_pipe < 0 && errno != ENOENT)
     766      caller->self->Err("open rt_pipe error: %s %i\n", filename.c_str(), errno);
     767    usleep(1000);
     768  }
     769  filename = NRT_PIPE_PATH + caller->self->ObjectName() + "-log_data";
     770  while (data_pipe < 0) {
     771    data_pipe = open(filename.c_str(), O_RDWR);
     772    if (data_pipe < 0 && errno != ENOENT)
     773      caller->self->Err("open rt_pipe error: %s %i\n", filename.c_str(), errno);
     774    usleep(1000);
     775  }
     776#else
     777  cmd_pipe = caller->cmd_pipe[0];
     778  data_pipe = caller->data_pipe[0];
     779#endif
     780
     781  while (caller->continuer == true) {
     782    FD_ZERO(&set);
     783    FD_SET(cmd_pipe, &set);
     784    FD_SET(data_pipe, &set);
     785
     786    timeout.tv_sec = 0;
     787    timeout.tv_usec = SELECT_TIMEOUT_MS * 1000;
     788    rv = select(FD_SETSIZE, &set, NULL, NULL, &timeout);
     789
     790    if (rv == -1) {
     791      caller->Err("select error\n"); // an error accured
     792    } else if (rv == 0) {
     793      // printf("timeout write_log_user %s\n",caller->ObjectName().c_str()); //
     794      // a timeout occured
     795    } else {
     796      if (FD_ISSET(cmd_pipe, &set)) {
     797        log_desc_t tmp;
     798        read(cmd_pipe, &tmp, sizeof(log_desc_t));
     799
     800        if (tmp.running == true) // start logging
     801        {
     802          filename =
     803              caller->log_path + "/" + caller->FileName(tmp.device) + ".dbt";
     804          printf("Creating log file %s (log size %i)\n", filename.c_str(),
     805                 (int)tmp.size);
     806          tmp.dbtFile = inithdFile((char *)filename.c_str(), UAV, tmp.size);
     807          logs.push_back(tmp);
     808
     809          if (logs.size() == 1) {
     810            filename = caller->log_path + "/setup.xml";
     811            xmlSaveFile(filename.c_str(), caller->file_doc);
     812          }
     813        } else // stop logging
     814        {
     815          for (size_t i = 0; i < logs.size(); i++) {
     816            if (logs.at(i).dbtFile != NULL) {
     817              close_hdfile(logs.at(i).dbtFile);
     818
     819              filename = caller->FileName(logs.at(i).device) + ".dbt";
     820              caller->SendFile(caller->log_path, filename);
     821
     822              fstream txt_file;
     823              filename = caller->FileName(logs.at(i).device) + ".txt";
     824              txt_file.open((caller->log_path + "/" + filename).c_str(),
     825                            fstream::out);
     826              txt_file << "1: time (us)\n2: time (ns)\n";
     827              int index = 3;
     828              logs.at(i).device->pimpl_->WriteLogsDescriptors(txt_file, &index);
     829              txt_file.close();
     830
     831              caller->SendFile(caller->log_path, filename);
     832            }
     833          }
     834          // a revoir celui ci est le xml enregistré et pas forcement l'actuel
     835          // if(caller->xml_file!="") caller->SendFile(caller->xml_file);
     836          caller->FinishSending();
     837
     838          logs.clear();
    161839        }
    162 
    163 #ifdef __XENO__
    164         status=rt_heap_delete(&log_heap);
    165         if(status!=0) {
    166             Err("rt_heap_delete error (%s)\n",strerror(-status));
     840      }
     841
     842      if (FD_ISSET(data_pipe, &set)) {
     843        log_header_t header;
     844        read(data_pipe, &header, sizeof(log_header_t));
     845
     846        for (size_t i = 0; i < logs.size(); i++) {
     847          if (logs.at(i).device == header.device) {
     848            char *buf = (char *)malloc(header.size);
     849            read(data_pipe, buf, header.size);
     850            // printf("%s \n",header.device->ObjectName().c_str());
     851            // for(int i=0;i<header.size;i++) printf("%x ",buf[i]);
     852            // printf("\n");
     853            if (logs.at(i).size == header.size) {
     854              if (logs.at(i).dbtFile != NULL) {
     855                write_hdfile(
     856                    logs.at(i).dbtFile, buf, (road_time_t)(header.time / 1000),
     857                    (road_timerange_t)(header.time % 1000), header.size);
     858              } else {
     859                printf("err\n");
     860              }
     861            } else {
     862              caller->self->Err("%s log size is not correct %i/%i\n",
     863                                header.device->ObjectName().c_str(),
     864                                header.size, logs.at(i).size);
     865            }
     866            free(buf);
     867          }
    167868        }
    168 #endif
    169 
    170         logs.clear();
    171     }
    172 
    173     if(file_doc!=NULL) xmlFreeDoc(file_doc);
    174 
    175     if(ui_defined) delete top_layout;
    176 
    177     if(com!=NULL) {
    178         delete com;
    179         status=UDT::close(com_sock);
    180         if(status!=0) printf("Error udt::close %s",UDT::getlasterror().getErrorMessage());
    181 
    182         status=UDT::close(file_sock);
    183         if(status!=0) printf("Error udt::close %s",UDT::getlasterror().getErrorMessage());
    184 
    185         SleepMS(200);//a revoir, sinon UDT::cleanup bloque en RT
    186         UDT::cleanup();
    187     }
    188 
    189     //Printf("destruction FrameworkManager_impl ok\n");
    190 }
    191 
    192 
    193 void FrameworkManager_impl::SetupConnection(string address,uint16_t port,size_t rcv_buf_size) {
    194     UDT::startup();
    195     this->rcv_buf_size=rcv_buf_size;
    196 
    197     //socket file_socket, doit être créé en premier, cf station sol
    198     Printf("Connecting to %s:%i\n",address.c_str(),port);
    199     file_sock=GetSocket(address,port);
    200     com_sock=GetSocket(address,port);
    201 
    202     //receive buffer allocation
    203     rcv_buf=(char*)malloc(rcv_buf_size);
    204     if(rcv_buf==NULL) {
    205         Err("receive buffer malloc error\n");
    206     }
    207 
    208     com=new ui_com(this,com_sock);
    209 
    210     //file managment
    211     bool blocking = true;
    212     UDT::setsockopt(file_sock, 0, UDT_SNDSYN, &blocking, sizeof(bool));
    213     UDT::setsockopt(file_sock, 0, UDT_RCVSYN, &blocking, sizeof(bool));
    214 
    215     //configure timeout of blocking receive, short timeout to have priority on the other socket (see run method)
    216     int timeout=1;//ms
    217     UDT::setsockopt(file_sock, 0, UDT_RCVTIMEO, &timeout, sizeof(int));
    218 
    219     timeout=100;//ms
    220     UDT::setsockopt(com_sock, 0, UDT_RCVTIMEO, &timeout, sizeof(int));
    221 
    222     Start();
    223 
    224     //watchdog for connection with ground station
    225     connection_lost=false;
    226     gcs_watchdog=new Watchdog(this,std::bind(&FrameworkManager_impl::ConnectionLost,this),(Time)1000000000);
    227     gcs_watchdog->Start();
    228 }
    229 
    230 void FrameworkManager_impl::SetupUserInterface(string xml_file) {
    231     ui_defined=true;
    232     this->xml_file=xml_file;
    233 
    234     //top_layout=new Layout(NULL,XML_ROOT_ELEMENT,XML_ROOT_TYPE);
    235     top_layout=new Layout(NULL,self->ObjectName(),XML_ROOT_TYPE);
    236 
    237     //xml setup of the main widget
    238     if(xml_file!="") {
    239         xmlNodePtr* file_node=&(((Widget*)(top_layout))->pimpl_->file_node);
    240         file_doc = xmlParseFile(xml_file.c_str());
    241         if (file_doc == NULL ) {
    242             self->Warn("XML document not parsed successfully. Creating a new one.\n");
    243             file_doc = xmlNewDoc((xmlChar*)"1.0");
    244             *file_node=xmlNewNode(NULL, (xmlChar*)XML_ROOT_TYPE);
    245             xmlSetProp(*file_node,(xmlChar*)"name",(xmlChar*)ObjectName().c_str());
    246             xmlDocSetRootElement(file_doc, *file_node);
    247             //PrintXml();
    248         } else {
    249             *file_node=xmlDocGetRootElement(file_doc);
    250             if(xmlStrcmp((*file_node)->name,(xmlChar*)XML_ROOT_TYPE)) {
    251                 self->Warn("%s, no match found in xml file\n",XML_ROOT_TYPE);
    252                 *file_node=xmlNewNode(NULL, (xmlChar*)XML_ROOT_TYPE);
    253                 xmlSetProp(*file_node,(xmlChar*)"name",(xmlChar*)ObjectName().c_str());
    254                 xmlDocSetRootElement(file_doc, *file_node);
    255             }
    256         }
    257     } else {
    258         self->Err("xml file not defined\n");
    259     }
    260 
    261     //gui
    262     tabwidget=new TabWidget(top_layout->At(0,0),XML_MAIN_TABWIDGET,TabWidget::North);
    263     save_button=new PushButton(top_layout->At(1,0),"save config on target (" + self->ObjectName() + ")");
    264     //load_button=new PushButton(top_layout->At(1,1),"load config on target (" + self->ObjectName() + ")");
    265 }
    266 
    267 //in case of RT, this thread switches to secondary mode when calling com->Receive
    268 //it switches back to RT in ProcessXML when mutex are locked
    269 void FrameworkManager_impl::Run(void)
    270 {
    271     while(!ToBeStopped())
    272     {
    273         ssize_t bytesRead;
    274 
    275         bytesRead=com->Receive(rcv_buf,rcv_buf_size);
    276 
    277         if(bytesRead==(ssize_t)rcv_buf_size) Err("FrameworkManager max receive size, augmenter le buffer size!\n");
    278 
    279         if(bytesRead>0)
    280         {
    281             //printf("recu %ld, trame %x\n",bytesRead,(uint8_t)rcv_buf[0]);
    282             //rcv_buf[bytesRead-1]=0;//pour affichage
    283             //printf("%s\n",rcv_buf);
    284 
    285             switch((uint8_t)rcv_buf[0])
    286             {
    287                 case XML_HEADER:
    288                 {
    289                     xmlDoc *doc;
    290                     rcv_buf[bytesRead]=0;
    291 
    292                     doc = xmlReadMemory(rcv_buf, (int)bytesRead, "include.xml", "ISO-8859-1", 0);
    293                     xmlNode *cur_node = NULL;
    294 
    295                     for (cur_node = xmlDocGetRootElement(doc); cur_node; cur_node = cur_node->next)
    296                     {
    297                         if (cur_node->type == XML_ELEMENT_NODE)
    298                         {
    299                             if(!xmlStrcmp(cur_node->name,(xmlChar*)XML_ROOT_TYPE) )
    300                             {
    301 #ifdef __XENO__
    302                                 WarnUponSwitches(true);//ProcessXML should not switch to secondary mode
    303 #endif
    304                                 top_layout->Widget::pimpl_->ProcessXML(cur_node->children);
    305 #ifdef __XENO__
    306                                 WarnUponSwitches(false);//other parts of this thread can switch to secondary mode
    307 #endif
    308                                 break;
    309                             }
    310                         }
    311                     }
    312 
    313                     xmlFreeDoc(doc);
    314 
    315                     if(save_button->Clicked()) SaveXml();
    316 
    317                     SaveXmlChange(rcv_buf);
    318 
    319                     break;
    320                 }
    321                 case WATCHDOG_HEADER: {
    322                     gcs_watchdog->Touch();
    323                     break;
    324                 }
    325                 default:
    326                     Err("unknown id: %x\n",(uint8_t)rcv_buf[0]);
    327                     break;
    328             }
    329         } else {
    330             if(com->ConnectionLost()) SleepMS(10);//avoid infinite loop in this case
    331         }
    332     }
    333 }
    334 
    335 void FrameworkManager_impl::SaveXml(void)
    336 {
    337     if(ui_defined) xmlSaveFormatFile(xml_file.c_str(),file_doc,1);
    338 }
    339 
    340 void FrameworkManager_impl::SaveXmlChange(char* buf)
    341 {
    342     if(is_logging==true)
    343     {
    344         FILE *xml_change;
    345         char filename[256];
    346         Time time=GetTime();
    347 
    348         sprintf(filename,"%s/changes_at_%lld.xml",log_path.c_str(),time);
    349 
    350         xml_change=fopen(filename,"a");
    351         fprintf(xml_change,"%s",buf);
    352         fclose(xml_change);
    353 
    354         sprintf(filename,"changes_at_%lld.xml",time);
    355         xml_changes.push_back(filename);
    356     }
    357 }
    358 
    359 void FrameworkManager_impl::SendFile(string path,string name)
    360 {
    361     char *buf,*more_buf;
    362     int size;
    363     filebuf *pbuf;
    364     ssize_t nb_write;
    365     string filename=path+ "/" +name;
    366 
    367     // open the file
    368     fstream ifs(filename.c_str(), ios::in | ios::binary);
    369     ifs.seekg(0, ios::end);
    370     size = ifs.tellg();
    371     ifs.seekg(0, ios::beg);
    372     pbuf=ifs.rdbuf();
    373 
    374     if(size<=0)
    375     {
    376         Err("error opening file %s\n",filename.c_str());
    377         return;
    378     }
    379 
    380     buf=(char*)malloc(sizeof(uint8_t)+sizeof(int)+name.size());
    381     if(buf==NULL)
    382     {
    383         Err("malloc error, not sending file\n");
    384         return;
    385     }
    386 
    387     if(IsBigEndian())
    388     {
    389         buf[0]=FILE_INFO_BIG_ENDIAN;
    390     }
    391     else
    392     {
    393         buf[0]=FILE_INFO_LITTLE_ENDIAN;
    394     }
    395 
    396     memcpy(buf+1,&size,sizeof(int));
    397     memcpy(buf+1+sizeof(int),name.c_str(),name.size());
    398     Printf("sending %s, size: %i\n",filename.c_str(),size);
    399     // send file information
    400     UDT::sendmsg(file_sock, buf, sizeof(uint8_t)+sizeof(int)+name.size(), -1,true);
    401 
    402     more_buf=(char*)realloc((void*)buf,size);
    403     if(more_buf==NULL)
    404     {
    405         Err("realloc error, not sending file\n");
    406         free(buf);
    407         return;
    408     }
    409     else
    410     {
    411         buf=more_buf;
    412     }
    413 
    414     pbuf->sgetn (buf,size);
    415     // send the file
    416     nb_write=UDT::sendmsg(file_sock, buf, size,-1,true);
    417 
    418     if(nb_write<0)
    419     {
    420         Err("UDT::sendmsg error (%s)\n",UDT::getlasterror().getErrorMessage());
    421     }
    422     else if (nb_write != size)
    423     {
    424         Err("UDT::sendmsg error, sent %ld/%i\n",nb_write,size);
    425     }
    426 
    427     ifs.close();
    428     free(buf);
    429 }
    430 
    431 void FrameworkManager_impl::FinishSending()
    432 {
    433     char rm_cmd[256];
    434 
    435     //send orignal xml
    436     SendFile(log_path,"setup.xml");
    437     sprintf(rm_cmd,"rm %s/setup.xml",log_path.c_str());
    438     system(rm_cmd);
    439 
    440     //send xml changes
    441     for(size_t i=0;i<xml_changes.size();i++)
    442     {
    443         //Printf("%s\n",xml_changes.at(i).c_str());
    444         SendFile(log_path,xml_changes.at(i).c_str());
    445         sprintf(rm_cmd,"rm %s/%s",log_path.c_str(),xml_changes.at(i).c_str());
    446         system(rm_cmd);
    447     }
    448     xml_changes.clear();
    449 
    450     //end notify
    451     char buf=END;
    452     int nb_write=UDT::sendmsg(file_sock, &buf, 1,-1,true);
    453 
    454     if(nb_write<0)
    455     {
    456         Err("UDT::sendmsg error (%s)\n",UDT::getlasterror().getErrorMessage());
    457     }
    458     else if (nb_write != 1)
    459     {
    460         Err("UDT::sendmsg error, sent %i/%i\n",nb_write,1);
    461     }
    462 }
    463 
    464 UDTSOCKET FrameworkManager_impl::GetSocket(string address,uint16_t port) {
    465     while(1) {
    466         UDTSOCKET new_fd;
    467         new_fd = UDT::socket(AF_INET, SOCK_DGRAM, 0);
    468         if(new_fd==UDT::INVALID_SOCK) {
    469             Err("socket error: %s\n",UDT::getlasterror().getErrorMessage());
    470             return 0;
    471         }
    472 
    473         sockaddr_in serv_addr;
    474         serv_addr.sin_family = AF_INET;
    475         serv_addr.sin_port = htons(short(port));
    476 
    477         if (inet_pton(AF_INET,address.c_str(), &serv_addr.sin_addr) <= 0) {
    478             Err("incorrect network address\n");
    479             return 0;
    480         }
    481 
    482         memset(&(serv_addr.sin_zero), '\0', 8);
    483 
    484         if (UDT::ERROR == UDT::connect(new_fd, (sockaddr*)&serv_addr, sizeof(serv_addr))) {
    485             //Printf("connect error: %s %i\n",UDT::getlasterror().getErrorMessage(),UDT::getlasterror().getErrorCode());
    486             UDT::close(new_fd);
    487             if(UDT::getlasterror().getErrorCode()!=1001 && UDT::getlasterror().getErrorCode()!=1002) {
    488                 Err("connect error: %s\n",UDT::getlasterror().getErrorMessage());
    489                 return 0;
    490             }
    491         } else {
    492             //printf("connected to %s:%i\n",inet_ntoa(serv_addr.sin_addr),serv_addr.sin_port);
    493             return new_fd;
    494         }
    495     }
    496 }
    497 
    498 #ifdef __XENO__
    499 int FrameworkManager_impl::CreatePipe(RT_PIPE* fd,string name)
    500 {
    501     name=self->ObjectName()+ "-" + name;
    502     //xenomai limitation
    503     if(name.size()>31) self->Err("rt_pipe_create error (%s is too long)\n",name.c_str());
    504     //start log writter
    505 #ifdef RT_PIPE_SIZE
    506     return rt_pipe_create(fd, name.c_str(), P_MINOR_AUTO,RT_PIPE_SIZE);
    507 #else
    508     return rt_pipe_create(fd, name.c_str(), P_MINOR_AUTO,0);
    509 #endif
    510 }
    511 #else
    512 int FrameworkManager_impl::CreatePipe(int (*fd)[2],string name)
    513 {
    514     //if(pipe2(fd[0],O_NONBLOCK) == -1)
    515     if(pipe(fd[0]) == -1)
    516     {
    517         return errno;
    518     }
    519     else
    520     {
    521         int attr=fcntl((*fd)[0], F_GETFL,0);
    522         if(attr == -1)
    523         {
    524             return errno;
    525         }
    526         if(fcntl((*fd)[0], F_SETFL, attr|O_NONBLOCK)== -1)
    527         {
    528             return errno;
    529         }
    530         attr=fcntl((*fd)[1], F_GETFL,0);
    531         if(attr == -1)
    532         {
    533             return errno;
    534         }
    535         if(fcntl((*fd)[1], F_SETFL, attr|O_NONBLOCK)== -1)
    536         {
    537             return errno;
    538         }
    539 
    540         return 0;
    541     }
    542 }
    543 #endif
    544 
    545 #ifdef __XENO__
    546 int FrameworkManager_impl::DeletePipe(RT_PIPE* fd)
    547 {
    548     return rt_pipe_delete(fd);
    549 }
    550 #else
    551 int FrameworkManager_impl::DeletePipe(int (*fd)[2])
    552 {
    553     int status1=close((*fd)[0]);
    554     int status2=close((*fd)[1]);
    555     if(status1==0 && status2==0) return 0;
    556     if(status1!=0) return status1;
    557     if(status2!=0) return status2;
    558 }
    559 #endif
    560 
    561 void FrameworkManager_impl::SetupLogger(string log_path) {
    562     if(logger_defined==true) {
    563         Warn("SetupLogger() was already called.\n");
    564         return;
    565     }
    566 
    567     this->log_path=log_path;
    568 
    569     int status=CreatePipe(&cmd_pipe,"log_cmd");
    570     if(status!=0) {
    571         Err("Error creating pipe (%s)\n",strerror(-status));
    572         return;
    573     }
    574 
    575     status=CreatePipe(&data_pipe,"log_data");
    576     if(status!=0) {
    577         Err("Error creating pipe (%s)\n",strerror(-status));
    578         return;
    579     }
    580 
    581 #ifdef __XENO__
    582     string tmp_name;
    583     tmp_name=self->ObjectName() + "-log_heap";
    584     status=rt_heap_create(&log_heap,tmp_name.c_str(),LOG_HEAP,H_FIFO);
    585     if(status!=0) {
    586         Err("rt_heap_create error (%s)\n",strerror(-status));
    587         return;
    588     }
    589 #endif //__XENO__
    590 
    591     continuer=true;
    592 
    593 #ifdef NRT_STACK_SIZE
    594     // Initialize thread creation attributes
    595     pthread_attr_t attr;
    596     if(pthread_attr_init(&attr) != 0) {
    597         Err("pthread_attr_init error\n");
    598         return;
    599     }
    600 
    601     if(pthread_attr_setstacksize(&attr, NRT_STACK_SIZE) != 0) {
    602        Err("pthread_attr_setstacksize error\n");
    603        return;
    604     }
    605 
    606     if(pthread_create(&log_th,  &attr, write_log_user, (void*)this) < 0)
    607 #else
    608     if(pthread_create(&log_th, NULL, write_log_user, (void*)this) < 0)
    609 #endif
    610     {
    611         Err("pthread_create error\n");
    612         return;
    613     }
    614 #ifdef NRT_STACK_SIZE
    615     if(pthread_attr_destroy(&attr) != 0) {
    616         Err("pthread_attr_destroy error\n");
    617         return;
    618     }
    619 #endif
    620 
    621     logger_defined=true;
    622 }
    623 
    624 void FrameworkManager_impl::AddDeviceToLog(IODevice *device)
    625 {
    626     if(logger_defined==false)
    627     {
    628         Err("SetupLogger() was not called, not starting log\n");
    629         return;
    630     }
    631 
    632     if(is_logging==false)
    633     {
    634         if(device->pimpl_->SetToBeLogged()) {
    635             log_desc_t tmp;
    636             tmp.device=device;
    637             logs.push_back(tmp);
    638         } else {
    639             Warn("not adding it twice\n");
    640         }
    641     }
    642     else
    643     {
    644         Err("impossible while logging\n");
    645     }
    646 }
    647 
    648 void FrameworkManager_impl::StartLog(void)
    649 {
    650     if(logger_defined==false)
    651     {
    652         Err("SetupLogger() was not called, not starting log\n");
    653         return;
    654     }
    655 
    656     ssize_t written;
    657     size_t nb_err=0;
    658 
    659     if(logs.size()==0)
    660     {
    661         Warn("Not starting log: nothing to log!\n");
    662         return;
    663     }
    664 
    665     if(is_logging==false)
    666     {
    667         for(size_t i=0;i<logs.size();i++)
    668         {
    669 
    670             logs.at(i).running=true;
    671             logs.at(i).dbtFile=NULL;
    672             logs.at(i).size=logs.at(i).device->pimpl_->LogSize();
    673 #ifdef __XENO__
    674             written=rt_pipe_write(&cmd_pipe,&logs.at(i),sizeof(log_desc_t),P_NORMAL);
    675 #else
    676             written=write(cmd_pipe[1],&logs.at(i),sizeof(log_desc_t));
    677 #endif
    678 
    679             if(written<0)
    680             {
    681                 Err("write pipe error (%s)\n",strerror(-written));
    682                 nb_err++;
    683                 logs.at(i).running=false;
    684             }
    685             else if (written != sizeof(log_desc_t))
    686             {
    687                 Err("write pipe error %ld/%ld\n",written,sizeof(log_desc_t));
    688                 nb_err++;
    689                 logs.at(i).running=false;
    690             }
    691         }
    692 
    693         if(nb_err!=logs.size()) is_logging=true;
    694     }
    695     else
    696     {
    697         Warn("Already logging\n");
    698     }
    699 }
    700 
    701 void FrameworkManager_impl::StopLog(void)
    702 {
    703     ssize_t written;
    704 
    705     if(is_logging==true)
    706     {
    707         for(size_t i=0;i<logs.size();i++)
    708         {
    709             logs.at(i).running=false;
    710         }
    711         //send only one running false condition, user thread will stop and send all
    712 #ifdef __XENO__
    713         written=rt_pipe_write(&cmd_pipe,&logs.at(0),sizeof(log_desc_t),P_NORMAL);
    714 #else
    715         written=write(cmd_pipe[1],&logs.at(0),sizeof(log_desc_t));
    716 #endif
    717 
    718         if(written<0)
    719         {
    720             Err("write pipe error (%s)\n",strerror(-written));
    721             return;
    722         }
    723         else if (written != sizeof(log_desc_t))
    724         {
    725             Err("write pipe error %ld/%ld\n",written,sizeof(log_desc_t));
    726             return;
    727         }
    728 
    729         is_logging=false;
    730     }
    731     else
    732     {
    733         Warn("Not logging\n");
    734     }
    735 }
    736 
    737 char* FrameworkManager_impl::GetBuffer(size_t sz)
    738 {
    739     //Printf("alloc %i\n",sz);
    740 #ifdef __XENO__
    741     void *ptr;
    742     int status=rt_heap_alloc(&log_heap,sz,TM_NONBLOCK ,&ptr);
    743     if(status!=0)
    744     {
    745         Err("rt_heap_alloc error (%s)\n",strerror(-status));
    746         ptr=NULL;
    747     }
    748     return (char*)ptr;
    749 #else
    750     return (char*)malloc(sz);
    751 #endif
    752 }
    753 
    754 void FrameworkManager_impl::ReleaseBuffer(char* buf)
    755 {
    756 #ifdef __XENO__
    757     int status=rt_heap_free(&log_heap,buf);
    758 
    759     if(status!=0)
    760     {
    761         Err("rt_heap_free error (%s)\n",strerror(-status));
    762     }
    763 #else
    764     free(buf);
    765 #endif
    766 }
    767 
    768 void FrameworkManager_impl::WriteLog(const char* buf,size_t size)
    769 {
    770     ssize_t written;
    771 
    772 #ifdef __XENO__
    773     written=rt_pipe_write(&data_pipe,buf,size,P_NORMAL);
    774 #else
    775     written=write(data_pipe[1],buf,size);
    776 #endif
    777 
    778     if(written<0)
    779     {
    780         Err("erreur write pipe (%s)\n",strerror(-written));
    781     }
    782     else if (written != (ssize_t)size)
    783     {
    784         Err("erreur write pipe %ld/%ld\n",written,size);
    785     }
    786 }
    787 
    788 void* FrameworkManager_impl::write_log_user(void * arg)
    789 {
    790     int cmd_pipe=-1;
    791     int data_pipe=-1;
    792     string filename;
    793     fd_set set;
    794     struct timeval timeout;
    795     FrameworkManager_impl *caller = (FrameworkManager_impl*)arg;
    796     int rv;
    797 
    798     vector<log_desc_t> logs;
    799 
    800 #ifdef __XENO__
    801     filename=NRT_PIPE_PATH+ caller->self->ObjectName() + "-log_cmd";
    802     while(cmd_pipe<0)
    803     {
    804         cmd_pipe = open(filename.c_str(), O_RDWR);
    805         if (cmd_pipe < 0 && errno!=ENOENT) caller->self->Err("open rt_pipe error: %s %i\n",filename.c_str(),errno);
    806         usleep(1000);
    807     }
    808     filename=NRT_PIPE_PATH+ caller->self->ObjectName() + "-log_data";
    809     while(data_pipe<0)
    810     {
    811         data_pipe = open(filename.c_str(), O_RDWR);
    812         if (data_pipe < 0 && errno!=ENOENT) caller->self->Err("open rt_pipe error: %s %i\n",filename.c_str(),errno);
    813         usleep(1000);
    814     }
    815 #else
    816     cmd_pipe=caller->cmd_pipe[0];
    817     data_pipe=caller->data_pipe[0];
    818 #endif
    819 
    820     while(caller->continuer==true)
    821     {
    822         FD_ZERO(&set);
    823         FD_SET(cmd_pipe, &set);
    824         FD_SET(data_pipe, &set);
    825 
    826         timeout.tv_sec = 0;
    827         timeout.tv_usec = SELECT_TIMEOUT_MS*1000;
    828         rv = select(FD_SETSIZE, &set, NULL, NULL, &timeout);
    829 
    830         if(rv == -1)
    831         {
    832             caller->Err("select error\n"); // an error accured
    833         }
    834         else if(rv == 0)
    835         {
    836             //printf("timeout write_log_user %s\n",caller->ObjectName().c_str()); // a timeout occured
    837         }
    838         else
    839         {
    840             if(FD_ISSET(cmd_pipe, &set))
    841             {
    842                 log_desc_t tmp;
    843                 read(cmd_pipe,&tmp,sizeof(log_desc_t));
    844 
    845                 if(tmp.running==true)//start logging
    846                 {
    847                     filename= caller->log_path + "/" +caller->FileName(tmp.device)+ ".dbt";
    848                     printf("Creating log file %s (log size %i)\n",filename.c_str(), (int)tmp.size);
    849                     tmp.dbtFile = inithdFile((char*)filename.c_str(),UAV,tmp.size);
    850                     logs.push_back(tmp);
    851 
    852                     if(logs.size()==1)
    853                     {
    854                         filename= caller->log_path + "/setup.xml";
    855                         xmlSaveFile(filename.c_str(),caller->file_doc);
    856                     }
    857                 }
    858                 else//stop logging
    859                 {
    860                     for(size_t i=0;i<logs.size();i++)
    861                     {
    862                         if(logs.at(i).dbtFile!=NULL)
    863                         {
    864                             close_hdfile(logs.at(i).dbtFile);
    865 
    866                             filename= caller->FileName(logs.at(i).device)+ ".dbt";
    867                             caller->SendFile(caller->log_path,filename);
    868 
    869                             fstream txt_file;
    870                             filename= caller->FileName(logs.at(i).device)+ ".txt";
    871                             txt_file.open((caller->log_path+"/"+filename).c_str(),fstream::out);
    872                             txt_file << "1: time (us)\n2: time (ns)\n";
    873                             int index=3;
    874                             logs.at(i).device->pimpl_->WriteLogsDescriptors(txt_file,&index);
    875                             txt_file.close();
    876 
    877                             caller->SendFile(caller->log_path,filename);
    878                         }
    879                     }
    880                     //a revoir celui ci est le xml enregistré et pas forcement l'actuel
    881                     //if(caller->xml_file!="") caller->SendFile(caller->xml_file);
    882                     caller->FinishSending();
    883 
    884                     logs.clear();
    885                 }
    886             }
    887 
    888             if(FD_ISSET(data_pipe, &set))
    889             {
    890                 log_header_t header;
    891                 read(data_pipe,&header,sizeof(log_header_t));
    892 
    893                 for(size_t i=0;i<logs.size();i++)
    894                 {
    895                     if(logs.at(i).device==header.device)
    896                     {
    897                         char* buf=(char*)malloc(header.size);
    898                         read(data_pipe,buf,header.size);
    899 //printf("%s \n",header.device->ObjectName().c_str());
    900 //for(int i=0;i<header.size;i++) printf("%x ",buf[i]);
    901 //printf("\n");
    902                         if(logs.at(i).size==header.size)
    903                         {
    904                             if(logs.at(i).dbtFile!=NULL)
    905                             {
    906                                 write_hdfile(logs.at(i).dbtFile,buf,(road_time_t)(header.time/1000),(road_timerange_t)(header.time%1000),header.size);
    907                             }
    908                             else
    909                             {
    910                                 printf("err\n");
    911                             }
    912                         }
    913                         else
    914                         {
    915                             caller->self->Err("%s log size is not correct %i/%i\n",header.device->ObjectName().c_str(),header.size,logs.at(i).size);
    916                         }
    917                         free(buf);
    918                     }
    919                 }
    920             }
    921         }
    922     }
    923 
    924 #ifdef __XENO__
    925     close(cmd_pipe);
    926     close(data_pipe);
    927 #endif
    928 
    929     pthread_exit(0);
    930 
    931 }
    932 
    933 string FrameworkManager_impl::FileName(IODevice* device)
    934 {
    935     return getFrameworkManager()->ObjectName() + "_"+device->ObjectName();
    936 }
    937 
    938 void FrameworkManager_impl::PrintXml(void) const
    939 {
    940     xmlChar *xmlbuff;
    941     int buffersize;
    942     xmlDocDumpFormatMemory(file_doc, &xmlbuff, &buffersize, 1);
    943     Printf("xml:\n%s\n",xmlbuff);
    944     xmlFree(xmlbuff);
    945 }
     869      }
     870    }
     871  }
     872
     873#ifdef __XENO__
     874  close(cmd_pipe);
     875  close(data_pipe);
     876#endif
     877
     878  pthread_exit(0);
     879}
     880
     881string FrameworkManager_impl::FileName(IODevice *device) {
     882  return getFrameworkManager()->ObjectName() + "_" + device->ObjectName();
     883}
     884
     885void FrameworkManager_impl::PrintXml(void) const {
     886  xmlChar *xmlbuff;
     887  int buffersize;
     888  xmlDocDumpFormatMemory(file_doc, &xmlbuff, &buffersize, 1);
     889  Printf("xml:\n%s\n", xmlbuff);
     890  xmlFree(xmlbuff);
     891}
Note: See TracChangeset for help on using the changeset viewer.