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                   UNS8 endianize)
  76 { /* DO NOT USE MSG_ERR because the macro may send a PDO -> infinite loop if it fails. */
  77   UNS32 errorCode;
  78   UNS8 szData;
  79   const indextable *ptrTable;
  80   ODCallback_t *Callback;
  81
  82   ptrTable = (*d->scanIndexOD)(wIndex, &errorCode, &Callback);
  83
  84   if (errorCode != OD_SUCCESSFUL)
  85     return errorCode;
  86   if( ptrTable->bSubCount <= bSubindex ) {
  87     /* Subindex not found */
  88     accessDictionaryError(wIndex, bSubindex, 0, 0, OD_NO_SUCH_SUBINDEX);
  89     return OD_NO_SUCH_SUBINDEX;
  90   }
  91
  92   if (checkAccess && !(ptrTable->pSubindex[bSubindex].bAccessType & WO)) {
  93         MSG_WAR(0x2B30, "Access Type : ", ptrTable->pSubindex[bSubindex].bAccessType);
  94     accessDictionaryError(wIndex, bSubindex, 0, 0, OD_WRITE_NOT_ALLOWED);
  95     return OD_READ_NOT_ALLOWED;
  96   }
  97
  98   *pDataType = ptrTable->pSubindex[bSubindex].bDataType;
  99    szData = ptrTable->pSubindex[bSubindex].size;
 100
 101    if(  *pExpectedSize == 0 ||
 102         *pExpectedSize == szData ||
 103         (*pDataType == visible_string && *pExpectedSize < szData)) {/* We allow to fetch a shorter string than expected */
 104
 105 #  ifdef CANOPEN_BIG_ENDIAN
 106      if(endianize && *pDataType > boolean && *pDataType < visible_string) {
 107        /* data must be transmited with low byte first */
 108        UNS8 i, j = 0;
 109        MSG_WAR(boolean, "data type ", *pDataType);
 110        MSG_WAR(visible_string, "data type ", *pDataType);
 111        for ( i = szData ; i > 0 ; i--) {
 112          MSG_WAR(i," ", j);
 113          ((UNS8*)pDestData)[j++] =
 114            ((UNS8*)ptrTable->pSubindex[bSubindex].pObject)[i-1];
 115        }
 116      }
 117      else /* It it is a visible string no endianisation to perform */
 118        memcpy(pDestData, ptrTable->pSubindex[bSubindex].pObject,szData);
 119 #  else
 120      memcpy(pDestData, ptrTable->pSubindex[bSubindex].pObject,szData);
 121 #  endif
 122
 123      *pExpectedSize = szData;
 124 #if 0
 125      /* Me laisser ça, please ! (FD) */
 126      {
 127        UNS8 i;
 128        for (i = 0 ; i < 10 ; i++) {
 129          MSG_WAR(*pExpectedSize, "dic data= ",
 130                  *(UNS8 *)(ptrTable->pSubindex[bSubindex].pObject + i));
 131        }
 132
 133      }
 134 #endif
 135      return OD_SUCCESSFUL;
 136    }
 137    else { /* Error ! */
 138      *pExpectedSize = szData;
 139      accessDictionaryError(wIndex, bSubindex, szData,
 140                            *pExpectedSize, OD_LENGTH_DATA_INVALID);
 141      return OD_LENGTH_DATA_INVALID;
 142    }
 143 }
 144
 145 UNS32 getODentry( CO_Data* d,
 146                   UNS16 wIndex,
 147                   UNS8 bSubindex,
 148                   void * pDestData,
 149                   UNS8 * pExpectedSize,
 150                   UNS8 * pDataType,
 151                   UNS8 checkAccess)
 152 {
 153         return _getODentry( d,
 154                   wIndex,
 155                   bSubindex,
 156                   pDestData,
 157                   pExpectedSize,
 158                   pDataType,
 159                   checkAccess,
 160                   1);//endianize
 161 }
 162
 163 UNS32 readLocalDict( CO_Data* d,
 164                   UNS16 wIndex,
 165                   UNS8 bSubindex,
 166                   void * pDestData,
 167                   UNS8 * pExpectedSize,
 168                   UNS8 * pDataType,
 169                   UNS8 checkAccess)
 170 {
 171                 return _getODentry( d,
 172                   wIndex,
 173                   bSubindex,
 174                   pDestData,
 175                   pExpectedSize,
 176                   pDataType,
 177                   checkAccess,
 178                   0);//do not endianize
 179 }
 180
 181 UNS32 _setODentry( CO_Data* d,
 182                   UNS16 wIndex,
 183                   UNS8 bSubindex,
 184                   void * pSourceData,
 185                   UNS8 * pExpectedSize,
 186                   UNS8 checkAccess,
 187                   UNS8 endianize)
 188 {
 189   UNS8 szData;
 190   UNS8 dataType;
 191   UNS32 errorCode;
 192   const indextable *ptrTable;
 193   ODCallback_t *Callback;
 194
 195   ptrTable =(*d->scanIndexOD)(wIndex, &errorCode, &Callback);
 196   if (errorCode != OD_SUCCESSFUL)
 197     return errorCode;
 198
 199   if( ptrTable->bSubCount <= bSubindex ) {
 200     /* Subindex not found */
 201     accessDictionaryError(wIndex, bSubindex, 0, *pExpectedSize, OD_NO_SUCH_SUBINDEX);
 202     return OD_NO_SUCH_SUBINDEX;
 203   }
 204   if (checkAccess && (ptrTable->pSubindex[bSubindex].bAccessType == RO)) {
 205         MSG_WAR(0x2B25, "Access Type : ", ptrTable->pSubindex[bSubindex].bAccessType);
 206     accessDictionaryError(wIndex, bSubindex, 0, *pExpectedSize, OD_WRITE_NOT_ALLOWED);
 207     return OD_WRITE_NOT_ALLOWED;
 208   }
 209
 210
 211    dataType = ptrTable->pSubindex[bSubindex].bDataType;
 212    szData = ptrTable->pSubindex[bSubindex].size;
 213
 214   if( *pExpectedSize == 0 ||
 215         *pExpectedSize == szData ||
 216         (dataType == visible_string && *pExpectedSize < szData)) /* We allow to store a shorter string than entry size */
 217   {
 218       #ifdef CANOPEN_BIG_ENDIAN
 219               if(endianize && dataType > boolean && dataType < visible_string)
 220               {
 221                         /* we invert the data source directly. This let us do range testing without */
 222                         /* additional temp variable */
 223                         UNS8 i;
 224                         for ( i = 0 ; i < ( ptrTable->pSubindex[bSubindex].size >> 1)  ; i++)
 225                         {
 226                         UNS8 tmp =((UNS8 *)pSourceData) [(ptrTable->pSubindex[bSubindex].size - 1) - i];
 227                         ((UNS8 *)pSourceData) [(ptrTable->pSubindex[bSubindex].size - 1) - i] = ((UNS8 *)pSourceData)[i];
 228                         ((UNS8 *)pSourceData)[i] = tmp;
 229                 }
 230                 }
 231       #endif
 232       errorCode = (*d->valueRangeTest)(dataType, pSourceData);
 233       if (errorCode) {
 234         accessDictionaryError(wIndex, bSubindex, szData, *pExpectedSize, errorCode);
 235         return errorCode;
 236       }
 237       memcpy(ptrTable->pSubindex[bSubindex].pObject,pSourceData, *pExpectedSize);
 238       *pExpectedSize = szData;
 239
 240       /* Callbacks */
 241       if(Callback && Callback[bSubindex]){
 242          (*Callback[bSubindex])(d, ptrTable, bSubindex);
 243       }
 244
 245       /* TODO : Store dans NVRAM */
 246       if (ptrTable->pSubindex[bSubindex].bAccessType & TO_BE_SAVE){
 247         (*d->storeODSubIndex)(wIndex, bSubindex);
 248       }
 249       return OD_SUCCESSFUL;
 250   }else{
 251       *pExpectedSize = szData;
 252       accessDictionaryError(wIndex, bSubindex, szData, *pExpectedSize, OD_LENGTH_DATA_INVALID);
 253       return OD_LENGTH_DATA_INVALID;
 254   }
 255 }
 256
 257 UNS32 setODentry( CO_Data* d,
 258                   UNS16 wIndex,
 259                   UNS8 bSubindex,
 260                   void * pSourceData,
 261                   UNS8 * pExpectedSize,
 262                   UNS8 checkAccess)
 263 {
 264         return _setODentry( d,
 265                   wIndex,
 266                   bSubindex,
 267                   pSourceData,
 268                   pExpectedSize,
 269                   checkAccess,
 270                   1);//endianize
 271 }
 272
 273 UNS32 writeLocalDict( CO_Data* d,
 274                   UNS16 wIndex,
 275                   UNS8 bSubindex,
 276                   void * pSourceData,
 277                   UNS8 * pExpectedSize,
 278                   UNS8 checkAccess)
 279 {
 280         return _setODentry( d,
 281                   wIndex,
 282                   bSubindex,
 283                   pSourceData,
 284                   pExpectedSize,
 285                   checkAccess,
 286                   0);//do not endianize
 287 }
 288
 289
 290
 291
 292 const indextable * scanIndexOD (CO_Data* d, UNS16 wIndex, UNS32 *errorCode, ODCallback_t **Callback)
 293 {
 294   return (*d->scanIndexOD)(wIndex, errorCode, Callback);
 295 }
 296
 297 UNS32 RegisterSetODentryCallBack(CO_Data* d, UNS16 wIndex, UNS8 bSubindex, ODCallback_t Callback)
 298 {
 299         UNS32 errorCode;
 300         ODCallback_t *CallbackList;
 301
 302         scanIndexOD (d, wIndex, &errorCode, &CallbackList);
 303         if(errorCode == OD_SUCCESSFUL && CallbackList)
 304                 CallbackList[bSubindex] = Callback;
 305         return errorCode;
 306 }
 307
 308 void _storeODSubIndex (UNS16 wIndex, UNS8 bSubindex){}