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