/*
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
*/
/* #define DEBUG_WAR_CONSOLE_ON */
/* #define DEBUG_ERR_CONSOLE_ON */
#include "objacces.h"
UNS8 accessDictionaryError(UNS16 index, UNS8 subIndex,
UNS8 sizeDataDict, UNS8 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,
UNS8 * 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;
UNS8 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_WRITE_NOT_ALLOWED);
return OD_READ_NOT_ALLOWED;
}
*pDataType = ptrTable->pSubindex[bSubindex].bDataType;
szData = ptrTable->pSubindex[bSubindex].size;
if( *pExpectedSize == 0 ||
*pExpectedSize == szData ||
(*pDataType == visible_string && *pExpectedSize < szData)) {/* We allow to fetch a shorter string than expected */
# ifdef CANOPEN_BIG_ENDIAN
if(endianize && *pDataType > boolean && *pDataType < visible_string) {
/* 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];
}
}
else /* It it is a visible string no endianisation to perform */
memcpy(pDestData, ptrTable->pSubindex[bSubindex].pObject,szData);
# else
memcpy(pDestData, ptrTable->pSubindex[bSubindex].pObject,szData);
# endif
*pExpectedSize = szData;
#if 0
/* Me laisser ça, please ! (FD) */
{
UNS8 i;
for (i = 0 ; i < 10 ; i++) {
MSG_WAR(*pExpectedSize, "dic data= ",
*(UNS8 *)(ptrTable->pSubindex[bSubindex].pObject + i));
}
}
#endif
return OD_SUCCESSFUL;
}
else { /* Error ! */
*pExpectedSize = szData;
accessDictionaryError(wIndex, bSubindex, szData,
*pExpectedSize, OD_LENGTH_DATA_INVALID);
return OD_LENGTH_DATA_INVALID;
}
}
UNS32 getODentry( CO_Data* d,
UNS16 wIndex,
UNS8 bSubindex,
void * pDestData,
UNS8 * pExpectedSize,
UNS8 * pDataType,
UNS8 checkAccess)
{
return _getODentry( d,
wIndex,
bSubindex,
pDestData,
pExpectedSize,
pDataType,
checkAccess,
1);//endianize
}
UNS32 readLocalDict( CO_Data* d,
UNS16 wIndex,
UNS8 bSubindex,
void * pDestData,
UNS8 * pExpectedSize,
UNS8 * pDataType,
UNS8 checkAccess)
{
return _getODentry( d,
wIndex,
bSubindex,
pDestData,
pExpectedSize,
pDataType,
checkAccess,
0);//do not endianize
}
UNS32 _setODentry( CO_Data* d,
UNS16 wIndex,
UNS8 bSubindex,
void * pSourceData,
UNS8 * pExpectedSize,
UNS8 checkAccess,
UNS8 endianize)
{
UNS8 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 ||
(dataType == visible_string && *pExpectedSize < szData)) /* We allow to store a shorter string than entry size */
{
#ifdef CANOPEN_BIG_ENDIAN
if(endianize && dataType > boolean && dataType < visible_string)
{
/* 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);
*pExpectedSize = szData;
/* Callbacks */
if(Callback && Callback[bSubindex]){
(*Callback[bSubindex])(d, ptrTable, bSubindex);
}
/* TODO : Store dans NVRAM */
if (ptrTable->pSubindex[bSubindex].bAccessType & TO_BE_SAVE){
(*d->storeODSubIndex)(wIndex, bSubindex);
}
return OD_SUCCESSFUL;
}else{
*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,
UNS8 * pExpectedSize,
UNS8 checkAccess)
{
return _setODentry( d,
wIndex,
bSubindex,
pSourceData,
pExpectedSize,
checkAccess,
1);//endianize
}
UNS32 writeLocalDict( CO_Data* d,
UNS16 wIndex,
UNS8 bSubindex,
void * pSourceData,
UNS8 * pExpectedSize,
UNS8 checkAccess)
{
return _setODentry( d,
wIndex,
bSubindex,
pSourceData,
pExpectedSize,
checkAccess,
0);//do not endianize
}
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;
scanIndexOD (d, wIndex, &errorCode, &CallbackList);
if(errorCode == OD_SUCCESSFUL && CallbackList)
CallbackList[bSubindex] = Callback;
return errorCode;
}
void _storeODSubIndex (UNS16 wIndex, UNS8 bSubindex){}