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@0: #include "objacces.h"
etisserant@0: 
dejoigny@8: 
nico@208: /*!
nico@208: **
nico@208: **
nico@208: ** @param index
nico@208: ** @param subIndex
nico@208: ** @param sizeDataDict
nico@208: ** @param sizeDataGiven
nico@208: ** @param code
nico@208: **
nico@208: ** @return
nico@208: **/
nico@208: UNS8 accessDictionaryError(UNS16 index, UNS8 subIndex,
nico@208:                            UNS8 sizeDataDict, UNS8 sizeDataGiven, UNS32 code)
etisserant@0: {
frdupin@75: #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:   }
frdupin@75: #endif
nico@208:   return 0;
nico@208: }
nico@208: 
nico@208: /*!
nico@208: **
nico@208: **
nico@208: ** @param d
nico@208: ** @param wIndex
nico@208: ** @param bSubindex
nico@208: ** @param pDestData
nico@208: ** @param pExpectedSize
nico@208: ** @param pDataType
nico@208: ** @param checkAccess
nico@208: ** @param endianize
nico@208: **
nico@208: ** @return
nico@208: **/
nico@208: UNS32 _getODentry( CO_Data* d,
nico@208:                    UNS16 wIndex,
nico@208:                    UNS8 bSubindex,
nico@208:                    void * pDestData,
nico@208:                    UNS8 * 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@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 ) {
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@0:   if (checkAccess && !(ptrTable->pSubindex[bSubindex].bAccessType & WO)) {
nico@208:     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;
nico@208:   szData = ptrTable->pSubindex[bSubindex].size;
nico@208: 
nico@208:   if(*pExpectedSize == 0 ||
nico@208:      *pExpectedSize == szData ||
nico@208:      (*pDataType == visible_string && *pExpectedSize < szData)) {
nico@215:     /* We
nico@208:       allow to fetch a shorter string than expected */
nico@208: 
dejoigny@12: #  ifdef CANOPEN_BIG_ENDIAN
nico@208:     if(endianize && *pDataType > boolean && *pDataType < visible_string) {
nico@215:       /* data must be transmited with low byte first */
nico@208:       UNS8 i, j = 0;
nico@208:       MSG_WAR(boolean, "data type ", *pDataType);
nico@208:       MSG_WAR(visible_string, "data type ", *pDataType);
nico@208:       for ( i = szData ; i > 0 ; i--) {
nico@208:         MSG_WAR(i," ", j);
nico@208:         ((UNS8*)pDestData)[j++] =
nico@208:           ((UNS8*)ptrTable->pSubindex[bSubindex].pObject)[i-1];
nico@208:       }
nico@208:     }
nico@215:     else /* It it is a visible string no endianisation to perform */
nico@208:       memcpy(pDestData, ptrTable->pSubindex[bSubindex].pObject,szData);
dejoigny@12: #  else
nico@208:     memcpy(pDestData, ptrTable->pSubindex[bSubindex].pObject,szData);
dejoigny@12: #  endif
nico@208: 
nico@208:     *pExpectedSize = szData;
dejoigny@12: #if 0
nico@215:     /* Me laisser a, please ! (FD) */
nico@208:     {
nico@208:       UNS8 i;
nico@208:       for (i = 0 ; i < 10 ; i++) {
nico@208:         MSG_WAR(*pExpectedSize, "dic data= ",
nico@208:                 *(UNS8 *)(ptrTable->pSubindex[bSubindex].pObject + i));
nico@208:       }
nico@208: 
nico@208:     }
dejoigny@12: #endif
nico@208:     return OD_SUCCESSFUL;
nico@208:   }
nico@215:   else { /* Error ! */
nico@208:     *pExpectedSize = szData;
nico@208:     accessDictionaryError(wIndex, bSubindex, szData,
nico@208:                           *pExpectedSize, OD_LENGTH_DATA_INVALID);
nico@208:     return OD_LENGTH_DATA_INVALID;
nico@208:   }
nico@208: }
nico@208: 
nico@208: /*!
nico@208: **
nico@208: **
nico@208: ** @param d
nico@208: ** @param wIndex
nico@208: ** @param bSubindex
nico@208: ** @param pDestData
nico@208: ** @param pExpectedSize
nico@208: ** @param pDataType
nico@208: ** @param checkAccess
nico@208: **
nico@208: ** @return
nico@208: **/
nico@208: UNS32 getODentry( CO_Data* d,
etisserant@175:                   UNS16 wIndex,
nico@208:                   UNS8 bSubindex,
nico@208:                   void * pDestData,
nico@208:                   UNS8 * pExpectedSize,
nico@208:                   UNS8 * pDataType,
nico@208:                   UNS8 checkAccess)
nico@208: {
nico@208:   return _getODentry( d,
nico@208:                       wIndex,
nico@208:                       bSubindex,
nico@208:                       pDestData,
nico@208:                       pExpectedSize,
nico@208:                       pDataType,
nico@208:                       checkAccess,
nico@208:                       1);//endianize
nico@208: }
nico@208: 
nico@208: /*!
nico@208: **
nico@208: **
nico@208: ** @param d
nico@208: ** @param wIndex
nico@208: ** @param bSubindex
nico@208: ** @param pDestData
nico@208: ** @param pExpectedSize
nico@208: ** @param pDataType
nico@208: ** @param checkAccess
nico@208: **
nico@208: ** @return
nico@208: **/
nico@208: UNS32 readLocalDict( CO_Data* d,
nico@208:                      UNS16 wIndex,
nico@208:                      UNS8 bSubindex,
nico@208:                      void * pDestData,
nico@208:                      UNS8 * pExpectedSize,
nico@208:                      UNS8 * pDataType,
nico@208:                      UNS8 checkAccess)
nico@208: {
nico@208:   return _getODentry( d,
nico@208:                       wIndex,
nico@208:                       bSubindex,
nico@208:                       pDestData,
nico@208:                       pExpectedSize,
nico@208:                       pDataType,
nico@208:                       checkAccess,
nico@208:                       0);//do not endianize
nico@208: }
nico@208: 
nico@208: /*!
nico@208: **
nico@208: **
nico@208: ** @param d
nico@208: ** @param wIndex
nico@208: ** @param bSubindex
nico@208: ** @param pSourceData
nico@208: ** @param pExpectedSize
nico@208: ** @param checkAccess
nico@208: ** @param endianize
nico@208: **
nico@208: ** @return
nico@208: **/
nico@208: UNS32 _setODentry( CO_Data* d,
nico@208:                    UNS16 wIndex,
nico@208:                    UNS8 bSubindex,
nico@208:                    void * pSourceData,
nico@208:                    UNS8 * pExpectedSize,
nico@208:                    UNS8 checkAccess,
nico@208:                    UNS8 endianize)
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 ) {
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 ||
nico@208:       (dataType == visible_string && *pExpectedSize < szData)) /* We
nico@208:                                                                   allow to store a shorter string than entry size */
nico@208:     {
nico@208: #ifdef CANOPEN_BIG_ENDIAN
nico@208:       if(endianize && dataType > boolean && dataType < visible_string)
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@0:       *pExpectedSize = szData;
nico@208: 
nico@215:       /* Callbacks */
etisserant@0:       if(Callback && Callback[bSubindex]){
nico@208:         (*Callback[bSubindex])(d, ptrTable, bSubindex);
etisserant@0:       }
nico@208: 
nico@215:       /* TODO : Store dans NVRAM */
etisserant@161:       if (ptrTable->pSubindex[bSubindex].bAccessType & TO_BE_SAVE){
nico@208:         (*d->storeODSubIndex)(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: 
nico@208: /*!
nico@208: **
nico@208: **
nico@208: ** @param d
nico@208: ** @param wIndex
nico@208: ** @param bSubindex
nico@208: ** @param pSourceData
nico@208: ** @param pExpectedSize
nico@208: ** @param checkAccess
nico@208: **
nico@208: ** @return
nico@208: **/
nico@208: UNS32 setODentry( CO_Data* d,
etisserant@175:                   UNS16 wIndex,
nico@208:                   UNS8 bSubindex,
nico@208:                   void * pSourceData,
nico@208:                   UNS8 * pExpectedSize,
nico@208:                   UNS8 checkAccess)
nico@208: {
nico@208:   return _setODentry( d,
nico@208:                       wIndex,
nico@208:                       bSubindex,
nico@208:                       pSourceData,
nico@208:                       pExpectedSize,
nico@208:                       checkAccess,
nico@208:                       1);//endianize
nico@208: }
nico@208: 
nico@208: /*!
nico@208: **
nico@208: **
nico@208: ** @param d
nico@208: ** @param wIndex
nico@208: ** @param bSubindex
nico@208: ** @param pSourceData
nico@208: ** @param pExpectedSize
nico@208: ** @param checkAccess
nico@208: **
nico@208: ** @return
nico@208: **/
nico@208: UNS32 writeLocalDict( CO_Data* d,
nico@208:                       UNS16 wIndex,
nico@208:                       UNS8 bSubindex,
nico@208:                       void * pSourceData,
nico@208:                       UNS8 * pExpectedSize,
nico@208:                       UNS8 checkAccess)
nico@208: {
nico@208:   return _setODentry( d,
nico@208:                       wIndex,
nico@208:                       bSubindex,
nico@208:                       pSourceData,
nico@208:                       pExpectedSize,
nico@208:                       checkAccess,
nico@208:                       0);//do not endianize
nico@208: }
nico@208: 
nico@208: /*!
nico@208: **
nico@208: **
nico@208: ** @param d
nico@208: ** @param wIndex
nico@208: ** @param errorCode
nico@208: ** @param Callback
nico@208: **
nico@208: ** @return
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: 
nico@208: /*!
nico@208: **
nico@208: **
nico@208: ** @param d
nico@208: ** @param wIndex
nico@208: ** @param bSubindex
nico@208: ** @param Callback
nico@208: **
nico@208: ** @return
nico@208: **/
etisserant@0: UNS32 RegisterSetODentryCallBack(CO_Data* d, UNS16 wIndex, UNS8 bSubindex, ODCallback_t Callback)
etisserant@0: {
nico@208:   UNS32 errorCode;
nico@208:   ODCallback_t *CallbackList;
nico@208: 
nico@208:   scanIndexOD (d, wIndex, &errorCode, &CallbackList);
nico@208:   if(errorCode == OD_SUCCESSFUL && CallbackList)
nico@208:     CallbackList[bSubindex] = Callback;
nico@208:   return errorCode;
nico@208: }
nico@208: 
nico@208: /*!
nico@208: **
nico@208: **
nico@208: ** @param wIndex
nico@208: ** @param bSubindex
nico@208: **/
etisserant@161: void _storeODSubIndex (UNS16 wIndex, UNS8 bSubindex){}