etisserant@343: /* etisserant@343: This file is part of CanFestival, a library implementing CanOpen Stack. etisserant@343: etisserant@343: etisserant@343: Copyright (C): Jorge Berzosa etisserant@343: etisserant@343: etisserant@343: See COPYING file for copyrights details. etisserant@343: etisserant@343: etisserant@343: This library is free software; you can redistribute it and/or etisserant@343: modify it under the terms of the GNU Lesser General Public etisserant@343: License as published by the Free Software Foundation; either etisserant@343: version 2.1 of the License, or (at your option) any later version. etisserant@343: etisserant@343: etisserant@343: This library is distributed in the hope that it will be useful, etisserant@343: but WITHOUT ANY WARRANTY; without even the implied warranty of etisserant@343: MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU etisserant@343: Lesser General Public License for more details. etisserant@343: etisserant@343: etisserant@343: You should have received a copy of the GNU Lesser General Public etisserant@343: License along with this library; if not, write to the Free Software etisserant@343: Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA etisserant@343: */ etisserant@343: etisserant@343: etisserant@343: /*! etisserant@343: ** @file lss.c etisserant@343: ** @author Jorge Berzosa etisserant@343: ** @date Mon Oct 22 05:44:32 2007 etisserant@343: ** etisserant@343: ** @brief etisserant@343: ** etisserant@343: ** etisserant@343: */ etisserant@343: etisserant@343: etisserant@343: #include "data.h" etisserant@343: #include "lss.h" etisserant@343: #include "canfestival.h" etisserant@370: #include "sysdep.h" etisserant@343: etisserant@482: #ifdef CO_ENABLE_LSS etisserant@482: groke6@361: //#define LSS_TIMEOUT_MS (TIMEVAL)1000 /* ms */ groke6@361: //#define LSS_FS_TIMEOUT_MS (TIMEVAL)100 /* ms */ etisserant@343: etisserant@343: /* Returns the LSS ident field from a Message struct */ etisserant@436: #define getLSSIdent(msg) (((UNS32)msg->data[4] << 24) | ((UNS32)msg->data[3] << 16) | (msg->data[2] << 8) | (msg->data[1])) etisserant@436: etisserant@343: etisserant@343: /* Returns the LSS switch delay field from a Message struct */ etisserant@343: #define getLSSDelay(msg) ((msg->data[2] << 8) | (msg->data[1])) etisserant@343: etisserant@343: /* Returns the LSS FastScan BitCheck field from a Message struct */ etisserant@343: #define getLSSBitCheck(msg) msg->data[5] etisserant@343: etisserant@343: /* Returns the LSS FastScan LSSSub field from a Message struct */ etisserant@343: #define getLSSSub(msg) msg->data[6] etisserant@343: etisserant@343: /* Returns the LSS FastScan LSSNext field from a Message struct */ etisserant@343: #define getLSSNext(msg) msg->data[7] etisserant@343: etisserant@343: /* Prototypes for internals functions */ groke6@381: UNS8 sendMasterLSSMessage(CO_Data* d, UNS8 command,void *dat1,void *dat2); groke6@381: void LssAlarmMSG(CO_Data* d, UNS32 id); groke6@381: void LssAlarmSDELAY(CO_Data* d, UNS32 id); groke6@381: groke6@381: groke6@381: #define StopLSS_MSG_TIMER(){\ groke6@381: MSG_WAR(0x3D01, "StopLSS_MSG_TIMER", 0);\ groke6@381: d->lss_transfer.timerMSG = DelAlarm(d->lss_transfer.timerMSG);} etisserant@343: etisserant@343: #define StartLSS_MSG_TIMER(){\ groke6@381: MSG_WAR(0x3D02, "StartLSS_MSG_TIMER",0);\ groke6@381: d->lss_transfer.timerMSG = SetAlarm(d,0,&LssAlarmMSG,MS_TO_TIMEVAL(LSS_TIMEOUT_MS),0);} groke6@381: groke6@381: #define StopLSS_SDELAY_TIMER(){\ groke6@381: MSG_WAR(0x3D03, "StopLSS_SDELAY_TIMER", 0);\ groke6@381: d->lss_transfer.timerSDELAY = DelAlarm(d->lss_transfer.timerSDELAY);} etisserant@343: etisserant@343: #define StartLSS_SDELAY_TIMER(){\ groke6@381: MSG_WAR(0x3D04, "StartLSS_SDELAY_TIMER",0);\ groke6@381: d->lss_transfer.timerSDELAY= SetAlarm(d,0,&LssAlarmSDELAY,MS_TO_TIMEVAL(d->lss_transfer.switchDelay),MS_TO_TIMEVAL(d->lss_transfer.switchDelay));} groke6@381: groke6@381: groke6@381: #ifdef CO_ENABLE_LSS_FS groke6@381: /* Prototypes for internals functions */ groke6@381: void LssAlarmFS(CO_Data* d, UNS32 id); groke6@381: groke6@381: #define StopLSS_FS_TIMER(){\ groke6@381: MSG_WAR(0x3D05, "StopLSS_FS_TIMER", id);\ groke6@381: d->lss_transfer.timerFS = DelAlarm(d->lss_transfer.timerFS);} etisserant@343: groke6@361: #define StartLSS_FS_TIMER(){\ groke6@381: MSG_WAR(0x3D06, "StartLSS_FS_TIMER",0);\ groke6@381: d->lss_transfer.timerFS = SetAlarm(d,0,&LssAlarmFS,MS_TO_TIMEVAL(LSS_FS_TIMEOUT_MS),0);} groke6@381: #endif groke6@381: groke6@381: groke6@381: void LssAlarmMSG(CO_Data* d, UNS32 id) groke6@381: { groke6@381: StopLSS_MSG_TIMER(); groke6@381: #ifdef CO_ENABLE_LSS_FS groke6@381: if(d->lss_transfer.command==LSS_IDENT_FASTSCAN){ groke6@381: if(d->lss_transfer.FastScan_SM==LSS_FS_RESET){ groke6@381: /* if at least one node had answered before the timer expired, start the FastScan protocol*/ groke6@381: if(d->lss_transfer.LSSanswer!=0){ groke6@517: UNS32 Mask=0xFFFFFFFF; groke6@381: d->lss_transfer.LSSanswer=0; groke6@517: d->lss_transfer.BitChecked=d->lss_transfer.lss_fs_transfer.FS_BitChecked[0]; groke6@517: Mask=(UNS32)((UNS64)Mask<<(d->lss_transfer.BitChecked+1)); groke6@517: d->lss_transfer.IDNumber=d->lss_transfer.lss_fs_transfer.FS_LSS_ID[0] & Mask; groke6@381: d->lss_transfer.FastScan_SM=LSS_FS_PROCESSING; groke6@517: //printf("BitChecked=%d, IDNumber=%x MASK=%x\n",d->lss_transfer.BitChecked,d->lss_transfer.IDNumber,Mask); groke6@381: StartLSS_FS_TIMER(); groke6@381: sendMasterLSSMessage(d,LSS_IDENT_FASTSCAN,0,0); groke6@381: return; groke6@381: } groke6@381: else{ groke6@381: groke6@381: d->lss_transfer.state = LSS_FINISHED; groke6@381: /* Inform the application that there aren't not configured nodes in the net */ groke6@381: d->lss_transfer.dat1=1; groke6@381: } groke6@381: } groke6@381: else{ groke6@381: /* This should not happen, an error ocurred*/ groke6@381: MSG_ERR(0x1D07, "LSS FastScan timeout. FastScan_SM inconsisten state.", d->lss_transfer.FastScan_SM); groke6@517: d->lss_transfer.state = LSS_ABORTED_INTERNAL; groke6@517: d->lss_transfer.FastScan_SM=LSS_FS_RESET; groke6@381: } groke6@381: } groke6@381: else groke6@381: #endif groke6@381: if(d->lss_transfer.command==LSS_IDENT_REMOTE_NON_CONF){ groke6@381: MSG_WAR(0x2D08, "LSS timeout. There are not no-configured slaves in the net", 0); groke6@381: d->lss_transfer.state = LSS_FINISHED; groke6@381: d->lss_transfer.dat1=1; groke6@381: } groke6@381: else{ groke6@381: MSG_ERR(0x1D09, "LSS timeout. LSS response not received.", 0); groke6@381: MSG_WAR(0x2D0A, "LSS timeout command specifier : ", d->lss_transfer.command); groke6@381: /* Set aborted state */ groke6@381: d->lss_transfer.state = LSS_ABORTED_INTERNAL; groke6@517: #ifdef CO_ENABLE_LSS_FS groke6@517: d->lss_transfer.FastScan_SM = LSS_FS_RESET; groke6@517: #endif groke6@381: } groke6@381: groke6@381: /* Call the user function to inform of the problem.*/ groke6@381: if(d->lss_transfer.Callback){ groke6@381: /*If there is a callback, it is responsible of the error*/ groke6@381: (*d->lss_transfer.Callback)(d,d->lss_transfer.command); groke6@381: } groke6@381: } groke6@381: groke6@381: etisserant@343: /*! etisserant@343: ** etisserant@343: ** etisserant@343: ** @param d etisserant@343: ** @param id etisserant@343: **/ groke6@381: void LssAlarmSDELAY(CO_Data* d, UNS32 id) etisserant@343: { groke6@381: groke6@381: /* The first switch_delay period expired. Store the node state, change it groke6@381: * so no CAN messages will be sent or received, call the ChangeBaudRate function*/ groke6@381: if(d->lss_transfer.switchDelayState==SDELAY_FIRST){ groke6@381: MSG_WAR(0x3D0B, "LSS switch delay first period expired",0); groke6@381: d->lss_transfer.switchDelayState=SDELAY_SECOND; groke6@384: //(*d->lss_ChangeBaudRate)(d,d->lss_transfer.baudRate); groke6@384: canChangeBaudRate(d->lss_transfer.canHandle_t, d->lss_transfer.baudRate); groke6@381: } groke6@381: else{ /* d->lss_transfer.switchDelayState==SDELAY_SECOND */ groke6@381: MSG_WAR(0x3D0C, "LSS switch delay second period expired",0); groke6@381: d->lss_transfer.switchDelayState=SDELAY_OFF; groke6@381: StopLSS_SDELAY_TIMER(); groke6@381: groke6@381: if (*(d->iam_a_slave)) groke6@381: d->canHandle=d->lss_transfer.canHandle_t; groke6@381: else{ groke6@381: d->lss_transfer.dat1=0; groke6@381: d->lss_transfer.state=LSS_FINISHED; groke6@381: /* Call the user function */ groke6@381: if(d->lss_transfer.Callback){ groke6@381: (*d->lss_transfer.Callback)(d,d->lss_transfer.command); groke6@381: } groke6@381: } groke6@381: } groke6@381: } groke6@381: groke6@361: #ifdef CO_ENABLE_LSS_FS groke6@381: /*! groke6@381: ** groke6@381: ** groke6@381: ** @param d groke6@381: ** @param id groke6@381: **/ groke6@381: void LssAlarmFS(CO_Data* d, UNS32 id) groke6@381: { groke6@381: StopLSS_FS_TIMER(); groke6@381: groke6@381: switch(d->lss_transfer.FastScan_SM){ groke6@381: case LSS_FS_RESET: groke6@381: { groke6@381: /* This should not happen, an error ocurred*/ groke6@381: MSG_ERR(0x1D0D, "LSS FastScan timeout. FastScan_SM inconsisten state.", d->lss_transfer.FastScan_SM); groke6@381: } groke6@381: break; groke6@381: case LSS_FS_PROCESSING: groke6@381: { groke6@381: /* If there isn't any answer, set the bit */ groke6@381: if(d->lss_transfer.LSSanswer==0){ groke6@381: UNS32 Mask=0x1; groke6@381: Mask<<=d->lss_transfer.BitChecked; groke6@381: d->lss_transfer.IDNumber|=Mask; groke6@381: } groke6@381: groke6@381: if(d->lss_transfer.BitChecked==0){ groke6@381: /* We finished with the current LSS-ID[sub], confirm it */ groke6@381: d->lss_transfer.FastScan_SM=LSS_FS_CONFIRMATION; groke6@381: if(d->lss_transfer.LSSNext<3)d->lss_transfer.LSSNext++; groke6@381: } groke6@381: else{ groke6@381: d->lss_transfer.BitChecked--; groke6@381: } groke6@517: //printf("BitChecked=%d, IDNumber=%x\n",d->lss_transfer.BitChecked,d->lss_transfer.IDNumber); groke6@381: d->lss_transfer.LSSanswer=0; groke6@381: StartLSS_FS_TIMER(); groke6@381: sendMasterLSSMessage(d,LSS_IDENT_FASTSCAN,0,0); groke6@381: return; groke6@381: } groke6@381: break; groke6@381: case LSS_FS_CONFIRMATION: groke6@381: { groke6@381: if(d->lss_transfer.LSSanswer!=0){ groke6@381: d->lss_transfer.LSSanswer=0; groke6@381: groke6@381: if(d->lss_transfer.LSSSub==3){ groke6@381: /* The LSS FastScan protocol finished correctly. Restore the parameters */ groke6@381: d->lss_transfer.BitChecked=128; groke6@381: d->lss_transfer.FastScan_SM=LSS_FS_RESET; groke6@381: d->lss_transfer.LSSSub=0; groke6@381: d->lss_transfer.LSSNext=0; groke6@381: d->lss_transfer.IDNumber=0; groke6@381: groke6@381: /* Inform the application that the FastScan finished correctly */ groke6@381: d->lss_transfer.state = LSS_FINISHED; groke6@381: d->lss_transfer.dat1=0; groke6@381: } groke6@381: else{ groke6@517: UNS32 Mask=0xFFFFFFFF; groke6@381: /* Start with the next LSS-ID[sub] */ groke6@381: d->lss_transfer.LSSSub++; groke6@517: d->lss_transfer.BitChecked=d->lss_transfer.lss_fs_transfer.FS_BitChecked[d->lss_transfer.LSSSub]; groke6@517: Mask=(UNS32)((UNS64)Mask<<(d->lss_transfer.BitChecked+1)); groke6@517: d->lss_transfer.IDNumber=d->lss_transfer.lss_fs_transfer.FS_LSS_ID[d->lss_transfer.LSSSub] & Mask; groke6@381: d->lss_transfer.FastScan_SM=LSS_FS_PROCESSING; groke6@517: //printf("BitChecked=%d, IDNumber=%x MASK=%x\n",d->lss_transfer.BitChecked,d->lss_transfer.IDNumber,Mask); groke6@381: StartLSS_FS_TIMER(); groke6@381: sendMasterLSSMessage(d,LSS_IDENT_FASTSCAN,0,0); groke6@381: return; groke6@381: } groke6@381: } groke6@381: else{ groke6@381: /* This should not happen, an error ocurred*/ groke6@381: MSG_ERR(0x1D0E, "LSS FastScan timeout. FastScan response not received.", 0); groke6@517: MSG_ERR(0x1D0E, "There is not any node with LSS_ID# =>", d->lss_transfer.LSSSub); groke6@517: MSG_ERR(0x1D0E, "with the value =>", d->lss_transfer.IDNumber); groke6@381: /* Set aborted state */ groke6@381: d->lss_transfer.state = LSS_ABORTED_INTERNAL; groke6@517: d->lss_transfer.FastScan_SM = LSS_FS_RESET; groke6@381: } groke6@381: } groke6@381: break; groke6@381: } groke6@381: groke6@381: /* Call the user function to inform of the problem.*/ groke6@381: if(d->lss_transfer.Callback){ groke6@381: /*If there is a callback, it is responsible of the error*/ groke6@381: (*d->lss_transfer.Callback)(d,d->lss_transfer.command); groke6@381: } groke6@381: } groke6@361: #endif groke6@381: groke6@381: etisserant@343: /*! etisserant@343: ** etisserant@343: ** etisserant@343: ** @param d etisserant@343: **/ etisserant@343: void startLSS(CO_Data* d) etisserant@343: { etisserant@343: /*MSG_WAR(0x3D09, "LSS services started",0);*/ etisserant@343: } etisserant@343: etisserant@343: /*! etisserant@343: ** etisserant@343: ** etisserant@343: ** @param d etisserant@343: **/ etisserant@343: void stopLSS(CO_Data* d) etisserant@343: { etisserant@343: /*MSG_WAR(0x3D09, "LSS services stopped",0);*/ etisserant@343: } etisserant@343: etisserant@343: /*! etisserant@343: ** etisserant@343: ** etisserant@343: ** @param d etisserant@343: ** @param cob_id etisserant@343: ** etisserant@343: ** @return etisserant@343: **/ etisserant@343: UNS8 sendSlaveLSSMessage(CO_Data* d, UNS8 command,void *dat1,void *dat2) etisserant@343: { etisserant@343: Message m; etisserant@343: UNS8 i; etisserant@343: etisserant@343: if (!d->CurrentCommunicationState.csLSS){ groke6@381: MSG_WAR(0x2D17, "unable to send the LSS message, not in the proper state =>", d->nodeState); groke6@361: return 0xFF; etisserant@343: } etisserant@343: etisserant@343: for(i=1;i<8;i++)m.data[i]=0; etisserant@343: m.len = 8; etisserant@343: m.rtr = NOT_A_REQUEST; etisserant@343: m.data[0]=command; etisserant@370: m.cob_id=UNS16_LE(SLSS_ADRESS); etisserant@343: etisserant@343: /* Tha data sent with the msg depends on the command */ etisserant@343: switch(command){ etisserant@343: case LSS_INQ_NODE_ID: /* Inquire Node-ID */ etisserant@343: m.data[1]=*(UNS8 *)dat1; etisserant@343: break; etisserant@343: case LSS_CONF_NODE_ID: /* Configure Node-ID */ etisserant@343: case LSS_CONF_BIT_TIMING: /* Configure Bit Timing Parameters */ etisserant@343: case LSS_CONF_STORE: /* Store Configured Parameters */ etisserant@343: m.data[1]=*(UNS8 *)dat1; etisserant@343: m.data[2]=*(UNS8 *)dat2; etisserant@343: break; etisserant@343: case LSS_INQ_VENDOR_ID: /* Inquire Identity Vendor-ID */ etisserant@343: case LSS_INQ_PRODUCT_CODE: /* Inquire Identity Product-Code */ etisserant@343: case LSS_INQ_REV_NUMBER: /* Inquire Identity Revision-Number */ etisserant@343: case LSS_INQ_SERIAL_NUMBER: /* Inquire Identity Serial-Number */ etisserant@343: m.data[1]=(UNS8)(*(UNS32*)dat1 & 0xFF); etisserant@343: m.data[2]=(UNS8)(*(UNS32*)dat1>>8 & 0xFF); etisserant@343: m.data[3]=(UNS8)(*(UNS32*)dat1>>16 & 0xFF); etisserant@343: m.data[4]=(UNS8)(*(UNS32*)dat1>>24 & 0xFF); etisserant@343: break; etisserant@343: case LSS_SM_SELECTIVE_RESP: /* Switch Mode Selective response*/ etisserant@343: case LSS_IDENT_SLAVE: /* LSS Identify Slave */ etisserant@343: case LSS_IDENT_NON_CONF_SLAVE: /* LSS identify non-configured remote slave */ etisserant@343: break; etisserant@343: default: groke6@381: MSG_ERR(0x1D18, "send Slave LSS command not implemented", command); etisserant@343: return 0xFF; etisserant@343: } etisserant@343: etisserant@343: return canSend(d->canHandle,&m); etisserant@343: } groke6@384: groke6@384: /* If a baud rate is not supported just comment the line. */ groke6@384: static UNS8 CO_TranslateBaudRate(char* optarg){ groke6@384: if(!strcmp( optarg, "1M")) return 0x00; groke6@384: if(!strcmp( optarg, "800K")) return 0x01; groke6@384: if(!strcmp( optarg, "500K")) return 0x02; groke6@384: if(!strcmp( optarg, "250K")) return 0x03; groke6@384: if(!strcmp( optarg, "125K")) return 0x04; groke6@384: if(!strcmp( optarg, "100K")) return 0x05; groke6@384: if(!strcmp( optarg, "50K")) return 0x06; groke6@384: if(!strcmp( optarg, "20K")) return 0x07; groke6@384: if(!strcmp( optarg, "10K")) return 0x08; groke6@399: return 0xFF; groke6@384: } etisserant@343: etisserant@343: /*! etisserant@343: ** etisserant@343: ** etisserant@343: ** @param d etisserant@343: ** @param cob_id etisserant@343: ** etisserant@343: ** @return etisserant@343: **/ etisserant@343: UNS8 sendMasterLSSMessage(CO_Data* d, UNS8 command,void *dat1,void *dat2) etisserant@343: { etisserant@343: Message m; etisserant@343: UNS8 i; groke6@381: UNS8 res; groke6@381: UNS8 hasResponse=0; groke6@381: etisserant@343: for(i=1;i<8;i++)m.data[i]=0; etisserant@343: m.len = 8; etisserant@343: m.rtr = NOT_A_REQUEST; etisserant@343: m.data[0]=command; etisserant@370: m.cob_id=UNS16_LE(MLSS_ADRESS); etisserant@343: etisserant@343: /* Tha data sent with the msg depends on the command */ etisserant@343: switch(command){ groke6@381: case LSS_CONF_NODE_ID: /* Configure Node-ID */ groke6@381: hasResponse=1; etisserant@343: case LSS_SM_GLOBAL: /* Switch Mode Global */ etisserant@343: m.data[1]=*(UNS8 *)dat1; etisserant@343: break; etisserant@343: case LSS_CONF_BIT_TIMING: /* Configure Bit Timing Parameters */ groke6@384: etisserant@343: m.data[1]=*(UNS8 *)dat1; groke6@384: d->lss_transfer.baudRate=*(char **)dat2; groke6@384: groke6@399: if((m.data[2]=CO_TranslateBaudRate(d->lss_transfer.baudRate))!=0xFF){ groke6@384: hasResponse=1; groke6@384: break; groke6@384: } groke6@384: groke6@384: MSG_ERR(0x1D19, "Master-> Baud rate not supported",0); groke6@384: d->lss_transfer.dat1=0xFF; groke6@384: groke6@384: /* if bit timing is not supported comment the previous code and uncomment the following one*/ groke6@384: /*{ groke6@381: MSG_ERR(0x1D1A, "Master-> Bit timing not supported",0); groke6@381: d->lss_transfer.dat1=0x01; groke6@384: }*/ groke6@384: groke6@381: d->lss_transfer.dat2=0; groke6@381: /* If there is a callback, it is responsible of the error */ groke6@381: if(d->lss_transfer.Callback) groke6@384: (*d->lss_transfer.Callback)(d,d->lss_transfer.command); groke6@381: return 0xFF; groke6@399: //break; etisserant@343: case LSS_CONF_ACT_BIT_TIMING: /* Activate Bit Timing Parameters */ etisserant@343: m.data[1]=(UNS8)(*(UNS32*)dat1 & 0xFF); etisserant@343: m.data[2]=(UNS8)(*(UNS32*)dat1>>8 & 0xFF); groke6@381: if(d->lss_transfer.baudRate!="none"){ groke6@381: d->lss_transfer.switchDelay=(UNS16)(*(UNS32*)dat1 & 0xFFFF); groke6@381: d->lss_transfer.switchDelayState=SDELAY_FIRST; groke6@384: d->lss_transfer.canHandle_t=d->canHandle; groke6@381: res=canSend(d->canHandle,&m); groke6@381: if(res==0){ groke6@381: StartLSS_SDELAY_TIMER(); groke6@381: d->lss_transfer.state=LSS_TRANS_IN_PROGRESS; groke6@381: } groke6@381: return res; groke6@381: } groke6@381: else{ groke6@381: MSG_ERR(0x1D1B, "Master-> Baud rate not specified",0); groke6@381: d->lss_transfer.dat1=1; groke6@381: /* If there is a callback, it is responsible of the error */ groke6@381: if(d->lss_transfer.Callback){ groke6@381: (*d->lss_transfer.Callback)(d,d->lss_transfer.command); groke6@381: } groke6@381: return 0xFF; groke6@381: } groke6@399: //break; groke6@381: case LSS_SM_SELECTIVE_SERIAL: groke6@381: case LSS_IDENT_REMOTE_SERIAL_HIGH: groke6@381: hasResponse=1; etisserant@343: case LSS_SM_SELECTIVE_VENDOR: /* Switch Mode Selective */ etisserant@343: case LSS_SM_SELECTIVE_PRODUCT: etisserant@343: case LSS_SM_SELECTIVE_REVISION: etisserant@343: case LSS_IDENT_REMOTE_VENDOR: /* LSS Identify Remote Slaves */ etisserant@343: case LSS_IDENT_REMOTE_PRODUCT: etisserant@343: case LSS_IDENT_REMOTE_REV_LOW: etisserant@343: case LSS_IDENT_REMOTE_REV_HIGH: etisserant@343: case LSS_IDENT_REMOTE_SERIAL_LOW: etisserant@343: m.data[1]=(UNS8)(*(UNS32*)dat1 & 0xFF); etisserant@343: m.data[2]=(UNS8)(*(UNS32*)dat1>>8 & 0xFF); etisserant@343: m.data[3]=(UNS8)(*(UNS32*)dat1>>16 & 0xFF); etisserant@343: m.data[4]=(UNS8)(*(UNS32*)dat1>>24 & 0xFF); etisserant@343: break; groke6@381: etisserant@343: case LSS_CONF_STORE: /* Store Configured Parameters */ etisserant@343: case LSS_IDENT_REMOTE_NON_CONF: /* LSS identify non-configured remote slave */ etisserant@343: case LSS_INQ_VENDOR_ID: /* Inquire Identity Vendor-ID */ etisserant@343: case LSS_INQ_PRODUCT_CODE: /* Inquire Identity Product-Code */ etisserant@343: case LSS_INQ_REV_NUMBER: /* Inquire Identity Revision-Number */ etisserant@343: case LSS_INQ_SERIAL_NUMBER: /* Inquire Identity Serial-Number */ etisserant@343: case LSS_INQ_NODE_ID: /* Inquire Node-ID */ groke6@381: hasResponse=1; etisserant@343: break; groke6@361: #ifdef CO_ENABLE_LSS_FS etisserant@343: case LSS_IDENT_FASTSCAN: groke6@517: if(d->lss_transfer.FastScan_SM==LSS_FS_RESET){ groke6@517: UNS8 i; groke6@517: /* Initialize the lss_fs_transfer FastScan parameters */ groke6@517: for(i=0;i<4;i++){ groke6@517: d->lss_transfer.lss_fs_transfer.FS_LSS_ID[i]=(*(lss_fs_transfer_t*)dat1).FS_LSS_ID[i]; groke6@517: d->lss_transfer.lss_fs_transfer.FS_BitChecked[i]=(*(lss_fs_transfer_t*)dat1).FS_BitChecked[i]; groke6@517: /* Adjust BitChecked from 32-1 to 31-0 */ groke6@517: if(d->lss_transfer.lss_fs_transfer.FS_BitChecked[i]>0)d->lss_transfer.lss_fs_transfer.FS_BitChecked[i]--; groke6@517: } groke6@517: groke6@517: d->lss_transfer.IDNumber=0; groke6@517: d->lss_transfer.BitChecked=128; groke6@517: d->lss_transfer.LSSSub=0; groke6@517: d->lss_transfer.LSSNext=0; groke6@517: groke6@517: /* it will generate a response only if it is the start of the FastScan protocol*/ groke6@517: hasResponse=1; groke6@517: } etisserant@343: m.data[1]=(UNS8)(d->lss_transfer.IDNumber & 0xFF); etisserant@343: m.data[2]=(UNS8)(d->lss_transfer.IDNumber>>8 & 0xFF); etisserant@343: m.data[3]=(UNS8)(d->lss_transfer.IDNumber>>16 & 0xFF); etisserant@343: m.data[4]=(UNS8)(d->lss_transfer.IDNumber>>24 & 0xFF); etisserant@343: m.data[5]=d->lss_transfer.BitChecked; etisserant@343: m.data[6]=d->lss_transfer.LSSSub; etisserant@343: m.data[7]=d->lss_transfer.LSSNext; etisserant@343: break; groke6@361: #endif etisserant@343: default: groke6@381: MSG_ERR(0x1D1C, "send Master LSS command not implemented", command); etisserant@343: return 0xFF; etisserant@343: } etisserant@343: groke6@381: res=canSend(d->canHandle,&m); groke6@381: if(res==0 && hasResponse==1){ groke6@381: StartLSS_MSG_TIMER(); groke6@381: d->lss_transfer.state=LSS_TRANS_IN_PROGRESS; groke6@381: } groke6@381: return res; etisserant@343: } etisserant@343: etisserant@343: /*! etisserant@343: ** etisserant@343: ** etisserant@343: ** @param d etisserant@343: ** @param cob_id etisserant@343: ** etisserant@343: ** @return etisserant@343: **/ etisserant@343: UNS8 sendLSS(CO_Data* d, UNS8 command,void *dat1,void *dat2) etisserant@343: { etisserant@343: UNS8 res=1; etisserant@343: etisserant@343: /* Tha data sent with the msg depends on the command and if the sender is a master or a slave */ etisserant@343: if (*(d->iam_a_slave)){ etisserant@343: res = sendSlaveLSSMessage(d, command,dat1,dat2); etisserant@343: } etisserant@343: else {/* It is a Master */ etisserant@343: res = sendMasterLSSMessage(d, command,dat1,dat2); etisserant@343: } etisserant@343: return res ; etisserant@343: } etisserant@343: etisserant@343: etisserant@343: /*! etisserant@343: ** etisserant@343: ** etisserant@343: ** @param d etisserant@343: ** @param m etisserant@343: ** etisserant@343: ** @return etisserant@343: **/ etisserant@343: UNS8 proceedLSS_Master(CO_Data* d, Message* m ) etisserant@343: { etisserant@343: UNS8 msg_cs; etisserant@343: UNS32 Dat1=0; etisserant@343: UNS8 Dat2=0; etisserant@343: groke6@361: if(d->lss_transfer.state!=LSS_TRANS_IN_PROGRESS) groke6@361: { groke6@361: //MSG_WAR(0x3D0D, "MasterLSS proceedLSS; unexpected message arrived;command ", m->data[0]); groke6@361: //return 0; groke6@361: goto ErrorProcessMaster; groke6@361: } groke6@361: groke6@381: MSG_WAR(0x3D1E, "MasterLSS proceedLSS; command ", m->data[0]); etisserant@343: etisserant@343: switch(msg_cs=m->data[0]){ etisserant@343: case LSS_INQ_NODE_ID: /* Inquire Node-ID */ groke6@361: if(d->lss_transfer.command!=LSS_INQ_NODE_ID)goto ErrorProcessMaster; etisserant@343: Dat1=m->data[1]; etisserant@343: break; etisserant@343: case LSS_CONF_NODE_ID: /* Configure Node-ID */ etisserant@343: case LSS_CONF_BIT_TIMING: /* Configure Bit Timing Parameters */ etisserant@343: case LSS_CONF_STORE: /* Store Configured Parameters */ groke6@361: if(d->lss_transfer.command!=msg_cs)goto ErrorProcessMaster; etisserant@343: Dat1=m->data[1]; etisserant@343: Dat2=m->data[2]; etisserant@343: break; etisserant@343: case LSS_INQ_VENDOR_ID: /* Inquire Identity Vendor-ID */ etisserant@343: case LSS_INQ_PRODUCT_CODE: /* Inquire Identity Product-Code */ etisserant@343: case LSS_INQ_REV_NUMBER: /* Inquire Identity Revision-Number */ etisserant@343: case LSS_INQ_SERIAL_NUMBER: /* Inquire Identity Serial-Number */ groke6@361: if(d->lss_transfer.command!=msg_cs)goto ErrorProcessMaster; etisserant@343: Dat1=getLSSIdent(m); etisserant@343: break; etisserant@343: case LSS_IDENT_SLAVE: /* LSS Identify Slave */ groke6@361: #ifdef CO_ENABLE_LSS_FS groke6@361: if(d->lss_transfer.command==LSS_IDENT_FASTSCAN){ groke6@361: /* A message arrived during the timer period */ groke6@361: d->lss_transfer.LSSanswer=1; groke6@361: return 0; groke6@361: } groke6@361: else groke6@361: #endif groke6@361: if(d->lss_transfer.command!=LSS_IDENT_REMOTE_VENDOR && \ groke6@361: d->lss_transfer.command!=LSS_IDENT_REMOTE_PRODUCT && \ groke6@361: d->lss_transfer.command!=LSS_IDENT_REMOTE_REV_LOW && \ groke6@361: d->lss_transfer.command!=LSS_IDENT_REMOTE_REV_HIGH && \ groke6@361: d->lss_transfer.command!=LSS_IDENT_REMOTE_SERIAL_LOW && \ groke6@361: d->lss_transfer.command!=LSS_IDENT_REMOTE_SERIAL_HIGH ) groke6@361: goto ErrorProcessMaster; etisserant@343: break; etisserant@343: case LSS_SM_SELECTIVE_RESP: /* Switch Mode Selective response */ groke6@361: if(d->lss_transfer.command!=LSS_SM_SELECTIVE_VENDOR && \ groke6@361: d->lss_transfer.command!=LSS_SM_SELECTIVE_PRODUCT && \ groke6@361: d->lss_transfer.command!=LSS_SM_SELECTIVE_REVISION && \ groke6@361: d->lss_transfer.command!=LSS_SM_SELECTIVE_SERIAL ) groke6@361: goto ErrorProcessMaster; groke6@361: break; etisserant@343: case LSS_IDENT_NON_CONF_SLAVE: /* LSS identify non-configured remote slave */ groke6@361: if(d->lss_transfer.command!=LSS_IDENT_REMOTE_NON_CONF)goto ErrorProcessMaster; etisserant@343: break; etisserant@343: default: groke6@381: MSG_ERR(0x1D1F, "Master LSS command not implemented", msg_cs); etisserant@343: return 0xFF; etisserant@343: } groke6@361: groke6@381: StopLSS_MSG_TIMER(); groke6@361: d->lss_transfer.state = LSS_FINISHED; groke6@361: etisserant@343: d->lss_transfer.dat1=Dat1; etisserant@343: d->lss_transfer.dat2=Dat2; etisserant@343: /* If there is a callback, it is responsible of the received response */ etisserant@343: if(d->lss_transfer.Callback) groke6@361: (*d->lss_transfer.Callback)(d,d->lss_transfer.command); etisserant@343: etisserant@343: return 0; groke6@361: groke6@361: ErrorProcessMaster: groke6@381: MSG_WAR(0x3D20, "MasterLSS proceedLSS; unexpected message arrived;command ", m->data[0]); groke6@361: return 0xFF; groke6@361: etisserant@343: } etisserant@343: etisserant@343: /*! etisserant@343: ** etisserant@343: ** etisserant@343: ** @param d etisserant@343: ** @param m etisserant@343: ** etisserant@343: ** @return etisserant@343: **/ etisserant@343: UNS8 proceedLSS_Slave(CO_Data* d, Message* m ) etisserant@343: { etisserant@343: UNS8 msg_cs; etisserant@343: groke6@381: MSG_WAR(0x3D21, "SlaveLSS proceedLSS; command ", m->data[0]); etisserant@343: etisserant@343: switch(msg_cs=m->data[0]){ etisserant@343: case LSS_SM_GLOBAL: /* Switch Mode Global */ etisserant@343: /* if there is not a mode change break*/ etisserant@343: if(m->data[1] == d->lss_transfer.mode){ groke6@381: MSG_WAR(0x3D22, "SlaveLSS already in the mode ", m->data[1]); etisserant@343: break; etisserant@343: } etisserant@343: etisserant@343: if(m->data[1]==LSS_CONFIGURATION_MODE) { groke6@381: MSG_WAR(0x3D23, "SlaveLSS switching to configuration mode ", 0); etisserant@343: /* Store the NodeId in case it will be changed */ groke6@381: //d->lss_transfer.nodeID=getNodeId(d); etisserant@343: d->lss_transfer.mode=LSS_CONFIGURATION_MODE; etisserant@343: } etisserant@343: else if(m->data[1]==LSS_WAITING_MODE){ groke6@381: MSG_WAR(0x3D24, "SlaveLSS switching to operational mode ", 0); etisserant@343: groke6@381: /* If the nodeID has changed update it and put the node state to Initialisation. */ groke6@381: if(d->lss_transfer.nodeID!=getNodeId(d)){ groke6@381: if(getNodeId(d)==0xFF){/* The nodeID was 0xFF; initialize the application*/ groke6@381: MSG_WAR(0x3D25, "The node Id has changed. Reseting to Initialisation state",0); etisserant@343: setNodeId(d, d->lss_transfer.nodeID); etisserant@343: setState(d, Initialisation); etisserant@343: } groke6@517: else{/* The nodeID will be changed on NMT_Reset_Comunication Request*/ groke6@381: } etisserant@343: } etisserant@343: d->lss_transfer.mode=LSS_WAITING_MODE; etisserant@343: } etisserant@343: break; etisserant@343: case LSS_CONF_NODE_ID: /* Configure Node-ID */ etisserant@343: { etisserant@343: UNS8 error_code=0; etisserant@343: UNS8 spec_error=0; etisserant@343: etisserant@343: if(d->lss_transfer.mode==LSS_CONFIGURATION_MODE){ etisserant@343: if(m->data[1]>127 && m->data[1]!=0xFF){ groke6@381: MSG_ERR(0x1D26, "NodeID out of range",0); etisserant@343: error_code=1; /* NodeID out of range */ etisserant@343: } etisserant@343: else{ etisserant@343: d->lss_transfer.nodeID=m->data[1]; etisserant@343: } etisserant@343: } etisserant@343: else{ groke6@381: MSG_WAR(0x3D27, "SlaveLSS not in configuration mode",0); groke6@361: //error_code=0xFF; groke6@361: break; etisserant@343: } etisserant@343: sendSlaveLSSMessage(d,msg_cs,&error_code,&spec_error); etisserant@343: } etisserant@343: break; etisserant@343: case LSS_CONF_BIT_TIMING: /* Configure Bit Timing Parameters */ etisserant@343: { etisserant@343: UNS8 error_code=0; etisserant@343: UNS8 spec_error=0; etisserant@343: etisserant@343: if(d->lss_transfer.mode==LSS_CONFIGURATION_MODE){ groke6@384: /* If a baud rate is not supported just comment the line. */ groke6@384: switch(m->data[2]){ groke6@384: case 0x00:d->lss_transfer.baudRate="1M";break; groke6@384: case 0x01:d->lss_transfer.baudRate="800K";break; groke6@384: case 0x02:d->lss_transfer.baudRate="500K";break; groke6@384: case 0x03:d->lss_transfer.baudRate="250K";break; groke6@384: case 0x04:d->lss_transfer.baudRate="125K";break; groke6@384: case 0x05:d->lss_transfer.baudRate="100K";break; groke6@384: case 0x06:d->lss_transfer.baudRate="50K";break; groke6@384: case 0x07:d->lss_transfer.baudRate="20K";break; groke6@384: case 0x08:d->lss_transfer.baudRate="10K";break; groke6@384: default: groke6@384: MSG_ERR(0x1D28, "Baud rate not supported",0); groke6@384: error_code=0xFF; /* Baud rate not supported*/ groke6@384: break; etisserant@343: } etisserant@343: } etisserant@343: else{ groke6@381: MSG_WAR(0x3D2A, "SlaveLSS not in configuration mode",0); groke6@361: //error_code=0xFF; groke6@361: break; etisserant@343: } etisserant@343: groke6@384: /* if bit timing is not supported comment the previous code and uncomment the following */ groke6@384: /*{ groke6@384: MSG_ERR(0x1D29, "Bit timing not supported",0); groke6@384: error_code=0x01; // bit timing not supported groke6@384: }*/ groke6@384: etisserant@343: sendSlaveLSSMessage(d,msg_cs,&error_code,&spec_error); etisserant@343: } etisserant@343: break; etisserant@343: case LSS_CONF_ACT_BIT_TIMING: /* Activate Bit Timing Parameters */ etisserant@343: etisserant@343: if(d->lss_transfer.mode!=LSS_CONFIGURATION_MODE){ groke6@381: MSG_ERR(0x3D2B, "SlaveLSS not in configuration mode",0); etisserant@343: break; etisserant@343: } etisserant@343: etisserant@343: if(d->lss_transfer.baudRate!="none"){ etisserant@343: d->lss_transfer.switchDelay=getLSSDelay(m); groke6@381: MSG_WAR(0x3D2C, "Slave Switch Delay set to: ",d->lss_transfer.switchDelay); etisserant@343: d->lss_transfer.switchDelayState=SDELAY_FIRST; groke6@381: //d->lss_transfer.currentState=getState(d); groke6@381: //setState(d, LssTimingDelay); groke6@381: d->lss_transfer.canHandle_t=d->canHandle; groke6@381: d->canHandle=NULL; etisserant@343: StartLSS_SDELAY_TIMER(); etisserant@343: } etisserant@343: break; etisserant@343: case LSS_CONF_STORE: /* Store Configured Parameters */ etisserant@343: { etisserant@343: UNS8 error_code=0; etisserant@343: UNS8 spec_error=0; etisserant@343: etisserant@343: if(d->lss_transfer.mode==LSS_CONFIGURATION_MODE){ etisserant@343: if(d->lss_StoreConfiguration){ etisserant@343: /* call lss_StoreConfiguration with NodeId */ groke6@381: (*d->lss_StoreConfiguration)(d,&error_code,&spec_error); etisserant@343: } etisserant@343: else{ groke6@381: MSG_ERR(0x1D2E, "Store configuration not supported",0); etisserant@343: error_code=1; /* store configuration is not supported */ etisserant@343: } etisserant@343: } etisserant@343: else{ groke6@381: MSG_WAR(0x3D2F, "SlaveLSS not in configuration mode",0); groke6@361: //error_code=0xFF; groke6@361: break; etisserant@343: } etisserant@343: sendSlaveLSSMessage(d,msg_cs,&error_code,&spec_error); etisserant@343: } etisserant@343: break; etisserant@343: case LSS_SM_SELECTIVE_VENDOR: /* Switch Mode Selective */ etisserant@343: case LSS_SM_SELECTIVE_PRODUCT: etisserant@343: case LSS_SM_SELECTIVE_REVISION: etisserant@343: case LSS_SM_SELECTIVE_SERIAL: etisserant@343: { etisserant@343: UNS32 errorCode; etisserant@343: const indextable *ptrTable; etisserant@343: ODCallback_t *Callback; etisserant@343: UNS32 _SpecificNodeInfo; etisserant@343: etisserant@343: if(d->lss_transfer.mode==LSS_CONFIGURATION_MODE) etisserant@343: { groke6@381: MSG_ERR(0x1D30, "Switch Mode Selective only supported in operational mode",0); etisserant@343: break; etisserant@343: } etisserant@343: etisserant@343: _SpecificNodeInfo=getLSSIdent(m); etisserant@343: etisserant@343: ptrTable = (*d->scanIndexOD)(0x1018, &errorCode, &Callback); etisserant@343: if(_SpecificNodeInfo==*(UNS32*)ptrTable->pSubindex[msg_cs-(LSS_SM_SELECTIVE_VENDOR-1)].pObject){ etisserant@343: etisserant@343: d->lss_transfer.addr_sel_match|=(0x01<<(msg_cs-LSS_SM_SELECTIVE_VENDOR)); etisserant@343: /* If all the fields has been set */ etisserant@343: if(d->lss_transfer.addr_sel_match==0x0F){ etisserant@343: groke6@381: MSG_WAR(0x3D31, "SlaveLSS switching to configuration mode ", 0); etisserant@343: d->lss_transfer.addr_sel_match=0; etisserant@343: d->lss_transfer.nodeID=getNodeId(d); etisserant@343: d->lss_transfer.mode=LSS_CONFIGURATION_MODE; etisserant@343: etisserant@343: sendSlaveLSSMessage(d,LSS_SM_SELECTIVE_RESP,0,0); etisserant@343: } etisserant@343: } etisserant@343: else { groke6@381: MSG_WAR(0x3D32, "LSS identity field doesn't match ", _SpecificNodeInfo); etisserant@343: d->lss_transfer.addr_sel_match=0; etisserant@343: } etisserant@343: } etisserant@343: break; etisserant@343: case LSS_IDENT_REMOTE_VENDOR: /* LSS Identify Remote Slaves */ etisserant@343: case LSS_IDENT_REMOTE_PRODUCT: etisserant@343: case LSS_IDENT_REMOTE_REV_LOW: etisserant@343: case LSS_IDENT_REMOTE_REV_HIGH: etisserant@343: case LSS_IDENT_REMOTE_SERIAL_LOW: etisserant@343: case LSS_IDENT_REMOTE_SERIAL_HIGH: etisserant@343: { etisserant@343: UNS32 errorCode; etisserant@343: const indextable *ptrTable; etisserant@343: ODCallback_t *Callback; etisserant@343: UNS32 _SpecificNodeInfo; etisserant@343: etisserant@343: _SpecificNodeInfo=getLSSIdent(m); etisserant@343: etisserant@343: ptrTable = (*d->scanIndexOD)(0x1018, &errorCode, &Callback); etisserant@343: etisserant@343: /* Check if the data match the identity object. */ etisserant@343: switch(msg_cs){ etisserant@343: 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; etisserant@343: 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; etisserant@343: 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; etisserant@343: 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; etisserant@343: 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; etisserant@343: 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; etisserant@343: } etisserant@343: /* If all the fields has been set.. */ etisserant@343: if(d->lss_transfer.addr_ident_match==0x3F){ groke6@381: MSG_WAR(0x3D33, "SlaveLSS identified ", 0); etisserant@343: d->lss_transfer.addr_ident_match=0; etisserant@343: sendSlaveLSSMessage(d,LSS_IDENT_SLAVE,0,0); etisserant@343: } etisserant@343: else if(d->lss_transfer.addr_ident_match==0){ groke6@381: MSG_WAR(0x3D34, "LSS identify field doesn't match ", _SpecificNodeInfo); etisserant@343: } etisserant@343: } etisserant@343: break; etisserant@343: case LSS_IDENT_REMOTE_NON_CONF: /* LSS identify non-configured remote slave */ etisserant@343: { etisserant@343: if(getNodeId(d)==0xFF){ groke6@381: MSG_WAR(0x3D35, "SlaveLSS non-configured ", 0); etisserant@343: sendSlaveLSSMessage(d,LSS_IDENT_NON_CONF_SLAVE,0,0); etisserant@343: } etisserant@343: else{ groke6@381: MSG_WAR(0x3D36, "SlaveLSS already configured ", 0); etisserant@343: } etisserant@343: } etisserant@343: break; etisserant@343: case LSS_INQ_VENDOR_ID: /* Inquire Identity Vendor-ID */ etisserant@343: case LSS_INQ_PRODUCT_CODE: /* Inquire Identity Product-Code */ etisserant@343: case LSS_INQ_REV_NUMBER: /* Inquire Identity Revision-Number */ etisserant@343: case LSS_INQ_SERIAL_NUMBER: /* Inquire Identity Serial-Number */ etisserant@343: if(d->lss_transfer.mode==LSS_CONFIGURATION_MODE) etisserant@343: { etisserant@343: etisserant@343: UNS32 errorCode; etisserant@343: const indextable *ptrTable; etisserant@343: ODCallback_t *Callback; etisserant@343: UNS32 _SpecificNodeInfo; etisserant@343: etisserant@343: ptrTable = (*d->scanIndexOD)(0x1018, &errorCode, &Callback); etisserant@343: _SpecificNodeInfo=*(UNS32*)ptrTable->pSubindex[msg_cs-(LSS_INQ_VENDOR_ID-1)].pObject; groke6@381: MSG_WAR(0x3D37, "SlaveLSS identity field inquired ", _SpecificNodeInfo); etisserant@343: etisserant@343: sendSlaveLSSMessage(d,msg_cs,&_SpecificNodeInfo,0); etisserant@343: } etisserant@343: break; etisserant@343: case LSS_INQ_NODE_ID: /* Inquire Node-ID */ etisserant@343: if(d->lss_transfer.mode==LSS_CONFIGURATION_MODE) etisserant@343: { etisserant@343: UNS8 NodeID; etisserant@343: etisserant@343: NodeID=getNodeId(d); groke6@381: MSG_WAR(0x3D38, "SlaveLSS Node ID inquired ", NodeID); etisserant@343: sendSlaveLSSMessage(d,msg_cs,&NodeID,0); etisserant@343: } etisserant@343: else{ groke6@381: MSG_WAR(0x3D39, "SlaveLSS not in configuration mode",0); groke6@361: } groke6@361: break; groke6@361: #ifdef CO_ENABLE_LSS_FS etisserant@343: case LSS_IDENT_FASTSCAN: etisserant@343: { groke6@361: /* If the nodeID isn't 0xFF the slave shall not participate */ groke6@361: if(getNodeId(d)!=0xFF)break; groke6@361: if(getLSSBitCheck(m)==128)d->lss_transfer.FastScan_SM=LSS_FS_RESET; groke6@361: groke6@361: switch(d->lss_transfer.FastScan_SM){ groke6@361: case LSS_FS_RESET: groke6@361: { groke6@361: UNS32 errorCode; groke6@361: const indextable *ptrTable; groke6@361: ODCallback_t *Callback; groke6@361: groke6@381: MSG_WAR(0x3D3A, "SlaveLSS Reseting LSSPos", 0); groke6@361: d->lss_transfer.LSSPos=0; groke6@361: d->lss_transfer.FastScan_SM=LSS_FS_PROCESSING; groke6@361: groke6@361: ptrTable = (*d->scanIndexOD)(0x1018, &errorCode, &Callback); groke6@361: d->lss_transfer.IDNumber=*(UNS32*)ptrTable->pSubindex[d->lss_transfer.LSSPos+1].pObject; groke6@361: groke6@361: sendSlaveLSSMessage(d,LSS_IDENT_SLAVE,0,0); groke6@361: } groke6@361: break; groke6@361: case LSS_FS_PROCESSING:/*if(getLSSBitCheck(m)<32)*/ groke6@361: if(d->lss_transfer.LSSPos==getLSSSub(m)) groke6@361: { groke6@361: UNS32 Mask=0xFFFFFFFF<lss_transfer.IDNumber); groke6@361: groke6@361: if((getLSSIdent(m) & Mask)==(d->lss_transfer.IDNumber & Mask)) groke6@361: { groke6@361: sendSlaveLSSMessage(d,LSS_IDENT_SLAVE,0,0); groke6@361: } groke6@361: groke6@361: if(getLSSBitCheck(m)==0) groke6@361: { groke6@361: d->lss_transfer.FastScan_SM=LSS_FS_CONFIRMATION; groke6@361: } groke6@361: } groke6@361: break; groke6@361: case LSS_FS_CONFIRMATION: groke6@361: if(d->lss_transfer.LSSPos==getLSSSub(m)) groke6@361: { groke6@361: if(getLSSIdent(m)==d->lss_transfer.IDNumber) groke6@361: { groke6@361: /* Current LSS-ID[sub] confirmed correctly */ groke6@381: MSG_WAR(0x3D3E, "SlaveLSS FastScan IDNumber and LSS-ID match=>", d->lss_transfer.IDNumber); groke6@361: if(d->lss_transfer.LSSPos==3) groke6@361: { groke6@361: /* All LSS-ID[sub] identified correctly, switching to configuration mode */ groke6@381: MSG_WAR(0x3D3F, "SlaveLSS switching to configuration mode ", 0); groke6@361: d->lss_transfer.nodeID=getNodeId(d); groke6@361: d->lss_transfer.mode=LSS_CONFIGURATION_MODE; groke6@361: d->lss_transfer.FastScan_SM=LSS_FS_RESET; groke6@361: d->lss_transfer.LSSPos=0xFF; groke6@361: } groke6@361: else groke6@361: { groke6@361: /* Switch to the next LSS-ID[sub] */ groke6@361: UNS32 errorCode; groke6@361: const indextable *ptrTable; groke6@361: ODCallback_t *Callback; groke6@361: groke6@361: d->lss_transfer.LSSPos=getLSSNext(m); groke6@361: ptrTable = (*d->scanIndexOD)(0x1018, &errorCode, &Callback); groke6@361: d->lss_transfer.IDNumber=*(UNS32*)ptrTable->pSubindex[d->lss_transfer.LSSPos+1].pObject; groke6@361: d->lss_transfer.FastScan_SM=LSS_FS_PROCESSING; groke6@361: } groke6@361: sendSlaveLSSMessage(d,LSS_IDENT_SLAVE,0,0); groke6@361: } groke6@361: } groke6@361: break; groke6@361: } etisserant@343: } etisserant@343: break; groke6@361: #endif etisserant@343: default: groke6@381: MSG_ERR(0x1D40, "SlaveLSS command not implemented", msg_cs); etisserant@343: return 0xFF; etisserant@343: } etisserant@343: etisserant@343: return 0; etisserant@343: } etisserant@343: groke6@381: /*UNS8 configNetworkNode(CO_Data* d, UNS8 command, void *dat1, void* dat2) etisserant@343: { etisserant@343: return sendMasterLSSMessage(d,command,dat1,dat2); groke6@381: }*/ groke6@381: groke6@381: UNS8 configNetworkNode (CO_Data* d, UNS8 command, void *dat1, void* dat2, LSSCallback_t Callback) etisserant@343: { groke6@381: //d->lss_transfer.state=LSS_TRANS_IN_PROGRESS; etisserant@343: d->lss_transfer.Callback=Callback; etisserant@343: d->lss_transfer.command=command; etisserant@343: groke6@381: StopLSS_MSG_TIMER(); groke6@381: //StartLSS_MSG_TIMER(); etisserant@343: etisserant@343: return sendMasterLSSMessage(d,command,dat1,dat2); etisserant@343: } etisserant@343: etisserant@343: UNS8 getConfigResultNetworkNode (CO_Data* d, UNS8 command, UNS32* dat1, UNS8* dat2) etisserant@343: { etisserant@343: *dat1=d->lss_transfer.dat1; etisserant@343: *dat2=d->lss_transfer.dat2; etisserant@343: return d->lss_transfer.state; etisserant@343: } etisserant@343: etisserant@343: //void _lss_StoreConfiguration(UNS8 *error, UNS8 *spec_error){printf("_lss_StoreConfiguration\n");} etisserant@343: etisserant@343: #endif