greg@178: /*
greg@178: This file is part of CanFestival, a library implementing CanOpen Stack. 
greg@178: 
greg@178: Copyright (C): Edouard TISSERANT and Francis DUPIN
greg@178: 
greg@178: See COPYING file for copyrights details.
greg@178: 
greg@178: This library is free software; you can redistribute it and/or
greg@178: modify it under the terms of the GNU Lesser General Public
greg@178: License as published by the Free Software Foundation; either
greg@178: version 2.1 of the License, or (at your option) any later version.
greg@178: 
greg@178: This library is distributed in the hope that it will be useful,
greg@178: but WITHOUT ANY WARRANTY; without even the implied warranty of
greg@178: MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
greg@178: Lesser General Public License for more details.
greg@178: 
greg@178: You should have received a copy of the GNU Lesser General Public
greg@178: License along with this library; if not, write to the Free Software
greg@178: Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
greg@178: */
greg@178: 
greg@178: #include "objacces.h"
greg@178: #include "sdo.h"
greg@178: #include "dcf.h"
greg@178: 
greg@178: const indextable *ptrTable;
greg@178: 
greg@178: static void CheckSDOAndContinue(CO_Data* d, UNS8 nodeId)
greg@178: {
greg@178: 	UNS32 abortCode;
greg@178: 	
greg@178: 	if(getWriteResultNetworkDict (d, nodeId, &abortCode) != SDO_FINISHED)
etisserant@191: 	{
etisserant@191: 		MSG_ERR(0x1A01, "SDO error in consise DCF", abortCode);
etisserant@191: 		MSG_WAR(0x2A02, "server node : ", nodeId);
etisserant@191: 	}
etisserant@191: 
greg@178: 	closeSDOtransfer(d, nodeId, SDO_CLIENT);
etisserant@191: 	decompo_dcf(d,nodeId);
greg@178: }
greg@178: 
greg@178: UNS32 decompo_dcf(CO_Data* d,UNS8 nodeId)
greg@178: {
greg@178: 		UNS32 errorCode;
greg@178: 		UNS16 target_Index;
greg@178: 		UNS8 target_Subindex;
greg@178: 		UNS32 target_Size;
greg@178: 		UNS32 res;
greg@178:   		ODCallback_t *Callback;
greg@178: 
greg@178: 		ptrTable = (*d->scanIndexOD)(0x1F22, &errorCode, &Callback);
greg@178: 		if (errorCode != OD_SUCCESSFUL)
greg@178: 		{
greg@178:     		return errorCode;
greg@178: 		}
greg@178: 
greg@178: 		/*Loop on all Nodes supported in DCF subindexes*/
greg@178: 		while (nodeId < ptrTable->bSubCount){
greg@178: 			UNS32 nb_targets;
greg@178: 	  		
greg@178: 	  		UNS8 szData = ptrTable->pSubindex[nodeId].size;
greg@178: 	  		void* dcfend;
greg@178: 	  		
greg@178: 		  	{
greg@178: 			  	void* dcf = *((void**)ptrTable->pSubindex[nodeId].pObject);
greg@178: 		  		dcfend = dcf + szData;
greg@178: 				if (!d->dcf_cursor)	{
greg@178: 					d->dcf_cursor = dcf + 4;
greg@178: 					d->dcf_count_targets = 0;
greg@178: 				}
greg@178: 	#ifdef CANOPEN_BIG_ENDIAN
greg@178: 				nb_targets = ((UNS8*)d->dcf++) | ((UNS8*)d->dcf++) << 8 | ((UNS8*)d->dcf++) << 16 | ((UNS8*)d->dcf++) << 24;
greg@178: 	#else
greg@178: 				nb_targets = *((UNS32*)dcf);
greg@178: 	#endif
greg@178: 		  	}
greg@178: 			
greg@178: 			// condition on consise DCF string for NodeID, if big enough
greg@178: 			if(d->dcf_cursor + 7 < dcfend && d->dcf_count_targets < nb_targets)
greg@178: 			{
greg@178: 				// pointer to the DCF string for NodeID
greg@178: 	#ifdef CANOPEN_BIG_ENDIAN
greg@178: 				target_Index = ((UNS8*)d->dcf_cursor++) | ((UNS8*)d->dcf_cursor++) << 8;
greg@178: 				target_Subindex = ((UNS8*)d->dcf_cursor++);
greg@178: 				target_Size = ((UNS8*)d->dcf_cursor++) | ((UNS8*)d->dcf_cursor++) << 8 | ((UNS8*)d->dcf_cursor++) << 16 | ((UNS8*)d->dcf_cursor++) << 24;
greg@178: 	#else
greg@178: 				target_Index = *((UNS16*)(d->dcf_cursor)); d->dcf_cursor += 2;
greg@178: 				target_Subindex = *((UNS8*)(d->dcf_cursor++));
greg@178: 				target_Size = *((UNS32*)(d->dcf_cursor)); d->dcf_cursor += 4;
greg@178: 	#endif
greg@178: 				
etisserant@191: 					/*printf("Master : ConfigureSlaveNode %2.2x (Concise DCF)\n",nodeId);*/
greg@178: 					res = writeNetworkDictCallBack(d, /*CO_Data* d*/
greg@178: 							nodeId, /*UNS8 nodeId*/
greg@178: 							target_Index, /*UNS16 index*/
greg@178: 							target_Subindex, /*UNS8 subindex*/
greg@178: 							target_Size, /*UNS8 count*/
greg@178: 							0, /*UNS8 dataType*/
greg@178: 							d->dcf_cursor,/*void *data*/
greg@178: 							CheckSDOAndContinue); /*SDOCallback_t Callback*/					
greg@178: 					/*Push d->dcf_cursor to the end of data*/
greg@178: 					
greg@178: 					d->dcf_cursor += target_Size;
greg@178: 					d->dcf_count_targets++;
greg@178: 					
greg@178: 					return ;
greg@178: 			}			
greg@178: 				nodeId++;
greg@178: 				d->dcf_cursor = NULL;
greg@178: 		}
greg@178: 		/* Switch Master to preOperational state */
greg@178: 		(*d->preOperational)();
greg@178: 		
greg@178: }