51 #define CAN_CLOSE close |
52 #define CAN_CLOSE close |
52 #define CAN_RECV recv |
53 #define CAN_RECV recv |
53 #define CAN_SEND send |
54 #define CAN_SEND send |
54 #define CAN_BIND bind |
55 #define CAN_BIND bind |
55 #define CAN_IOCTL ioctl |
56 #define CAN_IOCTL ioctl |
|
57 #define CAN_ERRNO(err) errno |
56 #endif |
58 #endif |
57 |
59 |
58 #include "can_driver.h" |
60 #include "can_driver.h" |
59 |
61 |
60 /*********functions which permit to communicate with the board****************/ |
62 /*********functions which permit to communicate with the board****************/ |
61 UNS8 canReceive_driver(CAN_HANDLE fd0, Message *m) |
63 UNS8 |
62 { |
64 canReceive_driver (CAN_HANDLE fd0, Message * m) |
63 int res; |
65 { |
64 struct can_frame frame; |
66 int res; |
65 |
67 struct can_frame frame; |
66 res = CAN_RECV(*(int*)fd0, &frame, sizeof(frame), 0); |
68 |
67 if (res < 0) |
69 res = CAN_RECV (*(int *) fd0, &frame, sizeof (frame), 0); |
68 return 1; |
70 if (res < 0) |
69 |
71 { |
70 m->cob_id.w = frame.can_id & CAN_EFF_MASK; |
72 fprintf (stderr, "Recv failed: %s\n", strerror (CAN_ERRNO (res))); |
71 m->len = frame.can_dlc; |
73 return 1; |
72 if (frame.can_id & CAN_RTR_FLAG) |
74 } |
73 m->rtr = 1; |
75 |
74 else |
76 m->cob_id.w = frame.can_id & CAN_EFF_MASK; |
75 m->rtr = 0; |
77 m->len = frame.can_dlc; |
76 memcpy(m->data, frame.data, 8); |
78 if (frame.can_id & CAN_RTR_FLAG) |
77 |
79 m->rtr = 1; |
78 return 0; |
80 else |
79 } |
81 m->rtr = 0; |
80 |
82 memcpy (m->data, frame.data, 8); |
81 |
83 |
82 /***************************************************************************/ |
84 return 0; |
83 UNS8 canSend_driver(CAN_HANDLE fd0, Message *m) |
85 } |
84 { |
86 |
85 int res; |
87 |
86 struct can_frame frame; |
88 /***************************************************************************/ |
87 |
89 UNS8 |
88 frame.can_id = m->cob_id.w; |
90 canSend_driver (CAN_HANDLE fd0, Message * m) |
89 if (frame.can_id >= 0x800) |
91 { |
90 frame.can_id |= CAN_EFF_FLAG; |
92 int res; |
91 frame.can_dlc = m->len; |
93 struct can_frame frame; |
92 if (m->rtr) |
94 |
93 frame.can_id |= CAN_RTR_FLAG; |
95 frame.can_id = m->cob_id.w; |
94 else |
96 if (frame.can_id >= 0x800) |
95 memcpy(frame.data, m->data, 8); |
97 frame.can_id |= CAN_EFF_FLAG; |
96 |
98 frame.can_dlc = m->len; |
97 res = CAN_SEND(*(int*)fd0, &frame, sizeof(frame), 0); |
99 if (m->rtr) |
98 if (res < 0) |
100 frame.can_id |= CAN_RTR_FLAG; |
99 return 1; |
101 else |
100 |
102 memcpy (frame.data, m->data, 8); |
101 return 0; |
103 |
102 } |
104 res = CAN_SEND (*(int *) fd0, &frame, sizeof (frame), 0); |
103 |
105 if (res < 0) |
104 /***************************************************************************/ |
106 { |
105 #ifdef RTCAN_SOCKET |
107 fprintf (stderr, "Send failed: %s\n", strerror (CAN_ERRNO (res))); |
106 int TranslateBaudRate(const char* optarg) { |
108 return 1; |
107 int baudrate; |
109 } |
108 int val, len; |
110 |
109 char *pos = NULL; |
111 return 0; |
110 |
112 } |
111 len = strlen(optarg); |
113 |
112 if (!len) |
114 /***************************************************************************/ |
113 return 0; |
115 #ifdef RTCAN_SOCKET |
114 |
116 int |
115 switch ((int)optarg[len - 1]) { |
117 TranslateBaudRate (const char *optarg) |
116 case 'M': |
118 { |
117 baudrate = 1000000; |
119 int baudrate; |
118 break; |
120 int val, len; |
119 case 'K': |
121 char *pos = NULL; |
120 baudrate = 1000; |
122 |
121 break; |
123 len = strlen (optarg); |
122 default: |
124 if (!len) |
123 baudrate = 1; |
125 return 0; |
124 break; |
126 |
125 } |
127 switch ((int) optarg[len - 1]) |
126 if ((sscanf(optarg, "%i", &val)) == 1) |
128 { |
127 baudrate *= val; |
129 case 'M': |
128 else |
130 baudrate = 1000000; |
129 baudrate = 0;; |
131 break; |
130 |
132 case 'K': |
131 printf("baudrate=%d (%s)\n", baudrate, optarg); |
133 baudrate = 1000; |
132 return baudrate; |
134 break; |
133 } |
135 default: |
134 #endif |
136 baudrate = 1; |
135 |
137 break; |
136 /***************************************************************************/ |
138 } |
137 CAN_HANDLE canOpen_driver(s_BOARD *board) |
139 if ((sscanf (optarg, "%i", &val)) == 1) |
138 { |
140 baudrate *= val; |
139 struct ifreq ifr; |
141 else |
140 struct sockaddr_can addr; |
142 baudrate = 0;; |
141 int err; |
143 |
142 CAN_HANDLE fd0 = malloc(sizeof(int)); |
144 return baudrate; |
143 #ifdef RTCAN_SOCKET |
145 } |
144 can_baudrate_t *baudrate; |
146 #endif |
145 can_mode_t *mode; |
147 |
146 #endif |
148 /***************************************************************************/ |
147 |
149 CAN_HANDLE |
148 *(int*)fd0 = CAN_SOCKET(PF_CAN, SOCK_RAW, CAN_RAW); |
150 canOpen_driver (s_BOARD * board) |
149 if (*(int*)fd0 < 0) { |
151 { |
150 perror("Socket creation failed"); |
152 struct ifreq ifr; |
151 goto error_ret; |
153 struct sockaddr_can addr; |
152 } |
154 int err; |
153 |
155 CAN_HANDLE fd0 = malloc (sizeof (int)); |
154 if (*board->busname >= '0' && *board->busname <= '9') |
156 #ifdef RTCAN_SOCKET |
155 snprintf(ifr.ifr_name, IFNAMSIZ, CAN_IFNAME, |
157 can_baudrate_t *baudrate; |
156 board->busname); |
158 can_mode_t *mode; |
157 else |
159 #endif |
158 strncpy(ifr.ifr_name, board->busname, IFNAMSIZ); |
160 |
159 err = CAN_IOCTL(*(int*)fd0, SIOCGIFINDEX, &ifr); |
161 *(int *) fd0 = CAN_SOCKET (PF_CAN, SOCK_RAW, CAN_RAW); |
160 if (err) { |
162 if (*(int *) fd0 < 0) |
161 fprintf(stderr, "Getting IF index for %s failed: %s\n", |
163 { |
162 ifr.ifr_name, strerror(errno)); |
164 fprintf (stderr, "Socket creation failed: %s\n", |
163 goto error_close; |
165 strerror (CAN_ERRNO (*(int *) fd0))); |
164 } |
166 goto error_ret; |
165 |
167 } |
166 addr.can_family = AF_CAN; |
168 |
167 addr.can_ifindex = ifr.ifr_ifindex; |
169 if (*board->busname >= '0' && *board->busname <= '9') |
168 err = CAN_BIND(*(int*)fd0, (struct sockaddr *)&addr, |
170 snprintf (ifr.ifr_name, IFNAMSIZ, CAN_IFNAME, board->busname); |
169 sizeof(addr)); |
171 else |
170 if (err) { |
172 strncpy (ifr.ifr_name, board->busname, IFNAMSIZ); |
171 perror("Binding failed"); |
173 err = CAN_IOCTL (*(int *) fd0, SIOCGIFINDEX, &ifr); |
172 goto error_close; |
174 if (err) |
173 } |
175 { |
174 |
176 fprintf (stderr, "Getting IF index for %s failed: %s\n", |
175 #ifdef RTCAN_SOCKET |
177 ifr.ifr_name, strerror (CAN_ERRNO (err))); |
176 baudrate = (can_baudrate_t *)&ifr.ifr_ifru; |
178 goto error_close; |
177 *baudrate = TranslateBaudRate(board->baudrate); |
179 } |
178 if (!*baudrate) |
180 |
179 goto error_close; |
181 addr.can_family = AF_CAN; |
180 |
182 addr.can_ifindex = ifr.ifr_ifindex; |
181 err = CAN_IOCTL(*(int*)fd0, SIOCSCANBAUDRATE, &ifr); |
183 err = CAN_BIND (*(int *) fd0, (struct sockaddr *) &addr, sizeof (addr)); |
182 if (err) { |
184 if (err) |
183 fprintf(stderr, |
185 { |
184 "Setting baudrate %d failed: %s\n", |
186 fprintf (stderr, "Binding failed: %s\n", strerror (CAN_ERRNO (err))); |
185 *baudrate, strerror(errno)); |
187 goto error_close; |
186 goto error_close; |
188 } |
187 } |
189 |
188 |
190 #ifdef RTCAN_SOCKET |
189 mode = (can_mode_t *)&ifr.ifr_ifru; |
191 baudrate = (can_baudrate_t *) & ifr.ifr_ifru; |
190 *mode = CAN_MODE_START; |
192 *baudrate = TranslateBaudRate (board->baudrate); |
191 err = CAN_IOCTL(*(int*)fd0, SIOCSCANMODE, &ifr); |
193 if (!*baudrate) |
192 if (err) { |
194 goto error_close; |
193 perror("Starting CAN device failed"); |
195 |
194 goto error_close; |
196 err = CAN_IOCTL (*(int *) fd0, SIOCSCANBAUDRATE, &ifr); |
195 } |
197 if (err) |
196 #endif |
198 { |
197 |
199 fprintf (stderr, |
198 return fd0; |
200 "Setting baudrate %d failed: %s\n", |
199 |
201 *baudrate, strerror (CAN_ERRNO (err))); |
200 error_close: |
202 goto error_close; |
201 CAN_CLOSE(*(int*)fd0); |
203 } |
202 |
204 |
203 error_ret: |
205 mode = (can_mode_t *) & ifr.ifr_ifru; |
204 free(fd0); |
206 *mode = CAN_MODE_START; |
205 return NULL; |
207 err = CAN_IOCTL (*(int *) fd0, SIOCSCANMODE, &ifr); |
206 } |
208 if (err) |
207 |
209 { |
208 /***************************************************************************/ |
210 fprintf (stderr, "Starting CAN device failed: %s\n", |
209 int canClose_driver(CAN_HANDLE fd0) |
211 strerror (CAN_ERRNO (err))); |
210 { |
212 goto error_close; |
211 if (fd0) { |
213 } |
212 CAN_CLOSE(*(int*)fd0); |
214 #endif |
213 free(fd0); |
215 |
214 } |
216 return fd0; |
215 return 0; |
217 |
216 } |
218 error_close: |
|
219 CAN_CLOSE (*(int *) fd0); |
|
220 |
|
221 error_ret: |
|
222 free (fd0); |
|
223 return NULL; |
|
224 } |
|
225 |
|
226 /***************************************************************************/ |
|
227 int |
|
228 canClose_driver (CAN_HANDLE fd0) |
|
229 { |
|
230 if (fd0) |
|
231 { |
|
232 CAN_CLOSE (*(int *) fd0); |
|
233 free (fd0); |
|
234 } |
|
235 return 0; |
|
236 } |