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 "data.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      /* allow to fetch a shorter string than expected */
 142      (*pDataType >= visible_string && *pExpectedSize < szData)) {
 143
 144 #  ifdef CANOPEN_BIG_ENDIAN
 145      if(endianize && dataType > boolean && !(
 146             dataType >= visible_string &&
 147             domain <= dataType)) {
 148       /* data must be transmited with low byte first */
 149       UNS8 i, j = 0;
 150       MSG_WAR(boolean, "data type ", *pDataType);
 151       MSG_WAR(visible_string, "data type ", *pDataType);
 152       for ( i = szData ; i > 0 ; i--) {
 153         MSG_WAR(i," ", j);
 154         ((UNS8*)pDestData)[j++] =
 155           ((UNS8*)ptrTable->pSubindex[bSubindex].pObject)[i-1];
 156       }
 157     }
 158     else /* no endianisation change */
 159 #  endif
 160     if(*pDataType != visible_string) {
 161         memcpy(pDestData, ptrTable->pSubindex[bSubindex].pObject,szData);
 162         *pExpectedSize = szData;
 163     }else{
 164         /* TODO : CONFORM TO DS-301 :
 165          *  - stop using NULL terminated strings
 166          *  - store string size in td_subindex
 167          * */
 168         /* Copy null terminated string to user, and return discovered size */
 169         UNS8 *ptr = (UNS8*)ptrTable->pSubindex[bSubindex].pObject;
 170         UNS8 *ptr_start = ptr;
 171         UNS8 *ptr_end = ptr + *pExpectedSize; /* *pExpectedSize IS < szData */
 172         UNS8 *ptr_dest = (UNS8*)pDestData;
 173         while( *ptr && ptr < ptr_end){
 174             *(ptr_dest++) = *(ptr++);
 175         }
 176
 177         *pExpectedSize = ptr - ptr_start;
 178         /* terminate string if not maximum length */
 179         if (*pExpectedSize < szData)
 180             *(ptr) = 0;
 181     }
 182
 183     return OD_SUCCESSFUL;
 184   }
 185   else { /* Error ! */
 186     *pExpectedSize = szData;
 187     accessDictionaryError(wIndex, bSubindex, szData,
 188                           *pExpectedSize, OD_LENGTH_DATA_INVALID);
 189     return OD_LENGTH_DATA_INVALID;
 190   }
 191 }
 192
 193 /*!
 194 **
 195 **
 196 ** @param d
 197 ** @param wIndex
 198 ** @param bSubindex
 199 ** @param pDestData
 200 ** @param pExpectedSize
 201 ** @param pDataType
 202 ** @param checkAccess
 203 **
 204 ** @return
 205 **/
 206 UNS32 getODentry( CO_Data* d,
 207                   UNS16 wIndex,
 208                   UNS8 bSubindex,
 209                   void * pDestData,
 210                   UNS8 * pExpectedSize,
 211                   UNS8 * pDataType,
 212                   UNS8 checkAccess)
 213 {
 214   return _getODentry( d,
 215                       wIndex,
 216                       bSubindex,
 217                       pDestData,
 218                       pExpectedSize,
 219                       pDataType,
 220                       checkAccess,
 221                       1);//endianize
 222 }
 223
 224 /*!
 225 **
 226 **
 227 ** @param d
 228 ** @param wIndex
 229 ** @param bSubindex
 230 ** @param pDestData
 231 ** @param pExpectedSize
 232 ** @param pDataType
 233 ** @param checkAccess
 234 **
 235 ** @return
 236 **/
 237 UNS32 readLocalDict( CO_Data* d,
 238                      UNS16 wIndex,
 239                      UNS8 bSubindex,
 240                      void * pDestData,
 241                      UNS8 * pExpectedSize,
 242                      UNS8 * pDataType,
 243                      UNS8 checkAccess)
 244 {
 245   return _getODentry( d,
 246                       wIndex,
 247                       bSubindex,
 248                       pDestData,
 249                       pExpectedSize,
 250                       pDataType,
 251                       checkAccess,
 252                       0);//do not endianize
 253 }
 254
 255 /*!
 256 **
 257 **
 258 ** @param d
 259 ** @param wIndex
 260 ** @param bSubindex
 261 ** @param pSourceData
 262 ** @param pExpectedSize
 263 ** @param checkAccess
 264 ** @param endianize
 265 **
 266 ** @return
 267 **/
 268 UNS32 _setODentry( CO_Data* d,
 269                    UNS16 wIndex,
 270                    UNS8 bSubindex,
 271                    void * pSourceData,
 272                    UNS8 * pExpectedSize,
 273                    UNS8 checkAccess,
 274                    UNS8 endianize)
 275 {
 276   UNS8 szData;
 277   UNS8 dataType;
 278   UNS32 errorCode;
 279   const indextable *ptrTable;
 280   ODCallback_t *Callback;
 281
 282   ptrTable =(*d->scanIndexOD)(wIndex, &errorCode, &Callback);
 283   if (errorCode != OD_SUCCESSFUL)
 284     return errorCode;
 285
 286   if( ptrTable->bSubCount <= bSubindex ) {
 287     /* Subindex not found */
 288     accessDictionaryError(wIndex, bSubindex, 0, *pExpectedSize, OD_NO_SUCH_SUBINDEX);
 289     return OD_NO_SUCH_SUBINDEX;
 290   }
 291   if (checkAccess && (ptrTable->pSubindex[bSubindex].bAccessType == RO)) {
 292     MSG_WAR(0x2B25, "Access Type : ", ptrTable->pSubindex[bSubindex].bAccessType);
 293     accessDictionaryError(wIndex, bSubindex, 0, *pExpectedSize, OD_WRITE_NOT_ALLOWED);
 294     return OD_WRITE_NOT_ALLOWED;
 295   }
 296
 297
 298   dataType = ptrTable->pSubindex[bSubindex].bDataType;
 299   szData = ptrTable->pSubindex[bSubindex].size;
 300
 301   if( *pExpectedSize == 0 ||
 302       *pExpectedSize == szData ||
 303       /* allow to store a shorter string than entry size */
 304       (dataType == visible_string && *pExpectedSize < szData))
 305     {
 306 #ifdef CANOPEN_BIG_ENDIAN
 307       /* re-endianize do not occur for bool, strings time and domains */
 308       if(endianize && dataType > boolean && !(
 309             dataType >= visible_string &&
 310             domain <= dataType))
 311         {
 312           /* we invert the data source directly. This let us do range
 313             testing without */
 314           /* additional temp variable */
 315           UNS8 i;
 316           for ( i = 0 ; i < ( ptrTable->pSubindex[bSubindex].size >> 1)  ; i++)
 317             {
 318               UNS8 tmp =((UNS8 *)pSourceData) [(ptrTable->pSubindex[bSubindex].size - 1) - i];
 319               ((UNS8 *)pSourceData) [(ptrTable->pSubindex[bSubindex].size - 1) - i] = ((UNS8 *)pSourceData)[i];
 320               ((UNS8 *)pSourceData)[i] = tmp;
 321             }
 322         }
 323 #endif
 324       errorCode = (*d->valueRangeTest)(dataType, pSourceData);
 325       if (errorCode) {
 326         accessDictionaryError(wIndex, bSubindex, szData, *pExpectedSize, errorCode);
 327         return errorCode;
 328       }
 329       memcpy(ptrTable->pSubindex[bSubindex].pObject,pSourceData, *pExpectedSize);
 330      /* TODO : CONFORM TO DS-301 :
 331       *  - stop using NULL terminated strings
 332       *  - store string size in td_subindex
 333       * */
 334       /* terminate visible_string with '\0' */
 335       if(dataType == visible_string && *pExpectedSize < szData)
 336         ((UNS8*)ptrTable->pSubindex[bSubindex].pObject)[*pExpectedSize] = 0;
 337
 338       *pExpectedSize = szData;
 339
 340       /* Callbacks */
 341       if(Callback && Callback[bSubindex]){
 342         (*Callback[bSubindex])(d, ptrTable, bSubindex);
 343       }
 344
 345       /* TODO : Store dans NVRAM */
 346       if (ptrTable->pSubindex[bSubindex].bAccessType & TO_BE_SAVE){
 347         (*d->storeODSubIndex)(d, wIndex, bSubindex);
 348       }
 349       return OD_SUCCESSFUL;
 350     }else{
 351       *pExpectedSize = szData;
 352       accessDictionaryError(wIndex, bSubindex, szData, *pExpectedSize, OD_LENGTH_DATA_INVALID);
 353       return OD_LENGTH_DATA_INVALID;
 354     }
 355 }
 356
 357 /*!
 358 **
 359 **
 360 ** @param d
 361 ** @param wIndex
 362 ** @param bSubindex
 363 ** @param pSourceData
 364 ** @param pExpectedSize
 365 ** @param checkAccess
 366 **
 367 ** @return
 368 **/
 369 UNS32 setODentry( CO_Data* d,
 370                   UNS16 wIndex,
 371                   UNS8 bSubindex,
 372                   void * pSourceData,
 373                   UNS8 * pExpectedSize,
 374                   UNS8 checkAccess)
 375 {
 376   return _setODentry( d,
 377                       wIndex,
 378                       bSubindex,
 379                       pSourceData,
 380                       pExpectedSize,
 381                       checkAccess,
 382                       1);//endianize
 383 }
 384
 385 /*!
 386 **
 387 **
 388 ** @param d
 389 ** @param wIndex
 390 ** @param bSubindex
 391 ** @param pSourceData
 392 ** @param pExpectedSize
 393 ** @param checkAccess
 394 **
 395 ** @return
 396 **/
 397 UNS32 writeLocalDict( CO_Data* d,
 398                       UNS16 wIndex,
 399                       UNS8 bSubindex,
 400                       void * pSourceData,
 401                       UNS8 * pExpectedSize,
 402                       UNS8 checkAccess)
 403 {
 404   return _setODentry( d,
 405                       wIndex,
 406                       bSubindex,
 407                       pSourceData,
 408                       pExpectedSize,
 409                       checkAccess,
 410                       0);//do not endianize
 411 }
 412
 413 /*!
 414 **
 415 **
 416 ** @param d
 417 ** @param wIndex
 418 ** @param errorCode
 419 ** @param Callback
 420 **
 421 ** @return
 422 **/
 423 const indextable * scanIndexOD (CO_Data* d, UNS16 wIndex, UNS32 *errorCode, ODCallback_t **Callback)
 424 {
 425   return (*d->scanIndexOD)(wIndex, errorCode, Callback);
 426 }
 427
 428 /*!
 429 **
 430 **
 431 ** @param d
 432 ** @param wIndex
 433 ** @param bSubindex
 434 ** @param Callback
 435 **
 436 ** @return
 437 **/
 438 UNS32 RegisterSetODentryCallBack(CO_Data* d, UNS16 wIndex, UNS8 bSubindex, ODCallback_t Callback)
 439 {
 440   UNS32 errorCode;
 441   ODCallback_t *CallbackList;
 442
 443   scanIndexOD (d, wIndex, &errorCode, &CallbackList);
 444   if(errorCode == OD_SUCCESSFUL && CallbackList)
 445     CallbackList[bSubindex] = Callback;
 446   return errorCode;
 447 }
 448
 449 /*!
 450 **
 451 **
 452 ** @param wIndex
 453 ** @param bSubindex
 454 **/
 455 void _storeODSubIndex (CO_Data* d, UNS16 wIndex, UNS8 bSubindex){}