author | Christian Taedcke |
Thu, 09 Sep 2010 15:38:20 +0200 | |
changeset 635 | 30a0e926bb2e |
parent 517 | 003679edc437 |
permissions | -rw-r--r-- |
343 | 1 |
/* |
2 |
This file is part of CanFestival, a library implementing CanOpen Stack. |
|
3 |
||
4 |
||
5 |
Copyright (C): Jorge Berzosa |
|
6 |
||
7 |
||
8 |
See COPYING file for copyrights details. |
|
9 |
||
10 |
||
11 |
This library is free software; you can redistribute it and/or |
|
12 |
modify it under the terms of the GNU Lesser General Public |
|
13 |
License as published by the Free Software Foundation; either |
|
14 |
version 2.1 of the License, or (at your option) any later version. |
|
15 |
||
16 |
||
17 |
This library is distributed in the hope that it will be useful, |
|
18 |
but WITHOUT ANY WARRANTY; without even the implied warranty of |
|
19 |
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU |
|
20 |
Lesser General Public License for more details. |
|
21 |
||
22 |
||
23 |
You should have received a copy of the GNU Lesser General Public |
|
24 |
License along with this library; if not, write to the Free Software |
|
25 |
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA |
|
26 |
*/ |
|
27 |
||
28 |
||
29 |
/*! |
|
30 |
** @file lss.c |
|
31 |
** @author Jorge Berzosa |
|
32 |
** @date Mon Oct 22 05:44:32 2007 |
|
33 |
** |
|
34 |
** @brief |
|
35 |
** |
|
36 |
** |
|
37 |
*/ |
|
38 |
||
39 |
||
40 |
#include "data.h" |
|
41 |
#include "lss.h" |
|
42 |
#include "canfestival.h" |
|
370
6fecf36df407
Fixed some endianization problems caused by switch to UNS16 for cob_id member in CAN messages. To be continued.
etisserant
parents:
365
diff
changeset
|
43 |
#include "sysdep.h" |
343 | 44 |
|
482
ec49f7132e36
LSS patch from Edward. Now, SetNodeId is not called from nmtslave anymore in case of LSS.
etisserant
parents:
436
diff
changeset
|
45 |
#ifdef CO_ENABLE_LSS |
ec49f7132e36
LSS patch from Edward. Now, SetNodeId is not called from nmtslave anymore in case of LSS.
etisserant
parents:
436
diff
changeset
|
46 |
|
361 | 47 |
//#define LSS_TIMEOUT_MS (TIMEVAL)1000 /* ms */ |
48 |
//#define LSS_FS_TIMEOUT_MS (TIMEVAL)100 /* ms */ |
|
343 | 49 |
|
50 |
/* Returns the LSS ident field from a Message struct */ |
|
436 | 51 |
#define getLSSIdent(msg) (((UNS32)msg->data[4] << 24) | ((UNS32)msg->data[3] << 16) | (msg->data[2] << 8) | (msg->data[1])) |
52 |
||
343 | 53 |
|
54 |
/* Returns the LSS switch delay field from a Message struct */ |
|
55 |
#define getLSSDelay(msg) ((msg->data[2] << 8) | (msg->data[1])) |
|
56 |
||
57 |
/* Returns the LSS FastScan BitCheck field from a Message struct */ |
|
58 |
#define getLSSBitCheck(msg) msg->data[5] |
|
59 |
||
60 |
/* Returns the LSS FastScan LSSSub field from a Message struct */ |
|
61 |
#define getLSSSub(msg) msg->data[6] |
|
62 |
||
63 |
/* Returns the LSS FastScan LSSNext field from a Message struct */ |
|
64 |
#define getLSSNext(msg) msg->data[7] |
|
65 |
||
66 |
/* Prototypes for internals functions */ |
|
381 | 67 |
UNS8 sendMasterLSSMessage(CO_Data* d, UNS8 command,void *dat1,void *dat2); |
68 |
void LssAlarmMSG(CO_Data* d, UNS32 id); |
|
69 |
void LssAlarmSDELAY(CO_Data* d, UNS32 id); |
|
70 |
||
71 |
||
72 |
#define StopLSS_MSG_TIMER(){\ |
|
73 |
MSG_WAR(0x3D01, "StopLSS_MSG_TIMER", 0);\ |
|
74 |
d->lss_transfer.timerMSG = DelAlarm(d->lss_transfer.timerMSG);} |
|
343 | 75 |
|
76 |
#define StartLSS_MSG_TIMER(){\ |
|
381 | 77 |
MSG_WAR(0x3D02, "StartLSS_MSG_TIMER",0);\ |
78 |
d->lss_transfer.timerMSG = SetAlarm(d,0,&LssAlarmMSG,MS_TO_TIMEVAL(LSS_TIMEOUT_MS),0);} |
|
79 |
||
80 |
#define StopLSS_SDELAY_TIMER(){\ |
|
81 |
MSG_WAR(0x3D03, "StopLSS_SDELAY_TIMER", 0);\ |
|
82 |
d->lss_transfer.timerSDELAY = DelAlarm(d->lss_transfer.timerSDELAY);} |
|
343 | 83 |
|
84 |
#define StartLSS_SDELAY_TIMER(){\ |
|
381 | 85 |
MSG_WAR(0x3D04, "StartLSS_SDELAY_TIMER",0);\ |
86 |
d->lss_transfer.timerSDELAY= SetAlarm(d,0,&LssAlarmSDELAY,MS_TO_TIMEVAL(d->lss_transfer.switchDelay),MS_TO_TIMEVAL(d->lss_transfer.switchDelay));} |
|
87 |
||
88 |
||
89 |
#ifdef CO_ENABLE_LSS_FS |
|
90 |
/* Prototypes for internals functions */ |
|
91 |
void LssAlarmFS(CO_Data* d, UNS32 id); |
|
92 |
||
93 |
#define StopLSS_FS_TIMER(){\ |
|
94 |
MSG_WAR(0x3D05, "StopLSS_FS_TIMER", id);\ |
|
95 |
d->lss_transfer.timerFS = DelAlarm(d->lss_transfer.timerFS);} |
|
343 | 96 |
|
361 | 97 |
#define StartLSS_FS_TIMER(){\ |
381 | 98 |
MSG_WAR(0x3D06, "StartLSS_FS_TIMER",0);\ |
99 |
d->lss_transfer.timerFS = SetAlarm(d,0,&LssAlarmFS,MS_TO_TIMEVAL(LSS_FS_TIMEOUT_MS),0);} |
|
100 |
#endif |
|
101 |
||
102 |
||
103 |
void LssAlarmMSG(CO_Data* d, UNS32 id) |
|
104 |
{ |
|
105 |
StopLSS_MSG_TIMER(); |
|
106 |
#ifdef CO_ENABLE_LSS_FS |
|
107 |
if(d->lss_transfer.command==LSS_IDENT_FASTSCAN){ |
|
108 |
if(d->lss_transfer.FastScan_SM==LSS_FS_RESET){ |
|
109 |
/* if at least one node had answered before the timer expired, start the FastScan protocol*/ |
|
110 |
if(d->lss_transfer.LSSanswer!=0){ |
|
517 | 111 |
UNS32 Mask=0xFFFFFFFF; |
381 | 112 |
d->lss_transfer.LSSanswer=0; |
517 | 113 |
d->lss_transfer.BitChecked=d->lss_transfer.lss_fs_transfer.FS_BitChecked[0]; |
114 |
Mask=(UNS32)((UNS64)Mask<<(d->lss_transfer.BitChecked+1)); |
|
115 |
d->lss_transfer.IDNumber=d->lss_transfer.lss_fs_transfer.FS_LSS_ID[0] & Mask; |
|
381 | 116 |
d->lss_transfer.FastScan_SM=LSS_FS_PROCESSING; |
517 | 117 |
//printf("BitChecked=%d, IDNumber=%x MASK=%x\n",d->lss_transfer.BitChecked,d->lss_transfer.IDNumber,Mask); |
381 | 118 |
StartLSS_FS_TIMER(); |
119 |
sendMasterLSSMessage(d,LSS_IDENT_FASTSCAN,0,0); |
|
120 |
return; |
|
121 |
} |
|
122 |
else{ |
|
123 |
||
124 |
d->lss_transfer.state = LSS_FINISHED; |
|
125 |
/* Inform the application that there aren't not configured nodes in the net */ |
|
126 |
d->lss_transfer.dat1=1; |
|
127 |
} |
|
128 |
} |
|
129 |
else{ |
|
130 |
/* This should not happen, an error ocurred*/ |
|
131 |
MSG_ERR(0x1D07, "LSS FastScan timeout. FastScan_SM inconsisten state.", d->lss_transfer.FastScan_SM); |
|
517 | 132 |
d->lss_transfer.state = LSS_ABORTED_INTERNAL; |
133 |
d->lss_transfer.FastScan_SM=LSS_FS_RESET; |
|
381 | 134 |
} |
135 |
} |
|
136 |
else |
|
137 |
#endif |
|
138 |
if(d->lss_transfer.command==LSS_IDENT_REMOTE_NON_CONF){ |
|
139 |
MSG_WAR(0x2D08, "LSS timeout. There are not no-configured slaves in the net", 0); |
|
140 |
d->lss_transfer.state = LSS_FINISHED; |
|
141 |
d->lss_transfer.dat1=1; |
|
142 |
} |
|
143 |
else{ |
|
144 |
MSG_ERR(0x1D09, "LSS timeout. LSS response not received.", 0); |
|
145 |
MSG_WAR(0x2D0A, "LSS timeout command specifier : ", d->lss_transfer.command); |
|
146 |
/* Set aborted state */ |
|
147 |
d->lss_transfer.state = LSS_ABORTED_INTERNAL; |
|
517 | 148 |
#ifdef CO_ENABLE_LSS_FS |
149 |
d->lss_transfer.FastScan_SM = LSS_FS_RESET; |
|
150 |
#endif |
|
381 | 151 |
} |
152 |
||
153 |
/* Call the user function to inform of the problem.*/ |
|
154 |
if(d->lss_transfer.Callback){ |
|
155 |
/*If there is a callback, it is responsible of the error*/ |
|
156 |
(*d->lss_transfer.Callback)(d,d->lss_transfer.command); |
|
157 |
} |
|
158 |
} |
|
159 |
||
160 |
||
343 | 161 |
/*! |
162 |
** |
|
163 |
** |
|
164 |
** @param d |
|
165 |
** @param id |
|
166 |
**/ |
|
381 | 167 |
void LssAlarmSDELAY(CO_Data* d, UNS32 id) |
343 | 168 |
{ |
381 | 169 |
|
170 |
/* The first switch_delay period expired. Store the node state, change it |
|
171 |
* so no CAN messages will be sent or received, call the ChangeBaudRate function*/ |
|
172 |
if(d->lss_transfer.switchDelayState==SDELAY_FIRST){ |
|
173 |
MSG_WAR(0x3D0B, "LSS switch delay first period expired",0); |
|
174 |
d->lss_transfer.switchDelayState=SDELAY_SECOND; |
|
384 | 175 |
//(*d->lss_ChangeBaudRate)(d,d->lss_transfer.baudRate); |
176 |
canChangeBaudRate(d->lss_transfer.canHandle_t, d->lss_transfer.baudRate); |
|
381 | 177 |
} |
178 |
else{ /* d->lss_transfer.switchDelayState==SDELAY_SECOND */ |
|
179 |
MSG_WAR(0x3D0C, "LSS switch delay second period expired",0); |
|
180 |
d->lss_transfer.switchDelayState=SDELAY_OFF; |
|
181 |
StopLSS_SDELAY_TIMER(); |
|
182 |
||
183 |
if (*(d->iam_a_slave)) |
|
184 |
d->canHandle=d->lss_transfer.canHandle_t; |
|
185 |
else{ |
|
186 |
d->lss_transfer.dat1=0; |
|
187 |
d->lss_transfer.state=LSS_FINISHED; |
|
188 |
/* Call the user function */ |
|
189 |
if(d->lss_transfer.Callback){ |
|
190 |
(*d->lss_transfer.Callback)(d,d->lss_transfer.command); |
|
191 |
} |
|
192 |
} |
|
193 |
} |
|
194 |
} |
|
195 |
||
361 | 196 |
#ifdef CO_ENABLE_LSS_FS |
381 | 197 |
/*! |
198 |
** |
|
199 |
** |
|
200 |
** @param d |
|
201 |
** @param id |
|
202 |
**/ |
|
203 |
void LssAlarmFS(CO_Data* d, UNS32 id) |
|
204 |
{ |
|
205 |
StopLSS_FS_TIMER(); |
|
206 |
||
207 |
switch(d->lss_transfer.FastScan_SM){ |
|
208 |
case LSS_FS_RESET: |
|
209 |
{ |
|
210 |
/* This should not happen, an error ocurred*/ |
|
211 |
MSG_ERR(0x1D0D, "LSS FastScan timeout. FastScan_SM inconsisten state.", d->lss_transfer.FastScan_SM); |
|
212 |
} |
|
213 |
break; |
|
214 |
case LSS_FS_PROCESSING: |
|
215 |
{ |
|
216 |
/* If there isn't any answer, set the bit */ |
|
217 |
if(d->lss_transfer.LSSanswer==0){ |
|
218 |
UNS32 Mask=0x1; |
|
219 |
Mask<<=d->lss_transfer.BitChecked; |
|
220 |
d->lss_transfer.IDNumber|=Mask; |
|
221 |
} |
|
222 |
||
223 |
if(d->lss_transfer.BitChecked==0){ |
|
224 |
/* We finished with the current LSS-ID[sub], confirm it */ |
|
225 |
d->lss_transfer.FastScan_SM=LSS_FS_CONFIRMATION; |
|
226 |
if(d->lss_transfer.LSSNext<3)d->lss_transfer.LSSNext++; |
|
227 |
} |
|
228 |
else{ |
|
229 |
d->lss_transfer.BitChecked--; |
|
230 |
} |
|
517 | 231 |
//printf("BitChecked=%d, IDNumber=%x\n",d->lss_transfer.BitChecked,d->lss_transfer.IDNumber); |
381 | 232 |
d->lss_transfer.LSSanswer=0; |
233 |
StartLSS_FS_TIMER(); |
|
234 |
sendMasterLSSMessage(d,LSS_IDENT_FASTSCAN,0,0); |
|
235 |
return; |
|
236 |
} |
|
237 |
break; |
|
238 |
case LSS_FS_CONFIRMATION: |
|
239 |
{ |
|
240 |
if(d->lss_transfer.LSSanswer!=0){ |
|
241 |
d->lss_transfer.LSSanswer=0; |
|
242 |
||
243 |
if(d->lss_transfer.LSSSub==3){ |
|
244 |
/* The LSS FastScan protocol finished correctly. Restore the parameters */ |
|
245 |
d->lss_transfer.BitChecked=128; |
|
246 |
d->lss_transfer.FastScan_SM=LSS_FS_RESET; |
|
247 |
d->lss_transfer.LSSSub=0; |
|
248 |
d->lss_transfer.LSSNext=0; |
|
249 |
d->lss_transfer.IDNumber=0; |
|
250 |
||
251 |
/* Inform the application that the FastScan finished correctly */ |
|
252 |
d->lss_transfer.state = LSS_FINISHED; |
|
253 |
d->lss_transfer.dat1=0; |
|
254 |
} |
|
255 |
else{ |
|
517 | 256 |
UNS32 Mask=0xFFFFFFFF; |
381 | 257 |
/* Start with the next LSS-ID[sub] */ |
258 |
d->lss_transfer.LSSSub++; |
|
517 | 259 |
d->lss_transfer.BitChecked=d->lss_transfer.lss_fs_transfer.FS_BitChecked[d->lss_transfer.LSSSub]; |
260 |
Mask=(UNS32)((UNS64)Mask<<(d->lss_transfer.BitChecked+1)); |
|
261 |
d->lss_transfer.IDNumber=d->lss_transfer.lss_fs_transfer.FS_LSS_ID[d->lss_transfer.LSSSub] & Mask; |
|
381 | 262 |
d->lss_transfer.FastScan_SM=LSS_FS_PROCESSING; |
517 | 263 |
//printf("BitChecked=%d, IDNumber=%x MASK=%x\n",d->lss_transfer.BitChecked,d->lss_transfer.IDNumber,Mask); |
381 | 264 |
StartLSS_FS_TIMER(); |
265 |
sendMasterLSSMessage(d,LSS_IDENT_FASTSCAN,0,0); |
|
266 |
return; |
|
267 |
} |
|
268 |
} |
|
269 |
else{ |
|
270 |
/* This should not happen, an error ocurred*/ |
|
271 |
MSG_ERR(0x1D0E, "LSS FastScan timeout. FastScan response not received.", 0); |
|
517 | 272 |
MSG_ERR(0x1D0E, "There is not any node with LSS_ID# =>", d->lss_transfer.LSSSub); |
273 |
MSG_ERR(0x1D0E, "with the value =>", d->lss_transfer.IDNumber); |
|
381 | 274 |
/* Set aborted state */ |
275 |
d->lss_transfer.state = LSS_ABORTED_INTERNAL; |
|
517 | 276 |
d->lss_transfer.FastScan_SM = LSS_FS_RESET; |
381 | 277 |
} |
278 |
} |
|
279 |
break; |
|
280 |
} |
|
281 |
||
282 |
/* Call the user function to inform of the problem.*/ |
|
283 |
if(d->lss_transfer.Callback){ |
|
284 |
/*If there is a callback, it is responsible of the error*/ |
|
285 |
(*d->lss_transfer.Callback)(d,d->lss_transfer.command); |
|
286 |
} |
|
287 |
} |
|
361 | 288 |
#endif |
381 | 289 |
|
290 |
||
343 | 291 |
/*! |
292 |
** |
|
293 |
** |
|
294 |
** @param d |
|
295 |
**/ |
|
296 |
void startLSS(CO_Data* d) |
|
297 |
{ |
|
298 |
/*MSG_WAR(0x3D09, "LSS services started",0);*/ |
|
299 |
} |
|
300 |
||
301 |
/*! |
|
302 |
** |
|
303 |
** |
|
304 |
** @param d |
|
305 |
**/ |
|
306 |
void stopLSS(CO_Data* d) |
|
307 |
{ |
|
308 |
/*MSG_WAR(0x3D09, "LSS services stopped",0);*/ |
|
309 |
} |
|
310 |
||
311 |
/*! |
|
312 |
** |
|
313 |
** |
|
314 |
** @param d |
|
315 |
** @param cob_id |
|
316 |
** |
|
317 |
** @return |
|
318 |
**/ |
|
319 |
UNS8 sendSlaveLSSMessage(CO_Data* d, UNS8 command,void *dat1,void *dat2) |
|
320 |
{ |
|
321 |
Message m; |
|
322 |
UNS8 i; |
|
323 |
||
324 |
if (!d->CurrentCommunicationState.csLSS){ |
|
381 | 325 |
MSG_WAR(0x2D17, "unable to send the LSS message, not in the proper state =>", d->nodeState); |
361 | 326 |
return 0xFF; |
343 | 327 |
} |
328 |
||
329 |
for(i=1;i<8;i++)m.data[i]=0; |
|
330 |
m.len = 8; |
|
331 |
m.rtr = NOT_A_REQUEST; |
|
332 |
m.data[0]=command; |
|
370
6fecf36df407
Fixed some endianization problems caused by switch to UNS16 for cob_id member in CAN messages. To be continued.
etisserant
parents:
365
diff
changeset
|
333 |
m.cob_id=UNS16_LE(SLSS_ADRESS); |
343 | 334 |
|
335 |
/* Tha data sent with the msg depends on the command */ |
|
336 |
switch(command){ |
|
337 |
case LSS_INQ_NODE_ID: /* Inquire Node-ID */ |
|
338 |
m.data[1]=*(UNS8 *)dat1; |
|
339 |
break; |
|
340 |
case LSS_CONF_NODE_ID: /* Configure Node-ID */ |
|
341 |
case LSS_CONF_BIT_TIMING: /* Configure Bit Timing Parameters */ |
|
342 |
case LSS_CONF_STORE: /* Store Configured Parameters */ |
|
343 |
m.data[1]=*(UNS8 *)dat1; |
|
344 |
m.data[2]=*(UNS8 *)dat2; |
|
345 |
break; |
|
346 |
case LSS_INQ_VENDOR_ID: /* Inquire Identity Vendor-ID */ |
|
347 |
case LSS_INQ_PRODUCT_CODE: /* Inquire Identity Product-Code */ |
|
348 |
case LSS_INQ_REV_NUMBER: /* Inquire Identity Revision-Number */ |
|
349 |
case LSS_INQ_SERIAL_NUMBER: /* Inquire Identity Serial-Number */ |
|
350 |
m.data[1]=(UNS8)(*(UNS32*)dat1 & 0xFF); |
|
351 |
m.data[2]=(UNS8)(*(UNS32*)dat1>>8 & 0xFF); |
|
352 |
m.data[3]=(UNS8)(*(UNS32*)dat1>>16 & 0xFF); |
|
353 |
m.data[4]=(UNS8)(*(UNS32*)dat1>>24 & 0xFF); |
|
354 |
break; |
|
355 |
case LSS_SM_SELECTIVE_RESP: /* Switch Mode Selective response*/ |
|
356 |
case LSS_IDENT_SLAVE: /* LSS Identify Slave */ |
|
357 |
case LSS_IDENT_NON_CONF_SLAVE: /* LSS identify non-configured remote slave */ |
|
358 |
break; |
|
359 |
default: |
|
381 | 360 |
MSG_ERR(0x1D18, "send Slave LSS command not implemented", command); |
343 | 361 |
return 0xFF; |
362 |
} |
|
363 |
||
364 |
return canSend(d->canHandle,&m); |
|
365 |
} |
|
384 | 366 |
|
367 |
/* If a baud rate is not supported just comment the line. */ |
|
368 |
static UNS8 CO_TranslateBaudRate(char* optarg){ |
|
369 |
if(!strcmp( optarg, "1M")) return 0x00; |
|
370 |
if(!strcmp( optarg, "800K")) return 0x01; |
|
371 |
if(!strcmp( optarg, "500K")) return 0x02; |
|
372 |
if(!strcmp( optarg, "250K")) return 0x03; |
|
373 |
if(!strcmp( optarg, "125K")) return 0x04; |
|
374 |
if(!strcmp( optarg, "100K")) return 0x05; |
|
375 |
if(!strcmp( optarg, "50K")) return 0x06; |
|
376 |
if(!strcmp( optarg, "20K")) return 0x07; |
|
377 |
if(!strcmp( optarg, "10K")) return 0x08; |
|
399 | 378 |
return 0xFF; |
384 | 379 |
} |
343 | 380 |
|
381 |
/*! |
|
382 |
** |
|
383 |
** |
|
384 |
** @param d |
|
385 |
** @param cob_id |
|
386 |
** |
|
387 |
** @return |
|
388 |
**/ |
|
389 |
UNS8 sendMasterLSSMessage(CO_Data* d, UNS8 command,void *dat1,void *dat2) |
|
390 |
{ |
|
391 |
Message m; |
|
392 |
UNS8 i; |
|
381 | 393 |
UNS8 res; |
394 |
UNS8 hasResponse=0; |
|
395 |
||
343 | 396 |
for(i=1;i<8;i++)m.data[i]=0; |
397 |
m.len = 8; |
|
398 |
m.rtr = NOT_A_REQUEST; |
|
399 |
m.data[0]=command; |
|
370
6fecf36df407
Fixed some endianization problems caused by switch to UNS16 for cob_id member in CAN messages. To be continued.
etisserant
parents:
365
diff
changeset
|
400 |
m.cob_id=UNS16_LE(MLSS_ADRESS); |
343 | 401 |
|
402 |
/* Tha data sent with the msg depends on the command */ |
|
403 |
switch(command){ |
|
381 | 404 |
case LSS_CONF_NODE_ID: /* Configure Node-ID */ |
405 |
hasResponse=1; |
|
343 | 406 |
case LSS_SM_GLOBAL: /* Switch Mode Global */ |
407 |
m.data[1]=*(UNS8 *)dat1; |
|
408 |
break; |
|
409 |
case LSS_CONF_BIT_TIMING: /* Configure Bit Timing Parameters */ |
|
384 | 410 |
|
343 | 411 |
m.data[1]=*(UNS8 *)dat1; |
384 | 412 |
d->lss_transfer.baudRate=*(char **)dat2; |
413 |
||
399 | 414 |
if((m.data[2]=CO_TranslateBaudRate(d->lss_transfer.baudRate))!=0xFF){ |
384 | 415 |
hasResponse=1; |
416 |
break; |
|
417 |
} |
|
418 |
||
419 |
MSG_ERR(0x1D19, "Master-> Baud rate not supported",0); |
|
420 |
d->lss_transfer.dat1=0xFF; |
|
421 |
||
422 |
/* if bit timing is not supported comment the previous code and uncomment the following one*/ |
|
423 |
/*{ |
|
381 | 424 |
MSG_ERR(0x1D1A, "Master-> Bit timing not supported",0); |
425 |
d->lss_transfer.dat1=0x01; |
|
384 | 426 |
}*/ |
427 |
||
381 | 428 |
d->lss_transfer.dat2=0; |
429 |
/* If there is a callback, it is responsible of the error */ |
|
430 |
if(d->lss_transfer.Callback) |
|
384 | 431 |
(*d->lss_transfer.Callback)(d,d->lss_transfer.command); |
381 | 432 |
return 0xFF; |
399 | 433 |
//break; |
343 | 434 |
case LSS_CONF_ACT_BIT_TIMING: /* Activate Bit Timing Parameters */ |
435 |
m.data[1]=(UNS8)(*(UNS32*)dat1 & 0xFF); |
|
436 |
m.data[2]=(UNS8)(*(UNS32*)dat1>>8 & 0xFF); |
|
381 | 437 |
if(d->lss_transfer.baudRate!="none"){ |
438 |
d->lss_transfer.switchDelay=(UNS16)(*(UNS32*)dat1 & 0xFFFF); |
|
439 |
d->lss_transfer.switchDelayState=SDELAY_FIRST; |
|
384 | 440 |
d->lss_transfer.canHandle_t=d->canHandle; |
381 | 441 |
res=canSend(d->canHandle,&m); |
442 |
if(res==0){ |
|
443 |
StartLSS_SDELAY_TIMER(); |
|
444 |
d->lss_transfer.state=LSS_TRANS_IN_PROGRESS; |
|
445 |
} |
|
446 |
return res; |
|
447 |
} |
|
448 |
else{ |
|
449 |
MSG_ERR(0x1D1B, "Master-> Baud rate not specified",0); |
|
450 |
d->lss_transfer.dat1=1; |
|
451 |
/* If there is a callback, it is responsible of the error */ |
|
452 |
if(d->lss_transfer.Callback){ |
|
453 |
(*d->lss_transfer.Callback)(d,d->lss_transfer.command); |
|
454 |
} |
|
455 |
return 0xFF; |
|
456 |
} |
|
399 | 457 |
//break; |
381 | 458 |
case LSS_SM_SELECTIVE_SERIAL: |
459 |
case LSS_IDENT_REMOTE_SERIAL_HIGH: |
|
460 |
hasResponse=1; |
|
343 | 461 |
case LSS_SM_SELECTIVE_VENDOR: /* Switch Mode Selective */ |
462 |
case LSS_SM_SELECTIVE_PRODUCT: |
|
463 |
case LSS_SM_SELECTIVE_REVISION: |
|
464 |
case LSS_IDENT_REMOTE_VENDOR: /* LSS Identify Remote Slaves */ |
|
465 |
case LSS_IDENT_REMOTE_PRODUCT: |
|
466 |
case LSS_IDENT_REMOTE_REV_LOW: |
|
467 |
case LSS_IDENT_REMOTE_REV_HIGH: |
|
468 |
case LSS_IDENT_REMOTE_SERIAL_LOW: |
|
469 |
m.data[1]=(UNS8)(*(UNS32*)dat1 & 0xFF); |
|
470 |
m.data[2]=(UNS8)(*(UNS32*)dat1>>8 & 0xFF); |
|
471 |
m.data[3]=(UNS8)(*(UNS32*)dat1>>16 & 0xFF); |
|
472 |
m.data[4]=(UNS8)(*(UNS32*)dat1>>24 & 0xFF); |
|
473 |
break; |
|
381 | 474 |
|
343 | 475 |
case LSS_CONF_STORE: /* Store Configured Parameters */ |
476 |
case LSS_IDENT_REMOTE_NON_CONF: /* LSS identify non-configured remote slave */ |
|
477 |
case LSS_INQ_VENDOR_ID: /* Inquire Identity Vendor-ID */ |
|
478 |
case LSS_INQ_PRODUCT_CODE: /* Inquire Identity Product-Code */ |
|
479 |
case LSS_INQ_REV_NUMBER: /* Inquire Identity Revision-Number */ |
|
480 |
case LSS_INQ_SERIAL_NUMBER: /* Inquire Identity Serial-Number */ |
|
481 |
case LSS_INQ_NODE_ID: /* Inquire Node-ID */ |
|
381 | 482 |
hasResponse=1; |
343 | 483 |
break; |
361 | 484 |
#ifdef CO_ENABLE_LSS_FS |
343 | 485 |
case LSS_IDENT_FASTSCAN: |
517 | 486 |
if(d->lss_transfer.FastScan_SM==LSS_FS_RESET){ |
487 |
UNS8 i; |
|
488 |
/* Initialize the lss_fs_transfer FastScan parameters */ |
|
489 |
for(i=0;i<4;i++){ |
|
490 |
d->lss_transfer.lss_fs_transfer.FS_LSS_ID[i]=(*(lss_fs_transfer_t*)dat1).FS_LSS_ID[i]; |
|
491 |
d->lss_transfer.lss_fs_transfer.FS_BitChecked[i]=(*(lss_fs_transfer_t*)dat1).FS_BitChecked[i]; |
|
492 |
/* Adjust BitChecked from 32-1 to 31-0 */ |
|
493 |
if(d->lss_transfer.lss_fs_transfer.FS_BitChecked[i]>0)d->lss_transfer.lss_fs_transfer.FS_BitChecked[i]--; |
|
494 |
} |
|
495 |
||
496 |
d->lss_transfer.IDNumber=0; |
|
497 |
d->lss_transfer.BitChecked=128; |
|
498 |
d->lss_transfer.LSSSub=0; |
|
499 |
d->lss_transfer.LSSNext=0; |
|
500 |
||
501 |
/* it will generate a response only if it is the start of the FastScan protocol*/ |
|
502 |
hasResponse=1; |
|
503 |
} |
|
343 | 504 |
m.data[1]=(UNS8)(d->lss_transfer.IDNumber & 0xFF); |
505 |
m.data[2]=(UNS8)(d->lss_transfer.IDNumber>>8 & 0xFF); |
|
506 |
m.data[3]=(UNS8)(d->lss_transfer.IDNumber>>16 & 0xFF); |
|
507 |
m.data[4]=(UNS8)(d->lss_transfer.IDNumber>>24 & 0xFF); |
|
508 |
m.data[5]=d->lss_transfer.BitChecked; |
|
509 |
m.data[6]=d->lss_transfer.LSSSub; |
|
510 |
m.data[7]=d->lss_transfer.LSSNext; |
|
511 |
break; |
|
361 | 512 |
#endif |
343 | 513 |
default: |
381 | 514 |
MSG_ERR(0x1D1C, "send Master LSS command not implemented", command); |
343 | 515 |
return 0xFF; |
516 |
} |
|
517 |
||
381 | 518 |
res=canSend(d->canHandle,&m); |
519 |
if(res==0 && hasResponse==1){ |
|
520 |
StartLSS_MSG_TIMER(); |
|
521 |
d->lss_transfer.state=LSS_TRANS_IN_PROGRESS; |
|
522 |
} |
|
523 |
return res; |
|
343 | 524 |
} |
525 |
||
526 |
/*! |
|
527 |
** |
|
528 |
** |
|
529 |
** @param d |
|
530 |
** @param cob_id |
|
531 |
** |
|
532 |
** @return |
|
533 |
**/ |
|
534 |
UNS8 sendLSS(CO_Data* d, UNS8 command,void *dat1,void *dat2) |
|
535 |
{ |
|
536 |
UNS8 res=1; |
|
537 |
||
538 |
/* Tha data sent with the msg depends on the command and if the sender is a master or a slave */ |
|
539 |
if (*(d->iam_a_slave)){ |
|
540 |
res = sendSlaveLSSMessage(d, command,dat1,dat2); |
|
541 |
} |
|
542 |
else {/* It is a Master */ |
|
543 |
res = sendMasterLSSMessage(d, command,dat1,dat2); |
|
544 |
} |
|
545 |
return res ; |
|
546 |
} |
|
547 |
||
548 |
||
549 |
/*! |
|
550 |
** |
|
551 |
** |
|
552 |
** @param d |
|
553 |
** @param m |
|
554 |
** |
|
555 |
** @return |
|
556 |
**/ |
|
557 |
UNS8 proceedLSS_Master(CO_Data* d, Message* m ) |
|
558 |
{ |
|
559 |
UNS8 msg_cs; |
|
560 |
UNS32 Dat1=0; |
|
561 |
UNS8 Dat2=0; |
|
562 |
||
361 | 563 |
if(d->lss_transfer.state!=LSS_TRANS_IN_PROGRESS) |
564 |
{ |
|
565 |
//MSG_WAR(0x3D0D, "MasterLSS proceedLSS; unexpected message arrived;command ", m->data[0]); |
|
566 |
//return 0; |
|
567 |
goto ErrorProcessMaster; |
|
568 |
} |
|
569 |
||
381 | 570 |
MSG_WAR(0x3D1E, "MasterLSS proceedLSS; command ", m->data[0]); |
343 | 571 |
|
572 |
switch(msg_cs=m->data[0]){ |
|
573 |
case LSS_INQ_NODE_ID: /* Inquire Node-ID */ |
|
361 | 574 |
if(d->lss_transfer.command!=LSS_INQ_NODE_ID)goto ErrorProcessMaster; |
343 | 575 |
Dat1=m->data[1]; |
576 |
break; |
|
577 |
case LSS_CONF_NODE_ID: /* Configure Node-ID */ |
|
578 |
case LSS_CONF_BIT_TIMING: /* Configure Bit Timing Parameters */ |
|
579 |
case LSS_CONF_STORE: /* Store Configured Parameters */ |
|
361 | 580 |
if(d->lss_transfer.command!=msg_cs)goto ErrorProcessMaster; |
343 | 581 |
Dat1=m->data[1]; |
582 |
Dat2=m->data[2]; |
|
583 |
break; |
|
584 |
case LSS_INQ_VENDOR_ID: /* Inquire Identity Vendor-ID */ |
|
585 |
case LSS_INQ_PRODUCT_CODE: /* Inquire Identity Product-Code */ |
|
586 |
case LSS_INQ_REV_NUMBER: /* Inquire Identity Revision-Number */ |
|
587 |
case LSS_INQ_SERIAL_NUMBER: /* Inquire Identity Serial-Number */ |
|
361 | 588 |
if(d->lss_transfer.command!=msg_cs)goto ErrorProcessMaster; |
343 | 589 |
Dat1=getLSSIdent(m); |
590 |
break; |
|
591 |
case LSS_IDENT_SLAVE: /* LSS Identify Slave */ |
|
361 | 592 |
#ifdef CO_ENABLE_LSS_FS |
593 |
if(d->lss_transfer.command==LSS_IDENT_FASTSCAN){ |
|
594 |
/* A message arrived during the timer period */ |
|
595 |
d->lss_transfer.LSSanswer=1; |
|
596 |
return 0; |
|
597 |
} |
|
598 |
else |
|
599 |
#endif |
|
600 |
if(d->lss_transfer.command!=LSS_IDENT_REMOTE_VENDOR && \ |
|
601 |
d->lss_transfer.command!=LSS_IDENT_REMOTE_PRODUCT && \ |
|
602 |
d->lss_transfer.command!=LSS_IDENT_REMOTE_REV_LOW && \ |
|
603 |
d->lss_transfer.command!=LSS_IDENT_REMOTE_REV_HIGH && \ |
|
604 |
d->lss_transfer.command!=LSS_IDENT_REMOTE_SERIAL_LOW && \ |
|
605 |
d->lss_transfer.command!=LSS_IDENT_REMOTE_SERIAL_HIGH ) |
|
606 |
goto ErrorProcessMaster; |
|
343 | 607 |
break; |
608 |
case LSS_SM_SELECTIVE_RESP: /* Switch Mode Selective response */ |
|
361 | 609 |
if(d->lss_transfer.command!=LSS_SM_SELECTIVE_VENDOR && \ |
610 |
d->lss_transfer.command!=LSS_SM_SELECTIVE_PRODUCT && \ |
|
611 |
d->lss_transfer.command!=LSS_SM_SELECTIVE_REVISION && \ |
|
612 |
d->lss_transfer.command!=LSS_SM_SELECTIVE_SERIAL ) |
|
613 |
goto ErrorProcessMaster; |
|
614 |
break; |
|
343 | 615 |
case LSS_IDENT_NON_CONF_SLAVE: /* LSS identify non-configured remote slave */ |
361 | 616 |
if(d->lss_transfer.command!=LSS_IDENT_REMOTE_NON_CONF)goto ErrorProcessMaster; |
343 | 617 |
break; |
618 |
default: |
|
381 | 619 |
MSG_ERR(0x1D1F, "Master LSS command not implemented", msg_cs); |
343 | 620 |
return 0xFF; |
621 |
} |
|
361 | 622 |
|
381 | 623 |
StopLSS_MSG_TIMER(); |
361 | 624 |
d->lss_transfer.state = LSS_FINISHED; |
625 |
||
343 | 626 |
d->lss_transfer.dat1=Dat1; |
627 |
d->lss_transfer.dat2=Dat2; |
|
628 |
/* If there is a callback, it is responsible of the received response */ |
|
629 |
if(d->lss_transfer.Callback) |
|
361 | 630 |
(*d->lss_transfer.Callback)(d,d->lss_transfer.command); |
343 | 631 |
|
632 |
return 0; |
|
361 | 633 |
|
634 |
ErrorProcessMaster: |
|
381 | 635 |
MSG_WAR(0x3D20, "MasterLSS proceedLSS; unexpected message arrived;command ", m->data[0]); |
361 | 636 |
return 0xFF; |
637 |
||
343 | 638 |
} |
639 |
||
640 |
/*! |
|
641 |
** |
|
642 |
** |
|
643 |
** @param d |
|
644 |
** @param m |
|
645 |
** |
|
646 |
** @return |
|
647 |
**/ |
|
648 |
UNS8 proceedLSS_Slave(CO_Data* d, Message* m ) |
|
649 |
{ |
|
650 |
UNS8 msg_cs; |
|
651 |
||
381 | 652 |
MSG_WAR(0x3D21, "SlaveLSS proceedLSS; command ", m->data[0]); |
343 | 653 |
|
654 |
switch(msg_cs=m->data[0]){ |
|
655 |
case LSS_SM_GLOBAL: /* Switch Mode Global */ |
|
656 |
/* if there is not a mode change break*/ |
|
657 |
if(m->data[1] == d->lss_transfer.mode){ |
|
381 | 658 |
MSG_WAR(0x3D22, "SlaveLSS already in the mode ", m->data[1]); |
343 | 659 |
break; |
660 |
} |
|
661 |
||
662 |
if(m->data[1]==LSS_CONFIGURATION_MODE) { |
|
381 | 663 |
MSG_WAR(0x3D23, "SlaveLSS switching to configuration mode ", 0); |
343 | 664 |
/* Store the NodeId in case it will be changed */ |
381 | 665 |
//d->lss_transfer.nodeID=getNodeId(d); |
343 | 666 |
d->lss_transfer.mode=LSS_CONFIGURATION_MODE; |
667 |
} |
|
668 |
else if(m->data[1]==LSS_WAITING_MODE){ |
|
381 | 669 |
MSG_WAR(0x3D24, "SlaveLSS switching to operational mode ", 0); |
343 | 670 |
|
381 | 671 |
/* If the nodeID has changed update it and put the node state to Initialisation. */ |
672 |
if(d->lss_transfer.nodeID!=getNodeId(d)){ |
|
673 |
if(getNodeId(d)==0xFF){/* The nodeID was 0xFF; initialize the application*/ |
|
674 |
MSG_WAR(0x3D25, "The node Id has changed. Reseting to Initialisation state",0); |
|
343 | 675 |
setNodeId(d, d->lss_transfer.nodeID); |
676 |
setState(d, Initialisation); |
|
677 |
} |
|
517 | 678 |
else{/* The nodeID will be changed on NMT_Reset_Comunication Request*/ |
381 | 679 |
} |
343 | 680 |
} |
681 |
d->lss_transfer.mode=LSS_WAITING_MODE; |
|
682 |
} |
|
683 |
break; |
|
684 |
case LSS_CONF_NODE_ID: /* Configure Node-ID */ |
|
685 |
{ |
|
686 |
UNS8 error_code=0; |
|
687 |
UNS8 spec_error=0; |
|
688 |
||
689 |
if(d->lss_transfer.mode==LSS_CONFIGURATION_MODE){ |
|
690 |
if(m->data[1]>127 && m->data[1]!=0xFF){ |
|
381 | 691 |
MSG_ERR(0x1D26, "NodeID out of range",0); |
343 | 692 |
error_code=1; /* NodeID out of range */ |
693 |
} |
|
694 |
else{ |
|
695 |
d->lss_transfer.nodeID=m->data[1]; |
|
696 |
} |
|
697 |
} |
|
698 |
else{ |
|
381 | 699 |
MSG_WAR(0x3D27, "SlaveLSS not in configuration mode",0); |
361 | 700 |
//error_code=0xFF; |
701 |
break; |
|
343 | 702 |
} |
703 |
sendSlaveLSSMessage(d,msg_cs,&error_code,&spec_error); |
|
704 |
} |
|
705 |
break; |
|
706 |
case LSS_CONF_BIT_TIMING: /* Configure Bit Timing Parameters */ |
|
707 |
{ |
|
708 |
UNS8 error_code=0; |
|
709 |
UNS8 spec_error=0; |
|
710 |
||
711 |
if(d->lss_transfer.mode==LSS_CONFIGURATION_MODE){ |
|
384 | 712 |
/* If a baud rate is not supported just comment the line. */ |
713 |
switch(m->data[2]){ |
|
714 |
case 0x00:d->lss_transfer.baudRate="1M";break; |
|
715 |
case 0x01:d->lss_transfer.baudRate="800K";break; |
|
716 |
case 0x02:d->lss_transfer.baudRate="500K";break; |
|
717 |
case 0x03:d->lss_transfer.baudRate="250K";break; |
|
718 |
case 0x04:d->lss_transfer.baudRate="125K";break; |
|
719 |
case 0x05:d->lss_transfer.baudRate="100K";break; |
|
720 |
case 0x06:d->lss_transfer.baudRate="50K";break; |
|
721 |
case 0x07:d->lss_transfer.baudRate="20K";break; |
|
722 |
case 0x08:d->lss_transfer.baudRate="10K";break; |
|
723 |
default: |
|
724 |
MSG_ERR(0x1D28, "Baud rate not supported",0); |
|
725 |
error_code=0xFF; /* Baud rate not supported*/ |
|
726 |
break; |
|
343 | 727 |
} |
728 |
} |
|
729 |
else{ |
|
381 | 730 |
MSG_WAR(0x3D2A, "SlaveLSS not in configuration mode",0); |
361 | 731 |
//error_code=0xFF; |
732 |
break; |
|
343 | 733 |
} |
734 |
||
384 | 735 |
/* if bit timing is not supported comment the previous code and uncomment the following */ |
736 |
/*{ |
|
737 |
MSG_ERR(0x1D29, "Bit timing not supported",0); |
|
738 |
error_code=0x01; // bit timing not supported |
|
739 |
}*/ |
|
740 |
||
343 | 741 |
sendSlaveLSSMessage(d,msg_cs,&error_code,&spec_error); |
742 |
} |
|
743 |
break; |
|
744 |
case LSS_CONF_ACT_BIT_TIMING: /* Activate Bit Timing Parameters */ |
|
745 |
||
746 |
if(d->lss_transfer.mode!=LSS_CONFIGURATION_MODE){ |
|
381 | 747 |
MSG_ERR(0x3D2B, "SlaveLSS not in configuration mode",0); |
343 | 748 |
break; |
749 |
} |
|
750 |
||
751 |
if(d->lss_transfer.baudRate!="none"){ |
|
752 |
d->lss_transfer.switchDelay=getLSSDelay(m); |
|
381 | 753 |
MSG_WAR(0x3D2C, "Slave Switch Delay set to: ",d->lss_transfer.switchDelay); |
343 | 754 |
d->lss_transfer.switchDelayState=SDELAY_FIRST; |
381 | 755 |
//d->lss_transfer.currentState=getState(d); |
756 |
//setState(d, LssTimingDelay); |
|
757 |
d->lss_transfer.canHandle_t=d->canHandle; |
|
758 |
d->canHandle=NULL; |
|
343 | 759 |
StartLSS_SDELAY_TIMER(); |
760 |
} |
|
761 |
break; |
|
762 |
case LSS_CONF_STORE: /* Store Configured Parameters */ |
|
763 |
{ |
|
764 |
UNS8 error_code=0; |
|
765 |
UNS8 spec_error=0; |
|
766 |
||
767 |
if(d->lss_transfer.mode==LSS_CONFIGURATION_MODE){ |
|
768 |
if(d->lss_StoreConfiguration){ |
|
769 |
/* call lss_StoreConfiguration with NodeId */ |
|
381 | 770 |
(*d->lss_StoreConfiguration)(d,&error_code,&spec_error); |
343 | 771 |
} |
772 |
else{ |
|
381 | 773 |
MSG_ERR(0x1D2E, "Store configuration not supported",0); |
343 | 774 |
error_code=1; /* store configuration is not supported */ |
775 |
} |
|
776 |
} |
|
777 |
else{ |
|
381 | 778 |
MSG_WAR(0x3D2F, "SlaveLSS not in configuration mode",0); |
361 | 779 |
//error_code=0xFF; |
780 |
break; |
|
343 | 781 |
} |
782 |
sendSlaveLSSMessage(d,msg_cs,&error_code,&spec_error); |
|
783 |
} |
|
784 |
break; |
|
785 |
case LSS_SM_SELECTIVE_VENDOR: /* Switch Mode Selective */ |
|
786 |
case LSS_SM_SELECTIVE_PRODUCT: |
|
787 |
case LSS_SM_SELECTIVE_REVISION: |
|
788 |
case LSS_SM_SELECTIVE_SERIAL: |
|
789 |
{ |
|
790 |
UNS32 errorCode; |
|
791 |
const indextable *ptrTable; |
|
792 |
ODCallback_t *Callback; |
|
793 |
UNS32 _SpecificNodeInfo; |
|
794 |
||
795 |
if(d->lss_transfer.mode==LSS_CONFIGURATION_MODE) |
|
796 |
{ |
|
381 | 797 |
MSG_ERR(0x1D30, "Switch Mode Selective only supported in operational mode",0); |
343 | 798 |
break; |
799 |
} |
|
800 |
||
801 |
_SpecificNodeInfo=getLSSIdent(m); |
|
802 |
||
803 |
ptrTable = (*d->scanIndexOD)(0x1018, &errorCode, &Callback); |
|
804 |
if(_SpecificNodeInfo==*(UNS32*)ptrTable->pSubindex[msg_cs-(LSS_SM_SELECTIVE_VENDOR-1)].pObject){ |
|
805 |
||
806 |
d->lss_transfer.addr_sel_match|=(0x01<<(msg_cs-LSS_SM_SELECTIVE_VENDOR)); |
|
807 |
/* If all the fields has been set */ |
|
808 |
if(d->lss_transfer.addr_sel_match==0x0F){ |
|
809 |
||
381 | 810 |
MSG_WAR(0x3D31, "SlaveLSS switching to configuration mode ", 0); |
343 | 811 |
d->lss_transfer.addr_sel_match=0; |
812 |
d->lss_transfer.nodeID=getNodeId(d); |
|
813 |
d->lss_transfer.mode=LSS_CONFIGURATION_MODE; |
|
814 |
||
815 |
sendSlaveLSSMessage(d,LSS_SM_SELECTIVE_RESP,0,0); |
|
816 |
} |
|
817 |
} |
|
818 |
else { |
|
381 | 819 |
MSG_WAR(0x3D32, "LSS identity field doesn't match ", _SpecificNodeInfo); |
343 | 820 |
d->lss_transfer.addr_sel_match=0; |
821 |
} |
|
822 |
} |
|
823 |
break; |
|
824 |
case LSS_IDENT_REMOTE_VENDOR: /* LSS Identify Remote Slaves */ |
|
825 |
case LSS_IDENT_REMOTE_PRODUCT: |
|
826 |
case LSS_IDENT_REMOTE_REV_LOW: |
|
827 |
case LSS_IDENT_REMOTE_REV_HIGH: |
|
828 |
case LSS_IDENT_REMOTE_SERIAL_LOW: |
|
829 |
case LSS_IDENT_REMOTE_SERIAL_HIGH: |
|
830 |
{ |
|
831 |
UNS32 errorCode; |
|
832 |
const indextable *ptrTable; |
|
833 |
ODCallback_t *Callback; |
|
834 |
UNS32 _SpecificNodeInfo; |
|
835 |
||
836 |
_SpecificNodeInfo=getLSSIdent(m); |
|
837 |
||
838 |
ptrTable = (*d->scanIndexOD)(0x1018, &errorCode, &Callback); |
|
839 |
||
840 |
/* Check if the data match the identity object. */ |
|
841 |
switch(msg_cs){ |
|
842 |
case LSS_IDENT_REMOTE_VENDOR:d->lss_transfer.addr_ident_match=(_SpecificNodeInfo == *(UNS32*)ptrTable->pSubindex[1].pObject)? d->lss_transfer.addr_ident_match|0x01:0; break; |
|
843 |
case LSS_IDENT_REMOTE_PRODUCT:d->lss_transfer.addr_ident_match=(_SpecificNodeInfo == *(UNS32*)ptrTable->pSubindex[2].pObject)? d->lss_transfer.addr_ident_match|0x02:0; break; |
|
844 |
case LSS_IDENT_REMOTE_REV_LOW:d->lss_transfer.addr_ident_match=(_SpecificNodeInfo <= *(UNS32*)ptrTable->pSubindex[3].pObject)? d->lss_transfer.addr_ident_match|0x04:0; break; |
|
845 |
case LSS_IDENT_REMOTE_REV_HIGH:d->lss_transfer.addr_ident_match=(_SpecificNodeInfo >= *(UNS32*)ptrTable->pSubindex[3].pObject)? d->lss_transfer.addr_ident_match|0x08:0; break; |
|
846 |
case LSS_IDENT_REMOTE_SERIAL_LOW:d->lss_transfer.addr_ident_match=(_SpecificNodeInfo <= *(UNS32*)ptrTable->pSubindex[4].pObject)? d->lss_transfer.addr_ident_match|0x10:0; break; |
|
847 |
case LSS_IDENT_REMOTE_SERIAL_HIGH:d->lss_transfer.addr_ident_match=(_SpecificNodeInfo >= *(UNS32*)ptrTable->pSubindex[4].pObject)? d->lss_transfer.addr_ident_match|0x20:0; break; |
|
848 |
} |
|
849 |
/* If all the fields has been set.. */ |
|
850 |
if(d->lss_transfer.addr_ident_match==0x3F){ |
|
381 | 851 |
MSG_WAR(0x3D33, "SlaveLSS identified ", 0); |
343 | 852 |
d->lss_transfer.addr_ident_match=0; |
853 |
sendSlaveLSSMessage(d,LSS_IDENT_SLAVE,0,0); |
|
854 |
} |
|
855 |
else if(d->lss_transfer.addr_ident_match==0){ |
|
381 | 856 |
MSG_WAR(0x3D34, "LSS identify field doesn't match ", _SpecificNodeInfo); |
343 | 857 |
} |
858 |
} |
|
859 |
break; |
|
860 |
case LSS_IDENT_REMOTE_NON_CONF: /* LSS identify non-configured remote slave */ |
|
861 |
{ |
|
862 |
if(getNodeId(d)==0xFF){ |
|
381 | 863 |
MSG_WAR(0x3D35, "SlaveLSS non-configured ", 0); |
343 | 864 |
sendSlaveLSSMessage(d,LSS_IDENT_NON_CONF_SLAVE,0,0); |
865 |
} |
|
866 |
else{ |
|
381 | 867 |
MSG_WAR(0x3D36, "SlaveLSS already configured ", 0); |
343 | 868 |
} |
869 |
} |
|
870 |
break; |
|
871 |
case LSS_INQ_VENDOR_ID: /* Inquire Identity Vendor-ID */ |
|
872 |
case LSS_INQ_PRODUCT_CODE: /* Inquire Identity Product-Code */ |
|
873 |
case LSS_INQ_REV_NUMBER: /* Inquire Identity Revision-Number */ |
|
874 |
case LSS_INQ_SERIAL_NUMBER: /* Inquire Identity Serial-Number */ |
|
875 |
if(d->lss_transfer.mode==LSS_CONFIGURATION_MODE) |
|
876 |
{ |
|
877 |
||
878 |
UNS32 errorCode; |
|
879 |
const indextable *ptrTable; |
|
880 |
ODCallback_t *Callback; |
|
881 |
UNS32 _SpecificNodeInfo; |
|
882 |
||
883 |
ptrTable = (*d->scanIndexOD)(0x1018, &errorCode, &Callback); |
|
884 |
_SpecificNodeInfo=*(UNS32*)ptrTable->pSubindex[msg_cs-(LSS_INQ_VENDOR_ID-1)].pObject; |
|
381 | 885 |
MSG_WAR(0x3D37, "SlaveLSS identity field inquired ", _SpecificNodeInfo); |
343 | 886 |
|
887 |
sendSlaveLSSMessage(d,msg_cs,&_SpecificNodeInfo,0); |
|
888 |
} |
|
889 |
break; |
|
890 |
case LSS_INQ_NODE_ID: /* Inquire Node-ID */ |
|
891 |
if(d->lss_transfer.mode==LSS_CONFIGURATION_MODE) |
|
892 |
{ |
|
893 |
UNS8 NodeID; |
|
894 |
||
895 |
NodeID=getNodeId(d); |
|
381 | 896 |
MSG_WAR(0x3D38, "SlaveLSS Node ID inquired ", NodeID); |
343 | 897 |
sendSlaveLSSMessage(d,msg_cs,&NodeID,0); |
898 |
} |
|
899 |
else{ |
|
381 | 900 |
MSG_WAR(0x3D39, "SlaveLSS not in configuration mode",0); |
361 | 901 |
} |
902 |
break; |
|
903 |
#ifdef CO_ENABLE_LSS_FS |
|
343 | 904 |
case LSS_IDENT_FASTSCAN: |
905 |
{ |
|
361 | 906 |
/* If the nodeID isn't 0xFF the slave shall not participate */ |
907 |
if(getNodeId(d)!=0xFF)break; |
|
908 |
if(getLSSBitCheck(m)==128)d->lss_transfer.FastScan_SM=LSS_FS_RESET; |
|
909 |
||
910 |
switch(d->lss_transfer.FastScan_SM){ |
|
911 |
case LSS_FS_RESET: |
|
912 |
{ |
|
913 |
UNS32 errorCode; |
|
914 |
const indextable *ptrTable; |
|
915 |
ODCallback_t *Callback; |
|
916 |
||
381 | 917 |
MSG_WAR(0x3D3A, "SlaveLSS Reseting LSSPos", 0); |
361 | 918 |
d->lss_transfer.LSSPos=0; |
919 |
d->lss_transfer.FastScan_SM=LSS_FS_PROCESSING; |
|
920 |
||
921 |
ptrTable = (*d->scanIndexOD)(0x1018, &errorCode, &Callback); |
|
922 |
d->lss_transfer.IDNumber=*(UNS32*)ptrTable->pSubindex[d->lss_transfer.LSSPos+1].pObject; |
|
923 |
||
924 |
sendSlaveLSSMessage(d,LSS_IDENT_SLAVE,0,0); |
|
925 |
} |
|
926 |
break; |
|
927 |
case LSS_FS_PROCESSING:/*if(getLSSBitCheck(m)<32)*/ |
|
928 |
if(d->lss_transfer.LSSPos==getLSSSub(m)) |
|
929 |
{ |
|
930 |
UNS32 Mask=0xFFFFFFFF<<getLSSBitCheck(m); |
|
931 |
||
381 | 932 |
MSG_WAR(0x3D3B, "SlaveLSS FastScan IDNumber", getLSSIdent(m)); |
933 |
MSG_WAR(0x3D3C, "SlaveLSS FastScan BitMask ", Mask); |
|
934 |
MSG_WAR(0x3D3D, "SlaveLSS FastScan LSS-ID ", d->lss_transfer.IDNumber); |
|
361 | 935 |
|
936 |
if((getLSSIdent(m) & Mask)==(d->lss_transfer.IDNumber & Mask)) |
|
937 |
{ |
|
938 |
sendSlaveLSSMessage(d,LSS_IDENT_SLAVE,0,0); |
|
939 |
} |
|
940 |
||
941 |
if(getLSSBitCheck(m)==0) |
|
942 |
{ |
|
943 |
d->lss_transfer.FastScan_SM=LSS_FS_CONFIRMATION; |
|
944 |
} |
|
945 |
} |
|
946 |
break; |
|
947 |
case LSS_FS_CONFIRMATION: |
|
948 |
if(d->lss_transfer.LSSPos==getLSSSub(m)) |
|
949 |
{ |
|
950 |
if(getLSSIdent(m)==d->lss_transfer.IDNumber) |
|
951 |
{ |
|
952 |
/* Current LSS-ID[sub] confirmed correctly */ |
|
381 | 953 |
MSG_WAR(0x3D3E, "SlaveLSS FastScan IDNumber and LSS-ID match=>", d->lss_transfer.IDNumber); |
361 | 954 |
if(d->lss_transfer.LSSPos==3) |
955 |
{ |
|
956 |
/* All LSS-ID[sub] identified correctly, switching to configuration mode */ |
|
381 | 957 |
MSG_WAR(0x3D3F, "SlaveLSS switching to configuration mode ", 0); |
361 | 958 |
d->lss_transfer.nodeID=getNodeId(d); |
959 |
d->lss_transfer.mode=LSS_CONFIGURATION_MODE; |
|
960 |
d->lss_transfer.FastScan_SM=LSS_FS_RESET; |
|
961 |
d->lss_transfer.LSSPos=0xFF; |
|
962 |
} |
|
963 |
else |
|
964 |
{ |
|
965 |
/* Switch to the next LSS-ID[sub] */ |
|
966 |
UNS32 errorCode; |
|
967 |
const indextable *ptrTable; |
|
968 |
ODCallback_t *Callback; |
|
969 |
||
970 |
d->lss_transfer.LSSPos=getLSSNext(m); |
|
971 |
ptrTable = (*d->scanIndexOD)(0x1018, &errorCode, &Callback); |
|
972 |
d->lss_transfer.IDNumber=*(UNS32*)ptrTable->pSubindex[d->lss_transfer.LSSPos+1].pObject; |
|
973 |
d->lss_transfer.FastScan_SM=LSS_FS_PROCESSING; |
|
974 |
} |
|
975 |
sendSlaveLSSMessage(d,LSS_IDENT_SLAVE,0,0); |
|
976 |
} |
|
977 |
} |
|
978 |
break; |
|
979 |
} |
|
343 | 980 |
} |
981 |
break; |
|
361 | 982 |
#endif |
343 | 983 |
default: |
381 | 984 |
MSG_ERR(0x1D40, "SlaveLSS command not implemented", msg_cs); |
343 | 985 |
return 0xFF; |
986 |
} |
|
987 |
||
988 |
return 0; |
|
989 |
} |
|
990 |
||
381 | 991 |
/*UNS8 configNetworkNode(CO_Data* d, UNS8 command, void *dat1, void* dat2) |
343 | 992 |
{ |
993 |
return sendMasterLSSMessage(d,command,dat1,dat2); |
|
381 | 994 |
}*/ |
995 |
||
996 |
UNS8 configNetworkNode (CO_Data* d, UNS8 command, void *dat1, void* dat2, LSSCallback_t Callback) |
|
343 | 997 |
{ |
381 | 998 |
//d->lss_transfer.state=LSS_TRANS_IN_PROGRESS; |
343 | 999 |
d->lss_transfer.Callback=Callback; |
1000 |
d->lss_transfer.command=command; |
|
1001 |
||
381 | 1002 |
StopLSS_MSG_TIMER(); |
1003 |
//StartLSS_MSG_TIMER(); |
|
343 | 1004 |
|
1005 |
return sendMasterLSSMessage(d,command,dat1,dat2); |
|
1006 |
} |
|
1007 |
||
1008 |
UNS8 getConfigResultNetworkNode (CO_Data* d, UNS8 command, UNS32* dat1, UNS8* dat2) |
|
1009 |
{ |
|
1010 |
*dat1=d->lss_transfer.dat1; |
|
1011 |
*dat2=d->lss_transfer.dat2; |
|
1012 |
return d->lss_transfer.state; |
|
1013 |
} |
|
1014 |
||
1015 |
//void _lss_StoreConfiguration(UNS8 *error, UNS8 *spec_error){printf("_lss_StoreConfiguration\n");} |
|
1016 |
||
1017 |
#endif |