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
  27 #include "objacces.h"
  28
  29
  30 #ifdef DEBUG_WAR_CONSOLE_ON
  31 UNS8 accessDictionaryError(UNS16 index, UNS8 subIndex,
  32                              UNS8 sizeDataDict, UNS8 sizeDataGiven, UNS32 code)
  33 {
  34   MSG_WAR(0x2B09,"Dictionary index : ", index);
  35   MSG_WAR(0X2B10,"           subindex : ", subIndex);
  36   switch (code) {
  37     case  OD_NO_SUCH_OBJECT:
  38       MSG_WAR(0x2B11,"Index not found ", index);
  39       break;
  40     case OD_NO_SUCH_SUBINDEX :
  41       MSG_WAR(0x2B12,"SubIndex not found ", subIndex);
  42       break;
  43     case OD_WRITE_NOT_ALLOWED :
  44       MSG_WAR(0x2B13,"Write not allowed, data is read only ", index);
  45       break;
  46     case OD_LENGTH_DATA_INVALID :
  47       MSG_WAR(0x2B14,"Conflict size data. Should be (bytes)  : ", sizeDataDict);
  48       MSG_WAR(0x2B15,"But you have given the size  : ", sizeDataGiven);
  49       break;
  50     case OD_NOT_MAPPABLE :
  51       MSG_WAR(0x2B16,"Not mappable data in a PDO at index    : ", index);
  52       break;
  53     case OD_VALUE_TOO_LOW :
  54       MSG_WAR(0x2B17,"Value range error : value too low. SDOabort : ", code);
  55       break;
  56     case OD_VALUE_TOO_HIGH :
  57       MSG_WAR(0x2B18,"Value range error : value too high. SDOabort : ", code);
  58       break;
  59   default :
  60     MSG_WAR(0x2B20, "Unknown error code : ", code);
  61   }
  62   return 0;
  63 }
  64 #endif
  65
  66 UNS32 getODentry( CO_Data* d,
  67                   UNS16 wIndex,
  68                   UNS8 bSubindex,
  69                   void * pDestData,
  70                   UNS8 * pExpectedSize,
  71                   UNS8 * pDataType,
  72                   UNS8 checkAccess)
  73 { // DO NOT USE MSG_ERR because the macro may send a PDO -> infinite loop if it fails.
  74   UNS32 errorCode;
  75   UNS8 szData;
  76   const indextable *ptrTable;
  77   ODCallback_t *Callback;
  78
  79   ptrTable = (*d->scanIndexOD)(wIndex, &errorCode, &Callback);
  80
  81   if (errorCode != OD_SUCCESSFUL)
  82     return errorCode;
  83   if( ptrTable->bSubCount <= bSubindex ) {
  84     // Subindex not found
  85     accessDictionaryError(wIndex, bSubindex, 0, 0, OD_NO_SUCH_SUBINDEX);
  86     return OD_NO_SUCH_SUBINDEX;
  87   }
  88
  89   if (checkAccess && !(ptrTable->pSubindex[bSubindex].bAccessType & WO)) {
  90         MSG_WAR(0x2B30, "Access Type : ", ptrTable->pSubindex[bSubindex].bAccessType);
  91     accessDictionaryError(wIndex, bSubindex, 0, 0, OD_WRITE_NOT_ALLOWED);
  92     return OD_READ_NOT_ALLOWED;
  93   }
  94
  95   *pDataType = ptrTable->pSubindex[bSubindex].bDataType;
  96    szData = ptrTable->pSubindex[bSubindex].size;
  97
  98    if(  *pExpectedSize == 0 ||
  99         *pExpectedSize == szData ||
 100         (*pDataType == visible_string && *pExpectedSize < szData)) {// We allow to fetch a shorter string than expected
 101
 102 #  ifdef CANOPEN_BIG_ENDIAN
 103      if(*pDataType > boolean && *pDataType < visible_string) {
 104        // data must be transmited with low byte first
 105        UNS8 i, j = 0;
 106        MSG_WAR(boolean, "data type ", *pDataType);
 107        MSG_WAR(visible_string, "data type ", *pDataType);
 108        for ( i = szData ; i > 0 ; i--) {
 109          MSG_WAR(i," ", j);
 110          ((UNS8*)pDestData)[j++] =
 111            ((UNS8*)ptrTable->pSubindex[bSubindex].pObject)[i-1];
 112        }
 113      }
 114      else // It it is a visible string no endianisation to perform
 115        memcpy(pDestData, ptrTable->pSubindex[bSubindex].pObject,szData);
 116 #  else
 117      memcpy(pDestData, ptrTable->pSubindex[bSubindex].pObject,szData);
 118 #  endif
 119
 120      *pExpectedSize = szData;
 121 #if 0
 122      // Me laisser ça, please ! (FD)
 123      {
 124        UNS8 i;
 125        for (i = 0 ; i < 10 ; i++) {
 126          MSG_WAR(*pExpectedSize, "dic data= ",
 127                  *(UNS8 *)(ptrTable->pSubindex[bSubindex].pObject + i));
 128        }
 129
 130      }
 131 #endif
 132      return OD_SUCCESSFUL;
 133    }
 134    else { // Error !
 135      *pExpectedSize = szData;
 136      accessDictionaryError(wIndex, bSubindex, szData,
 137                            *pExpectedSize, OD_LENGTH_DATA_INVALID);
 138      return OD_LENGTH_DATA_INVALID;
 139    }
 140 }
 141
 142 UNS32 setODentry( CO_Data* d,
 143                   UNS16 wIndex,
 144                   UNS8 bSubindex,
 145                   void * pSourceData,
 146                   UNS8 * pExpectedSize,
 147                   UNS8 checkAccess)
 148 {
 149   UNS8 szData;
 150   UNS8 dataType;
 151   UNS32 errorCode;
 152   const indextable *ptrTable;
 153   ODCallback_t *Callback;
 154
 155   ptrTable =(*d->scanIndexOD)(wIndex, &errorCode, &Callback);
 156   if (errorCode != OD_SUCCESSFUL)
 157     return errorCode;
 158
 159   if( ptrTable->bSubCount <= bSubindex ) {
 160     // Subindex not found
 161     accessDictionaryError(wIndex, bSubindex, 0, *pExpectedSize, OD_NO_SUCH_SUBINDEX);
 162     return OD_NO_SUCH_SUBINDEX;
 163   }
 164   if (checkAccess && (ptrTable->pSubindex[bSubindex].bAccessType == RO)) {
 165         MSG_WAR(0x2B25, "Access Type : ", ptrTable->pSubindex[bSubindex].bAccessType);
 166     accessDictionaryError(wIndex, bSubindex, 0, *pExpectedSize, OD_WRITE_NOT_ALLOWED);
 167     return OD_WRITE_NOT_ALLOWED;
 168   }
 169
 170
 171    dataType = ptrTable->pSubindex[bSubindex].bDataType;
 172    szData = ptrTable->pSubindex[bSubindex].size;
 173
 174   if( *pExpectedSize == 0 ||
 175         *pExpectedSize == szData ||
 176         (dataType == visible_string && *pExpectedSize < szData)) // We allow to store a shorter string than entry size
 177   {
 178       #ifdef CANOPEN_BIG_ENDIAN
 179               if(dataType > boolean && dataType < visible_string)
 180               {
 181                         // we invert the data source directly. This let us do range testing without
 182                         // additional temp variable
 183                         UNS8 i;
 184                         for ( i = 0 ; i < ( ptrTable->pSubindex[bSubindex].size >> 1)  ; i++)
 185                         {
 186                         UNS8 tmp =((UNS8 *)pSourceData) [(ptrTable->pSubindex[bSubindex].size - 1) - i];
 187                         ((UNS8 *)pSourceData) [(ptrTable->pSubindex[bSubindex].size - 1) - i] = ((UNS8 *)pSourceData)[i];
 188                         ((UNS8 *)pSourceData)[i] = tmp;
 189                 }
 190                 }
 191       #endif
 192       errorCode = (*d->valueRangeTest)(dataType, pSourceData);
 193       if (errorCode) {
 194         accessDictionaryError(wIndex, bSubindex, szData, *pExpectedSize, errorCode);
 195         return errorCode;
 196       }
 197       memcpy(ptrTable->pSubindex[bSubindex].pObject,pSourceData, *pExpectedSize);
 198       *pExpectedSize = szData;
 199
 200       // Callbacks
 201       if(Callback && Callback[bSubindex]){
 202          (*Callback[bSubindex])(d, ptrTable, bSubindex);
 203       }
 204
 205       // TODO : Store dans NVRAM
 206       // if (ptrTable->pSubindex[bSubindex].bAccessType & TO_BE_SAVED)
 207       return OD_SUCCESSFUL;
 208   }else{
 209       *pExpectedSize = szData;
 210       accessDictionaryError(wIndex, bSubindex, szData, *pExpectedSize, OD_LENGTH_DATA_INVALID);
 211       return OD_LENGTH_DATA_INVALID;
 212   }
 213 }
 214
 215
 216 const indextable * scanIndexOD (CO_Data* d, UNS16 wIndex, UNS32 *errorCode, ODCallback_t **Callback)
 217 {
 218   return (*d->scanIndexOD)(wIndex, errorCode, Callback);
 219 }
 220
 221 UNS32 RegisterSetODentryCallBack(CO_Data* d, UNS16 wIndex, UNS8 bSubindex, ODCallback_t Callback)
 222 {
 223         UNS32 errorCode;
 224         ODCallback_t *CallbackList;
 225
 226         scanIndexOD (d, wIndex, &errorCode, &CallbackList);
 227         if(errorCode == OD_SUCCESSFUL && CallbackList)
 228                 CallbackList[bSubindex] = Callback;
 229         return errorCode;
 230 }
 231
 232