etisserant@0: /*
etisserant@0: This file is part of CanFestival, a library implementing CanOpen Stack. 
etisserant@0: 
etisserant@0: Copyright (C): Edouard TISSERANT and Francis DUPIN
etisserant@0: 
etisserant@0: See COPYING file for copyrights details.
etisserant@0: 
etisserant@0: This library is free software; you can redistribute it and/or
etisserant@0: modify it under the terms of the GNU Lesser General Public
etisserant@0: License as published by the Free Software Foundation; either
etisserant@0: version 2.1 of the License, or (at your option) any later version.
etisserant@0: 
etisserant@0: This library is distributed in the hope that it will be useful,
etisserant@0: but WITHOUT ANY WARRANTY; without even the implied warranty of
etisserant@0: MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
etisserant@0: Lesser General Public License for more details.
etisserant@0: 
etisserant@0: You should have received a copy of the GNU Lesser General Public
etisserant@0: License along with this library; if not, write to the Free Software
etisserant@0: Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
etisserant@0: */
etisserant@0: 
etisserant@0: #include "data.h"
etisserant@0: #include "sync.h"
etisserant@0: 
frdupin@53: // Prototypes for internals functions
frdupin@53: void SyncAlarm(CO_Data* d, UNS32 id);
frdupin@53: UNS32 OnCOB_ID_SyncUpdate(CO_Data* d, const indextable * unsused_indextable, 
frdupin@53: 	UNS8 unsused_bSubindex);
frdupin@53: 
frdupin@53: 
frdupin@53: 
frdupin@53: /*****************************************************************************/
etisserant@0: void SyncAlarm(CO_Data* d, UNS32 id)
etisserant@0: {
etisserant@0: 	sendSYNC(d, *d->COB_ID_Sync & 0x1FFFFFFF) ;
etisserant@0: }
etisserant@0: 
frdupin@53: /*****************************************************************************/
etisserant@0: // This is called when Index 0x1005 is updated.
etisserant@0: UNS32 OnCOB_ID_SyncUpdate(CO_Data* d, const indextable * unsused_indextable, UNS8 unsused_bSubindex)
etisserant@0: {
etisserant@0: 	startSYNC(d);
lbessard@31: 	return 0;
etisserant@0: }
etisserant@0: 
frdupin@53: /*****************************************************************************/
etisserant@0: void startSYNC(CO_Data* d)
etisserant@0: {
etisserant@0: 	RegisterSetODentryCallBack(d, 0x1005, 0, &OnCOB_ID_SyncUpdate);
etisserant@0: 	RegisterSetODentryCallBack(d, 0x1006, 0, &OnCOB_ID_SyncUpdate);
etisserant@0: 
etisserant@0: 	if(d->syncTimer != TIMER_NONE){
etisserant@0: 		stopSYNC(d);
etisserant@0: 	}
etisserant@0: 	
etisserant@0: 	if(*d->COB_ID_Sync & 0x40000000 && *d->Sync_Cycle_Period)
etisserant@0: 	{
etisserant@0: 		d->syncTimer = SetAlarm(
etisserant@0: 				d,
etisserant@0: 				0/*No id needed*/,
etisserant@0: 				&SyncAlarm,
etisserant@0: 				US_TO_TIMEVAL(*d->Sync_Cycle_Period), 
etisserant@0: 				US_TO_TIMEVAL(*d->Sync_Cycle_Period));
etisserant@0: 	}
etisserant@0: }
etisserant@0: 
frdupin@53: /*****************************************************************************/
etisserant@0: void stopSYNC(CO_Data* d)
etisserant@0: {
etisserant@0: 	d->syncTimer = DelAlarm(d->syncTimer);
etisserant@0: }
etisserant@0: 
etisserant@0: /*********************************************************************/
etisserant@0: UNS8 sendSYNC(CO_Data* d, UNS32 cob_id)
etisserant@0: {
etisserant@0:   Message m;
etisserant@0:   UNS8 resultat ;
etisserant@0:   
etisserant@0:   MSG_WAR(0x3001, "sendSYNC ", 0);
etisserant@0:   
etisserant@0:   m.cob_id.w = cob_id ;
etisserant@0:   m.rtr = NOT_A_REQUEST;
etisserant@0:   m.len = 0;
etisserant@0:   resultat = (*d->canSend)(&m) ;
etisserant@0:   proceedSYNC(d, &m) ; 
etisserant@0:   return resultat ;
etisserant@0: }
etisserant@0: 
etisserant@0: /*****************************************************************************/
etisserant@0: UNS8 proceedSYNC(CO_Data* d, Message *m)
etisserant@0: {
etisserant@0: 
etisserant@0:   MSG_WAR(0x3002, "SYNC received. Proceed. ", 0);
etisserant@0: 
etisserant@0:   UNS8 	pdoNum,       // number of the actual processed pdo-nr.
etisserant@0:     prp_j;
etisserant@0: 
etisserant@0:   const UNS8 *     pMappingCount = NULL;      // count of mapped objects...
etisserant@0:   // pointer to the var which is mapped to a pdo
etisserant@0: //  void *     pMappedAppObject = NULL; 
etisserant@0:   // pointer fo the var which holds the mapping parameter of an mapping entry  
etisserant@0:   UNS32 *    pMappingParameter = NULL;  
etisserant@0:   // pointer to the transmissiontype...
etisserant@0:   UNS8 *     pTransmissionType = NULL;  
etisserant@0:   UNS32 *    pwCobId = NULL;	
etisserant@0: 
etisserant@0:   UNS8 *    pSize;
etisserant@0:   UNS8      size;
etisserant@0:   UNS8      dataType;
etisserant@0:   UNS16 index;
etisserant@0:   UNS8 subIndex;
etisserant@0:   UNS8 offset;
etisserant@0:   UNS8 status;
etisserant@0:   UNS8 sizeData;
etisserant@0:   pSize = &size;
etisserant@0:   UNS32   objDict;	
etisserant@0:   status = state3;
etisserant@0:   pdoNum=0x00;
etisserant@0:   prp_j=0x00;
etisserant@0:   offset = 0x00;
etisserant@0:   UNS16 offsetObjdict;
etisserant@0:   UNS16 offsetObjdictMap;
etisserant@0:   UNS16 lastIndex;
etisserant@0: 
etisserant@0:   /* only operational state allows PDO transmission */
etisserant@0:   if( d->nodeState != Operational ) 
etisserant@0:     return 0;
etisserant@0: 
etisserant@0:    (*d->post_sync)();
etisserant@0:   
etisserant@0:   /* So, the node is in operational state */
etisserant@0:   /* study all PDO stored in the objects dictionary */	
etisserant@0:  
etisserant@0:   offsetObjdict = d->firstIndex->PDO_TRS;
etisserant@0:   lastIndex = d->lastIndex->PDO_TRS;
etisserant@0:   offsetObjdictMap = d->firstIndex->PDO_TRS_MAP;
etisserant@0:   
etisserant@0:   if(offsetObjdict) while( offsetObjdict <= lastIndex) {  
etisserant@0:     switch( status ) {
etisserant@0:                     
etisserant@0:     case state3:    /* get the PDO transmission type */
etisserant@0:       if (d->objdict[offsetObjdict].bSubCount <= 2) {
etisserant@0: 	  MSG_ERR(0x1004, "Subindex 2  not found at index ", 0x1800 + pdoNum);
etisserant@0: 	  return 0xFF;
etisserant@0: 	}
etisserant@0:       pTransmissionType = d->objdict[offsetObjdict].pSubindex[2].pObject;    
etisserant@0:       MSG_WAR(0x3005, "Reading PDO at index : ", 0x1800 + pdoNum);
etisserant@0:       status = state4; 
etisserant@0:       break;     
etisserant@0:     case state4:	/* check if transmission type is after (this) SYNC */
etisserant@0:                         /* The message may not be transmited every SYNC but every n SYNC */      
etisserant@0:       if( (*pTransmissionType >= TRANS_SYNC_MIN) && (*pTransmissionType <= TRANS_SYNC_MAX) &&
etisserant@0:           (++d->count_sync[pdoNum] == *pTransmissionType) ) {	
etisserant@0: 	d->count_sync[pdoNum] = 0;
etisserant@0: 	MSG_WAR(0x3007, "  PDO is on SYNCHRO. Trans type : ", *pTransmissionType);
etisserant@0: 	status = state5;
etisserant@0: 	break;
etisserant@0:       }
etisserant@0:       else {
etisserant@0: 	MSG_WAR(0x3008, "  Not on synchro or not at this SYNC. Trans type : ", 
etisserant@0: 		*pTransmissionType);
etisserant@0: 	pdoNum++;
etisserant@0: 	offsetObjdict++;
etisserant@0: 	offsetObjdictMap++;
etisserant@0: 	status = state11;
etisserant@0: 	break;
etisserant@0:       }      
etisserant@0:     case state5:	/* get PDO CobId */
etisserant@0:         pwCobId = d->objdict[offsetObjdict].pSubindex[1].pObject;     
etisserant@0: 	MSG_WAR(0x3009, "  PDO CobId is : ", *pwCobId);
etisserant@0: 	status = state7;
etisserant@0: 	break;     
etisserant@0:     case state7:  /* get mapped objects number to transmit with this PDO */
etisserant@0:       pMappingCount = d->objdict[offsetObjdictMap].pSubindex[0].pObject;
etisserant@0: 	MSG_WAR(0x300D, "  Number of objects mapped : ",*pMappingCount );
etisserant@0: 	status = state8;
etisserant@0:     case state8:	/* get mapping parameters */
etisserant@0:       pMappingParameter = d->objdict[offsetObjdictMap].pSubindex[prp_j + 1].pObject;
etisserant@0: 	MSG_WAR(0x300F, "  got mapping parameter : ", *pMappingParameter);
etisserant@0: 	MSG_WAR(0x3050, "    at index : ", 0x1A00 + pdoNum);
etisserant@0: 	MSG_WAR(0x3051, "    sub-index : ", prp_j + 1);
etisserant@0: 	status = state9;
etisserant@0:     
etisserant@0:     case state9:	/* get data to transmit */ 
etisserant@0:       index = (UNS16)((*pMappingParameter) >> 16);
etisserant@0:       subIndex = (UNS8)(( (*pMappingParameter) >> (UNS8)8 ) & (UNS32)0x000000FF);
etisserant@0:       // <<3 because in *pMappingParameter the size is in bits
etisserant@0:       sizeData = (UNS8) ((*pMappingParameter & (UNS32)0x000000FF) >> 3) ;
etisserant@0: 
etisserant@0:         objDict = getODentry(d, index, subIndex, (void *)&d->process_var.data[offset], &sizeData, &dataType, 0 ); 
etisserant@0: 
etisserant@0:         if( objDict != OD_SUCCESSFUL ){
etisserant@0:           MSG_ERR(0x1013, " Couldn't find mapped variable at index-subindex-size : ", (UNS16)(*pMappingParameter));
etisserant@0:           return 0xFF;
etisserant@0:         }
etisserant@0: 	
etisserant@0: 	offset += sizeData ;
etisserant@0: 	d->process_var.count = offset;
etisserant@0: 	prp_j++;
etisserant@0: 	status = state10;	 
etisserant@0: 	break;					
etisserant@0:       
etisserant@0:     case state10:	/* loop to get all the data to transmit */
etisserant@0:       if( prp_j < *pMappingCount ){
etisserant@0: 	MSG_WAR(0x3014, "  next variable mapped : ", prp_j);
etisserant@0: 	status = state8;
etisserant@0: 	break;
etisserant@0:       }
etisserant@0:       else {
etisserant@0: 	MSG_WAR(0x3015, "  End scan mapped variable", 0);
etisserant@0: 	PDOmGR( d, *pwCobId );	
etisserant@0: 	MSG_WAR(0x3016, "  End of this pdo. Should have been sent", 0);
etisserant@0: 	pdoNum++;
etisserant@0: 	offsetObjdict++;
etisserant@0: 	offsetObjdictMap++;
etisserant@0: 	offset = 0x00;
etisserant@0: 	prp_j = 0x00;
etisserant@0: 	status = state11;
etisserant@0: 	break;
etisserant@0:       }
etisserant@0:       
etisserant@0:     case state11:     
etisserant@0:       MSG_WAR(0x3017, "next pdo index : ", pdoNum);
etisserant@0:       status = state3;
etisserant@0:       break;
etisserant@0:       
etisserant@0:     default:
etisserant@0:       MSG_ERR(0x1019,"Unknown state has been reached : %d",status);
etisserant@0:       return 0xFF;
etisserant@0:     }// end switch case
etisserant@0:     
etisserant@0:   }// end while( prp_i<dict_cstes.max_count_of_PDO_transmit )
etisserant@0:    
etisserant@0:   (*d->post_TPDO)();
etisserant@0: 
etisserant@0:   return 0;
etisserant@0: }
etisserant@0: 
etisserant@0: