src/objacces.c

   1 /*
   2   This file is part of CanFestival, a library implementing CanOpen
   3   Stack.
   4
   5   Copyright (C): Edouard TISSERANT and Francis DUPIN
   6
   7   See COPYING file for copyrights details.
   8
   9   This library is free software; you can redistribute it and/or
  10   modify it under the terms of the GNU Lesser General Public
  11   License as published by the Free Software Foundation; either
  12   version 2.1 of the License, or (at your option) any later version.
  13
  14   This library is distributed in the hope that it will be useful,
  15   but WITHOUT ANY WARRANTY; without even the implied warranty of
  16   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  17   Lesser General Public License for more details.
  18
  19   You should have received a copy of the GNU Lesser General Public
  20   License along with this library; if not, write to the Free Software
  21   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307
  22   USA
  23 */
  24 /*!
  25 ** @file   objacces.c
  26 ** @author Edouard TISSERANT and Francis DUPIN
  27 ** @date   Tue Jun  5 08:55:23 2007
  28 **
  29 ** @brief
  30 **
  31 **
  32 */
  33
  34
  35
  36
  37 /* #define DEBUG_WAR_CONSOLE_ON */
  38 /* #define DEBUG_ERR_CONSOLE_ON */
  39
  40
  41 #include "objacces.h"
  42
  43
  44 /*!
  45 **
  46 **
  47 ** @param index
  48 ** @param subIndex
  49 ** @param sizeDataDict
  50 ** @param sizeDataGiven
  51 ** @param code
  52 **
  53 ** @return
  54 **/
  55 UNS8 accessDictionaryError(UNS16 index, UNS8 subIndex,
  56                            UNS8 sizeDataDict, UNS8 sizeDataGiven, UNS32 code)
  57 {
  58 #ifdef DEBUG_WAR_CONSOLE_ON
  59   MSG_WAR(0x2B09,"Dictionary index : ", index);
  60   MSG_WAR(0X2B10,"           subindex : ", subIndex);
  61   switch (code) {
  62   case  OD_NO_SUCH_OBJECT:
  63     MSG_WAR(0x2B11,"Index not found ", index);
  64     break;
  65   case OD_NO_SUCH_SUBINDEX :
  66     MSG_WAR(0x2B12,"SubIndex not found ", subIndex);
  67     break;
  68   case OD_WRITE_NOT_ALLOWED :
  69     MSG_WAR(0x2B13,"Write not allowed, data is read only ", index);
  70     break;
  71   case OD_LENGTH_DATA_INVALID :
  72     MSG_WAR(0x2B14,"Conflict size data. Should be (bytes)  : ", sizeDataDict);
  73     MSG_WAR(0x2B15,"But you have given the size  : ", sizeDataGiven);
  74     break;
  75   case OD_NOT_MAPPABLE :
  76     MSG_WAR(0x2B16,"Not mappable data in a PDO at index    : ", index);
  77     break;
  78   case OD_VALUE_TOO_LOW :
  79     MSG_WAR(0x2B17,"Value range error : value too low. SDOabort : ", code);
  80     break;
  81   case OD_VALUE_TOO_HIGH :
  82     MSG_WAR(0x2B18,"Value range error : value too high. SDOabort : ", code);
  83     break;
  84   default :
  85     MSG_WAR(0x2B20, "Unknown error code : ", code);
  86   }
  87 #endif
  88   return 0;
  89 }
  90
  91 /*!
  92 **
  93 **
  94 ** @param d
  95 ** @param wIndex
  96 ** @param bSubindex
  97 ** @param pDestData
  98 ** @param pExpectedSize
  99 ** @param pDataType
 100 ** @param checkAccess
 101 ** @param endianize
 102 **
 103 ** @return
 104 **/
 105 UNS32 _getODentry( CO_Data* d,
 106                    UNS16 wIndex,
 107                    UNS8 bSubindex,
 108                    void * pDestData,
 109                    UNS8 * pExpectedSize,
 110                    UNS8 * pDataType,
 111                    UNS8 checkAccess,
 112                    UNS8 endianize)
 113 { /* DO NOT USE MSG_ERR because the macro may send a PDO -> infinite
 114     loop if it fails. */
 115   UNS32 errorCode;
 116   UNS8 szData;
 117   const indextable *ptrTable;
 118   ODCallback_t *Callback;
 119
 120   ptrTable = (*d->scanIndexOD)(wIndex, &errorCode, &Callback);
 121
 122   if (errorCode != OD_SUCCESSFUL)
 123     return errorCode;
 124   if( ptrTable->bSubCount <= bSubindex ) {
 125     /* Subindex not found */
 126     accessDictionaryError(wIndex, bSubindex, 0, 0, OD_NO_SUCH_SUBINDEX);
 127     return OD_NO_SUCH_SUBINDEX;
 128   }
 129
 130   if (checkAccess && !(ptrTable->pSubindex[bSubindex].bAccessType & WO)) {
 131     MSG_WAR(0x2B30, "Access Type : ", ptrTable->pSubindex[bSubindex].bAccessType);
 132     accessDictionaryError(wIndex, bSubindex, 0, 0, OD_WRITE_NOT_ALLOWED);
 133     return OD_READ_NOT_ALLOWED;
 134   }
 135
 136   *pDataType = ptrTable->pSubindex[bSubindex].bDataType;
 137   szData = ptrTable->pSubindex[bSubindex].size;
 138
 139   if(*pExpectedSize == 0 ||
 140      *pExpectedSize == szData ||
 141      (*pDataType == visible_string && *pExpectedSize < szData)) {
 142     /* We
 143       allow to fetch a shorter string than expected */
 144
 145 #  ifdef CANOPEN_BIG_ENDIAN
 146     if(endianize && *pDataType > boolean && *pDataType < visible_string) {
 147       /* data must be transmited with low byte first */
 148       UNS8 i, j = 0;
 149       MSG_WAR(boolean, "data type ", *pDataType);
 150       MSG_WAR(visible_string, "data type ", *pDataType);
 151       for ( i = szData ; i > 0 ; i--) {
 152         MSG_WAR(i," ", j);
 153         ((UNS8*)pDestData)[j++] =
 154           ((UNS8*)ptrTable->pSubindex[bSubindex].pObject)[i-1];
 155       }
 156     }
 157     else /* It it is a visible string no endianisation to perform */
 158       memcpy(pDestData, ptrTable->pSubindex[bSubindex].pObject,szData);
 159 #  else
 160     memcpy(pDestData, ptrTable->pSubindex[bSubindex].pObject,szData);
 161 #  endif
 162
 163     *pExpectedSize = szData;
 164 #if 0
 165     /* Me laisser a, please ! (FD) */
 166     {
 167       UNS8 i;
 168       for (i = 0 ; i < 10 ; i++) {
 169         MSG_WAR(*pExpectedSize, "dic data= ",
 170                 *(UNS8 *)(ptrTable->pSubindex[bSubindex].pObject + i));
 171       }
 172
 173     }
 174 #endif
 175     return OD_SUCCESSFUL;
 176   }
 177   else { /* Error ! */
 178     *pExpectedSize = szData;
 179     accessDictionaryError(wIndex, bSubindex, szData,
 180                           *pExpectedSize, OD_LENGTH_DATA_INVALID);
 181     return OD_LENGTH_DATA_INVALID;
 182   }
 183 }
 184
 185 /*!
 186 **
 187 **
 188 ** @param d
 189 ** @param wIndex
 190 ** @param bSubindex
 191 ** @param pDestData
 192 ** @param pExpectedSize
 193 ** @param pDataType
 194 ** @param checkAccess
 195 **
 196 ** @return
 197 **/
 198 UNS32 getODentry( CO_Data* d,
 199                   UNS16 wIndex,
 200                   UNS8 bSubindex,
 201                   void * pDestData,
 202                   UNS8 * pExpectedSize,
 203                   UNS8 * pDataType,
 204                   UNS8 checkAccess)
 205 {
 206   return _getODentry( d,
 207                       wIndex,
 208                       bSubindex,
 209                       pDestData,
 210                       pExpectedSize,
 211                       pDataType,
 212                       checkAccess,
 213                       1);//endianize
 214 }
 215
 216 /*!
 217 **
 218 **
 219 ** @param d
 220 ** @param wIndex
 221 ** @param bSubindex
 222 ** @param pDestData
 223 ** @param pExpectedSize
 224 ** @param pDataType
 225 ** @param checkAccess
 226 **
 227 ** @return
 228 **/
 229 UNS32 readLocalDict( CO_Data* d,
 230                      UNS16 wIndex,
 231                      UNS8 bSubindex,
 232                      void * pDestData,
 233                      UNS8 * pExpectedSize,
 234                      UNS8 * pDataType,
 235                      UNS8 checkAccess)
 236 {
 237   return _getODentry( d,
 238                       wIndex,
 239                       bSubindex,
 240                       pDestData,
 241                       pExpectedSize,
 242                       pDataType,
 243                       checkAccess,
 244                       0);//do not endianize
 245 }
 246
 247 /*!
 248 **
 249 **
 250 ** @param d
 251 ** @param wIndex
 252 ** @param bSubindex
 253 ** @param pSourceData
 254 ** @param pExpectedSize
 255 ** @param checkAccess
 256 ** @param endianize
 257 **
 258 ** @return
 259 **/
 260 UNS32 _setODentry( CO_Data* d,
 261                    UNS16 wIndex,
 262                    UNS8 bSubindex,
 263                    void * pSourceData,
 264                    UNS8 * pExpectedSize,
 265                    UNS8 checkAccess,
 266                    UNS8 endianize)
 267 {
 268   UNS8 szData;
 269   UNS8 dataType;
 270   UNS32 errorCode;
 271   const indextable *ptrTable;
 272   ODCallback_t *Callback;
 273
 274   ptrTable =(*d->scanIndexOD)(wIndex, &errorCode, &Callback);
 275   if (errorCode != OD_SUCCESSFUL)
 276     return errorCode;
 277
 278   if( ptrTable->bSubCount <= bSubindex ) {
 279     /* Subindex not found */
 280     accessDictionaryError(wIndex, bSubindex, 0, *pExpectedSize, OD_NO_SUCH_SUBINDEX);
 281     return OD_NO_SUCH_SUBINDEX;
 282   }
 283   if (checkAccess && (ptrTable->pSubindex[bSubindex].bAccessType == RO)) {
 284     MSG_WAR(0x2B25, "Access Type : ", ptrTable->pSubindex[bSubindex].bAccessType);
 285     accessDictionaryError(wIndex, bSubindex, 0, *pExpectedSize, OD_WRITE_NOT_ALLOWED);
 286     return OD_WRITE_NOT_ALLOWED;
 287   }
 288
 289
 290   dataType = ptrTable->pSubindex[bSubindex].bDataType;
 291   szData = ptrTable->pSubindex[bSubindex].size;
 292
 293   if( *pExpectedSize == 0 ||
 294       *pExpectedSize == szData ||
 295       (dataType == visible_string && *pExpectedSize < szData)) /* We
 296                                                                   allow to store a shorter string than entry size */
 297     {
 298 #ifdef CANOPEN_BIG_ENDIAN
 299       if(endianize && dataType > boolean && dataType < visible_string)
 300         {
 301           /* we invert the data source directly. This let us do range
 302             testing without */
 303           /* additional temp variable */
 304           UNS8 i;
 305           for ( i = 0 ; i < ( ptrTable->pSubindex[bSubindex].size >> 1)  ; i++)
 306             {
 307               UNS8 tmp =((UNS8 *)pSourceData) [(ptrTable->pSubindex[bSubindex].size - 1) - i];
 308               ((UNS8 *)pSourceData) [(ptrTable->pSubindex[bSubindex].size - 1) - i] = ((UNS8 *)pSourceData)[i];
 309               ((UNS8 *)pSourceData)[i] = tmp;
 310             }
 311         }
 312 #endif
 313       errorCode = (*d->valueRangeTest)(dataType, pSourceData);
 314       if (errorCode) {
 315         accessDictionaryError(wIndex, bSubindex, szData, *pExpectedSize, errorCode);
 316         return errorCode;
 317       }
 318       memcpy(ptrTable->pSubindex[bSubindex].pObject,pSourceData, *pExpectedSize);
 319       *pExpectedSize = szData;
 320
 321       /* Callbacks */
 322       if(Callback && Callback[bSubindex]){
 323         (*Callback[bSubindex])(d, ptrTable, bSubindex);
 324       }
 325
 326       /* TODO : Store dans NVRAM */
 327       if (ptrTable->pSubindex[bSubindex].bAccessType & TO_BE_SAVE){
 328         (*d->storeODSubIndex)(wIndex, bSubindex);
 329       }
 330       return OD_SUCCESSFUL;
 331     }else{
 332       *pExpectedSize = szData;
 333       accessDictionaryError(wIndex, bSubindex, szData, *pExpectedSize, OD_LENGTH_DATA_INVALID);
 334       return OD_LENGTH_DATA_INVALID;
 335     }
 336 }
 337
 338 /*!
 339 **
 340 **
 341 ** @param d
 342 ** @param wIndex
 343 ** @param bSubindex
 344 ** @param pSourceData
 345 ** @param pExpectedSize
 346 ** @param checkAccess
 347 **
 348 ** @return
 349 **/
 350 UNS32 setODentry( CO_Data* d,
 351                   UNS16 wIndex,
 352                   UNS8 bSubindex,
 353                   void * pSourceData,
 354                   UNS8 * pExpectedSize,
 355                   UNS8 checkAccess)
 356 {
 357   return _setODentry( d,
 358                       wIndex,
 359                       bSubindex,
 360                       pSourceData,
 361                       pExpectedSize,
 362                       checkAccess,
 363                       1);//endianize
 364 }
 365
 366 /*!
 367 **
 368 **
 369 ** @param d
 370 ** @param wIndex
 371 ** @param bSubindex
 372 ** @param pSourceData
 373 ** @param pExpectedSize
 374 ** @param checkAccess
 375 **
 376 ** @return
 377 **/
 378 UNS32 writeLocalDict( CO_Data* d,
 379                       UNS16 wIndex,
 380                       UNS8 bSubindex,
 381                       void * pSourceData,
 382                       UNS8 * pExpectedSize,
 383                       UNS8 checkAccess)
 384 {
 385   return _setODentry( d,
 386                       wIndex,
 387                       bSubindex,
 388                       pSourceData,
 389                       pExpectedSize,
 390                       checkAccess,
 391                       0);//do not endianize
 392 }
 393
 394 /*!
 395 **
 396 **
 397 ** @param d
 398 ** @param wIndex
 399 ** @param errorCode
 400 ** @param Callback
 401 **
 402 ** @return
 403 **/
 404 const indextable * scanIndexOD (CO_Data* d, UNS16 wIndex, UNS32 *errorCode, ODCallback_t **Callback)
 405 {
 406   return (*d->scanIndexOD)(wIndex, errorCode, Callback);
 407 }
 408
 409 /*!
 410 **
 411 **
 412 ** @param d
 413 ** @param wIndex
 414 ** @param bSubindex
 415 ** @param Callback
 416 **
 417 ** @return
 418 **/
 419 UNS32 RegisterSetODentryCallBack(CO_Data* d, UNS16 wIndex, UNS8 bSubindex, ODCallback_t Callback)
 420 {
 421   UNS32 errorCode;
 422   ODCallback_t *CallbackList;
 423
 424   scanIndexOD (d, wIndex, &errorCode, &CallbackList);
 425   if(errorCode == OD_SUCCESSFUL && CallbackList)
 426     CallbackList[bSubindex] = Callback;
 427   return errorCode;
 428 }
 429
 430 /*!
 431 **
 432 **
 433 ** @param wIndex
 434 ** @param bSubindex
 435 **/
 436 void _storeODSubIndex (UNS16 wIndex, UNS8 bSubindex){}