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