src/lss.c

   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(&current_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