author | etisserant |
Thu, 30 Aug 2007 17:01:11 +0200 | |
changeset 260 | 1eafe582b6a1 |
parent 196 | 65aa7a664f6f |
child 290 | 43c3b2bf3e32 |
permissions | -rw-r--r-- |
47
8a1047ab51f4
SOCKET-CAN support added. Many thanks to Jan Kiszka !
etisserant
parents:
diff
changeset
|
1 |
/* |
8a1047ab51f4
SOCKET-CAN support added. Many thanks to Jan Kiszka !
etisserant
parents:
diff
changeset
|
2 |
This file is part of CanFestival, a library implementing CanOpen Stack. |
8a1047ab51f4
SOCKET-CAN support added. Many thanks to Jan Kiszka !
etisserant
parents:
diff
changeset
|
3 |
|
8a1047ab51f4
SOCKET-CAN support added. Many thanks to Jan Kiszka !
etisserant
parents:
diff
changeset
|
4 |
Copyright (C): Edouard TISSERANT and Francis DUPIN |
8a1047ab51f4
SOCKET-CAN support added. Many thanks to Jan Kiszka !
etisserant
parents:
diff
changeset
|
5 |
|
8a1047ab51f4
SOCKET-CAN support added. Many thanks to Jan Kiszka !
etisserant
parents:
diff
changeset
|
6 |
See COPYING file for copyrights details. |
8a1047ab51f4
SOCKET-CAN support added. Many thanks to Jan Kiszka !
etisserant
parents:
diff
changeset
|
7 |
|
8a1047ab51f4
SOCKET-CAN support added. Many thanks to Jan Kiszka !
etisserant
parents:
diff
changeset
|
8 |
This library is free software; you can redistribute it and/or |
8a1047ab51f4
SOCKET-CAN support added. Many thanks to Jan Kiszka !
etisserant
parents:
diff
changeset
|
9 |
modify it under the terms of the GNU Lesser General Public |
8a1047ab51f4
SOCKET-CAN support added. Many thanks to Jan Kiszka !
etisserant
parents:
diff
changeset
|
10 |
License as published by the Free Software Foundation; either |
8a1047ab51f4
SOCKET-CAN support added. Many thanks to Jan Kiszka !
etisserant
parents:
diff
changeset
|
11 |
version 2.1 of the License, or (at your option) any later version. |
8a1047ab51f4
SOCKET-CAN support added. Many thanks to Jan Kiszka !
etisserant
parents:
diff
changeset
|
12 |
|
8a1047ab51f4
SOCKET-CAN support added. Many thanks to Jan Kiszka !
etisserant
parents:
diff
changeset
|
13 |
This library is distributed in the hope that it will be useful, |
8a1047ab51f4
SOCKET-CAN support added. Many thanks to Jan Kiszka !
etisserant
parents:
diff
changeset
|
14 |
but WITHOUT ANY WARRANTY; without even the implied warranty of |
8a1047ab51f4
SOCKET-CAN support added. Many thanks to Jan Kiszka !
etisserant
parents:
diff
changeset
|
15 |
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU |
8a1047ab51f4
SOCKET-CAN support added. Many thanks to Jan Kiszka !
etisserant
parents:
diff
changeset
|
16 |
Lesser General Public License for more details. |
8a1047ab51f4
SOCKET-CAN support added. Many thanks to Jan Kiszka !
etisserant
parents:
diff
changeset
|
17 |
|
8a1047ab51f4
SOCKET-CAN support added. Many thanks to Jan Kiszka !
etisserant
parents:
diff
changeset
|
18 |
You should have received a copy of the GNU Lesser General Public |
8a1047ab51f4
SOCKET-CAN support added. Many thanks to Jan Kiszka !
etisserant
parents:
diff
changeset
|
19 |
License along with this library; if not, write to the Free Software |
8a1047ab51f4
SOCKET-CAN support added. Many thanks to Jan Kiszka !
etisserant
parents:
diff
changeset
|
20 |
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA |
8a1047ab51f4
SOCKET-CAN support added. Many thanks to Jan Kiszka !
etisserant
parents:
diff
changeset
|
21 |
*/ |
8a1047ab51f4
SOCKET-CAN support added. Many thanks to Jan Kiszka !
etisserant
parents:
diff
changeset
|
22 |
|
8a1047ab51f4
SOCKET-CAN support added. Many thanks to Jan Kiszka !
etisserant
parents:
diff
changeset
|
23 |
#include <stdio.h> |
8a1047ab51f4
SOCKET-CAN support added. Many thanks to Jan Kiszka !
etisserant
parents:
diff
changeset
|
24 |
#include <string.h> |
8a1047ab51f4
SOCKET-CAN support added. Many thanks to Jan Kiszka !
etisserant
parents:
diff
changeset
|
25 |
#include <stdlib.h> |
196
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
26 |
#include <stddef.h> /* for NULL */ |
192 | 27 |
#include <errno.h> |
47
8a1047ab51f4
SOCKET-CAN support added. Many thanks to Jan Kiszka !
etisserant
parents:
diff
changeset
|
28 |
|
8a1047ab51f4
SOCKET-CAN support added. Many thanks to Jan Kiszka !
etisserant
parents:
diff
changeset
|
29 |
#include "config.h" |
8a1047ab51f4
SOCKET-CAN support added. Many thanks to Jan Kiszka !
etisserant
parents:
diff
changeset
|
30 |
|
8a1047ab51f4
SOCKET-CAN support added. Many thanks to Jan Kiszka !
etisserant
parents:
diff
changeset
|
31 |
#ifdef RTCAN_SOCKET |
8a1047ab51f4
SOCKET-CAN support added. Many thanks to Jan Kiszka !
etisserant
parents:
diff
changeset
|
32 |
#include "rtdm/rtcan.h" |
8a1047ab51f4
SOCKET-CAN support added. Many thanks to Jan Kiszka !
etisserant
parents:
diff
changeset
|
33 |
#define CAN_IFNAME "rtcan%s" |
8a1047ab51f4
SOCKET-CAN support added. Many thanks to Jan Kiszka !
etisserant
parents:
diff
changeset
|
34 |
#define CAN_SOCKET rt_dev_socket |
8a1047ab51f4
SOCKET-CAN support added. Many thanks to Jan Kiszka !
etisserant
parents:
diff
changeset
|
35 |
#define CAN_CLOSE rt_dev_close |
8a1047ab51f4
SOCKET-CAN support added. Many thanks to Jan Kiszka !
etisserant
parents:
diff
changeset
|
36 |
#define CAN_RECV rt_dev_recv |
8a1047ab51f4
SOCKET-CAN support added. Many thanks to Jan Kiszka !
etisserant
parents:
diff
changeset
|
37 |
#define CAN_SEND rt_dev_send |
8a1047ab51f4
SOCKET-CAN support added. Many thanks to Jan Kiszka !
etisserant
parents:
diff
changeset
|
38 |
#define CAN_BIND rt_dev_bind |
8a1047ab51f4
SOCKET-CAN support added. Many thanks to Jan Kiszka !
etisserant
parents:
diff
changeset
|
39 |
#define CAN_IOCTL rt_dev_ioctl |
196
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
40 |
#define CAN_ERRNO(err) (-err) |
47
8a1047ab51f4
SOCKET-CAN support added. Many thanks to Jan Kiszka !
etisserant
parents:
diff
changeset
|
41 |
#else |
187 | 42 |
#include <sys/socket.h> |
43 |
#include <sys/ioctl.h> |
|
44 |
#include "linux/can.h" |
|
45 |
#include "linux/can/raw.h" |
|
46 |
#include "net/if.h" |
|
47 |
#define PF_CAN 29 |
|
48 |
#define AF_CAN PF_CAN |
|
49 |
//#include "af_can.h" |
|
47
8a1047ab51f4
SOCKET-CAN support added. Many thanks to Jan Kiszka !
etisserant
parents:
diff
changeset
|
50 |
#define CAN_IFNAME "can%s" |
8a1047ab51f4
SOCKET-CAN support added. Many thanks to Jan Kiszka !
etisserant
parents:
diff
changeset
|
51 |
#define CAN_SOCKET socket |
8a1047ab51f4
SOCKET-CAN support added. Many thanks to Jan Kiszka !
etisserant
parents:
diff
changeset
|
52 |
#define CAN_CLOSE close |
8a1047ab51f4
SOCKET-CAN support added. Many thanks to Jan Kiszka !
etisserant
parents:
diff
changeset
|
53 |
#define CAN_RECV recv |
8a1047ab51f4
SOCKET-CAN support added. Many thanks to Jan Kiszka !
etisserant
parents:
diff
changeset
|
54 |
#define CAN_SEND send |
8a1047ab51f4
SOCKET-CAN support added. Many thanks to Jan Kiszka !
etisserant
parents:
diff
changeset
|
55 |
#define CAN_BIND bind |
8a1047ab51f4
SOCKET-CAN support added. Many thanks to Jan Kiszka !
etisserant
parents:
diff
changeset
|
56 |
#define CAN_IOCTL ioctl |
196
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
57 |
#define CAN_ERRNO(err) errno |
47
8a1047ab51f4
SOCKET-CAN support added. Many thanks to Jan Kiszka !
etisserant
parents:
diff
changeset
|
58 |
#endif |
8a1047ab51f4
SOCKET-CAN support added. Many thanks to Jan Kiszka !
etisserant
parents:
diff
changeset
|
59 |
|
8a1047ab51f4
SOCKET-CAN support added. Many thanks to Jan Kiszka !
etisserant
parents:
diff
changeset
|
60 |
#include "can_driver.h" |
8a1047ab51f4
SOCKET-CAN support added. Many thanks to Jan Kiszka !
etisserant
parents:
diff
changeset
|
61 |
|
8a1047ab51f4
SOCKET-CAN support added. Many thanks to Jan Kiszka !
etisserant
parents:
diff
changeset
|
62 |
/*********functions which permit to communicate with the board****************/ |
196
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
63 |
UNS8 |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
64 |
canReceive_driver (CAN_HANDLE fd0, Message * m) |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
65 |
{ |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
66 |
int res; |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
67 |
struct can_frame frame; |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
68 |
|
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
69 |
res = CAN_RECV (*(int *) fd0, &frame, sizeof (frame), 0); |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
70 |
if (res < 0) |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
71 |
{ |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
72 |
fprintf (stderr, "Recv failed: %s\n", strerror (CAN_ERRNO (res))); |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
73 |
return 1; |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
74 |
} |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
75 |
|
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
76 |
m->cob_id.w = frame.can_id & CAN_EFF_MASK; |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
77 |
m->len = frame.can_dlc; |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
78 |
if (frame.can_id & CAN_RTR_FLAG) |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
79 |
m->rtr = 1; |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
80 |
else |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
81 |
m->rtr = 0; |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
82 |
memcpy (m->data, frame.data, 8); |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
83 |
|
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
84 |
return 0; |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
85 |
} |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
86 |
|
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
87 |
|
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
88 |
/***************************************************************************/ |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
89 |
UNS8 |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
90 |
canSend_driver (CAN_HANDLE fd0, Message * m) |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
91 |
{ |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
92 |
int res; |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
93 |
struct can_frame frame; |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
94 |
|
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
95 |
frame.can_id = m->cob_id.w; |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
96 |
if (frame.can_id >= 0x800) |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
97 |
frame.can_id |= CAN_EFF_FLAG; |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
98 |
frame.can_dlc = m->len; |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
99 |
if (m->rtr) |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
100 |
frame.can_id |= CAN_RTR_FLAG; |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
101 |
else |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
102 |
memcpy (frame.data, m->data, 8); |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
103 |
|
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
104 |
res = CAN_SEND (*(int *) fd0, &frame, sizeof (frame), 0); |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
105 |
if (res < 0) |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
106 |
{ |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
107 |
fprintf (stderr, "Send failed: %s\n", strerror (CAN_ERRNO (res))); |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
108 |
return 1; |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
109 |
} |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
110 |
|
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
111 |
return 0; |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
112 |
} |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
113 |
|
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
114 |
/***************************************************************************/ |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
115 |
#ifdef RTCAN_SOCKET |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
116 |
int |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
117 |
TranslateBaudRate (const char *optarg) |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
118 |
{ |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
119 |
int baudrate; |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
120 |
int val, len; |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
121 |
char *pos = NULL; |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
122 |
|
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
123 |
len = strlen (optarg); |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
124 |
if (!len) |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
125 |
return 0; |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
126 |
|
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
127 |
switch ((int) optarg[len - 1]) |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
128 |
{ |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
129 |
case 'M': |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
130 |
baudrate = 1000000; |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
131 |
break; |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
132 |
case 'K': |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
133 |
baudrate = 1000; |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
134 |
break; |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
135 |
default: |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
136 |
baudrate = 1; |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
137 |
break; |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
138 |
} |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
139 |
if ((sscanf (optarg, "%i", &val)) == 1) |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
140 |
baudrate *= val; |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
141 |
else |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
142 |
baudrate = 0;; |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
143 |
|
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
144 |
return baudrate; |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
145 |
} |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
146 |
#endif |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
147 |
|
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
148 |
/***************************************************************************/ |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
149 |
CAN_HANDLE |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
150 |
canOpen_driver (s_BOARD * board) |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
151 |
{ |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
152 |
struct ifreq ifr; |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
153 |
struct sockaddr_can addr; |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
154 |
int err; |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
155 |
CAN_HANDLE fd0 = malloc (sizeof (int)); |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
156 |
#ifdef RTCAN_SOCKET |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
157 |
can_baudrate_t *baudrate; |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
158 |
can_mode_t *mode; |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
159 |
#endif |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
160 |
|
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
161 |
*(int *) fd0 = CAN_SOCKET (PF_CAN, SOCK_RAW, CAN_RAW); |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
162 |
if (*(int *) fd0 < 0) |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
163 |
{ |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
164 |
fprintf (stderr, "Socket creation failed: %s\n", |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
165 |
strerror (CAN_ERRNO (*(int *) fd0))); |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
166 |
goto error_ret; |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
167 |
} |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
168 |
|
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
169 |
if (*board->busname >= '0' && *board->busname <= '9') |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
170 |
snprintf (ifr.ifr_name, IFNAMSIZ, CAN_IFNAME, board->busname); |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
171 |
else |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
172 |
strncpy (ifr.ifr_name, board->busname, IFNAMSIZ); |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
173 |
err = CAN_IOCTL (*(int *) fd0, SIOCGIFINDEX, &ifr); |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
174 |
if (err) |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
175 |
{ |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
176 |
fprintf (stderr, "Getting IF index for %s failed: %s\n", |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
177 |
ifr.ifr_name, strerror (CAN_ERRNO (err))); |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
178 |
goto error_close; |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
179 |
} |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
180 |
|
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
181 |
addr.can_family = AF_CAN; |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
182 |
addr.can_ifindex = ifr.ifr_ifindex; |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
183 |
err = CAN_BIND (*(int *) fd0, (struct sockaddr *) &addr, sizeof (addr)); |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
184 |
if (err) |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
185 |
{ |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
186 |
fprintf (stderr, "Binding failed: %s\n", strerror (CAN_ERRNO (err))); |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
187 |
goto error_close; |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
188 |
} |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
189 |
|
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
190 |
#ifdef RTCAN_SOCKET |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
191 |
baudrate = (can_baudrate_t *) & ifr.ifr_ifru; |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
192 |
*baudrate = TranslateBaudRate (board->baudrate); |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
193 |
if (!*baudrate) |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
194 |
goto error_close; |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
195 |
|
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
196 |
err = CAN_IOCTL (*(int *) fd0, SIOCSCANBAUDRATE, &ifr); |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
197 |
if (err) |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
198 |
{ |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
199 |
fprintf (stderr, |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
200 |
"Setting baudrate %d failed: %s\n", |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
201 |
*baudrate, strerror (CAN_ERRNO (err))); |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
202 |
goto error_close; |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
203 |
} |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
204 |
|
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
205 |
mode = (can_mode_t *) & ifr.ifr_ifru; |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
206 |
*mode = CAN_MODE_START; |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
207 |
err = CAN_IOCTL (*(int *) fd0, SIOCSCANMODE, &ifr); |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
208 |
if (err) |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
209 |
{ |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
210 |
fprintf (stderr, "Starting CAN device failed: %s\n", |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
211 |
strerror (CAN_ERRNO (err))); |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
212 |
goto error_close; |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
213 |
} |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
214 |
#endif |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
215 |
|
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
216 |
return fd0; |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
217 |
|
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
218 |
error_close: |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
219 |
CAN_CLOSE (*(int *) fd0); |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
220 |
|
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
221 |
error_ret: |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
222 |
free (fd0); |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
223 |
return NULL; |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
224 |
} |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
225 |
|
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
226 |
/***************************************************************************/ |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
227 |
int |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
228 |
canClose_driver (CAN_HANDLE fd0) |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
229 |
{ |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
230 |
if (fd0) |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
231 |
{ |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
232 |
CAN_CLOSE (*(int *) fd0); |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
233 |
free (fd0); |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
234 |
} |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
235 |
return 0; |
65aa7a664f6f
Applied wolfgang changes, and re-indented socket_can.c file
etisserant
parents:
192
diff
changeset
|
236 |
} |