|
1 /* |
|
2 This file is part of CanFestival, a library implementing CanOpen Stack. |
|
3 |
|
4 Copyright (C): Jaroslav Fojtik |
|
5 */ |
|
6 |
|
7 #if defined(WIN32) && !defined(__CYGWIN__) |
|
8 #define usleep(micro) Sleep(micro%1000 ? (micro/1000) + 1 : (micro/1000)) |
|
9 #else |
|
10 #include <stdio.h> |
|
11 #include <string.h> |
|
12 #include <errno.h> |
|
13 #include <fcntl.h> |
|
14 #endif |
|
15 |
|
16 |
|
17 #include "cancfg.h" |
|
18 #include "can_driver.h" |
|
19 #include "def.h" |
|
20 |
|
21 UNS8 LIBAPI canSend_driver(CAN_HANDLE fd0, Message const *m); |
|
22 |
|
23 |
|
24 #define VERSION_2 |
|
25 |
|
26 /* dummy implementation for older version. */ |
|
27 #ifndef VERSION_2 |
|
28 void CAN_SetRcvEvent(HANDLE hEventx) |
|
29 { |
|
30 SetEvent(hEventx); |
|
31 } |
|
32 #endif |
|
33 |
|
34 |
|
35 #define SLAVE_COUNT 10 |
|
36 #define QueueSize 100 |
|
37 |
|
38 |
|
39 #ifndef extra_PCAN_init_params |
|
40 #define extra_PCAN_init_params /**/ |
|
41 #else |
|
42 long int print_getenv(const char* pcanparam) |
|
43 { |
|
44 char* param=NULL; |
|
45 long int res=0; |
|
46 |
|
47 param = getenv(pcanparam); |
|
48 if(param != NULL){ |
|
49 res = strtol(param,NULL,0); |
|
50 } |
|
51 else |
|
52 printf("Environment variable %s not defined !\n", pcanparam); |
|
53 printf("Found environment variable %s : %ld\n", pcanparam ,res); |
|
54 return res; |
|
55 } |
|
56 #define extra_PCAN_init_params\ |
|
57 ,print_getenv("PCANHwType")\ |
|
58 ,print_getenv("PCANIO_Port")\ |
|
59 ,print_getenv("PCANInterupt") |
|
60 #endif |
|
61 |
|
62 |
|
63 |
|
64 typedef struct |
|
65 { |
|
66 s_BOARD *board; |
|
67 Message MQueue[QueueSize]; |
|
68 unsigned QStart, QEnd; |
|
69 HANDLE hEventx; |
|
70 } QueueRecord; |
|
71 |
|
72 int initialisedQ = 0; |
|
73 QueueRecord Q_DATA[10]; |
|
74 |
|
75 |
|
76 /** Store message into a queue. */ |
|
77 static void PushMsgToQueue(QueueRecord *QR, Message *m) |
|
78 { |
|
79 if(QR==NULL || m==NULL) return; |
|
80 if(QR->board==NULL) return; // No Board assigned yet |
|
81 memcpy(&QR->MQueue[QR->QStart], m, sizeof(Message)); |
|
82 QR->QStart = (QR->QStart + 1) % QueueSize; |
|
83 if(QR->QEnd==QR->QStart) QR->QEnd = (QR->QEnd+1) % QueueSize; |
|
84 if(QR->hEventx) SetEvent(QR->hEventx); // Signalise internal flag |
|
85 } |
|
86 |
|
87 |
|
88 /** Get message from a queue. */ |
|
89 static int PopMsgFromQueue(QueueRecord *QR, Message *m) |
|
90 { |
|
91 if(QR==NULL || m==NULL) return 0; |
|
92 if(QR->QEnd == QR->QStart) return 0; |
|
93 |
|
94 memcpy(m, &QR->MQueue[QR->QEnd], sizeof(Message)); |
|
95 QR->QEnd = (QR->QEnd+1) % QueueSize; |
|
96 return 1; |
|
97 } |
|
98 |
|
99 |
|
100 /** Create the Event for the first board */ |
|
101 HANDLE hEvent1 = NULL; |
|
102 CRITICAL_SECTION InitLock; |
|
103 |
|
104 |
|
105 // Define for rtr CAN message |
|
106 #define CAN_INIT_TYPE_ST_RTR MSGTYPE_STANDARD | MSGTYPE_RTR |
|
107 |
|
108 |
|
109 /***************************************************************************/ |
|
110 static int TranslateBaudeRate(char* optarg) |
|
111 { |
|
112 if(!strcmp( optarg, "1M")) return CAN_BAUD_1M; |
|
113 if(!strcmp( optarg, "500K")) return CAN_BAUD_500K; |
|
114 if(!strcmp( optarg, "250K")) return CAN_BAUD_250K; |
|
115 if(!strcmp( optarg, "125K")) return CAN_BAUD_125K; |
|
116 if(!strcmp( optarg, "100K")) return CAN_BAUD_100K; |
|
117 if(!strcmp( optarg, "50K")) return CAN_BAUD_50K; |
|
118 if(!strcmp( optarg, "20K")) return CAN_BAUD_20K; |
|
119 if(!strcmp( optarg, "10K")) return CAN_BAUD_10K; |
|
120 if(!strcmp( optarg, "5K")) return CAN_BAUD_5K; |
|
121 if(!strcmp( optarg, "none")) return 0; |
|
122 return 0x0000; |
|
123 } |
|
124 |
|
125 |
|
126 static UNS8 canInit(s_BOARD *board) |
|
127 { |
|
128 int baudrate; |
|
129 int ret = 0; |
|
130 |
|
131 if(hEvent1==NULL) |
|
132 { //Create the Event for the first board |
|
133 hEvent1 = CreateEvent(NULL, // lpEventAttributes |
|
134 FALSE, // bManualReset |
|
135 FALSE, // bInitialState |
|
136 ""); // lpName |
|
137 InitializeCriticalSection(&InitLock); |
|
138 } |
|
139 |
|
140 EnterCriticalSection(&InitLock); |
|
141 |
|
142 if(baudrate = TranslateBaudeRate(board->baudrate)) |
|
143 { |
|
144 ret = CAN_Init(baudrate, CAN_INIT_TYPE_ST extra_PCAN_init_params); |
|
145 if(ret != CAN_ERR_OK) |
|
146 { |
|
147 LeaveCriticalSection(&InitLock); |
|
148 return 0; |
|
149 } |
|
150 } |
|
151 |
|
152 CAN_SetRcvEvent(hEvent1); //Set Event Handler for CANReadExt |
|
153 LeaveCriticalSection(&InitLock); |
|
154 return 1; |
|
155 } |
|
156 |
|
157 |
|
158 /********* functions which permit to communicate with the board ****************/ |
|
159 UNS8 LIBAPI canReceive_driver(CAN_HANDLE fd0, Message *m) |
|
160 { |
|
161 static int HeavyCounter = 0; |
|
162 int ret=0; |
|
163 UNS8 data; |
|
164 TPCANMsg peakMsg; |
|
165 DWORD Res; |
|
166 DWORD result; |
|
167 HANDLE hh[2]; |
|
168 int i; |
|
169 |
|
170 #ifdef CAN_READ_EX |
|
171 TPCANTimestamp peakRcvTime; |
|
172 #endif |
|
173 |
|
174 i = strtol(((s_BOARD *)fd0)->busname,NULL,0); |
|
175 if(i>=SLAVE_COUNT || i<0) return 1; // error |
|
176 if(Q_DATA[i].board!=(s_BOARD *)fd0) return 1; // error |
|
177 |
|
178 hh[0]=hEvent1; hh[1]=Q_DATA[i].hEventx; |
|
179 |
|
180 // loop until valid message or fatal error |
|
181 do |
|
182 { |
|
183 CONTINUE: |
|
184 if(PopMsgFromQueue(&Q_DATA[i],m)) return 0; //message is waiting in the internal queue |
|
185 |
|
186 // We read the queue looking for messages. |
|
187 #ifdef VERSION_2 |
|
188 result = WaitForMultipleObjects(2,hh,FALSE,15); |
|
189 if(Q_DATA[i].board==NULL) return 1; //exit hook, exit immediatelly when device is closed |
|
190 |
|
191 if(result == WAIT_OBJECT_0+1) |
|
192 goto CONTINUE; //look to a PopMsgFromQueue() (continue will check while(), goto skips it) |
|
193 |
|
194 if(result==WAIT_OBJECT_0 || result==WAIT_TIMEOUT) |
|
195 { |
|
196 #endif |
|
197 #ifdef CAN_READ_EX |
|
198 Res = CAN_ReadEx(&peakMsg, &peakRcvTime); |
|
199 #else |
|
200 Res = CAN_Read(&peakMsg); |
|
201 #endif |
|
202 // Exit receive thread when handle is no more valid |
|
203 #ifdef CAN_ERRMASK_ILLHANDLE |
|
204 if(Res & CAN_ERRMASK_ILLHANDLE) return 1; |
|
205 #else |
|
206 if(Res & CAN_ERR_ILLHANDLE) return 1; |
|
207 #endif |
|
208 |
|
209 #ifndef VERSION_2 |
|
210 if(Res != CAN_ERR_OK) |
|
211 result = WaitForSingleObject(hEvent1, 1); //pooling for pcan release<2 |
|
212 #endif |
|
213 if(Res==CAN_ERR_QRCVEMPTY) goto CONTINUE; |
|
214 #ifdef VERSION_2 |
|
215 } |
|
216 else |
|
217 { |
|
218 //if(result==WAIT_TIMEOUT || result==(WAIT_OBJECT_0+1)) |
|
219 // Res = CAN_ERR_BUSLIGHT; |
|
220 //else |
|
221 Res = CAN_ERR_UNKNOWN; |
|
222 } |
|
223 #endif |
|
224 |
|
225 if(Res==CAN_ERR_BUSHEAVY) |
|
226 { |
|
227 if(HeavyCounter++>10) |
|
228 { |
|
229 HeavyCounter = 0; |
|
230 Res=CAN_ERR_BUSOFF; |
|
231 } |
|
232 } |
|
233 |
|
234 if(Res & CAN_ERR_BUSOFF) |
|
235 { |
|
236 peakMsg.MSGTYPE = MSGTYPE_STATUS; |
|
237 peakMsg.DATA[3] = CAN_ERR_BUSOFF; |
|
238 Res = CAN_ERR_OK; |
|
239 } |
|
240 |
|
241 // A message was received : we process the message(s) |
|
242 if(Res == CAN_ERR_OK) |
|
243 { |
|
244 // if something different that 11bit or rtr... problem |
|
245 switch(peakMsg.MSGTYPE) |
|
246 { |
|
247 case MSGTYPE_STATUS: |
|
248 switch(peakMsg.DATA[3]) |
|
249 { |
|
250 case CAN_ERR_BUSHEAVY: |
|
251 break; |
|
252 case CAN_ERR_BUSOFF: |
|
253 printf ("Peak board read BUSOFF: re-init!!!\n"); |
|
254 canInit((s_BOARD*)fd0); |
|
255 usleep(2000); |
|
256 break; |
|
257 } |
|
258 return peakMsg.DATA[3]; /* if something different that 11bit or rtr... problem */ |
|
259 |
|
260 case MSGTYPE_STANDARD: /* bits of MSGTYPE_ */ |
|
261 case MSGTYPE_EXTENDED: |
|
262 m->rtr = 0; |
|
263 break; |
|
264 |
|
265 case MSGTYPE_RTR: /* bits of MSGTYPE_ */ |
|
266 m->rtr = 1; |
|
267 break; |
|
268 |
|
269 default: return CAN_ERR_OVERRUN; /* If status, return status if 29bit, return overrun. */ |
|
270 |
|
271 } |
|
272 |
|
273 m->cob_id = peakMsg.ID; |
|
274 |
|
275 if (peakMsg.MSGTYPE == CAN_INIT_TYPE_ST) /* bits of MSGTYPE_ */ |
|
276 m->rtr = 0; |
|
277 else |
|
278 m->rtr = 1; |
|
279 m->len = peakMsg.LEN; /* count of data bytes (0..8) */ |
|
280 for(data=0; data<peakMsg.LEN; data++) |
|
281 m->Data[data] = peakMsg.DATA[data]; /* data bytes, up to 8 */ |
|
282 #if defined DEBUG_MSG_CONSOLE_ON |
|
283 MSG("in : "); |
|
284 print_message(m); |
|
285 #endif |
|
286 } |
|
287 else |
|
288 { // not benign error => fatal error |
|
289 if(!(Res & CAN_ERR_QRCVEMPTY |
|
290 || Res & CAN_ERR_BUSLIGHT |
|
291 || Res & CAN_ERR_BUSHEAVY)) |
|
292 { |
|
293 printf ("canReceive returned error (%d)\n", Res); |
|
294 return 1; |
|
295 } |
|
296 } |
|
297 } while(Res != CAN_ERR_OK); |
|
298 |
|
299 |
|
300 // populate message received to other drivers |
|
301 for(i=0; i<SLAVE_COUNT; i++) |
|
302 { |
|
303 if(Q_DATA[i].board != (s_BOARD *)fd0) // do not populate to own queue |
|
304 { |
|
305 PushMsgToQueue(&Q_DATA[i],m); |
|
306 } |
|
307 } |
|
308 |
|
309 return 0; |
|
310 } |
|
311 |
|
312 |
|
313 /***************************************************************************/ |
|
314 UNS8 LIBAPI canSend_driver(CAN_HANDLE fd0, Message const *m) |
|
315 { |
|
316 UNS8 data; |
|
317 TPCANMsg peakMsg; |
|
318 int i, j; |
|
319 int loc_errno; |
|
320 int MaxLoops = 100; |
|
321 |
|
322 i = -1; |
|
323 for(j=0; j<SLAVE_COUNT; j++) |
|
324 { |
|
325 if(Q_DATA[j].board != (s_BOARD *)fd0) // store this message forr all other drivers |
|
326 { |
|
327 PushMsgToQueue(&Q_DATA[j],m); |
|
328 i = j; |
|
329 } |
|
330 } |
|
331 |
|
332 if(i<0) return 1; // no board found |
|
333 |
|
334 peakMsg.ID = m->cob_id; /* 11/29 bit code */ |
|
335 if(m->rtr == 0) |
|
336 { |
|
337 if(peakMsg.ID > 0x7FF) |
|
338 peakMsg.MSGTYPE = MSGTYPE_EXTENDED; /* bits of MSGTYPE_ */ |
|
339 else |
|
340 peakMsg.MSGTYPE = MSGTYPE_STANDARD; /* bits of MSGTYPE_ */ |
|
341 } |
|
342 else |
|
343 peakMsg.MSGTYPE = MSGTYPE_RTR; /* bits of MSGTYPE_ */ |
|
344 |
|
345 peakMsg.LEN = m->len; |
|
346 /* count of data bytes (0..8) */ |
|
347 for(data = 0; data < m->len; data++) |
|
348 peakMsg.DATA[data] = m->Data[data]; /* data bytes, up to 8 */ |
|
349 |
|
350 do |
|
351 { |
|
352 errno = loc_errno = CAN_Write(&peakMsg); |
|
353 |
|
354 if(loc_errno) |
|
355 { |
|
356 if(loc_errno==CAN_ERR_BUSOFF && (MaxLoops%20)==1) |
|
357 { |
|
358 #if defined DEBUG_MSG_CONSOLE_ON |
|
359 printf ("Peak board write: re-init!!!\n"); |
|
360 #endif |
|
361 canInit((s_BOARD*)fd0); |
|
362 usleep(1000); |
|
363 } |
|
364 usleep(80); |
|
365 } |
|
366 if(MaxLoops-- == 0) break; // limit max looping |
|
367 } while(loc_errno != CAN_ERR_OK); |
|
368 |
|
369 #if defined DEBUG_MSG_CONSOLE_ON |
|
370 MSG("out : "); |
|
371 print_message(m); |
|
372 #endif |
|
373 return 0; |
|
374 } |
|
375 |
|
376 |
|
377 /***************************************************************************/ |
|
378 UNS8 LIBAPI canChangeBaudRate_driver(CAN_HANDLE fd, char* baud) |
|
379 { |
|
380 printf("canChangeBaudRate not yet supported by this driver\n"); |
|
381 return 0; |
|
382 } |
|
383 |
|
384 |
|
385 /***************************************************************************/ |
|
386 LIBPUBLIC CAN_HANDLE LIBAPI canOpen_driver(s_BOARD * board) |
|
387 { |
|
388 int ret; |
|
389 int i; |
|
390 |
|
391 if(!initialisedQ) |
|
392 { |
|
393 memset(Q_DATA,0,sizeof(Q_DATA)); |
|
394 initialisedQ = 1; |
|
395 } |
|
396 |
|
397 i = strtol(board->busname,NULL,0); // Get slot name |
|
398 //printf ("Board Busname=%d.\n",strtol(board->busname, &pEnd,0)); |
|
399 if(i<SLAVE_COUNT && i>=0) |
|
400 { |
|
401 Q_DATA[i].board = board; |
|
402 //printf ("First Board selected\n"); |
|
403 if(Q_DATA[i].hEventx==NULL) // Create local event |
|
404 { |
|
405 Q_DATA[i].hEventx = CreateEvent(NULL, FALSE, FALSE, ""); |
|
406 } |
|
407 |
|
408 if(hEvent1!=NULL) return (CAN_HANDLE)board; // Create global event, if needed |
|
409 |
|
410 ret = canInit(board); |
|
411 if(ret) |
|
412 return (CAN_HANDLE)board; |
|
413 } |
|
414 |
|
415 return NULL; |
|
416 } |
|
417 |
|
418 |
|
419 /***************************************************************************/ |
|
420 int LIBAPI canClose_driver(CAN_HANDLE fd0) |
|
421 { |
|
422 s_BOARD *x_board = NULL; |
|
423 int ActiveBoards = 0; |
|
424 int i; |
|
425 |
|
426 if((s_BOARD *)fd0==NULL) return 0; |
|
427 for(i=0; i<SLAVE_COUNT; i++) |
|
428 { |
|
429 if(Q_DATA[i].board == (s_BOARD *)fd0) |
|
430 { |
|
431 x_board = Q_DATA[i].board; |
|
432 Q_DATA[i].board = NULL; |
|
433 CloseHandle(Q_DATA[i].hEventx); |
|
434 Q_DATA[i].hEventx = NULL; |
|
435 } |
|
436 else |
|
437 ActiveBoards++; |
|
438 } |
|
439 |
|
440 if(ActiveBoards<=0) |
|
441 { // No can device is used. |
|
442 CAN_SetRcvEvent(NULL); |
|
443 CAN_Close(); |
|
444 if(hEvent1) |
|
445 { |
|
446 SetEvent(hEvent1); |
|
447 CloseHandle(hEvent1); |
|
448 hEvent1 = NULL; |
|
449 } |
|
450 } |
|
451 else |
|
452 SetEvent(hEvent1); |
|
453 |
|
454 return 0; |
|
455 } |
|
456 |
|
457 |
|
458 |
|
459 |
|
460 |
|
461 |