![](/media/cache/profiles/IMG_2753.JPG.50x50_q85.jpg)
Can_open_slavenode
Diff: framework/src/objacces.c
- Revision:
- 0:6219434a0cb5
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/framework/src/objacces.c Mon May 30 07:14:41 2011 +0000 @@ -0,0 +1,281 @@ +/* + This file is part of CanFestival, a library implementing CanOpen Stack. + + Copyright (C): Edouard TISSERANT and Francis DUPIN + + See COPYING file for copyrights details. + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with this library; if not, write to the Free Software + Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 + USA +*/ +/*! +** @file objacces.c +** @author Edouard TISSERANT and Francis DUPIN +** @date Tue Jun 5 08:55:23 2007 +** +** @brief +** +** +*/ + + + + +/* #define DEBUG_WAR_CONSOLE_ON */ +/* #define DEBUG_ERR_CONSOLE_ON */ + + +#include "data.h" + +//We need the function implementation for linking +//Only a placeholder with a define isnt enough! +UNS8 accessDictionaryError(UNS16 index, UNS8 subIndex, + UNS32 sizeDataDict, UNS32 sizeDataGiven, UNS32 code) +{ +#ifdef DEBUG_WAR_CONSOLE_ON + MSG_WAR(0x2B09,"Dictionary index : ", index); + MSG_WAR(0X2B10," subindex : ", subIndex); + switch (code) { + case OD_NO_SUCH_OBJECT: + MSG_WAR(0x2B11,"Index not found ", index); + break; + case OD_NO_SUCH_SUBINDEX : + MSG_WAR(0x2B12,"SubIndex not found ", subIndex); + break; + case OD_WRITE_NOT_ALLOWED : + MSG_WAR(0x2B13,"Write not allowed, data is read only ", index); + break; + case OD_LENGTH_DATA_INVALID : + MSG_WAR(0x2B14,"Conflict size data. Should be (bytes) : ", sizeDataDict); + MSG_WAR(0x2B15,"But you have given the size : ", sizeDataGiven); + break; + case OD_NOT_MAPPABLE : + MSG_WAR(0x2B16,"Not mappable data in a PDO at index : ", index); + break; + case OD_VALUE_TOO_LOW : + MSG_WAR(0x2B17,"Value range error : value too low. SDOabort : ", code); + break; + case OD_VALUE_TOO_HIGH : + MSG_WAR(0x2B18,"Value range error : value too high. SDOabort : ", code); + break; + default : + MSG_WAR(0x2B20, "Unknown error code : ", code); + } + #endif + + return 0; +} + +UNS32 _getODentry( CO_Data* d, + UNS16 wIndex, + UNS8 bSubindex, + void * pDestData, + UNS32 * pExpectedSize, + UNS8 * pDataType, + UNS8 checkAccess, + UNS8 endianize) +{ /* DO NOT USE MSG_ERR because the macro may send a PDO -> infinite + loop if it fails. */ + UNS32 errorCode; + UNS32 szData; + const indextable *ptrTable; + ODCallback_t *Callback; + + ptrTable = (*d->scanIndexOD)(wIndex, &errorCode, &Callback); + + if (errorCode != OD_SUCCESSFUL) + return errorCode; + if( ptrTable->bSubCount <= bSubindex ) { + /* Subindex not found */ + accessDictionaryError(wIndex, bSubindex, 0, 0, OD_NO_SUCH_SUBINDEX); + return OD_NO_SUCH_SUBINDEX; + } + + if (checkAccess && (ptrTable->pSubindex[bSubindex].bAccessType & WO)) { + MSG_WAR(0x2B30, "Access Type : ", ptrTable->pSubindex[bSubindex].bAccessType); + accessDictionaryError(wIndex, bSubindex, 0, 0, OD_READ_NOT_ALLOWED); + return OD_READ_NOT_ALLOWED; + } + + *pDataType = ptrTable->pSubindex[bSubindex].bDataType; + szData = ptrTable->pSubindex[bSubindex].size; + + if(*pExpectedSize == 0 || + *pExpectedSize == szData || + /* allow to fetch a shorter string than expected */ + (*pDataType >= visible_string && *pExpectedSize < szData)) { + +# ifdef CANOPEN_BIG_ENDIAN + if(endianize && *pDataType > boolean && !( + *pDataType >= visible_string && + *pDataType <= domain)) { + /* data must be transmited with low byte first */ + UNS8 i, j = 0; + MSG_WAR(boolean, "data type ", *pDataType); + MSG_WAR(visible_string, "data type ", *pDataType); + for ( i = szData ; i > 0 ; i--) { + MSG_WAR(i," ", j); + ((UNS8*)pDestData)[j++] = + ((UNS8*)ptrTable->pSubindex[bSubindex].pObject)[i-1]; + } + *pExpectedSize = szData; + } + else /* no endianisation change */ +# endif + if(*pDataType != visible_string) { + memcpy(pDestData, ptrTable->pSubindex[bSubindex].pObject,szData); + *pExpectedSize = szData; + }else{ + /* TODO : CONFORM TO DS-301 : + * - stop using NULL terminated strings + * - store string size in td_subindex + * */ + /* Copy null terminated string to user, and return discovered size */ + UNS8 *ptr = (UNS8*)ptrTable->pSubindex[bSubindex].pObject; + UNS8 *ptr_start = ptr; + /* *pExpectedSize IS < szData . if null, use szData */ + UNS8 *ptr_end = ptr + (*pExpectedSize ? *pExpectedSize : szData) ; + UNS8 *ptr_dest = (UNS8*)pDestData; + while( *ptr && ptr < ptr_end){ + *(ptr_dest++) = *(ptr++); + } + + *pExpectedSize = (UNS32) (ptr - ptr_start); + /* terminate string if not maximum length */ + if (*pExpectedSize < szData) + *(ptr) = 0; + } + + return OD_SUCCESSFUL; + } + else { /* Error ! */ + *pExpectedSize = szData; + accessDictionaryError(wIndex, bSubindex, szData, + *pExpectedSize, OD_LENGTH_DATA_INVALID); + return OD_LENGTH_DATA_INVALID; + } +} + +UNS32 _setODentry( CO_Data* d, + UNS16 wIndex, + UNS8 bSubindex, + void * pSourceData, + UNS32 * pExpectedSize, + UNS8 checkAccess, + UNS8 endianize) +{ + UNS32 szData; + UNS8 dataType; + UNS32 errorCode; + const indextable *ptrTable; + ODCallback_t *Callback; + + ptrTable =(*d->scanIndexOD)(wIndex, &errorCode, &Callback); + if (errorCode != OD_SUCCESSFUL) + return errorCode; + + if( ptrTable->bSubCount <= bSubindex ) { + /* Subindex not found */ + accessDictionaryError(wIndex, bSubindex, 0, *pExpectedSize, OD_NO_SUCH_SUBINDEX); + return OD_NO_SUCH_SUBINDEX; + } + if (checkAccess && (ptrTable->pSubindex[bSubindex].bAccessType == RO)) { + MSG_WAR(0x2B25, "Access Type : ", ptrTable->pSubindex[bSubindex].bAccessType); + accessDictionaryError(wIndex, bSubindex, 0, *pExpectedSize, OD_WRITE_NOT_ALLOWED); + return OD_WRITE_NOT_ALLOWED; + } + + + dataType = ptrTable->pSubindex[bSubindex].bDataType; + szData = ptrTable->pSubindex[bSubindex].size; + + if( *pExpectedSize == 0 || + *pExpectedSize == szData || + /* allow to store a shorter string than entry size */ + (dataType == visible_string && *pExpectedSize < szData)) + { +#ifdef CANOPEN_BIG_ENDIAN + /* re-endianize do not occur for bool, strings time and domains */ + if(endianize && dataType > boolean && !( + dataType >= visible_string && + dataType <= domain)) + { + /* we invert the data source directly. This let us do range + testing without */ + /* additional temp variable */ + UNS8 i; + for ( i = 0 ; i < ( ptrTable->pSubindex[bSubindex].size >> 1) ; i++) + { + UNS8 tmp =((UNS8 *)pSourceData) [(ptrTable->pSubindex[bSubindex].size - 1) - i]; + ((UNS8 *)pSourceData) [(ptrTable->pSubindex[bSubindex].size - 1) - i] = ((UNS8 *)pSourceData)[i]; + ((UNS8 *)pSourceData)[i] = tmp; + } + } +#endif + errorCode = (*d->valueRangeTest)(dataType, pSourceData); + if (errorCode) { + accessDictionaryError(wIndex, bSubindex, szData, *pExpectedSize, errorCode); + return errorCode; + } + memcpy(ptrTable->pSubindex[bSubindex].pObject,pSourceData, *pExpectedSize); + /* TODO : CONFORM TO DS-301 : + * - stop using NULL terminated strings + * - store string size in td_subindex + * */ + /* terminate visible_string with '\0' */ + if(dataType == visible_string && *pExpectedSize < szData) + ((UNS8*)ptrTable->pSubindex[bSubindex].pObject)[*pExpectedSize] = 0; + + *pExpectedSize = szData; + + /* Callbacks */ + if(Callback && Callback[bSubindex]){ + errorCode = (Callback[bSubindex])(d, ptrTable, bSubindex); + if(errorCode != OD_SUCCESSFUL) + { + return errorCode; + } + } + + /* TODO : Store dans NVRAM */ + if (ptrTable->pSubindex[bSubindex].bAccessType & TO_BE_SAVE){ + (*d->storeODSubIndex)(d, wIndex, bSubindex); + } + return OD_SUCCESSFUL; + }else{ + *pExpectedSize = szData; + accessDictionaryError(wIndex, bSubindex, szData, *pExpectedSize, OD_LENGTH_DATA_INVALID); + return OD_LENGTH_DATA_INVALID; + } +} + +const indextable * scanIndexOD (CO_Data* d, UNS16 wIndex, UNS32 *errorCode, ODCallback_t **Callback) +{ + return (*d->scanIndexOD)(wIndex, errorCode, Callback); +} + +UNS32 RegisterSetODentryCallBack(CO_Data* d, UNS16 wIndex, UNS8 bSubindex, ODCallback_t Callback) +{ +UNS32 errorCode; +ODCallback_t *CallbackList; +const indextable *odentry; + + odentry = scanIndexOD (d, wIndex, &errorCode, &CallbackList); + if(errorCode == OD_SUCCESSFUL && CallbackList && bSubindex < odentry->bSubCount) + CallbackList[bSubindex] = Callback; + return errorCode; +} + +void _storeODSubIndex (CO_Data* d, UNS16 wIndex, UNS8 bSubindex){}