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