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