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