OpcUaCanOpen
CANopen OPC-UA server
SockCanScan.cpp
Go to the documentation of this file.
1 /*
2  * SockCanScan.cpp
3  *
4  * Created on: Jul 21, 2011
5  * Author: vfilimon
6  */
7 #include "SockCanScan.h"
8 #include <errno.h>
9 
10 #include <fstream>
11 #include <iostream>
12 #include <strstream>
13 
14 using namespace std;
15 
17 // Construction/Destruction
18 
19 #include <time.h>
20 #include <stdio.h>
21 #include <string.h>
22 #include <sys/time.h>
23 #include <sys/types.h>
24 #include <sys/stat.h>
25 #include <sys/socket.h>
26 #include <sys/ioctl.h>
27 #include <sys/uio.h>
28 #include <net/if.h>
29 #include "gettimeofday.h"
30 
31 #include <linux/can.h>
32 #include <linux/can/raw.h>
33 #include <linux/can/error.h>
34 
35 #ifndef peak_can
36 #include <libsocketcan.h>
37 #endif
38 
39 extern "C" CCanAccess *getCanbusAccess()
40 {
41 
42  cout << "getCanbusAccess called" << endl;
43  CCanAccess *cc;
44  cc = new CSockCanScan();
45  return cc;
46 }
47 
48 void* CSockCanScan::CanScanControlThread(void *pCanScan)
49 {
50  canMessage cmsg;
51  int timeErr;
52  struct can_frame sockmsg;
53  CSockCanScan *ppcs = reinterpret_cast<CSockCanScan *>(pCanScan);
54  ppcs->run_can = true;
55  int sock = ppcs->sock;
56  int nbytes;
57  int errorFlag=0;
58  int state = 0, ret;
59 
60  usleep(150); // wait to connection
61  struct timeval tv;
62  tv.tv_sec = 0;
63  tv.tv_usec = 300000;
64  setsockopt(sock,SOL_SOCKET,SO_RCVTIMEO,&tv ,sizeof(struct timeval));
65 
66  ppcs->clearErrorMessage(); // clear error flag
67 
68  while (ppcs->run_can) {
69 
70 /* these settings may be modified by recvmsg() */
71 
72  nbytes = read(sock, &sockmsg, sizeof(struct can_frame));
73  if (!ppcs->run_can)
74  break;
75 // printf("Read byte %s = %d\n",ppcs->channelName.c_str(),nbytes);
76  if (nbytes < 0) {
77  if (errno == EAGAIN)
78  continue;
79  perror("read error");
80  printf ("Slowed down readout loop for %s ...\n", ppcs->channelName.c_str());
81 
82  // close socket, FIXME
83  //
84  ret = can_get_state(ppcs->channelName.c_str(), &state);
85 
86 printf("State socket for %s ret=%d state=%d\n", ppcs->channelName.c_str(),ret,state);
87 
88  if ((ret != 0) || (state == CAN_STATE_STOPPED) || (state == CAN_STATE_SLEEPING)) {
89  printf ("Closing socket for %s ...\n", ppcs->channelName.c_str());
90  if (close(sock) < 0) {
91  perror("Socket close error!");
92  }
93  printf ("Socket for %s closed.\n", ppcs->channelName.c_str());
94 
95  // ppcs->waitCanDriver();
96  usleep(15000000);
97 
98  printf ("Opening new socket for %s ...\n", ppcs->channelName.c_str());
99 
100  // FIXME: handle errors
101  //
102  sock = ppcs->openCanPort();
103  ppcs->sock = sock;
104  printf ("Socket for %s open.\n", ppcs->channelName.c_str());
105  }
106 
107  continue;
108  }
109 
110  if (nbytes <(int) sizeof(struct can_frame)) {
111  cout << "read: incomplete CAN frame for " << ppcs->channelName.c_str() << endl;
112  continue;
113  }
114  if (sockmsg.can_id & CAN_ERR_FLAG) {
115  ppcs->sendErrorMessage(&sockmsg);
116  errorFlag = sockmsg.can_id;
117  continue;
118  }
119 
120  if (errorFlag) {
121  ret = can_get_state(ppcs->channelName.c_str(),&state);
122  if (state == 0) {
123  ppcs->clearErrorMessage();
124  errorFlag = 0;
125  }
126  }
127 
128  cmsg.c_id = sockmsg.can_id;
129  timeErr = ioctl(sock,SIOCGSTAMP,&cmsg.c_time);
130  cmsg.c_dlc = sockmsg.can_dlc;
131  memcpy(&cmsg.c_data[0],&sockmsg.data[0],8);
132  ppcs->canMessageCame(cmsg);
133  }
134 
135  // loop finished, close open socket
136  // These is a redundant code
137  if (sock) {
138  if (close(sock) < 0) {
139  perror("Socket close error!");
140  }
141  else printf ("Socket for %s closed.\n", ppcs->channelName.c_str());
142  }
143  fflush(stdout);
144  pthread_exit(NULL);
145  return NULL;
146 }
147 
148 CSockCanScan::~CSockCanScan()
149 {
150  time_t t = time(NULL);
151  struct tm *tm = localtime(&t);
152 
153  run_can = false;
154  pthread_join(m_hCanScanThread,0);
155  t = time(NULL);
156  tm = localtime(&t);
157  // socket will be closed in readout loop
158  //
159 }
160 
161 int CSockCanScan::configureCanboard(const char *name,const char *parameters)
162 {
163  const char *com; // Channel name
164  int delta;
165  com = strchr(name,':');
166  if (com == NULL) {
167 
168  com = name;
169  }
170  else {
171  delta = com - name;
172  com++;
173  }
174 
175  channelName = com;
176  param = parameters;
177  return openCanPort();
178 }
179 
180 int CSockCanScan::openCanPort()
181 {
182  unsigned int tseg1 = 0;
183  unsigned int tseg2 = 0;
184  unsigned int sjw = 0;
185  unsigned int noSamp = 0;
186  unsigned int syncmode = 0;
187  bool paramSpecified = true;
188  struct sockaddr_can addr;
189 
190  memset ((char*)&addr, 0, sizeof addr);
191 
192  struct ifreq ifr;
193 
194  int numPar,err;
195  unsigned int br;
196 
197  if (param.length() > 0) {
198  if (param != "Unspecified") {
199  numPar = sscanf(param.c_str(),"%d %d %d %d %d %d",&br,
200  &tseg1,&tseg2,&sjw,&noSamp,&syncmode);
201  }
202  else paramSpecified = false;
203  }
204 
205  /* Set baud rate to 125 Kbits/second. */
206 
207  if (paramSpecified) {
208  err = can_do_stop(channelName.c_str());
209  if (err < 0) {
210  perror("Cannot stop channel");
211  return err;
212  }
213  err = can_set_bitrate(channelName.c_str(),br);
214  if (err < 0) {
215  perror("Cannot set bit rate");
216  return err;
217  }
218  err = can_do_start(channelName.c_str());
219  if (err < 0) {
220  perror("Cannot start channel");
221  return err;
222  }
223  }
224 
225  sock = socket(PF_CAN, SOCK_RAW, CAN_RAW);
226  if (sock < 0) {
227  // fill out initialisation struct
228  perror("Cannot open the socket");
229  return 0;
230  }
231  memset(&ifr.ifr_name, 0, sizeof(ifr.ifr_name));
232  strncpy(ifr.ifr_name, channelName.c_str(), channelName.length());
233 
234  if (ioctl(sock, SIOCGIFINDEX, &ifr) < 0) {
235  perror("SIOCGIFINDEX");
236  return -1;
237  }
238 
239  can_err_mask_t err_mask = 0x1ff;
240 
241  setsockopt(sock, SOL_CAN_RAW, CAN_RAW_ERR_FILTER,
242  &err_mask, sizeof(err_mask));
243 
244  addr.can_family = AF_CAN;
245  addr.can_ifindex = ifr.ifr_ifindex;
246 
247  if (bind(sock, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
248  perror("bind");
249  return 0;
250  }
251  // Fetch initial state of the port
252  int ret;
253  if (ret = can_get_state(channelName.c_str(), &err))
254  {
255  cout << "can_get_state() failed with error code " << ret << ", it was not possible to obtain initial port state";
256  }
257 
258  if (err == 0)
259  clearErrorMessage();
260  else
261  {
262  timeval now;
263  gettimeofday(&now, 0);
264  canMessageError(err, "Initial port state: error", now);
265  }
266 
267  return sock;
268 }
269 
270 
271 bool CSockCanScan::sendMessage(short cobID, unsigned char len, unsigned char *message)
272 {
273  struct can_frame frame;
274 
275  memset ((char*) &frame, 0, sizeof(frame));
276  ssize_t nbytes;
277  unsigned char *buf = message;
278  frame.can_id = cobID;
279  frame.can_dlc = len;
280  int l, l1;
281  l = len;
282 
283  do {
284  if (l > 8) {
285  l1 = 8; l = l - 8;
286  }
287  else l1 = l;
288  frame.can_dlc = l1;
289  memcpy(frame.data,buf,l1);
290 
291  nbytes = write(sock, &frame, sizeof(struct can_frame));
292  if (nbytes < 0)
293  {
294  perror("while write() on socket");
295  if (errno == ENOBUFS)
296  {
297  printf ("ENOBUFS; waiting a jiffy ...\n");
298  // insert deleted systec buffer
299  /*
300  * struct ifreq ifr;
301  * strncpy(ifr.ifr_name, channelName.c_str(), sizeof(channelName.c_str()));
302  * ioctl(sock,SIODEVPRIVATE,&ifr); // stop sending and clear buffer
303  * ioctl(sock,SIODEVPRIVATE_1,&ifr); // start sebding
304  */
305  usleep(100000);
306  continue;
307  }
308  }
309  if (nbytes < (int)sizeof(struct can_frame)) {
310  return false;
311  }
312  buf = buf + l1;
313  }
314  while (l > 8);
315 
316  return true;
317 }
318 
319 bool CSockCanScan::sendRemoteRequest(short cobID)
320 {
321  struct can_frame frame;
322 
323  memset ((char*) &frame, 0, sizeof(frame));
324  int nbytes;
325  frame.can_id = cobID + CAN_RTR_FLAG ;
326  frame.can_dlc = 0;
327  do {
328  nbytes = write(sock, &frame, sizeof(struct can_frame));
329  if (nbytes < 0)
330  {
331  perror("while write() on socket");
332  if (errno == ENOBUFS)
333  {
334  printf ("ENOBUFS; waiting a jiffy ...\n");
335  // insert deleted systec buffer
336  usleep(100000);
337  continue;
338  }
339  }
340  else {
341  if (nbytes < (int)sizeof(struct can_frame))
342  {
343  cout << "Warning: sendRemoteRequest() sent less bytes than expected" << endl;
344  return false;
345  }
346  }
347  break;
348  }
349  while(true);
350 
351  return true;
352 }
353 
354 bool CSockCanScan::createBUS(const char *name ,const char *parameters)
355 {
356  name_of_port = string(name);
357  cout << "Create bus " << name << " " << parameters << endl;
358  if ((sock = configureCanboard(name,parameters)) < 0) {
359  return false;
360  }
361  cout << "Create main loop" << endl;
362  m_idCanScanThread =
363  pthread_create(&m_hCanScanThread,NULL,&CanScanControlThread,
364  (void *)this);
365 
366  return (!m_idCanScanThread);
367 }
368 
369 void CSockCanScan::sendErrorMessage(const struct can_frame *errFrame)
370 {
371  int timeErr;
372  timeval c_time;
373  char mesbuf[1024];
374  string errorMessage;
375 
376  timeErr = ioctl(sock,SIOCGSTAMP,&c_time);
377  unsigned int errFlag = errFrame->can_id & CAN_ERR_MASK;
378 
379  if (errFlag & CAN_ERR_TX_TIMEOUT)
380  {
381  errorMessage = "TX timeout (by netdevice driver)";
382  canMessageError(errFlag,errorMessage.c_str(),c_time);
383  return;
384  }
385 
386  if ( errFlag & CAN_ERR_LOSTARB )
387  {
388  sprintf(mesbuf,"%s %x","Lost arbitration",errFrame->data[0]);
389  errorMessage = mesbuf;
390  canMessageError(errFlag,errorMessage.c_str(),c_time);
391  return;
392  }
393 
394  if ((errFlag & CAN_ERR_CRTL) && (errFrame->data[1] & CAN_ERR_CRTL_RX_OVERFLOW) )
395  {
396  errorMessage = "RX buffer overflow";
397  canMessageError(errFlag,errorMessage.c_str(),c_time);
398  return;
399  }
400  if ((errFlag & CAN_ERR_CRTL) && (errFrame->data[1] & CAN_ERR_CRTL_TX_OVERFLOW) )
401  {
402  errorMessage = "TX buffer overflow";
403  canMessageError(errFlag,errorMessage.c_str(),c_time);
404  return;
405  }
406  if ((errFlag & CAN_ERR_CRTL) && (errFrame->data[1] & CAN_ERR_CRTL_RX_WARNING) )
407  {
408  errorMessage = "reached warning level for RX errors";
409  canMessageError(errFlag,errorMessage.c_str(),c_time);
410  return;
411  }
412  if ((errFlag & CAN_ERR_CRTL) && (errFrame->data[1] & CAN_ERR_CRTL_TX_WARNING) )
413  {
414  errorMessage = "reached warning level for TX errors";
415  canMessageError(errFlag,errorMessage.c_str(),c_time);
416  return;
417  }
418  if ((errFlag & CAN_ERR_CRTL) && (errFrame->data[1] & CAN_ERR_CRTL_RX_PASSIVE) )
419  {
420  errorMessage = "reached error passive status RX";
421  canMessageError(errFlag,errorMessage.c_str(),c_time);
422  return;
423  }
424  if ((errFlag & CAN_ERR_CRTL) && (errFrame->data[1] & CAN_ERR_CRTL_TX_PASSIVE) )
425  {
426  errorMessage = "reached error passive status TX";
427  canMessageError(errFlag,errorMessage.c_str(),c_time);
428  return;
429  }
430 
431  if (errFlag & CAN_ERR_PROT)
432  {
433  sprintf(mesbuf,"%s %x %x","protocol violations",errFrame->data[2],errFrame->data[3]);
434  errorMessage = mesbuf;
435  canMessageError(errFlag,errorMessage.c_str(),c_time);
436  return;
437  }
438 
439  if (errFlag & CAN_ERR_TRX)
440  {
441  sprintf(mesbuf,"%s %x","transceiver status",errFrame->data[4]);
442  errorMessage = mesbuf;
443  canMessageError(errFlag,errorMessage.c_str(),c_time);
444  return;
445  }
446  if (errFlag & CAN_ERR_ACK)
447  {
448  errorMessage = "received no ACK on transmission";
449  canMessageError(errFlag,errorMessage.c_str(),c_time);
450  return;
451  }
452  if (errFlag & CAN_ERR_BUSOFF)
453  {
454  errorMessage = "bus off";
455  canMessageError(errFlag,errorMessage.c_str(),c_time);
456  return;
457  }
458  if (errFlag & CAN_ERR_BUSERROR)
459  {
460  errorMessage = "bus error (may flood!)";
461  canMessageError(errFlag,errorMessage.c_str(),c_time);
462  return;
463  }
464  if (errFlag & CAN_ERR_RESTARTED)
465  {
466  errorMessage = "controller restarted";
467  canMessageError(errFlag,errorMessage.c_str(),c_time);
468  return;
469  }
470 }
471 
472 void CSockCanScan::clearErrorMessage()
473 {
474  string errorMessage = "";
475  int timeErr;
476  timeval c_time;
477 
478  timeErr = ioctl(sock,SIOCGSTAMP,&c_time);
479 
480  canMessageError(0,errorMessage.c_str(),c_time);
481  return;
482 }
483 
484 void CSockCanScan::sendErrorMessage(const char *mess)
485 {
486  int timeErr;
487  timeval c_time;
488 
489  timeErr = ioctl(sock,SIOCGSTAMP,&c_time);
490 
491  canMessageError(-1,mess,c_time);
492  return;
493 }
494 
495 void CSockCanScan::waitCanDriver()
496 {
497  struct stat statBuf;
498  int timeErr;
499  string errorMessage;
500  timeval c_time;
501  char procFile[] = "/proc/net/can/rcvlist_all";
502 
503  perror("Canbus Read error, Sockect close");
504 
505  timeErr = ioctl(sock,SIOCGSTAMP,&c_time);
506 
507  errorMessage = "Can Driver error";
508 
509  sendErrorMessage(errorMessage.c_str());
510 
511  string str;
512 
513  while (true) {
514 
515  usleep(200000);
516  ifstream input(procFile);
517  if (input) {
518 
519  // read data as a block:
520  string str((std::istreambuf_iterator<char>(input)), std::istreambuf_iterator<char>());
521 
522  int length = str.length();
523 
524  cout << "Reading " << length << " characters... " << endl;
525 
526  if (length > 0) {
527  cout << "all characters read successfully." << endl;
528  int sz = str.find(channelName.c_str());
529  if (sz > 0 ) {
530  input.close();
531  cout << "Network up..." << endl;
532  break;
533  }
534  else {
535  cout << "No Chennal..." << endl;
536  }
537  }
538  else
539  cout << "error: " << errno << endl;
540 
541  input.close();
542  }
543  else
544  cout << "No CAN driver..." << endl;
545 
546  }
547 
548  clearErrorMessage();
549  return;
550 }
551