Alex exoskeleton
ALEX SoftwareDocumentation
•All Classes Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Pages
CO_command.c
Go to the documentation of this file.
1 /*
2  * Socket command interface for CANopenSocket.
3  *
4  * @file CO_command.c
5  * @author Janez Paternoster
6  * @copyright 2015 Janez Paternoster
7  *
8  * This file is part of CANopenSocket, a Linux implementation of CANopen
9  * stack with master functionality. Project home page is
10  * <https://github.com/CANopenNode/CANopenSocket>. CANopenSocket is based
11  * on CANopenNode: <https://github.com/CANopenNode/CANopenNode>.
12  *
13  * CANopenSocket is free and open source software: you can redistribute
14  * it and/or modify it under the terms of the GNU General Public License
15  * as published by the Free Software Foundation, either version 2 of the
16  * License, or (at your option) any later version.
17  *
18  * This program is distributed in the hope that it will be useful,
19  * but WITHOUT ANY WARRANTY; without even the implied warranty of
20  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
21  * GNU General Public License for more details.
22  *
23  * You should have received a copy of the GNU General Public License
24  * along with this program. If not, see <http://www.gnu.org/licenses/>.
25  */
26 
27 #include "CO_command.h"
28 
29 #include <ctype.h>
30 #include <endian.h>
31 #include <errno.h>
32 #include <pthread.h>
33 #include <stdio.h>
34 #include <stdlib.h>
35 #include <string.h>
36 #include <sys/socket.h>
37 #include <sys/un.h>
38 #include <unistd.h>
39 
40 #include "CANopen.h"
41 #include "CO_comm_helpers.h"
42 #include "CO_master.h"
43 
44 /* Maximum size of Object Dictionary variable transmitted via SDO. */
45 #ifndef CO_COMMAND_SDO_BUFFER_SIZE
46 #define CO_COMMAND_SDO_BUFFER_SIZE 100000
47 #endif
48 
49 #define STRING_BUFFER_SIZE (CO_COMMAND_SDO_BUFFER_SIZE * 4 + 100)
50 #define LISTEN_BACKLOG 50
51 
52 /* Globals */
53 char *CO_command_socketPath = "/tmp/CO_command_socket"; /* Name of the local domain socket */
54 
55 /* Variables */
56 static void *command_thread(void *arg);
57 static pthread_t command_thread_id;
58 static void command_process(int fd, char *command, size_t commandLength);
59 static int fdSocket;
60 static unsigned short comm_net = 1; /* default CAN net number */
61 static uint8_t comm_node_default = 0xFF; /* CANopen Node ID number is undefined at startup. */
62 static uint16_t SDOtimeoutTime = 500; /* Timeout time for SDO transfer in milliseconds, if no response */
63 static uint8_t blockTransferEnable = 0; /* SDO block transfer enabled? */
64 static volatile int endProgram = 0;
65 /***/
66 /* send CANopen generic emergency message */
67 void CO_errorR(const uint32_t info) {
68  //CO_errorReport(CO->em, CO_EM_GENERIC_SOFTWARE_ERROR, CO_EMC_SOFTWARE_INTERNAL, info);
69  fprintf(stderr, "canopend generic error: 0x%X\n", info);
70 }
71 
72 /******************************************************************************/
73 int CO_command_init(void) {
74  struct sockaddr_un addr;
75 
76  if (CO == NULL || CO->SDOclient == NULL) {
77  perror("CO_command_init - Wrong arguments");
78  exit(EXIT_FAILURE);
79  }
80 
81  /* Create, bind and listen socket */
82  fdSocket = socket(AF_UNIX, SOCK_STREAM, 0);
83  if (fdSocket == -1) {
84  perror("CO_command_init - socket failed");
85  exit(EXIT_FAILURE);
86  }
87 
88  memset(&addr, 0, sizeof(struct sockaddr_un));
89  addr.sun_family = AF_UNIX;
90  strncpy(addr.sun_path, CO_command_socketPath, sizeof(addr.sun_path) - 1);
91 
92  if (bind(fdSocket, (struct sockaddr *)&addr, sizeof(struct sockaddr_un)) == -1) {
93  fprintf(stderr, "Can't bind Socket to path '%s'\n", CO_command_socketPath);
94  perror("CO_command_init");
95  exit(EXIT_FAILURE);
96  }
97 
98  if (listen(fdSocket, LISTEN_BACKLOG) == -1) {
99  perror("CO_command_init - listen failed");
100  exit(EXIT_FAILURE);
101  }
102 
103  /* Create thread */
104  endProgram = 0;
105  if (pthread_create(&command_thread_id, NULL, command_thread, NULL) != 0) {
106  perror("CO_command_init - thread creation failed");
107  exit(EXIT_FAILURE);
108  }
109 
110  return 0;
111 }
112 
113 /******************************************************************************/
114 int CO_command_clear(void) {
115  static struct sockaddr_un addr;
116  int fd;
117 
118  endProgram = 1;
119 
120  /* Establish a client socket connection to finish the command_thread. */
121  fd = socket(AF_UNIX, SOCK_STREAM, 0);
122  if (fd == -1) {
123  return -1;
124  }
125 
126  memset(&addr, 0, sizeof(struct sockaddr_un));
127  addr.sun_family = AF_UNIX;
128  strncpy(addr.sun_path, CO_command_socketPath, sizeof(addr.sun_path) - 1);
129 
130  if (connect(fd, (struct sockaddr *)&addr, sizeof(struct sockaddr_un)) == -1) {
131  return -1;
132  }
133 
134  close(fd);
135 
136  /* Wait for thread to finish. */
137  if (pthread_join(command_thread_id, NULL) != 0) {
138  return -1;
139  }
140 
141  close(fdSocket);
142 
143  /* Remove socket from filesystem. */
144  if (remove(CO_command_socketPath) == -1) {
145  return -1;
146  }
147 
148  return 0;
149 }
150 
151 /******************************************************************************/
152 static void *command_thread(void *arg) {
153  int fd;
154  ssize_t n;
155  char buf[STRING_BUFFER_SIZE];
156 
157  /* Almost endless loop */
158  while (endProgram == 0) {
159  /* wait for new command */
160  fd = accept(fdSocket, NULL, NULL);
161  if (fd == -1) {
162  CO_errorR(0x15100000L);
163  }
164 
165  /* Read command and send answer. */
166  while ((n = read(fd, buf, sizeof(buf) - 1)) > 0) {
167  buf[n++] = 0; /* terminate input string */
168  command_process(fd, buf, n);
169  }
170 
171  if (n == -1) {
172  CO_errorR(0x15800000L + errno);
173  }
174 
175  /* close current communication */
176  if (close(fd) == -1) {
177  CO_errorR(0x15900000L);
178  }
179  }
180 
181  return NULL;
182 }
183 
184 /******************************************************************************/
185 static void command_process(int fd, char *command, size_t commandLength) {
186  int err = 0; /* syntax or other error, true or false */
187  int emptyLine = 0;
188  char *token;
189  int i;
190  uint32_t ui[3];
191  uint8_t comm_node = 0xFF; /* undefined */
192 
193  char resp[STRING_BUFFER_SIZE];
194  int respLen = 0;
195  respErrorCode_t respErrorCode = respErrorNone;
196 
197  uint32_t sequence = 0;
198 
199  /* parse mandatory token '"["<sequence>"]"' */
200  if ((token = getTok(command, spaceDelim, &err)) == NULL) {
201  /* If empty line, respond with empty line. */
202  emptyLine = 1;
203  }
204  if (err == 0) {
205  if (token[0] != '[' || token[strlen(token) - 1] != ']') {
206  err = 1;
207  if (token[0] == '#') {
208  /* If comment, respond with empty line. */
209  emptyLine = 1;
210  }
211  } else {
212  token[strlen(token) - 1] = '\0';
213  sequence = getU32(token + 1, 0, 0xFFFFFFFF, &err);
214  }
215  }
216 
217  /* parse optional tokens '[[<net>] <node>]', both numerical. Then follows
218  * mandatory token <command>, which is not numerical. */
219  if (err == 0) {
220  for (i = 0; i < 3; i++) {
221  if ((token = getTok(NULL, spaceDelim, &err)) == NULL) {
222  break;
223  }
224  if (isdigit(token[0]) == 0) {
225  break;
226  }
227  ui[i] = getU32(token, 0, 0xFFFFFFFF, &err);
228  }
229  }
230  if (err == 0) {
231  switch (i) {
232  case 0: /* only <command> (pointed by token) */
233  comm_node = comm_node_default; /* may be undefined */
234  break;
235  case 1: /* <node> and <command> tokens */
236  if (ui[0] < 0 || ui[0] > 127) {
237  err = 1;
238  respErrorCode = respErrorUnsupportedNode;
239  } else {
240  comm_node = (uint8_t)ui[0];
241  }
242  break;
243  case 2: /* <net>, <node> and <command> tokens */
244  if (ui[0] < 1 || ui[0] > 1) {
245  err = 1;
246  respErrorCode = respErrorUnsupportedNet;
247  } else if (ui[1] < 0 || ui[1] > 127) {
248  err = 1;
249  respErrorCode = respErrorUnsupportedNode;
250  } else {
251  comm_net = (uint16_t)ui[0];
252  comm_node = (uint8_t)ui[1];
253  }
254  break;
255  case 3: /* <command> token contains digit */
256  err = 1;
257  break;
258  }
259  }
260 
261  /* Execute command */
262  if (err == 0) {
263  /* Upload SDO command - 'r[ead] <index> <subindex> <datatype>' */
264  if (strcmp(token, "r") == 0 || strcmp(token, "read") == 0) {
265  uint16_t idx;
266  uint8_t subidx;
267  const dataType_t *datatype; /* optional token */
268  int errTokDt = 0;
269  int errDt = 0;
270  uint32_t SDOabortCode = 1;
271 
272  uint8_t dataRx[CO_COMMAND_SDO_BUFFER_SIZE]; /* SDO receive buffer */
273  uint32_t dataRxLen; /* Length of received data */
274 
275  token = getTok(NULL, spaceDelim, &err);
276  idx = (uint16_t)getU32(token, 0, 0xFFFF, &err);
277 
278  token = getTok(NULL, spaceDelim, &err);
279  subidx = (uint8_t)getU32(token, 0, 0xFF, &err);
280 
281  token = getTok(NULL, spaceDelim, &errTokDt);
282  datatype = getDataType(token, &errDt);
283 
284  /* Datatype must be correct, if present. */
285  if (errTokDt == 0 && errDt != 0) {
286  err = 1;
287  }
288 
289  lastTok(NULL, spaceDelim, &err);
290 
291  if (err == 0 && (comm_node < 1 || comm_node > 127)) {
292  err = 1;
293  if (comm_node == 0xFF) {
294  respErrorCode = respErrorNoDefaultNodeSet;
295  } else {
296  respErrorCode = respErrorUnsupportedNode;
297  }
298  }
299 
300  /* Make CANopen SDO transfer */
301  if (err == 0) {
302  err = sdoClientUpload(
303  CO->SDOclient,
304  comm_node,
305  idx,
306  subidx,
307  dataRx,
308  sizeof(dataRx),
309  &dataRxLen,
310  &SDOabortCode,
311  SDOtimeoutTime,
312  blockTransferEnable);
313 
314  if (err != 0) {
315  respErrorCode = respErrorInternalState;
316  }
317  }
318 
319  /* output result */
320  if (err == 0) {
321  if (SDOabortCode == 0) {
322  respLen = sprintf(resp, "[%d] ", sequence);
323 
324  if (datatype == NULL || (datatype->length != 0 && datatype->length != dataRxLen)) {
325  respLen += dtpHex(resp + respLen, sizeof(resp) - respLen, (char *)dataRx, dataRxLen);
326  } else {
327  respLen += datatype->dataTypePrint(
328  resp + respLen, sizeof(resp) - respLen, (char *)dataRx, dataRxLen);
329  }
330  } else {
331  respLen = sprintf(resp, "[%d] ERROR: 0x%08X", sequence, SDOabortCode);
332  }
333  }
334  }
335 
336  /* Download SDO command - w[rite] <index> <subindex> <datatype> <value> */
337  else if (strcmp(token, "w") == 0 || strcmp(token, "write") == 0) {
338  uint16_t idx;
339  uint8_t subidx;
340  const dataType_t *datatype;
341  uint32_t SDOabortCode = 1;
342 
343  uint8_t dataTx[CO_COMMAND_SDO_BUFFER_SIZE]; /* SDO transmit buffer */
344  uint32_t dataTxLen = 0; /* Length of data to transmit. */
345 
346  token = getTok(NULL, spaceDelim, &err);
347  idx = (uint16_t)getU32(token, 0, 0xFFFF, &err);
348 
349  token = getTok(NULL, spaceDelim, &err);
350  subidx = (uint8_t)getU32(token, 0, 0xFF, &err);
351 
352  token = getTok(NULL, spaceDelim, &err);
353  datatype = getDataType(token, &err);
354 
355  if (err == 0) {
356  /* take whole string or single token, depending on datatype. Comment may follow. */
357  token = getTok(NULL, (datatype->length == 0) ? "\n\r\f" : spaceDelim, &err);
358  }
359 
360  if (err == 0) {
361  dataTxLen = datatype->dataTypeScan((char *)dataTx, sizeof(dataTx), token);
362 
363  /* Length must match and must not be zero. */
364  if ((datatype->length != 0 && datatype->length != dataTxLen) || dataTxLen == 0) {
365  err = 1;
366  }
367  }
368 
369  lastTok(NULL, spaceDelim, &err);
370 
371  if (err == 0 && (comm_node < 1 || comm_node > 127)) {
372  err = 1;
373  if (comm_node == 0xFF) {
374  respErrorCode = respErrorNoDefaultNodeSet;
375  } else {
376  respErrorCode = respErrorUnsupportedNode;
377  }
378  }
379 
380  /* Make CANopen SDO transfer */
381  if (err == 0) {
382  err = sdoClientDownload(
383  CO->SDOclient,
384  comm_node,
385  idx,
386  subidx,
387  dataTx,
388  dataTxLen,
389  &SDOabortCode,
390  SDOtimeoutTime,
391  blockTransferEnable);
392 
393  if (err != 0) {
394  respErrorCode = respErrorInternalState;
395  }
396  }
397 
398  /* output result */
399  if (err == 0) {
400  if (SDOabortCode == 0) {
401  respLen = sprintf(resp, "[%d] OK", sequence);
402  } else {
403  respLen = sprintf(resp, "[%d] ERROR: 0x%08X", sequence, SDOabortCode);
404  }
405  }
406  }
407 
408  /* NMT start node */
409  else if (strcmp(token, "start") == 0) {
410  lastTok(NULL, spaceDelim, &err);
411  if (err == 0 && comm_node > 127) {
412  err = 1;
413  respErrorCode = respErrorNoDefaultNodeSet;
414  }
415  if (err == 0) {
416  err = CO_sendNMTcommand(CO, CO_NMT_ENTER_OPERATIONAL, comm_node) ? 1 : 0;
417  if (err == 0) respLen = sprintf(resp, "[%d] OK", sequence);
418  }
419  }
420 
421  /* NMT stop node */
422  else if (strcmp(token, "stop") == 0) {
423  lastTok(NULL, spaceDelim, &err);
424  if (err == 0 && comm_node > 127) {
425  err = 1;
426  respErrorCode = respErrorNoDefaultNodeSet;
427  }
428  if (err == 0) {
429  err = CO_sendNMTcommand(CO, CO_NMT_ENTER_STOPPED, comm_node) ? 1 : 0;
430  if (err == 0) respLen = sprintf(resp, "[%d] OK", sequence);
431  }
432  }
433 
434  /* NMT Set node to pre-operational */
435  else if (strcmp(token, "preop") == 0 || strcmp(token, "preoperational") == 0) {
436  lastTok(NULL, spaceDelim, &err);
437  if (err == 0 && comm_node > 127) {
438  err = 1;
439  respErrorCode = respErrorNoDefaultNodeSet;
440  }
441  if (err == 0) {
442  err = CO_sendNMTcommand(CO, CO_NMT_ENTER_PRE_OPERATIONAL, comm_node) ? 1 : 0;
443  if (err == 0) respLen = sprintf(resp, "[%d] OK", sequence);
444  }
445  }
446 
447  /* NMT reset (node or communication) */
448  else if (strcmp(token, "reset") == 0) {
449  token = getTok(NULL, spaceDelim, &err);
450  if (err == 0 && comm_node > 127) {
451  err = 1;
452  respErrorCode = respErrorNoDefaultNodeSet;
453  }
454  if (err == 0) {
455  if (strcmp(token, "node") == 0) {
456  lastTok(NULL, spaceDelim, &err);
457  if (err == 0) {
458  err = CO_sendNMTcommand(CO, CO_NMT_RESET_NODE, comm_node) ? 1 : 0;
459  if (err == 0) respLen = sprintf(resp, "[%d] OK", sequence);
460  }
461  } else if (strcmp(token, "comm") == 0 || strcmp(token, "communication") == 0) {
462  lastTok(NULL, spaceDelim, &err);
463  if (err == 0) {
464  err = CO_sendNMTcommand(CO, CO_NMT_RESET_COMMUNICATION, comm_node) ? 1 : 0;
465  if (err == 0) respLen = sprintf(resp, "[%d] OK", sequence);
466  }
467  }
468 
469  else {
470  err = 1;
471  }
472  }
473  }
474 
475  /* set command - multiple settings */
476  else if (strcmp(token, "set") == 0) {
477  token = getTok(NULL, spaceDelim, &err);
478  if (err == 0) {
479  /* sdo_timeout <value> */
480  if (strcmp(token, "sdo_timeout") == 0) {
481  uint16_t tmout;
482 
483  token = getTok(NULL, spaceDelim, &err);
484  tmout = (uint16_t)getU32(token, 0, 10000, &err);
485 
486  lastTok(NULL, spaceDelim, &err);
487 
488  /* Write to variable */
489  if (err == 0) {
490  SDOtimeoutTime = tmout;
491  respLen = sprintf(resp, "[%d] OK", sequence);
492  }
493  }
494 
495  /* sdo_block <value> */
496  else if (strcmp(token, "sdo_block") == 0) {
497  uint8_t blk;
498 
499  token = getTok(NULL, spaceDelim, &err);
500  blk = (uint8_t)getU32(token, 0, 1, &err);
501 
502  lastTok(NULL, spaceDelim, &err);
503 
504  /* Write to variable */
505  if (err == 0) {
506  blockTransferEnable = blk;
507  respLen = sprintf(resp, "[%d] OK", sequence);
508  }
509  }
510 
511  /* node <value> */
512  else if (strcmp(token, "node") == 0) {
513  uint16_t node;
514 
515  token = getTok(NULL, spaceDelim, &err);
516  node = (uint16_t)getU32(token, 1, 127, &err);
517 
518  lastTok(NULL, spaceDelim, &err);
519 
520  /* Write to variable */
521  if (err == 0) {
522  comm_node_default = node;
523  respLen = sprintf(resp, "[%d] OK", sequence);
524  }
525  }
526 
527  /* Unknown command */
528  else {
529  err = 1;
530  }
531  }
532  }
533 
534  /* Unknown command */
535  else {
536  respErrorCode = respErrorReqNotSupported;
537  err = 1;
538  }
539  }
540 
541  /* Generate error response (or leave empty line response) */
542  if (err != 0 && emptyLine == 0) {
543  if (respErrorCode == respErrorNone) {
544  respErrorCode = respErrorSyntax;
545  }
546  respLen = sprintf(resp, "[%d] ERROR: %d", sequence, respErrorCode);
547  }
548 
549  /* Terminate string and send response */
550  resp[respLen++] = '\r';
551  resp[respLen++] = '\n';
552  resp[respLen++] = '\0';
553 
554  if (write(fd, resp, respLen) != respLen) {
555  CO_errorR(0x15200000L);
556  }
557 }
558 /******************************************************************************/
559 void cancomm_socketFree(char *command, char *ret) {
560  int err = 0; /* syntax or other error, true or false */
561  int emptyLine = 0;
562  char *token;
563  int i;
564  uint32_t ui[3];
565  uint8_t comm_node = 0xFF; /* undefined */
566 
567  char resp[STRING_BUFFER_SIZE];
568  int respLen = 0;
569  respErrorCode_t respErrorCode = respErrorNone;
570 
571  uint32_t sequence = 0;
572 
573  /* parse mandatory token '"["<sequence>"]"' */
574  if ((token = getTok(command, spaceDelim, &err)) == NULL) {
575  /* If empty line, respond with empty line. */
576  emptyLine = 1;
577  }
578  if (err == 0) {
579  if (token[0] != '[' || token[strlen(token) - 1] != ']') {
580  err = 1;
581  if (token[0] == '#') {
582  /* If comment, respond with empty line. */
583  emptyLine = 1;
584  }
585  } else {
586  token[strlen(token) - 1] = '\0';
587  sequence = getU32(token + 1, 0, 0xFFFFFFFF, &err);
588  }
589  }
590 
591  /* parse optional tokens '[[<net>] <node>]', both numerical. Then follows
592  * mandatory token <command>, which is not numerical. */
593  if (err == 0) {
594  for (i = 0; i < 3; i++) {
595  if ((token = getTok(NULL, spaceDelim, &err)) == NULL) {
596  break;
597  }
598  if (isdigit(token[0]) == 0) {
599  break;
600  }
601  ui[i] = getU32(token, 0, 0xFFFFFFFF, &err);
602  }
603  }
604  if (err == 0) {
605  switch (i) {
606  case 0: /* only <command> (pointed by token) */
607  comm_node = comm_node_default; /* may be undefined */
608  break;
609  case 1: /* <node> and <command> tokens */
610  if (ui[0] < 0 || ui[0] > 127) {
611  err = 1;
612  respErrorCode = respErrorUnsupportedNode;
613  } else {
614  comm_node = (uint8_t)ui[0];
615  }
616  break;
617  case 2: /* <net>, <node> and <command> tokens */
618  if (ui[0] < 1 || ui[0] > 1) {
619  err = 1;
620  respErrorCode = respErrorUnsupportedNet;
621  } else if (ui[1] < 0 || ui[1] > 127) {
622  err = 1;
623  respErrorCode = respErrorUnsupportedNode;
624  } else {
625  comm_net = (uint16_t)ui[0];
626  comm_node = (uint8_t)ui[1];
627  }
628  break;
629  case 3: /* <command> token contains digit */
630  err = 1;
631  break;
632  }
633  }
634  /* Execute command */
635  if (err == 0) {
636  //printf("Error is 0\n");
637 
638  /* Upload SDO command - 'r[ead] <index> <subindex> <datatype>' */
639  if (strcmp(token, "r") == 0 || strcmp(token, "read") == 0) {
640  uint16_t idx;
641  uint8_t subidx;
642  const dataType_t *datatype; /* optional token */
643  int errTokDt = 0;
644  int errDt = 0;
645  uint32_t SDOabortCode = 1;
646 
647  uint8_t dataRx[CO_COMMAND_SDO_BUFFER_SIZE]; /* SDO receive buffer */
648  uint32_t dataRxLen; /* Length of received data */
649 
650  token = getTok(NULL, spaceDelim, &err);
651  idx = (uint16_t)getU32(token, 0, 0xFFFF, &err);
652 
653  token = getTok(NULL, spaceDelim, &err);
654  subidx = (uint8_t)getU32(token, 0, 0xFF, &err);
655 
656  token = getTok(NULL, spaceDelim, &errTokDt);
657  datatype = getDataType(token, &errDt);
658 
659  /* Datatype must be correct, if present. */
660  if (errTokDt == 0 && errDt != 0) {
661  err = 1;
662  }
663 
664  lastTok(NULL, spaceDelim, &err);
665 
666  if (err == 0 && (comm_node < 1 || comm_node > 127)) {
667  err = 1;
668  if (comm_node == 0xFF) {
669  respErrorCode = respErrorNoDefaultNodeSet;
670  } else {
671  respErrorCode = respErrorUnsupportedNode;
672  }
673  }
674 
675  /* Make CANopen SDO transfer */
676  if (err == 0) {
677  err = sdoClientUpload(
678  CO->SDOclient,
679  comm_node,
680  idx,
681  subidx,
682  dataRx,
683  sizeof(dataRx),
684  &dataRxLen,
685  &SDOabortCode,
686  SDOtimeoutTime,
687  blockTransferEnable);
688 
689  if (err != 0) {
690  respErrorCode = respErrorInternalState;
691  }
692  }
693 
694  /* output result */
695  if (err == 0) {
696  if (SDOabortCode == 0) {
697  respLen = sprintf(resp, "[%d] ", sequence);
698 
699  if (datatype == NULL || (datatype->length != 0 && datatype->length != dataRxLen)) {
700  respLen += dtpHex(resp + respLen, sizeof(resp) - respLen, (char *)dataRx, dataRxLen);
701  } else {
702  respLen += datatype->dataTypePrint(
703  resp + respLen, sizeof(resp) - respLen, (char *)dataRx, dataRxLen);
704  }
705  } else {
706  respLen = sprintf(resp, "[%d] ERROR: 0x%08X", sequence, SDOabortCode);
707  }
708  }
709  }
710 
711  /* Download SDO command - w[rite] <index> <subindex> <datatype> <value> */
712  else if (strcmp(token, "w") == 0 || strcmp(token, "write") == 0) {
713  uint16_t idx;
714  uint8_t subidx;
715  const dataType_t *datatype;
716  uint32_t SDOabortCode = 1;
717 
718  uint8_t dataTx[CO_COMMAND_SDO_BUFFER_SIZE]; /* SDO transmit buffer */
719  uint32_t dataTxLen = 0; /* Length of data to transmit. */
720 
721  token = getTok(NULL, spaceDelim, &err);
722  idx = (uint16_t)getU32(token, 0, 0xFFFF, &err);
723 
724  token = getTok(NULL, spaceDelim, &err);
725  subidx = (uint8_t)getU32(token, 0, 0xFF, &err);
726 
727  token = getTok(NULL, spaceDelim, &err);
728  datatype = getDataType(token, &err);
729 
730  if (err == 0) {
731  /* take whole string or single token, depending on datatype. Comment may follow. */
732  token = getTok(NULL, (datatype->length == 0) ? "\n\r\f" : spaceDelim, &err);
733  }
734 
735  if (err == 0) {
736  dataTxLen = datatype->dataTypeScan((char *)dataTx, sizeof(dataTx), token);
737 
738  /* Length must match and must not be zero. */
739  if ((datatype->length != 0 && datatype->length != dataTxLen) || dataTxLen == 0) {
740  err = 1;
741  }
742  }
743 
744  lastTok(NULL, spaceDelim, &err);
745 
746  if (err == 0 && (comm_node < 1 || comm_node > 127)) {
747  err = 1;
748  if (comm_node == 0xFF) {
749  respErrorCode = respErrorNoDefaultNodeSet;
750  } else {
751  respErrorCode = respErrorUnsupportedNode;
752  }
753  }
754 
755  /* Make CANopen SDO transfer */
756  if (err == 0) {
757  err = sdoClientDownload(
758  CO->SDOclient,
759  comm_node,
760  idx,
761  subidx,
762  dataTx,
763  dataTxLen,
764  &SDOabortCode,
765  SDOtimeoutTime,
766  blockTransferEnable);
767 
768  if (err != 0) {
769  respErrorCode = respErrorInternalState;
770  }
771  }
772 
773  /* output result */
774  if (err == 0) {
775  if (SDOabortCode == 0) {
776  respLen = sprintf(resp, "[%d] OK", sequence);
777  } else {
778  respLen = sprintf(resp, "[%d] ERROR: 0x%08X", sequence, SDOabortCode);
779  }
780  }
781  }
782 
783  /* NMT start node */
784  else if (strcmp(token, "start") == 0) {
785  lastTok(NULL, spaceDelim, &err);
786  if (err == 0 && comm_node > 127) {
787  err = 1;
788  respErrorCode = respErrorNoDefaultNodeSet;
789  }
790  if (err == 0) {
791  err = CO_sendNMTcommand(CO, CO_NMT_ENTER_OPERATIONAL, comm_node) ? 1 : 0;
792  if (err == 0) respLen = sprintf(resp, "[%d] OK", sequence);
793  }
794  }
795 
796  /* NMT stop node */
797  else if (strcmp(token, "stop") == 0) {
798  lastTok(NULL, spaceDelim, &err);
799  if (err == 0 && comm_node > 127) {
800  err = 1;
801  respErrorCode = respErrorNoDefaultNodeSet;
802  }
803  if (err == 0) {
804  err = CO_sendNMTcommand(CO, CO_NMT_ENTER_STOPPED, comm_node) ? 1 : 0;
805  if (err == 0) respLen = sprintf(resp, "[%d] OK", sequence);
806  }
807  }
808 
809  /* NMT Set node to pre-operational */
810  else if (strcmp(token, "preop") == 0 || strcmp(token, "preoperational") == 0) {
811  lastTok(NULL, spaceDelim, &err);
812  if (err == 0 && comm_node > 127) {
813  err = 1;
814  respErrorCode = respErrorNoDefaultNodeSet;
815  }
816  if (err == 0) {
817  err = CO_sendNMTcommand(CO, CO_NMT_ENTER_PRE_OPERATIONAL, comm_node) ? 1 : 0;
818  if (err == 0) respLen = sprintf(resp, "[%d] OK", sequence);
819  }
820  }
821 
822  /* NMT reset (node or communication) */
823  else if (strcmp(token, "reset") == 0) {
824  token = getTok(NULL, spaceDelim, &err);
825  if (err == 0 && comm_node > 127) {
826  err = 1;
827  respErrorCode = respErrorNoDefaultNodeSet;
828  }
829  if (err == 0) {
830  if (strcmp(token, "node") == 0) {
831  lastTok(NULL, spaceDelim, &err);
832  if (err == 0) {
833  err = CO_sendNMTcommand(CO, CO_NMT_RESET_NODE, comm_node) ? 1 : 0;
834  if (err == 0) respLen = sprintf(resp, "[%d] OK", sequence);
835  }
836  } else if (strcmp(token, "comm") == 0 || strcmp(token, "communication") == 0) {
837  lastTok(NULL, spaceDelim, &err);
838  if (err == 0) {
839  err = CO_sendNMTcommand(CO, CO_NMT_RESET_COMMUNICATION, comm_node) ? 1 : 0;
840  if (err == 0) respLen = sprintf(resp, "[%d] OK", sequence);
841  }
842  }
843 
844  else {
845  err = 1;
846  }
847  }
848  }
849 
850  /* set command - multiple settings */
851  else if (strcmp(token, "set") == 0) {
852  token = getTok(NULL, spaceDelim, &err);
853  if (err == 0) {
854  /* sdo_timeout <value> */
855  if (strcmp(token, "sdo_timeout") == 0) {
856  uint16_t tmout;
857 
858  token = getTok(NULL, spaceDelim, &err);
859  tmout = (uint16_t)getU32(token, 0, 10000, &err);
860 
861  lastTok(NULL, spaceDelim, &err);
862 
863  /* Write to variable */
864  if (err == 0) {
865  SDOtimeoutTime = tmout;
866  respLen = sprintf(resp, "[%d] OK", sequence);
867  }
868  }
869 
870  /* sdo_block <value> */
871  else if (strcmp(token, "sdo_block") == 0) {
872  uint8_t blk;
873 
874  token = getTok(NULL, spaceDelim, &err);
875  blk = (uint8_t)getU32(token, 0, 1, &err);
876 
877  lastTok(NULL, spaceDelim, &err);
878 
879  /* Write to variable */
880  if (err == 0) {
881  blockTransferEnable = blk;
882  respLen = sprintf(resp, "[%d] OK", sequence);
883  }
884  }
885 
886  /* node <value> */
887  else if (strcmp(token, "node") == 0) {
888  uint16_t node;
889 
890  token = getTok(NULL, spaceDelim, &err);
891  node = (uint16_t)getU32(token, 1, 127, &err);
892 
893  lastTok(NULL, spaceDelim, &err);
894 
895  /* Write to variable */
896  if (err == 0) {
897  comm_node_default = node;
898  respLen = sprintf(resp, "[%d] OK", sequence);
899  }
900  }
901 
902  /* Unknown command */
903  else {
904  err = 1;
905  }
906  }
907  }
908 
909  /* Unknown command */
910  else {
911  respErrorCode = respErrorReqNotSupported;
912  err = 1;
913  }
914  }
915 
916  /* Generate error response (or leave empty line response) */
917  if (err != 0 && emptyLine == 0) {
918  if (respErrorCode == respErrorNone) {
919  respErrorCode = respErrorSyntax;
920  }
921  respLen = sprintf(resp, "[%d] ERROR: %d", sequence, respErrorCode);
922  }
923 
924  /* Terminate string and send response */
925  resp[respLen++] = '\r';
926  resp[respLen++] = '\n';
927  resp[respLen++] = '\0';
928  // print SDO response to terminal.
929  printf("RESPONSE: %s\n", resp);
930 }
respErrorInternalState
Definition: CO_comm_helpers.h:42
dataType_t::length
int length
Definition: CO_comm_helpers.h:51
getU32
uint32_t getU32(char *token, uint32_t min, uint32_t max, int *err)
Definition: CO_comm_helpers.c:271
getTok
char * getTok(char *initStr, const char *delim, int *err)
Definition: CO_comm_helpers.c:241
getDataType
const dataType_t * getDataType(char *token, int *err)
Definition: CO_comm_helpers.c:385
lastTok
void lastTok(char *initStr, const char *delim, int *err)
Definition: CO_comm_helpers.c:257
CO_command_socketPath
char * CO_command_socketPath
Definition: CO_command.c:53
ret
char ret[STRING_BUFFER_SIZE]
Definition: application.cpp:15
buf
char buf[STRING_BUFFER_SIZE]
Definition: application.cpp:14
respErrorUnsupportedNode
Definition: CO_comm_helpers.h:45
uint8_t
unsigned char uint8_t
Definition: CO_command.h:39
respErrorUnsupportedNet
Definition: CO_comm_helpers.h:44
CO_comm_helpers.h
CO_command_init
int CO_command_init(void)
Definition: CO_command.c:73
uint16_t
unsigned short uint16_t
Definition: CO_command.h:35
LISTEN_BACKLOG
#define LISTEN_BACKLOG
Definition: CO_command.c:50
uint32_t
unsigned int uint32_t
Definition: CO_command.h:31
dataType_t::dataTypeScan
int(* dataTypeScan)(char *bufSdo, int bufSdoSize, char *strin)
Definition: CO_comm_helpers.h:53
dataType_t
Data types structure - Defined in CANOpen Code.
Definition: CO_comm_helpers.h:49
CO_command_clear
int CO_command_clear(void)
Definition: CO_command.c:114
sdoClientDownload
int sdoClientDownload(CO_SDOclient_t *SDOclient, uint8_t nodeID, uint16_t idx, uint8_t subidx, uint8_t *dataTx, uint32_t dataTxLen, uint32_t *SDOabortCode, uint16_t SDOtimeoutTime, uint8_t blockTransferEnable)
Definition: CO_master.c:96
dataType_t::dataTypePrint
int(* dataTypePrint)(char *strout, int stroutSize, char *bufSdo, int bufLen)
Definition: CO_comm_helpers.h:52
dtpHex
int dtpHex(char *strout, int stroutSize, char *bufSdo, int bufLen)
Definition: CO_comm_helpers.c:44
respErrorSyntax
Definition: CO_comm_helpers.h:41
respErrorCode_t
respErrorCode_t
Definition: CO_comm_helpers.h:38
respErrorNoDefaultNodeSet
Definition: CO_comm_helpers.h:43
CO_COMMAND_SDO_BUFFER_SIZE
#define CO_COMMAND_SDO_BUFFER_SIZE
Definition: CO_command.c:46
STRING_BUFFER_SIZE
#define STRING_BUFFER_SIZE
Definition: CO_command.c:49
spaceDelim
const char spaceDelim[]
Definition: CO_comm_helpers.c:36
sdoClientUpload
int sdoClientUpload(CO_SDOclient_t *SDOclient, uint8_t nodeID, uint16_t idx, uint8_t subidx, uint8_t *dataRx, uint32_t dataRxSize, uint32_t *dataRxLen, uint32_t *SDOabortCode, uint16_t SDOtimeoutTime, uint8_t blockTransferEnable)
Definition: CO_master.c:32
CO_master.h
cancomm_socketFree
void cancomm_socketFree(char *command, char *ret)
Definition: CO_command.c:559
CO_command.h
respErrorNone
Definition: CO_comm_helpers.h:39
respErrorReqNotSupported
Definition: CO_comm_helpers.h:40
CO_errorR
void CO_errorR(const uint32_t info)
Definition: CO_command.c:67
Generated by   doxygen 1.8.11