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: 
dejoigny@11: //#define DEBUG_WAR_CONSOLE_ON
dejoigny@11: //#define DEBUG_ERR_CONSOLE_ON
dejoigny@11: 
etisserant@1: 
etisserant@0: #include "objacces.h"
etisserant@0: 
dejoigny@8: 
etisserant@0: #ifdef DEBUG_WAR_CONSOLE_ON
etisserant@0: UNS8 accessDictionaryError(UNS16 index, UNS8 subIndex, 
etisserant@0: 			     UNS8 sizeDataDict, UNS8 sizeDataGiven, UNS32 code)
etisserant@0: {
etisserant@0:   MSG_WAR(0x2B09,"Dictionary index : ", index);
etisserant@0:   MSG_WAR(0X2B10,"           subindex : ", subIndex);
etisserant@0:   switch (code) {
etisserant@0:     case  OD_NO_SUCH_OBJECT: 
etisserant@0:       MSG_WAR(0x2B11,"Index not found ", index);
etisserant@0:       break;
etisserant@0:     case OD_NO_SUCH_SUBINDEX :
etisserant@0:       MSG_WAR(0x2B12,"SubIndex not found ", subIndex);
etisserant@0:       break;   
etisserant@0:     case OD_WRITE_NOT_ALLOWED :
etisserant@0:       MSG_WAR(0x2B13,"Write not allowed, data is read only ", index);
etisserant@0:       break;         
etisserant@0:     case OD_LENGTH_DATA_INVALID :    
etisserant@0:       MSG_WAR(0x2B14,"Conflict size data. Should be (bytes)  : ", sizeDataDict);
etisserant@0:       MSG_WAR(0x2B15,"But you have given the size  : ", sizeDataGiven);
etisserant@0:       break;
etisserant@0:     case OD_NOT_MAPPABLE :
etisserant@0:       MSG_WAR(0x2B16,"Not mappable data in a PDO at index    : ", index);
etisserant@0:       break;
etisserant@0:     case OD_VALUE_TOO_LOW :
etisserant@0:       MSG_WAR(0x2B17,"Value range error : value too low. SDOabort : ", code);
etisserant@0:       break;
etisserant@0:     case OD_VALUE_TOO_HIGH :
etisserant@0:       MSG_WAR(0x2B18,"Value range error : value too high. SDOabort : ", code);
etisserant@0:       break;
etisserant@0:   default :
etisserant@0:     MSG_WAR(0x2B20, "Unknown error code : ", code);
etisserant@0:   }
etisserant@0:   return 0; 
etisserant@0: }	
etisserant@0: #endif
etisserant@0: 
etisserant@0: UNS32 getODentry( CO_Data* d, 
etisserant@0:                   UNS16 wIndex,
etisserant@0: 		  UNS8 bSubindex,
etisserant@0: 		  void * pDestData,
etisserant@0: 		  UNS8 * pExpectedSize,
etisserant@0: 		  UNS8 * pDataType,
etisserant@0: 		  UNS8 checkAccess)
etisserant@0: { // DO NOT USE MSG_ERR because the macro may send a PDO -> infinite loop if it fails.
etisserant@0:   UNS32 errorCode;
etisserant@0:   UNS8 szData;
etisserant@0:   const indextable *ptrTable;
etisserant@0:   ODCallback_t *Callback;
etisserant@0: 
etisserant@0:   ptrTable = (*d->scanIndexOD)(wIndex, &errorCode, &Callback);
etisserant@0: 
etisserant@0:   if (errorCode != OD_SUCCESSFUL)
etisserant@0:     return errorCode;
etisserant@0:   if( ptrTable->bSubCount <= bSubindex ) {
etisserant@0:     // Subindex not found
etisserant@0:     accessDictionaryError(wIndex, bSubindex, 0, 0, OD_NO_SUCH_SUBINDEX);
etisserant@0:     return OD_NO_SUCH_SUBINDEX;
etisserant@0:   }
etisserant@0:   
etisserant@0:   if (checkAccess && !(ptrTable->pSubindex[bSubindex].bAccessType & WO)) {
etisserant@1:   	MSG_WAR(0x2B30, "Access Type : ", ptrTable->pSubindex[bSubindex].bAccessType);
etisserant@0:     accessDictionaryError(wIndex, bSubindex, 0, 0, OD_WRITE_NOT_ALLOWED);
etisserant@0:     return OD_READ_NOT_ALLOWED;
etisserant@0:   }
etisserant@0: 
etisserant@0:   *pDataType = ptrTable->pSubindex[bSubindex].bDataType;
etisserant@0:    szData = ptrTable->pSubindex[bSubindex].size;
etisserant@0: 
dejoigny@12:    if(	*pExpectedSize == 0 ||
etisserant@0:   	*pExpectedSize == szData ||
dejoigny@12:   	(*pDataType == visible_string && *pExpectedSize < szData)) {// We allow to fetch a shorter string than expected
dejoigny@12:      
dejoigny@12: #  ifdef CANOPEN_BIG_ENDIAN
dejoigny@12:      if(*pDataType > boolean && *pDataType < visible_string) {
dejoigny@12:        // data must be transmited with low byte first
dejoigny@12:        UNS8 i, j = 0;
dejoigny@12:        MSG_WAR(boolean, "data type ", *pDataType);
dejoigny@12:        MSG_WAR(visible_string, "data type ", *pDataType);
dejoigny@12:        for ( i = szData ; i > 0 ; i--) {
dejoigny@12:          MSG_WAR(i," ", j);
dejoigny@12: 	 ((UNS8*)pDestData)[j++] = 
dejoigny@12: 	   ((UNS8*)ptrTable->pSubindex[bSubindex].pObject)[i-1];
dejoigny@12:        }
dejoigny@12:      }
dejoigny@12:      else // It it is a visible string no endianisation to perform
dejoigny@12:        memcpy(pDestData, ptrTable->pSubindex[bSubindex].pObject,szData);
dejoigny@12: #  else
dejoigny@12:      memcpy(pDestData, ptrTable->pSubindex[bSubindex].pObject,szData);
dejoigny@12: #  endif
dejoigny@12:      
dejoigny@12:      *pExpectedSize = szData;
dejoigny@12: #if 0
dejoigny@12:      // Me laisser ça, please ! (FD)
dejoigny@12:      {
dejoigny@12:        UNS8 i;
dejoigny@12:        for (i = 0 ; i < 10 ; i++) {
dejoigny@12: 	 MSG_WAR(*pExpectedSize, "dic data= ",
dejoigny@12: 		 *(UNS8 *)(ptrTable->pSubindex[bSubindex].pObject + i));
dejoigny@12:        }
dejoigny@12:       
dejoigny@12:      }
dejoigny@12: #endif
dejoigny@12:      return OD_SUCCESSFUL;
dejoigny@12:    }
dejoigny@12:    else { // Error !
dejoigny@12:      *pExpectedSize = szData;
dejoigny@12:      accessDictionaryError(wIndex, bSubindex, szData, 
dejoigny@12: 			   *pExpectedSize, OD_LENGTH_DATA_INVALID);
dejoigny@12:      return OD_LENGTH_DATA_INVALID;
dejoigny@12:    }
etisserant@0: }
etisserant@0: 
etisserant@0: UNS32 setODentry( CO_Data* d, 
etisserant@0:                   UNS16 wIndex,
etisserant@0: 		  UNS8 bSubindex, 
etisserant@0: 		  void * pSourceData, 
etisserant@0: 		  UNS8 * pExpectedSize, 
etisserant@0: 		  UNS8 checkAccess)
etisserant@0: {
etisserant@0:   UNS8 szData;
etisserant@0:   UNS8 dataType;
etisserant@0:   UNS32 errorCode;
etisserant@0:   const indextable *ptrTable;
etisserant@0:   ODCallback_t *Callback;
etisserant@0: 
etisserant@0:   ptrTable =(*d->scanIndexOD)(wIndex, &errorCode, &Callback);
etisserant@0:   if (errorCode != OD_SUCCESSFUL)
etisserant@0:     return errorCode;
etisserant@0: 
etisserant@0:   if( ptrTable->bSubCount <= bSubindex ) {
etisserant@0:     // Subindex not found
etisserant@0:     accessDictionaryError(wIndex, bSubindex, 0, *pExpectedSize, OD_NO_SUCH_SUBINDEX);
etisserant@0:     return OD_NO_SUCH_SUBINDEX;
etisserant@0:   }
etisserant@1:   if (checkAccess && (ptrTable->pSubindex[bSubindex].bAccessType == RO)) {
etisserant@1:   	MSG_WAR(0x2B25, "Access Type : ", ptrTable->pSubindex[bSubindex].bAccessType);
etisserant@0:     accessDictionaryError(wIndex, bSubindex, 0, *pExpectedSize, OD_WRITE_NOT_ALLOWED);
etisserant@0:     return OD_WRITE_NOT_ALLOWED;
etisserant@0:   }
etisserant@0: 
etisserant@0: 
etisserant@0:    dataType = ptrTable->pSubindex[bSubindex].bDataType;
etisserant@0:    szData = ptrTable->pSubindex[bSubindex].size;
etisserant@0: 
etisserant@0:   if( *pExpectedSize == 0 ||
etisserant@0:   	*pExpectedSize == szData ||
etisserant@0:   	(dataType == visible_string && *pExpectedSize < szData)) // We allow to store a shorter string than entry size
etisserant@0:   {
etisserant@2:       #ifdef CANOPEN_BIG_ENDIAN
dejoigny@11: 	      if(dataType > boolean && dataType < visible_string)
dejoigny@11: 	      {
dejoigny@11: 			// we invert the data source directly. This let us do range testing without
dejoigny@11: 			// additional temp variable
dejoigny@11: 		 	UNS8 tmp, i;
dejoigny@11: 	  		for ( i = 0 ; i < ( ptrTable->pSubindex[bSubindex].size >> 1)  ; i++) 
dejoigny@11: 	  		{
dejoigny@11: 	    		UNS8 tmp =((UNS8 *)pSourceData) [(ptrTable->pSubindex[bSubindex].size - 1) - i];
dejoigny@11: 	    		((UNS8 *)pSourceData) [(ptrTable->pSubindex[bSubindex].size - 1) - i] = ((UNS8 *)pSourceData)[i];
dejoigny@11: 	    		((UNS8 *)pSourceData)[i] = tmp;
dejoigny@11: 	    	}
dejoigny@11: 		}  
etisserant@2:       #endif
etisserant@0:       errorCode = (*d->valueRangeTest)(dataType, pSourceData);
etisserant@0:       if (errorCode) {
etisserant@0: 	accessDictionaryError(wIndex, bSubindex, szData, *pExpectedSize, errorCode);
etisserant@0: 	return errorCode;
etisserant@0:       }
etisserant@2:       memcpy(ptrTable->pSubindex[bSubindex].pObject,pSourceData, *pExpectedSize);
etisserant@0:       *pExpectedSize = szData;
etisserant@0:       
etisserant@0:       // Callbacks
etisserant@0:       if(Callback && Callback[bSubindex]){
etisserant@0:       	 (*Callback[bSubindex])(d, ptrTable, bSubindex);
etisserant@0:       }
etisserant@0:       
etisserant@0:       // TODO : Store dans NVRAM      
etisserant@0:       // if (ptrTable->pSubindex[bSubindex].bAccessType & TO_BE_SAVED)
etisserant@0:       return OD_SUCCESSFUL;
etisserant@0:   }else{
etisserant@0:       *pExpectedSize = szData;
etisserant@0:       accessDictionaryError(wIndex, bSubindex, szData, *pExpectedSize, OD_LENGTH_DATA_INVALID);
etisserant@0:       return OD_LENGTH_DATA_INVALID;
etisserant@0:   }
etisserant@0: }
etisserant@0: 
etisserant@0: 
etisserant@0: const indextable * scanIndexOD (CO_Data* d, UNS16 wIndex, UNS32 *errorCode, ODCallback_t **Callback)
etisserant@0: {
etisserant@0:   return (*d->scanIndexOD)(wIndex, errorCode, Callback);
etisserant@0: }
etisserant@0: 
etisserant@0: UNS32 RegisterSetODentryCallBack(CO_Data* d, UNS16 wIndex, UNS8 bSubindex, ODCallback_t Callback)
etisserant@0: {
etisserant@0: 	UNS32 errorCode;
etisserant@0: 	ODCallback_t *CallbackList;
etisserant@0: 
etisserant@0: 	scanIndexOD (d, wIndex, &errorCode, &CallbackList);
etisserant@0: 	if(errorCode == OD_SUCCESSFUL && CallbackList) 
etisserant@0: 		CallbackList[bSubindex] = Callback;
etisserant@0: 	return errorCode;
etisserant@0: }
etisserant@0: 
etisserant@1: