/home/epimerde/documents/tc11/CanFestival-3/src/objacces.c

Go to the documentation of this file.

/*
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){}

---