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 #ifdef DEBUG_WAR_CONSOLE_ON
  56 UNS8 accessDictionaryError(UNS16 index, UNS8 subIndex,
  57                            UNS8 sizeDataDict, UNS8 sizeDataGiven, UNS32 code)
  58 {
  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   return 0;
  88 }
  89 #else
  90 #define accessDictionaryError(index, subIndex, sizeDataDict, sizeDataGiven, code)
  91 #endif
  92
  93 /*!
  94 **
  95 **
  96 ** @param d
  97 ** @param wIndex
  98 ** @param bSubindex
  99 ** @param pDestData
 100 ** @param pExpectedSize
 101 ** @param pDataType
 102 ** @param checkAccess
 103 ** @param endianize
 104 **
 105 ** @return
 106 **/
 107 UNS32 _getODentry( CO_Data* d,
 108                    UNS16 wIndex,
 109                    UNS8 bSubindex,
 110                    void * pDestData,
 111                    UNS8 * pExpectedSize,
 112                    UNS8 * pDataType,
 113                    UNS8 checkAccess,
 114                    UNS8 endianize)
 115 { /* DO NOT USE MSG_ERR because the macro may send a PDO -> infinite
 116     loop if it fails. */
 117   UNS32 errorCode;
 118   UNS8 szData;
 119   const indextable *ptrTable;
 120   ODCallback_t *Callback;
 121
 122   ptrTable = (*d->scanIndexOD)(wIndex, &errorCode, &Callback);
 123
 124   if (errorCode != OD_SUCCESSFUL)
 125     return errorCode;
 126   if( ptrTable->bSubCount <= bSubindex ) {
 127     /* Subindex not found */
 128     accessDictionaryError(wIndex, bSubindex, 0, 0, OD_NO_SUCH_SUBINDEX);
 129     return OD_NO_SUCH_SUBINDEX;
 130   }
 131
 132   if (checkAccess && (ptrTable->pSubindex[bSubindex].bAccessType & WO)) {
 133     MSG_WAR(0x2B30, "Access Type : ", ptrTable->pSubindex[bSubindex].bAccessType);
 134     accessDictionaryError(wIndex, bSubindex, 0, 0, OD_READ_NOT_ALLOWED);
 135     return OD_READ_NOT_ALLOWED;
 136   }
 137
 138   *pDataType = ptrTable->pSubindex[bSubindex].bDataType;
 139   szData = ptrTable->pSubindex[bSubindex].size;
 140
 141   if(*pExpectedSize == 0 ||
 142      *pExpectedSize == szData ||
 143      /* allow to fetch a shorter string than expected */
 144      (*pDataType >= visible_string && *pExpectedSize < szData)) {
 145
 146 #  ifdef CANOPEN_BIG_ENDIAN
 147      if(endianize && *pDataType > boolean && !(
 148             *pDataType >= visible_string &&
 149             *pDataType <= domain)) {
 150       /* data must be transmited with low byte first */
 151       UNS8 i, j = 0;
 152       MSG_WAR(boolean, "data type ", *pDataType);
 153       MSG_WAR(visible_string, "data type ", *pDataType);
 154       for ( i = szData ; i > 0 ; i--) {
 155         MSG_WAR(i," ", j);
 156         ((UNS8*)pDestData)[j++] =
 157           ((UNS8*)ptrTable->pSubindex[bSubindex].pObject)[i-1];
 158       }
 159       *pExpectedSize = szData;
 160     }
 161     else /* no endianisation change */
 162 #  endif
 163     if(*pDataType != visible_string) {
 164         memcpy(pDestData, ptrTable->pSubindex[bSubindex].pObject,szData);
 165         *pExpectedSize = szData;
 166     }else{
 167         /* TODO : CONFORM TO DS-301 :
 168          *  - stop using NULL terminated strings
 169          *  - store string size in td_subindex
 170          * */
 171         /* Copy null terminated string to user, and return discovered size */
 172         UNS8 *ptr = (UNS8*)ptrTable->pSubindex[bSubindex].pObject;
 173         UNS8 *ptr_start = ptr;
 174         /* *pExpectedSize IS < szData . if null, use szData */
 175         UNS8 *ptr_end = ptr + (*pExpectedSize ? *pExpectedSize : szData) ;
 176         UNS8 *ptr_dest = (UNS8*)pDestData;
 177         while( *ptr && ptr < ptr_end){
 178             *(ptr_dest++) = *(ptr++);
 179         }
 180
 181         *pExpectedSize = ptr - ptr_start;
 182         /* terminate string if not maximum length */
 183         if (*pExpectedSize < szData)
 184             *(ptr) = 0;
 185     }
 186
 187     return OD_SUCCESSFUL;
 188   }
 189   else { /* Error ! */
 190     *pExpectedSize = szData;
 191     accessDictionaryError(wIndex, bSubindex, szData,
 192                           *pExpectedSize, OD_LENGTH_DATA_INVALID);
 193     return OD_LENGTH_DATA_INVALID;
 194   }
 195 }
 196
 197 /*!
 198 **
 199 **
 200 ** @param d
 201 ** @param wIndex
 202 ** @param bSubindex
 203 ** @param pSourceData
 204 ** @param pExpectedSize
 205 ** @param checkAccess
 206 ** @param endianize
 207 **
 208 ** @return
 209 **/
 210 UNS32 _setODentry( CO_Data* d,
 211                    UNS16 wIndex,
 212                    UNS8 bSubindex,
 213                    void * pSourceData,
 214                    UNS8 * pExpectedSize,
 215                    UNS8 checkAccess,
 216                    UNS8 endianize)
 217 {
 218   UNS8 szData;
 219   UNS8 dataType;
 220   UNS32 errorCode;
 221   const indextable *ptrTable;
 222   ODCallback_t *Callback;
 223
 224   ptrTable =(*d->scanIndexOD)(wIndex, &errorCode, &Callback);
 225   if (errorCode != OD_SUCCESSFUL)
 226     return errorCode;
 227
 228   if( ptrTable->bSubCount <= bSubindex ) {
 229     /* Subindex not found */
 230     accessDictionaryError(wIndex, bSubindex, 0, *pExpectedSize, OD_NO_SUCH_SUBINDEX);
 231     return OD_NO_SUCH_SUBINDEX;
 232   }
 233   if (checkAccess && (ptrTable->pSubindex[bSubindex].bAccessType == RO)) {
 234     MSG_WAR(0x2B25, "Access Type : ", ptrTable->pSubindex[bSubindex].bAccessType);
 235     accessDictionaryError(wIndex, bSubindex, 0, *pExpectedSize, OD_WRITE_NOT_ALLOWED);
 236     return OD_WRITE_NOT_ALLOWED;
 237   }
 238
 239
 240   dataType = ptrTable->pSubindex[bSubindex].bDataType;
 241   szData = ptrTable->pSubindex[bSubindex].size;
 242
 243   if( *pExpectedSize == 0 ||
 244       *pExpectedSize == szData ||
 245       /* allow to store a shorter string than entry size */
 246       (dataType == visible_string && *pExpectedSize < szData))
 247     {
 248 #ifdef CANOPEN_BIG_ENDIAN
 249       /* re-endianize do not occur for bool, strings time and domains */
 250       if(endianize && dataType > boolean && !(
 251             dataType >= visible_string &&
 252             dataType <= domain))
 253         {
 254           /* we invert the data source directly. This let us do range
 255             testing without */
 256           /* additional temp variable */
 257           UNS8 i;
 258           for ( i = 0 ; i < ( ptrTable->pSubindex[bSubindex].size >> 1)  ; i++)
 259             {
 260               UNS8 tmp =((UNS8 *)pSourceData) [(ptrTable->pSubindex[bSubindex].size - 1) - i];
 261               ((UNS8 *)pSourceData) [(ptrTable->pSubindex[bSubindex].size - 1) - i] = ((UNS8 *)pSourceData)[i];
 262               ((UNS8 *)pSourceData)[i] = tmp;
 263             }
 264         }
 265 #endif
 266       errorCode = (*d->valueRangeTest)(dataType, pSourceData);
 267       if (errorCode) {
 268         accessDictionaryError(wIndex, bSubindex, szData, *pExpectedSize, errorCode);
 269         return errorCode;
 270       }
 271       memcpy(ptrTable->pSubindex[bSubindex].pObject,pSourceData, *pExpectedSize);
 272      /* TODO : CONFORM TO DS-301 :
 273       *  - stop using NULL terminated strings
 274       *  - store string size in td_subindex
 275       * */
 276       /* terminate visible_string with '\0' */
 277       if(dataType == visible_string && *pExpectedSize < szData)
 278         ((UNS8*)ptrTable->pSubindex[bSubindex].pObject)[*pExpectedSize] = 0;
 279
 280       *pExpectedSize = szData;
 281
 282       /* Callbacks */
 283       if(Callback && Callback[bSubindex]){
 284         errorCode = (Callback[bSubindex])(d, ptrTable, bSubindex);
 285         if(errorCode != OD_SUCCESSFUL)
 286         {
 287             return errorCode;
 288         }
 289        }
 290
 291       /* TODO : Store dans NVRAM */
 292       if (ptrTable->pSubindex[bSubindex].bAccessType & TO_BE_SAVE){
 293         (*d->storeODSubIndex)(d, wIndex, bSubindex);
 294       }
 295       return OD_SUCCESSFUL;
 296     }else{
 297       *pExpectedSize = szData;
 298       accessDictionaryError(wIndex, bSubindex, szData, *pExpectedSize, OD_LENGTH_DATA_INVALID);
 299       return OD_LENGTH_DATA_INVALID;
 300     }
 301 }
 302
 303 /*!
 304 **
 305 **
 306 ** @param d
 307 ** @param wIndex
 308 ** @param errorCode
 309 ** @param Callback
 310 **
 311 ** @return
 312 **/
 313 const indextable * scanIndexOD (CO_Data* d, UNS16 wIndex, UNS32 *errorCode, ODCallback_t **Callback)
 314 {
 315   return (*d->scanIndexOD)(wIndex, errorCode, Callback);
 316 }
 317
 318 /*!
 319 **
 320 **
 321 ** @param d
 322 ** @param wIndex
 323 ** @param bSubindex
 324 ** @param Callback
 325 **
 326 ** @return
 327 **/
 328 UNS32 RegisterSetODentryCallBack(CO_Data* d, UNS16 wIndex, UNS8 bSubindex, ODCallback_t Callback)
 329 {
 330 UNS32 errorCode;
 331 ODCallback_t *CallbackList;
 332 const indextable *odentry;
 333
 334   odentry = scanIndexOD (d, wIndex, &errorCode, &CallbackList);
 335   if(errorCode == OD_SUCCESSFUL  &&  CallbackList  &&  bSubindex < odentry->bSubCount)
 336     CallbackList[bSubindex] = Callback;
 337   return errorCode;
 338 }
 339
 340 /*!
 341 **
 342 **
 343 ** @param wIndex
 344 ** @param bSubindex
 345 **/
 346 void _storeODSubIndex (CO_Data* d, UNS16 wIndex, UNS8 bSubindex){}