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