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