|
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 |
#ifdef CO_ENABLE_LSS
|
|
|
40 |
|
|
|
41 |
#include "data.h"
|
|
|
42 |
#include "lss.h"
|
|
|
43 |
#include "canfestival.h"
|
|
|
44 |
|
|
|
45 |
#define LSS_TIMEOUT_MS (TIMEVAL)1000 /* ms */
|
|
|
46 |
|
|
|
47 |
/* Returns the LSS ident field from a Message struct */
|
|
|
48 |
#define getLSSIdent(msg) ((msg->data[4] << 24) | (msg->data[3] << 16) | (msg->data[2] << 8) | (msg->data[1]))
|
|
|
49 |
|
|
|
50 |
/* Returns the LSS switch delay field from a Message struct */
|
|
|
51 |
#define getLSSDelay(msg) ((msg->data[2] << 8) | (msg->data[1]))
|
|
|
52 |
|
|
|
53 |
/* Returns the LSS FastScan BitCheck field from a Message struct */
|
|
|
54 |
#define getLSSBitCheck(msg) msg->data[5]
|
|
|
55 |
|
|
|
56 |
/* Returns the LSS FastScan LSSSub field from a Message struct */
|
|
|
57 |
#define getLSSSub(msg) msg->data[6]
|
|
|
58 |
|
|
|
59 |
/* Returns the LSS FastScan LSSNext field from a Message struct */
|
|
|
60 |
#define getLSSNext(msg) msg->data[7]
|
|
|
61 |
|
|
|
62 |
/* Prototypes for internals functions */
|
|
|
63 |
void LssAlarm(CO_Data* d, UNS32 id);
|
|
|
64 |
|
|
|
65 |
#define StopLSS_TIMER(id){\
|
|
|
66 |
MSG_WAR(0x3D01, "StopLSS_TIMER for timer : ", id);\
|
|
|
67 |
d->lss_transfer.timers[id] = DelAlarm(d->lss_transfer.timers[id]);}
|
|
|
68 |
|
|
|
69 |
#define StartLSS_MSG_TIMER(){\
|
|
|
70 |
MSG_WAR(0x3D02, "StartLSS_TIMER for MSG_TIMER",0);\
|
|
|
71 |
d->lss_transfer.timers[LSS_MSG_TIMER] = SetAlarm(d,LSS_MSG_TIMER,&LssAlarm,MS_TO_TIMEVAL(LSS_TIMEOUT_MS),0);}
|
|
|
72 |
|
|
|
73 |
#define StartLSS_SDELAY_TIMER(){\
|
|
|
74 |
MSG_WAR(0x3D03, "StartLSS_TIMER for SDELAY_TIMER",0);\
|
|
|
75 |
d->lss_transfer.timers[LSS_SWITCH_DELAY_TIMER] = SetAlarm(d,LSS_SWITCH_DELAY_TIMER,&LssAlarm,MS_TO_TIMEVAL(d->lss_transfer.switchDelay),MS_TO_TIMEVAL(d->lss_transfer.switchDelay));}
|
|
|
76 |
|
|
|
77 |
|
|
|
78 |
/*!
|
|
|
79 |
**
|
|
|
80 |
**
|
|
|
81 |
** @param d
|
|
|
82 |
** @param id
|
|
|
83 |
**/
|
|
|
84 |
//struct timeval current_time,init_time;
|
|
|
85 |
void LssAlarm(CO_Data* d, UNS32 id)
|
|
|
86 |
{
|
|
|
87 |
/*unsigned long time_period;
|
|
|
88 |
|
|
|
89 |
gettimeofday(¤t_time,NULL);
|
|
|
90 |
time_period=(current_time.tv_sec - init_time.tv_sec)* 1000000 + current_time.tv_usec - init_time.tv_usec;
|
|
|
91 |
printf("%3ld.%3ld.%3ld --",time_period/1000000,(time_period%1000000)/1000,time_period%1000);*/
|
|
|
92 |
|
|
|
93 |
switch(id){
|
|
|
94 |
case LSS_MSG_TIMER:
|
|
|
95 |
MSG_ERR(0x1D04, "LSS timeout. LSS response not received.", 0);
|
|
|
96 |
MSG_WAR(0x2D05, "LSS timeout command specifier : ", d->lss_transfer.command);
|
|
|
97 |
|
|
|
98 |
StopLSS_TIMER(LSS_MSG_TIMER);
|
|
|
99 |
/* Set aborted state */
|
|
|
100 |
d->lss_transfer.state = LSS_ABORTED_INTERNAL;
|
|
|
101 |
/* Call the user function to inform of the problem.*/
|
|
|
102 |
if(d->lss_transfer.Callback){
|
|
|
103 |
/*If there is a callback, it is responsible of the error*/
|
|
|
104 |
(*d->lss_transfer.Callback)(d,d->lss_transfer.command);
|
|
|
105 |
}
|
|
|
106 |
break;
|
|
|
107 |
case LSS_SWITCH_DELAY_TIMER:
|
|
|
108 |
/* The first switch_delay period has expired. Store the node state, change it
|
|
|
109 |
* so no CAN messages will be sent or received, call the ChangeBaudRate function*/
|
|
|
110 |
if(d->lss_transfer.switchDelayState==SDELAY_FIRST){
|
|
|
111 |
MSG_WAR(0x3D06, "LSS switch delay first period expired",0);
|
|
|
112 |
d->lss_transfer.switchDelayState=SDELAY_SECOND;
|
|
|
113 |
//d->lss_transfer.currentState=getState(d);
|
|
|
114 |
//setState(d, LssTimingDelay);
|
|
|
115 |
(*d->lss_ChangeBaudRate)(d->lss_transfer.baudRate);
|
|
|
116 |
}
|
|
|
117 |
else{ /* d->lss_transfer.switchDelayState==SDELAY_SECOND */
|
|
|
118 |
MSG_WAR(0x3D07, "LSS switch delay second period expired",0);
|
|
|
119 |
d->lss_transfer.switchDelayState=SDELAY_OFF;
|
|
|
120 |
StopLSS_TIMER(LSS_SWITCH_DELAY_TIMER);
|
|
|
121 |
|
|
|
122 |
setState(d, d->lss_transfer.currentState);
|
|
|
123 |
}
|
|
|
124 |
break;
|
|
|
125 |
}
|
|
|
126 |
}
|
|
|
127 |
|
|
|
128 |
/*!
|
|
|
129 |
**
|
|
|
130 |
**
|
|
|
131 |
** @param d
|
|
|
132 |
**/
|
|
|
133 |
void startLSS(CO_Data* d)
|
|
|
134 |
{
|
|
|
135 |
/*MSG_WAR(0x3D09, "LSS services started",0);*/
|
|
|
136 |
}
|
|
|
137 |
|
|
|
138 |
/*!
|
|
|
139 |
**
|
|
|
140 |
**
|
|
|
141 |
** @param d
|
|
|
142 |
**/
|
|
|
143 |
void stopLSS(CO_Data* d)
|
|
|
144 |
{
|
|
|
145 |
/*MSG_WAR(0x3D09, "LSS services stopped",0);*/
|
|
|
146 |
}
|
|
|
147 |
|
|
|
148 |
/*!
|
|
|
149 |
**
|
|
|
150 |
**
|
|
|
151 |
** @param d
|
|
|
152 |
** @param cob_id
|
|
|
153 |
**
|
|
|
154 |
** @return
|
|
|
155 |
**/
|
|
|
156 |
UNS8 sendSlaveLSSMessage(CO_Data* d, UNS8 command,void *dat1,void *dat2)
|
|
|
157 |
{
|
|
|
158 |
Message m;
|
|
|
159 |
UNS8 i;
|
|
|
160 |
|
|
|
161 |
if (!d->CurrentCommunicationState.csLSS){
|
|
|
162 |
MSG_WAR(0x2D0A, "unable to send the LSS message (not in stop or pre-op mode", d->nodeState);
|
|
|
163 |
return 0;
|
|
|
164 |
}
|
|
|
165 |
|
|
|
166 |
for(i=1;i<8;i++)m.data[i]=0;
|
|
|
167 |
m.len = 8;
|
|
|
168 |
m.rtr = NOT_A_REQUEST;
|
|
|
169 |
m.data[0]=command;
|
|
|
170 |
m.cob_id.w=SLSS_ADRESS;
|
|
|
171 |
|
|
|
172 |
/* Tha data sent with the msg depends on the command */
|
|
|
173 |
switch(command){
|
|
|
174 |
case LSS_INQ_NODE_ID: /* Inquire Node-ID */
|
|
|
175 |
m.data[1]=*(UNS8 *)dat1;
|
|
|
176 |
break;
|
|
|
177 |
case LSS_CONF_NODE_ID: /* Configure Node-ID */
|
|
|
178 |
case LSS_CONF_BIT_TIMING: /* Configure Bit Timing Parameters */
|
|
|
179 |
case LSS_CONF_STORE: /* Store Configured Parameters */
|
|
|
180 |
m.data[1]=*(UNS8 *)dat1;
|
|
|
181 |
m.data[2]=*(UNS8 *)dat2;
|
|
|
182 |
break;
|
|
|
183 |
case LSS_INQ_VENDOR_ID: /* Inquire Identity Vendor-ID */
|
|
|
184 |
case LSS_INQ_PRODUCT_CODE: /* Inquire Identity Product-Code */
|
|
|
185 |
case LSS_INQ_REV_NUMBER: /* Inquire Identity Revision-Number */
|
|
|
186 |
case LSS_INQ_SERIAL_NUMBER: /* Inquire Identity Serial-Number */
|
|
|
187 |
m.data[1]=(UNS8)(*(UNS32*)dat1 & 0xFF);
|
|
|
188 |
m.data[2]=(UNS8)(*(UNS32*)dat1>>8 & 0xFF);
|
|
|
189 |
m.data[3]=(UNS8)(*(UNS32*)dat1>>16 & 0xFF);
|
|
|
190 |
m.data[4]=(UNS8)(*(UNS32*)dat1>>24 & 0xFF);
|
|
|
191 |
break;
|
|
|
192 |
case LSS_SM_SELECTIVE_RESP: /* Switch Mode Selective response*/
|
|
|
193 |
case LSS_IDENT_SLAVE: /* LSS Identify Slave */
|
|
|
194 |
case LSS_IDENT_NON_CONF_SLAVE: /* LSS identify non-configured remote slave */
|
|
|
195 |
break;
|
|
|
196 |
default:
|
|
|
197 |
MSG_ERR(0x1D0B, "send Slave LSS command not implemented", command);
|
|
|
198 |
return 0xFF;
|
|
|
199 |
break;
|
|
|
200 |
}
|
|
|
201 |
|
|
|
202 |
return canSend(d->canHandle,&m);
|
|
|
203 |
}
|
|
|
204 |
|
|
|
205 |
/*!
|
|
|
206 |
**
|
|
|
207 |
**
|
|
|
208 |
** @param d
|
|
|
209 |
** @param cob_id
|
|
|
210 |
**
|
|
|
211 |
** @return
|
|
|
212 |
**/
|
|
|
213 |
UNS8 sendMasterLSSMessage(CO_Data* d, UNS8 command,void *dat1,void *dat2)
|
|
|
214 |
{
|
|
|
215 |
Message m;
|
|
|
216 |
UNS8 i;
|
|
|
217 |
|
|
|
218 |
for(i=1;i<8;i++)m.data[i]=0;
|
|
|
219 |
m.len = 8;
|
|
|
220 |
m.rtr = NOT_A_REQUEST;
|
|
|
221 |
m.data[0]=command;
|
|
|
222 |
m.cob_id.w=MLSS_ADRESS;
|
|
|
223 |
|
|
|
224 |
/* Tha data sent with the msg depends on the command */
|
|
|
225 |
switch(command){
|
|
|
226 |
case LSS_SM_GLOBAL: /* Switch Mode Global */
|
|
|
227 |
d->lss_transfer.state=LSS_FINISHED;
|
|
|
228 |
case LSS_CONF_NODE_ID: /* Configure Node-ID */
|
|
|
229 |
m.data[1]=*(UNS8 *)dat1;
|
|
|
230 |
break;
|
|
|
231 |
case LSS_CONF_BIT_TIMING: /* Configure Bit Timing Parameters */
|
|
|
232 |
m.data[1]=*(UNS8 *)dat1;
|
|
|
233 |
m.data[2]=*(UNS8 *)dat2;
|
|
|
234 |
break;
|
|
|
235 |
case LSS_CONF_ACT_BIT_TIMING: /* Activate Bit Timing Parameters */
|
|
|
236 |
m.data[1]=(UNS8)(*(UNS32*)dat1 & 0xFF);
|
|
|
237 |
m.data[2]=(UNS8)(*(UNS32*)dat1>>8 & 0xFF);
|
|
|
238 |
break;
|
|
|
239 |
case LSS_SM_SELECTIVE_VENDOR: /* Switch Mode Selective */
|
|
|
240 |
case LSS_SM_SELECTIVE_PRODUCT:
|
|
|
241 |
case LSS_SM_SELECTIVE_REVISION:
|
|
|
242 |
case LSS_SM_SELECTIVE_SERIAL:
|
|
|
243 |
case LSS_IDENT_REMOTE_VENDOR: /* LSS Identify Remote Slaves */
|
|
|
244 |
case LSS_IDENT_REMOTE_PRODUCT:
|
|
|
245 |
case LSS_IDENT_REMOTE_REV_LOW:
|
|
|
246 |
case LSS_IDENT_REMOTE_REV_HIGH:
|
|
|
247 |
case LSS_IDENT_REMOTE_SERIAL_LOW:
|
|
|
248 |
case LSS_IDENT_REMOTE_SERIAL_HIGH:
|
|
|
249 |
m.data[1]=(UNS8)(*(UNS32*)dat1 & 0xFF);
|
|
|
250 |
m.data[2]=(UNS8)(*(UNS32*)dat1>>8 & 0xFF);
|
|
|
251 |
m.data[3]=(UNS8)(*(UNS32*)dat1>>16 & 0xFF);
|
|
|
252 |
m.data[4]=(UNS8)(*(UNS32*)dat1>>24 & 0xFF);
|
|
|
253 |
break;
|
|
|
254 |
case LSS_CONF_STORE: /* Store Configured Parameters */
|
|
|
255 |
case LSS_IDENT_REMOTE_NON_CONF: /* LSS identify non-configured remote slave */
|
|
|
256 |
case LSS_INQ_VENDOR_ID: /* Inquire Identity Vendor-ID */
|
|
|
257 |
case LSS_INQ_PRODUCT_CODE: /* Inquire Identity Product-Code */
|
|
|
258 |
case LSS_INQ_REV_NUMBER: /* Inquire Identity Revision-Number */
|
|
|
259 |
case LSS_INQ_SERIAL_NUMBER: /* Inquire Identity Serial-Number */
|
|
|
260 |
case LSS_INQ_NODE_ID: /* Inquire Node-ID */
|
|
|
261 |
break;
|
|
|
262 |
case LSS_IDENT_FASTSCAN:
|
|
|
263 |
m.data[1]=(UNS8)(d->lss_transfer.IDNumber & 0xFF);
|
|
|
264 |
m.data[2]=(UNS8)(d->lss_transfer.IDNumber>>8 & 0xFF);
|
|
|
265 |
m.data[3]=(UNS8)(d->lss_transfer.IDNumber>>16 & 0xFF);
|
|
|
266 |
m.data[4]=(UNS8)(d->lss_transfer.IDNumber>>24 & 0xFF);
|
|
|
267 |
m.data[5]=d->lss_transfer.BitChecked;
|
|
|
268 |
m.data[6]=d->lss_transfer.LSSSub;
|
|
|
269 |
m.data[7]=d->lss_transfer.LSSNext;
|
|
|
270 |
break;
|
|
|
271 |
default:
|
|
|
272 |
MSG_ERR(0x1D0C, "send Master LSS command not implemented", command);
|
|
|
273 |
return 0xFF;
|
|
|
274 |
break;
|
|
|
275 |
}
|
|
|
276 |
|
|
|
277 |
return canSend(d->canHandle,&m);
|
|
|
278 |
}
|
|
|
279 |
|
|
|
280 |
/*!
|
|
|
281 |
**
|
|
|
282 |
**
|
|
|
283 |
** @param d
|
|
|
284 |
** @param cob_id
|
|
|
285 |
**
|
|
|
286 |
** @return
|
|
|
287 |
**/
|
|
|
288 |
UNS8 sendLSS(CO_Data* d, UNS8 command,void *dat1,void *dat2)
|
|
|
289 |
{
|
|
|
290 |
UNS8 res=1;
|
|
|
291 |
|
|
|
292 |
/* Tha data sent with the msg depends on the command and if the sender is a master or a slave */
|
|
|
293 |
if (*(d->iam_a_slave)){
|
|
|
294 |
res = sendSlaveLSSMessage(d, command,dat1,dat2);
|
|
|
295 |
}
|
|
|
296 |
else {/* It is a Master */
|
|
|
297 |
res = sendMasterLSSMessage(d, command,dat1,dat2);
|
|
|
298 |
}
|
|
|
299 |
return res ;
|
|
|
300 |
}
|
|
|
301 |
|
|
|
302 |
|
|
|
303 |
/*!
|
|
|
304 |
**
|
|
|
305 |
**
|
|
|
306 |
** @param d
|
|
|
307 |
** @param m
|
|
|
308 |
**
|
|
|
309 |
** @return
|
|
|
310 |
**/
|
|
|
311 |
UNS8 proceedLSS_Master(CO_Data* d, Message* m )
|
|
|
312 |
{
|
|
|
313 |
UNS8 msg_cs;
|
|
|
314 |
UNS32 Dat1=0;
|
|
|
315 |
UNS8 Dat2=0;
|
|
|
316 |
|
|
|
317 |
MSG_WAR(0x3D0D, "MasterLSS proceedLSS; command ", m->data[0]);
|
|
|
318 |
StopLSS_TIMER(LSS_MSG_TIMER);
|
|
|
319 |
/* Set state */
|
|
|
320 |
d->lss_transfer.state = LSS_FINISHED;
|
|
|
321 |
|
|
|
322 |
switch(msg_cs=m->data[0]){
|
|
|
323 |
case LSS_INQ_NODE_ID: /* Inquire Node-ID */
|
|
|
324 |
Dat1=m->data[1];
|
|
|
325 |
break;
|
|
|
326 |
case LSS_CONF_NODE_ID: /* Configure Node-ID */
|
|
|
327 |
case LSS_CONF_BIT_TIMING: /* Configure Bit Timing Parameters */
|
|
|
328 |
case LSS_CONF_STORE: /* Store Configured Parameters */
|
|
|
329 |
Dat1=m->data[1];
|
|
|
330 |
Dat2=m->data[2];
|
|
|
331 |
break;
|
|
|
332 |
case LSS_INQ_VENDOR_ID: /* Inquire Identity Vendor-ID */
|
|
|
333 |
case LSS_INQ_PRODUCT_CODE: /* Inquire Identity Product-Code */
|
|
|
334 |
case LSS_INQ_REV_NUMBER: /* Inquire Identity Revision-Number */
|
|
|
335 |
case LSS_INQ_SERIAL_NUMBER: /* Inquire Identity Serial-Number */
|
|
|
336 |
Dat1=getLSSIdent(m);
|
|
|
337 |
break;
|
|
|
338 |
case LSS_IDENT_SLAVE: /* LSS Identify Slave */
|
|
|
339 |
/*if(d->lss_transfer.command==LSS_IDENT_FASTSCAN){
|
|
|
340 |
* ************ TODO *******************
|
|
|
341 |
}*/
|
|
|
342 |
break;
|
|
|
343 |
case LSS_SM_SELECTIVE_RESP: /* Switch Mode Selective response */
|
|
|
344 |
case LSS_IDENT_NON_CONF_SLAVE: /* LSS identify non-configured remote slave */
|
|
|
345 |
break;
|
|
|
346 |
default:
|
|
|
347 |
MSG_ERR(0x1D0E, "Master LSS command not implemented", msg_cs);
|
|
|
348 |
return 0xFF;
|
|
|
349 |
break;
|
|
|
350 |
}
|
|
|
351 |
|
|
|
352 |
d->lss_transfer.dat1=Dat1;
|
|
|
353 |
d->lss_transfer.dat2=Dat2;
|
|
|
354 |
/* If there is a callback, it is responsible of the received response */
|
|
|
355 |
if(d->lss_transfer.Callback)
|
|
|
356 |
(*d->lss_transfer.Callback)(d,msg_cs);
|
|
|
357 |
|
|
|
358 |
return 0;
|
|
|
359 |
}
|
|
|
360 |
|
|
|
361 |
/*!
|
|
|
362 |
**
|
|
|
363 |
**
|
|
|
364 |
** @param d
|
|
|
365 |
** @param m
|
|
|
366 |
**
|
|
|
367 |
** @return
|
|
|
368 |
**/
|
|
|
369 |
UNS8 proceedLSS_Slave(CO_Data* d, Message* m )
|
|
|
370 |
{
|
|
|
371 |
UNS8 msg_cs;
|
|
|
372 |
|
|
|
373 |
MSG_WAR(0x3D0F, "SlaveLSS proceedLSS; command ", m->data[0]);
|
|
|
374 |
|
|
|
375 |
switch(msg_cs=m->data[0]){
|
|
|
376 |
case LSS_SM_GLOBAL: /* Switch Mode Global */
|
|
|
377 |
/* if there is not a mode change break*/
|
|
|
378 |
if(m->data[1] == d->lss_transfer.mode){
|
|
|
379 |
MSG_WAR(0x3D10, "SlaveLSS already in the mode ", m->data[1]);
|
|
|
380 |
break;
|
|
|
381 |
}
|
|
|
382 |
|
|
|
383 |
if(m->data[1]==LSS_CONFIGURATION_MODE) {
|
|
|
384 |
MSG_WAR(0x3D11, "SlaveLSS switching to configuration mode ", 0);
|
|
|
385 |
/* Store the NodeId in case it will be changed */
|
|
|
386 |
d->lss_transfer.nodeID=getNodeId(d);
|
|
|
387 |
d->lss_transfer.mode=LSS_CONFIGURATION_MODE;
|
|
|
388 |
}
|
|
|
389 |
else if(m->data[1]==LSS_WAITING_MODE){
|
|
|
390 |
MSG_WAR(0x3D12, "SlaveLSS switching to operational mode ", 0);
|
|
|
391 |
|
|
|
392 |
if(d->lss_transfer.switchDelayState==SDELAY_OFF){
|
|
|
393 |
/* If the nodeID has changed update it and put the node state to Initialisation. */
|
|
|
394 |
if(d->lss_transfer.nodeID!=getNodeId(d)){
|
|
|
395 |
MSG_WAR(0x3D13, "The node Id has changed. Reseting to Initialisation state",0);
|
|
|
396 |
setNodeId(d, d->lss_transfer.nodeID);
|
|
|
397 |
setState(d, Initialisation);
|
|
|
398 |
}
|
|
|
399 |
}
|
|
|
400 |
d->lss_transfer.mode=LSS_WAITING_MODE;
|
|
|
401 |
}
|
|
|
402 |
break;
|
|
|
403 |
case LSS_CONF_NODE_ID: /* Configure Node-ID */
|
|
|
404 |
{
|
|
|
405 |
UNS8 error_code=0;
|
|
|
406 |
UNS8 spec_error=0;
|
|
|
407 |
|
|
|
408 |
if(d->lss_transfer.mode==LSS_CONFIGURATION_MODE){
|
|
|
409 |
if(m->data[1]>127 && m->data[1]!=0xFF){
|
|
|
410 |
MSG_ERR(0x1D14, "NodeID out of range",0);
|
|
|
411 |
error_code=1; /* NodeID out of range */
|
|
|
412 |
}
|
|
|
413 |
else{
|
|
|
414 |
d->lss_transfer.nodeID=m->data[1];
|
|
|
415 |
}
|
|
|
416 |
}
|
|
|
417 |
else{
|
|
|
418 |
MSG_ERR(0x1D15, "SlaveLSS not in configuration mode",0);
|
|
|
419 |
error_code=0xFF;
|
|
|
420 |
}
|
|
|
421 |
sendSlaveLSSMessage(d,msg_cs,&error_code,&spec_error);
|
|
|
422 |
}
|
|
|
423 |
break;
|
|
|
424 |
case LSS_CONF_BIT_TIMING: /* Configure Bit Timing Parameters */
|
|
|
425 |
{
|
|
|
426 |
UNS8 error_code=0;
|
|
|
427 |
UNS8 spec_error=0;
|
|
|
428 |
|
|
|
429 |
if(d->lss_transfer.mode==LSS_CONFIGURATION_MODE){
|
|
|
430 |
/* Change the baud rate only if the function lss_ChangeBaudRate
|
|
|
431 |
* has been implemented. If not send an error.
|
|
|
432 |
*/
|
|
|
433 |
if(d->lss_ChangeBaudRate){
|
|
|
434 |
/* If a baud rate is not supported just comment the line. */
|
|
|
435 |
switch(m->data[2]){
|
|
|
436 |
case 0x00:d->lss_transfer.baudRate="1M";break;
|
|
|
437 |
case 0x01:d->lss_transfer.baudRate="800K";break;
|
|
|
438 |
case 0x02:d->lss_transfer.baudRate="500K";break;
|
|
|
439 |
case 0x03:d->lss_transfer.baudRate="250K";break;
|
|
|
440 |
case 0x04:d->lss_transfer.baudRate="125K";break;
|
|
|
441 |
case 0x05:d->lss_transfer.baudRate="100K";break;
|
|
|
442 |
case 0x06:d->lss_transfer.baudRate="50K";break;
|
|
|
443 |
case 0x07:d->lss_transfer.baudRate="20K";break;
|
|
|
444 |
case 0x08:d->lss_transfer.baudRate="10K";break;
|
|
|
445 |
default:
|
|
|
446 |
MSG_ERR(0x1D16, "Baud rate not supported",0);
|
|
|
447 |
error_code=0xFF; /* Baud rate not supported*/
|
|
|
448 |
break;
|
|
|
449 |
}
|
|
|
450 |
}
|
|
|
451 |
else
|
|
|
452 |
{
|
|
|
453 |
MSG_ERR(0x1D17, "Bit timing not supported",0);
|
|
|
454 |
error_code=0x01; /* bit timing not supported */
|
|
|
455 |
}
|
|
|
456 |
}
|
|
|
457 |
else{
|
|
|
458 |
MSG_ERR(0x1D18, "SlaveLSS not in configuration mode",0);
|
|
|
459 |
error_code=0xFF;
|
|
|
460 |
}
|
|
|
461 |
|
|
|
462 |
sendSlaveLSSMessage(d,msg_cs,&error_code,&spec_error);
|
|
|
463 |
}
|
|
|
464 |
break;
|
|
|
465 |
case LSS_CONF_ACT_BIT_TIMING: /* Activate Bit Timing Parameters */
|
|
|
466 |
|
|
|
467 |
if(d->lss_transfer.mode!=LSS_CONFIGURATION_MODE){
|
|
|
468 |
MSG_ERR(0x1D19, "SlaveLSS not in configuration mode",0);
|
|
|
469 |
break;
|
|
|
470 |
}
|
|
|
471 |
|
|
|
472 |
if(d->lss_transfer.baudRate!="none"){
|
|
|
473 |
d->lss_transfer.switchDelay=getLSSDelay(m);
|
|
|
474 |
MSG_WAR(0x3D1A, "Slave Switch Delay set to: ",d->lss_transfer.switchDelay);
|
|
|
475 |
d->lss_transfer.switchDelayState=SDELAY_FIRST;
|
|
|
476 |
d->lss_transfer.currentState=getState(d);
|
|
|
477 |
setState(d, LssTimingDelay);
|
|
|
478 |
StartLSS_SDELAY_TIMER();
|
|
|
479 |
}
|
|
|
480 |
break;
|
|
|
481 |
case LSS_CONF_STORE: /* Store Configured Parameters */
|
|
|
482 |
{
|
|
|
483 |
UNS8 error_code=0;
|
|
|
484 |
UNS8 spec_error=0;
|
|
|
485 |
|
|
|
486 |
if(d->lss_transfer.mode==LSS_CONFIGURATION_MODE){
|
|
|
487 |
if(d->lss_StoreConfiguration){
|
|
|
488 |
/* call lss_StoreConfiguration with NodeId */
|
|
|
489 |
(*d->lss_StoreConfiguration)(&error_code,&spec_error);
|
|
|
490 |
}
|
|
|
491 |
else{
|
|
|
492 |
MSG_ERR(0x1D1B, "Store configuration not supported",0);
|
|
|
493 |
error_code=1; /* store configuration is not supported */
|
|
|
494 |
}
|
|
|
495 |
}
|
|
|
496 |
else{
|
|
|
497 |
MSG_ERR(0x1D1C, "SlaveLSS not in configuration mode",0);
|
|
|
498 |
error_code=0xFF;
|
|
|
499 |
}
|
|
|
500 |
sendSlaveLSSMessage(d,msg_cs,&error_code,&spec_error);
|
|
|
501 |
}
|
|
|
502 |
break;
|
|
|
503 |
case LSS_SM_SELECTIVE_VENDOR: /* Switch Mode Selective */
|
|
|
504 |
case LSS_SM_SELECTIVE_PRODUCT:
|
|
|
505 |
case LSS_SM_SELECTIVE_REVISION:
|
|
|
506 |
case LSS_SM_SELECTIVE_SERIAL:
|
|
|
507 |
{
|
|
|
508 |
UNS32 errorCode;
|
|
|
509 |
const indextable *ptrTable;
|
|
|
510 |
ODCallback_t *Callback;
|
|
|
511 |
UNS32 _SpecificNodeInfo;
|
|
|
512 |
|
|
|
513 |
if(d->lss_transfer.mode==LSS_CONFIGURATION_MODE)
|
|
|
514 |
{
|
|
|
515 |
MSG_ERR(0x1D1D, "Switch Mode Selective only supported in operational mode",0);
|
|
|
516 |
break;
|
|
|
517 |
}
|
|
|
518 |
|
|
|
519 |
_SpecificNodeInfo=getLSSIdent(m);
|
|
|
520 |
|
|
|
521 |
ptrTable = (*d->scanIndexOD)(0x1018, &errorCode, &Callback);
|
|
|
522 |
if(_SpecificNodeInfo==*(UNS32*)ptrTable->pSubindex[msg_cs-(LSS_SM_SELECTIVE_VENDOR-1)].pObject){
|
|
|
523 |
|
|
|
524 |
d->lss_transfer.addr_sel_match|=(0x01<<(msg_cs-LSS_SM_SELECTIVE_VENDOR));
|
|
|
525 |
/* If all the fields has been set */
|
|
|
526 |
if(d->lss_transfer.addr_sel_match==0x0F){
|
|
|
527 |
|
|
|
528 |
MSG_WAR(0x3D1E, "SlaveLSS switching to configuration mode ", 0);
|
|
|
529 |
d->lss_transfer.addr_sel_match=0;
|
|
|
530 |
d->lss_transfer.nodeID=getNodeId(d);
|
|
|
531 |
d->lss_transfer.mode=LSS_CONFIGURATION_MODE;
|
|
|
532 |
|
|
|
533 |
sendSlaveLSSMessage(d,LSS_SM_SELECTIVE_RESP,0,0);
|
|
|
534 |
}
|
|
|
535 |
}
|
|
|
536 |
else {
|
|
|
537 |
MSG_WAR(0x3D1F, "LSS identity field doesn't match ", _SpecificNodeInfo);
|
|
|
538 |
d->lss_transfer.addr_sel_match=0;
|
|
|
539 |
}
|
|
|
540 |
}
|
|
|
541 |
break;
|
|
|
542 |
case LSS_IDENT_REMOTE_VENDOR: /* LSS Identify Remote Slaves */
|
|
|
543 |
case LSS_IDENT_REMOTE_PRODUCT:
|
|
|
544 |
case LSS_IDENT_REMOTE_REV_LOW:
|
|
|
545 |
case LSS_IDENT_REMOTE_REV_HIGH:
|
|
|
546 |
case LSS_IDENT_REMOTE_SERIAL_LOW:
|
|
|
547 |
case LSS_IDENT_REMOTE_SERIAL_HIGH:
|
|
|
548 |
{
|
|
|
549 |
UNS32 errorCode;
|
|
|
550 |
const indextable *ptrTable;
|
|
|
551 |
ODCallback_t *Callback;
|
|
|
552 |
UNS32 _SpecificNodeInfo;
|
|
|
553 |
|
|
|
554 |
_SpecificNodeInfo=getLSSIdent(m);
|
|
|
555 |
|
|
|
556 |
ptrTable = (*d->scanIndexOD)(0x1018, &errorCode, &Callback);
|
|
|
557 |
|
|
|
558 |
/* Check if the data match the identity object. */
|
|
|
559 |
switch(msg_cs){
|
|
|
560 |
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;
|
|
|
561 |
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;
|
|
|
562 |
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;
|
|
|
563 |
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;
|
|
|
564 |
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;
|
|
|
565 |
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;
|
|
|
566 |
}
|
|
|
567 |
/* If all the fields has been set.. */
|
|
|
568 |
if(d->lss_transfer.addr_ident_match==0x3F){
|
|
|
569 |
MSG_WAR(0x3D20, "SlaveLSS identified ", 0);
|
|
|
570 |
d->lss_transfer.addr_ident_match=0;
|
|
|
571 |
sendSlaveLSSMessage(d,LSS_IDENT_SLAVE,0,0);
|
|
|
572 |
}
|
|
|
573 |
else if(d->lss_transfer.addr_ident_match==0){
|
|
|
574 |
MSG_WAR(0x3D21, "LSS identify field doesn't match ", _SpecificNodeInfo);
|
|
|
575 |
}
|
|
|
576 |
}
|
|
|
577 |
break;
|
|
|
578 |
case LSS_IDENT_REMOTE_NON_CONF: /* LSS identify non-configured remote slave */
|
|
|
579 |
{
|
|
|
580 |
if(getNodeId(d)==0xFF){
|
|
|
581 |
MSG_WAR(0x3D22, "SlaveLSS non-configured ", 0);
|
|
|
582 |
sendSlaveLSSMessage(d,LSS_IDENT_NON_CONF_SLAVE,0,0);
|
|
|
583 |
}
|
|
|
584 |
else{
|
|
|
585 |
MSG_WAR(0x3D23, "SlaveLSS already configured ", 0);
|
|
|
586 |
}
|
|
|
587 |
}
|
|
|
588 |
break;
|
|
|
589 |
case LSS_INQ_VENDOR_ID: /* Inquire Identity Vendor-ID */
|
|
|
590 |
case LSS_INQ_PRODUCT_CODE: /* Inquire Identity Product-Code */
|
|
|
591 |
case LSS_INQ_REV_NUMBER: /* Inquire Identity Revision-Number */
|
|
|
592 |
case LSS_INQ_SERIAL_NUMBER: /* Inquire Identity Serial-Number */
|
|
|
593 |
if(d->lss_transfer.mode==LSS_CONFIGURATION_MODE)
|
|
|
594 |
{
|
|
|
595 |
|
|
|
596 |
UNS32 errorCode;
|
|
|
597 |
const indextable *ptrTable;
|
|
|
598 |
ODCallback_t *Callback;
|
|
|
599 |
UNS32 _SpecificNodeInfo;
|
|
|
600 |
|
|
|
601 |
ptrTable = (*d->scanIndexOD)(0x1018, &errorCode, &Callback);
|
|
|
602 |
_SpecificNodeInfo=*(UNS32*)ptrTable->pSubindex[msg_cs-(LSS_INQ_VENDOR_ID-1)].pObject;
|
|
|
603 |
MSG_WAR(0x3D24, "SlaveLSS identity field inquired ", _SpecificNodeInfo);
|
|
|
604 |
|
|
|
605 |
sendSlaveLSSMessage(d,msg_cs,&_SpecificNodeInfo,0);
|
|
|
606 |
}
|
|
|
607 |
break;
|
|
|
608 |
case LSS_INQ_NODE_ID: /* Inquire Node-ID */
|
|
|
609 |
if(d->lss_transfer.mode==LSS_CONFIGURATION_MODE)
|
|
|
610 |
{
|
|
|
611 |
UNS8 NodeID;
|
|
|
612 |
|
|
|
613 |
NodeID=getNodeId(d);
|
|
|
614 |
MSG_WAR(0x3D25, "SlaveLSS Node ID inquired ", NodeID);
|
|
|
615 |
sendSlaveLSSMessage(d,msg_cs,&NodeID,0);
|
|
|
616 |
}
|
|
|
617 |
else{
|
|
|
618 |
MSG_ERR(0x1D26, "SlaveLSS not in configuration mode",0);
|
|
|
619 |
}
|
|
|
620 |
break;
|
|
|
621 |
case LSS_IDENT_FASTSCAN:
|
|
|
622 |
{
|
|
|
623 |
/******* TODO *******/
|
|
|
624 |
}
|
|
|
625 |
break;
|
|
|
626 |
default:
|
|
|
627 |
MSG_ERR(0x1D27, "SlaveLSS command not implemented", msg_cs);
|
|
|
628 |
return 0xFF;
|
|
|
629 |
break;
|
|
|
630 |
}
|
|
|
631 |
|
|
|
632 |
return 0;
|
|
|
633 |
}
|
|
|
634 |
|
|
|
635 |
UNS8 configNetworkNode(CO_Data* d, UNS8 command, void *dat1, void* dat2)
|
|
|
636 |
{
|
|
|
637 |
return sendMasterLSSMessage(d,command,dat1,dat2);
|
|
|
638 |
}
|
|
|
639 |
|
|
|
640 |
UNS8 configNetworkNodeCallBack (CO_Data* d, UNS8 command, void *dat1, void* dat2, LSSCallback_t Callback)
|
|
|
641 |
{
|
|
|
642 |
d->lss_transfer.state=LSS_TRANS_IN_PROGRESS;
|
|
|
643 |
d->lss_transfer.Callback=Callback;
|
|
|
644 |
d->lss_transfer.command=command;
|
|
|
645 |
|
|
|
646 |
StopLSS_TIMER(LSS_MSG_TIMER);
|
|
|
647 |
StartLSS_MSG_TIMER();
|
|
|
648 |
|
|
|
649 |
return sendMasterLSSMessage(d,command,dat1,dat2);
|
|
|
650 |
}
|
|
|
651 |
|
|
|
652 |
UNS8 getConfigResultNetworkNode (CO_Data* d, UNS8 command, UNS32* dat1, UNS8* dat2)
|
|
|
653 |
{
|
|
|
654 |
*dat1=d->lss_transfer.dat1;
|
|
|
655 |
*dat2=d->lss_transfer.dat2;
|
|
|
656 |
return d->lss_transfer.state;
|
|
|
657 |
}
|
|
|
658 |
|
|
|
659 |
//void _lss_StoreConfiguration(UNS8 *error, UNS8 *spec_error){printf("_lss_StoreConfiguration\n");}
|
|
|
660 |
//void _lss_ChangeBaudRate(char *BaudRate){printf("_lss_ChangeBaudRate\n");}
|
|
|
661 |
|
|
|
662 |
#endif
|