src/objacces.c

   1 /*
   2 This file is part of CanFestival, a library implementing CanOpen Stack.
   3
   4 Copyright (C): Edouard TISSERANT and Francis DUPIN
   5
   6 See COPYING file for copyrights details.
   7
   8 This library is free software; you can redistribute it and/or
   9 modify it under the terms of the GNU Lesser General Public
  10 License as published by the Free Software Foundation; either
  11 version 2.1 of the License, or (at your option) any later version.
  12
  13 This library is distributed in the hope that it will be useful,
  14 but WITHOUT ANY WARRANTY; without even the implied warranty of
  15 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  16 Lesser General Public License for more details.
  17
  18 You should have received a copy of the GNU Lesser General Public
  19 License along with this library; if not, write to the Free Software
  20 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  21 */
  22
  23 //#define DEBUG_WAR_CONSOLE_ON
  24 //#define DEBUG_ERR_CONSOLE_ON
  25
  26 #include "objacces.h"
  27
  28 #ifdef DEBUG_WAR_CONSOLE_ON
  29 UNS8 accessDictionaryError(UNS16 index, UNS8 subIndex,
  30                              UNS8 sizeDataDict, UNS8 sizeDataGiven, UNS32 code)
  31 {
  32   MSG_WAR(0x2B09,"Dictionary index : ", index);
  33   MSG_WAR(0X2B10,"           subindex : ", subIndex);
  34   switch (code) {
  35     case  OD_NO_SUCH_OBJECT:
  36       MSG_WAR(0x2B11,"Index not found ", index);
  37       break;
  38     case OD_NO_SUCH_SUBINDEX :
  39       MSG_WAR(0x2B12,"SubIndex not found ", subIndex);
  40       break;
  41     case OD_WRITE_NOT_ALLOWED :
  42       MSG_WAR(0x2B13,"Write not allowed, data is read only ", index);
  43       break;
  44     case OD_LENGTH_DATA_INVALID :
  45       MSG_WAR(0x2B14,"Conflict size data. Should be (bytes)  : ", sizeDataDict);
  46       MSG_WAR(0x2B15,"But you have given the size  : ", sizeDataGiven);
  47       break;
  48     case OD_NOT_MAPPABLE :
  49       MSG_WAR(0x2B16,"Not mappable data in a PDO at index    : ", index);
  50       break;
  51     case OD_VALUE_TOO_LOW :
  52       MSG_WAR(0x2B17,"Value range error : value too low. SDOabort : ", code);
  53       break;
  54     case OD_VALUE_TOO_HIGH :
  55       MSG_WAR(0x2B18,"Value range error : value too high. SDOabort : ", code);
  56       break;
  57   default :
  58     MSG_WAR(0x2B20, "Unknown error code : ", code);
  59   }
  60   return 0;
  61 }
  62 #endif
  63
  64 UNS32 getODentry( CO_Data* d,
  65                   UNS16 wIndex,
  66                   UNS8 bSubindex,
  67                   void * pDestData,
  68                   UNS8 * pExpectedSize,
  69                   UNS8 * pDataType,
  70                   UNS8 checkAccess)
  71 { // DO NOT USE MSG_ERR because the macro may send a PDO -> infinite loop if it fails.
  72   UNS32 errorCode;
  73   UNS8 szData;
  74   const indextable *ptrTable;
  75   ODCallback_t *Callback;
  76
  77   ptrTable = (*d->scanIndexOD)(wIndex, &errorCode, &Callback);
  78
  79   if (errorCode != OD_SUCCESSFUL)
  80     return errorCode;
  81   if( ptrTable->bSubCount <= bSubindex ) {
  82     // Subindex not found
  83     accessDictionaryError(wIndex, bSubindex, 0, 0, OD_NO_SUCH_SUBINDEX);
  84     return OD_NO_SUCH_SUBINDEX;
  85   }
  86
  87   if (checkAccess && !(ptrTable->pSubindex[bSubindex].bAccessType & WO)) {
  88         MSG_WAR(0x2B30, "Access Type : ", ptrTable->pSubindex[bSubindex].bAccessType);
  89     accessDictionaryError(wIndex, bSubindex, 0, 0, OD_WRITE_NOT_ALLOWED);
  90     return OD_READ_NOT_ALLOWED;
  91   }
  92
  93   *pDataType = ptrTable->pSubindex[bSubindex].bDataType;
  94    szData = ptrTable->pSubindex[bSubindex].size;
  95
  96   if(   *pExpectedSize == 0 ||
  97         *pExpectedSize == szData ||
  98         (*pDataType == visible_string && *pExpectedSize > szData)) // We allow to fetch a shorter string than expected
  99   {
 100         #ifdef CANOPEN_BIG_ENDIAN
 101               {
 102                 // data must be transmited with low byte first
 103                 UNS8 i, j = 0;
 104                 for ( i = ptrTable->pSubindex[bSubindex].size ; i > 0 ; i--) {
 105                         ((char*)pDestData)[j++] = ((char*)ptrTable->pSubindex[bSubindex].pObject)[i-1];
 106                 }
 107               }
 108         #else
 109               memcpy(pDestData, ptrTable->pSubindex[bSubindex].pObject,*pExpectedSize);
 110         #endif
 111       *pExpectedSize = szData;
 112       return OD_SUCCESSFUL;
 113   }else{
 114       *pExpectedSize = szData;
 115       accessDictionaryError(wIndex, bSubindex, szData, *pExpectedSize, OD_LENGTH_DATA_INVALID);
 116       return OD_LENGTH_DATA_INVALID;
 117   }
 118 }
 119
 120 UNS32 setODentry( CO_Data* d,
 121                   UNS16 wIndex,
 122                   UNS8 bSubindex,
 123                   void * pSourceData,
 124                   UNS8 * pExpectedSize,
 125                   UNS8 checkAccess)
 126 {
 127   UNS8 szData;
 128   UNS8 dataType;
 129   UNS32 errorCode;
 130   const indextable *ptrTable;
 131   ODCallback_t *Callback;
 132
 133   ptrTable =(*d->scanIndexOD)(wIndex, &errorCode, &Callback);
 134   if (errorCode != OD_SUCCESSFUL)
 135     return errorCode;
 136
 137   if( ptrTable->bSubCount <= bSubindex ) {
 138     // Subindex not found
 139     accessDictionaryError(wIndex, bSubindex, 0, *pExpectedSize, OD_NO_SUCH_SUBINDEX);
 140     return OD_NO_SUCH_SUBINDEX;
 141   }
 142   if (checkAccess && (ptrTable->pSubindex[bSubindex].bAccessType == RO)) {
 143         MSG_WAR(0x2B25, "Access Type : ", ptrTable->pSubindex[bSubindex].bAccessType);
 144     accessDictionaryError(wIndex, bSubindex, 0, *pExpectedSize, OD_WRITE_NOT_ALLOWED);
 145     return OD_WRITE_NOT_ALLOWED;
 146   }
 147
 148
 149    dataType = ptrTable->pSubindex[bSubindex].bDataType;
 150    szData = ptrTable->pSubindex[bSubindex].size;
 151
 152   if( *pExpectedSize == 0 ||
 153         *pExpectedSize == szData ||
 154         (dataType == visible_string && *pExpectedSize < szData)) // We allow to store a shorter string than entry size
 155   {
 156       errorCode = (*d->valueRangeTest)(dataType, pSourceData);
 157       if (errorCode) {
 158         accessDictionaryError(wIndex, bSubindex, szData, *pExpectedSize, errorCode);
 159         return errorCode;
 160       }
 161       #ifdef CANOPEN_BIG_ENDIAN
 162               {
 163                 // data must be transmited with low byte first
 164                 UNS8 i, j = 0;
 165                 for ( i = ptrTable->pSubindex[bSubindex].size ; i > 0 ; i--) {
 166                         ((char*)ptrTable->pSubindex[bSubindex].pObject)[i - 1] = ((char*)pSourceData)[j++];
 167                 }
 168               }
 169       #else
 170               memcpy(ptrTable->pSubindex[bSubindex].pObject,pSourceData, *pExpectedSize);
 171       #endif
 172       *pExpectedSize = szData;
 173
 174       // Callbacks
 175       if(Callback && Callback[bSubindex]){
 176          (*Callback[bSubindex])(d, ptrTable, bSubindex);
 177       }
 178
 179       // TODO : Store dans NVRAM
 180       // if (ptrTable->pSubindex[bSubindex].bAccessType & TO_BE_SAVED)
 181       return OD_SUCCESSFUL;
 182   }else{
 183       *pExpectedSize = szData;
 184       accessDictionaryError(wIndex, bSubindex, szData, *pExpectedSize, OD_LENGTH_DATA_INVALID);
 185       return OD_LENGTH_DATA_INVALID;
 186   }
 187 }
 188
 189
 190 const indextable * scanIndexOD (CO_Data* d, UNS16 wIndex, UNS32 *errorCode, ODCallback_t **Callback)
 191 {
 192   return (*d->scanIndexOD)(wIndex, errorCode, Callback);
 193 }
 194
 195 UNS32 RegisterSetODentryCallBack(CO_Data* d, UNS16 wIndex, UNS8 bSubindex, ODCallback_t Callback)
 196 {
 197         UNS32 errorCode;
 198         ODCallback_t *CallbackList;
 199
 200         scanIndexOD (d, wIndex, &errorCode, &CallbackList);
 201         if(errorCode == OD_SUCCESSFUL && CallbackList)
 202                 CallbackList[bSubindex] = Callback;
 203         return errorCode;
 204 }
 205
 206