CanFestival: /home/epimerde/documents/tc11/CanFestival-3/src/objacces.c Source File

00001 /*
nico@207: 00002 This file is part of CanFestival, a library implementing CanOpen Stack. 
nico@207: 00003 
nico@207: 00004 Copyright (C): Edouard TISSERANT and Francis DUPIN
nico@207: 00005 
nico@207: 00006 See COPYING file for copyrights details.
nico@207: 00007 
nico@207: 00008 This library is free software; you can redistribute it and/or
nico@207: 00009 modify it under the terms of the GNU Lesser General Public
nico@207: 00010 License as published by the Free Software Foundation; either
nico@207: 00011 version 2.1 of the License, or (at your option) any later version.
nico@207: 00012 
nico@207: 00013 This library is distributed in the hope that it will be useful,
nico@207: 00014 but WITHOUT ANY WARRANTY; without even the implied warranty of
nico@207: 00015 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
nico@207: 00016 Lesser General Public License for more details.
nico@207: 00017 
nico@207: 00018 You should have received a copy of the GNU Lesser General Public
nico@207: 00019 License along with this library; if not, write to the Free Software
nico@207: 00020 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
nico@207: 00021 */
nico@207: 00022 
nico@207: 00023 /* #define DEBUG_WAR_CONSOLE_ON */
nico@207: 00024 /* #define DEBUG_ERR_CONSOLE_ON */
nico@207: 00025 
nico@207: 00026 
nico@207: 00027 #include "objacces.h"
nico@207: 00028 
nico@207: 00029 
nico@207: 00030 
nico@207: 00031 UNS8 accessDictionaryError(UNS16 index, UNS8 subIndex, 
nico@207: 00032                              UNS8 sizeDataDict, UNS8 sizeDataGiven, UNS32 code)
nico@207: 00033 {
nico@207: 00034 #ifdef DEBUG_WAR_CONSOLE_ON
nico@207: 00035   MSG_WAR(0x2B09,"Dictionary index : ", index);
nico@207: 00036   MSG_WAR(0X2B10,"           subindex : ", subIndex);
nico@207: 00037   switch (code) {
nico@207: 00038     case  OD_NO_SUCH_OBJECT: 
nico@207: 00039       MSG_WAR(0x2B11,"Index not found ", index);
nico@207: 00040       break;
nico@207: 00041     case OD_NO_SUCH_SUBINDEX :
nico@207: 00042       MSG_WAR(0x2B12,"SubIndex not found ", subIndex);
nico@207: 00043       break;   
nico@207: 00044     case OD_WRITE_NOT_ALLOWED :
nico@207: 00045       MSG_WAR(0x2B13,"Write not allowed, data is read only ", index);
nico@207: 00046       break;         
nico@207: 00047     case OD_LENGTH_DATA_INVALID :    
nico@207: 00048       MSG_WAR(0x2B14,"Conflict size data. Should be (bytes)  : ", sizeDataDict);
nico@207: 00049       MSG_WAR(0x2B15,"But you have given the size  : ", sizeDataGiven);
nico@207: 00050       break;
nico@207: 00051     case OD_NOT_MAPPABLE :
nico@207: 00052       MSG_WAR(0x2B16,"Not mappable data in a PDO at index    : ", index);
nico@207: 00053       break;
nico@207: 00054     case OD_VALUE_TOO_LOW :
nico@207: 00055       MSG_WAR(0x2B17,"Value range error : value too low. SDOabort : ", code);
nico@207: 00056       break;
nico@207: 00057     case OD_VALUE_TOO_HIGH :
nico@207: 00058       MSG_WAR(0x2B18,"Value range error : value too high. SDOabort : ", code);
nico@207: 00059       break;
nico@207: 00060   default :
nico@207: 00061     MSG_WAR(0x2B20, "Unknown error code : ", code);
nico@207: 00062   }
nico@207: 00063 #endif
nico@207: 00064   return 0; 
nico@207: 00065 }       
nico@207: 00066 
nico@207: 00067 
nico@207: 00068 UNS32 _getODentry( CO_Data* d, 
nico@207: 00069                   UNS16 wIndex,
nico@207: 00070                   UNS8 bSubindex,
nico@207: 00071                   void * pDestData,
nico@207: 00072                   UNS8 * pExpectedSize,
nico@207: 00073                   UNS8 * pDataType,
nico@207: 00074                   UNS8 checkAccess,
nico@207: 00075                   UNS8 endianize)
nico@207: 00076 { /* DO NOT USE MSG_ERR because the macro may send a PDO -> infinite loop if it fails. */
nico@207: 00077   UNS32 errorCode;
nico@207: 00078   UNS8 szData;
nico@207: 00079   const indextable *ptrTable;
nico@207: 00080   ODCallback_t *Callback;
nico@207: 00081 
nico@207: 00082   ptrTable = (*d->scanIndexOD)(wIndex, &errorCode, &Callback);
nico@207: 00083 
nico@207: 00084   if (errorCode != OD_SUCCESSFUL)
nico@207: 00085     return errorCode;
nico@207: 00086   if( ptrTable->bSubCount <= bSubindex ) {
nico@207: 00087     /* Subindex not found */
nico@207: 00088     accessDictionaryError(wIndex, bSubindex, 0, 0, OD_NO_SUCH_SUBINDEX);
nico@207: 00089     return OD_NO_SUCH_SUBINDEX;
nico@207: 00090   }
nico@207: 00091   
nico@207: 00092   if (checkAccess && !(ptrTable->pSubindex[bSubindex].bAccessType & WO)) {
nico@207: 00093         MSG_WAR(0x2B30, "Access Type : ", ptrTable->pSubindex[bSubindex].bAccessType);
nico@207: 00094     accessDictionaryError(wIndex, bSubindex, 0, 0, OD_WRITE_NOT_ALLOWED);
nico@207: 00095     return OD_READ_NOT_ALLOWED;
nico@207: 00096   }
nico@207: 00097 
nico@207: 00098   *pDataType = ptrTable->pSubindex[bSubindex].bDataType;
nico@207: 00099    szData = ptrTable->pSubindex[bSubindex].size;
nico@207: 00100 
nico@207: 00101    if(  *pExpectedSize == 0 ||
nico@207: 00102         *pExpectedSize == szData ||
nico@207: 00103         (*pDataType == visible_string && *pExpectedSize < szData)) {/* We allow to fetch a shorter string than expected */
nico@207: 00104      
nico@207: 00105 #  ifdef CANOPEN_BIG_ENDIAN
nico@207: 00106      if(endianize && *pDataType > boolean && *pDataType < visible_string) {
nico@207: 00107        /* data must be transmited with low byte first */
nico@207: 00108        UNS8 i, j = 0;
nico@207: 00109        MSG_WAR(boolean, "data type ", *pDataType);
nico@207: 00110        MSG_WAR(visible_string, "data type ", *pDataType);
nico@207: 00111        for ( i = szData ; i > 0 ; i--) {
nico@207: 00112          MSG_WAR(i," ", j);
nico@207: 00113          ((UNS8*)pDestData)[j++] = 
nico@207: 00114            ((UNS8*)ptrTable->pSubindex[bSubindex].pObject)[i-1];
nico@207: 00115        }
nico@207: 00116      }
nico@207: 00117      else /* It it is a visible string no endianisation to perform */
nico@207: 00118        memcpy(pDestData, ptrTable->pSubindex[bSubindex].pObject,szData);
nico@207: 00119 #  else
nico@207: 00120      memcpy(pDestData, ptrTable->pSubindex[bSubindex].pObject,szData);
nico@207: 00121 #  endif
nico@207: 00122      
nico@207: 00123      *pExpectedSize = szData;
nico@207: 00124 #if 0
nico@207: 00125      /* Me laisser ça, please ! (FD) */
nico@207: 00126      {
nico@207: 00127        UNS8 i;
nico@207: 00128        for (i = 0 ; i < 10 ; i++) {
nico@207: 00129          MSG_WAR(*pExpectedSize, "dic data= ",
nico@207: 00130                  *(UNS8 *)(ptrTable->pSubindex[bSubindex].pObject + i));
nico@207: 00131        }
nico@207: 00132       
nico@207: 00133      }
nico@207: 00134 #endif
nico@207: 00135      return OD_SUCCESSFUL;
nico@207: 00136    }
nico@207: 00137    else { /* Error ! */
nico@207: 00138      *pExpectedSize = szData;
nico@207: 00139      accessDictionaryError(wIndex, bSubindex, szData, 
nico@207: 00140                            *pExpectedSize, OD_LENGTH_DATA_INVALID);
nico@207: 00141      return OD_LENGTH_DATA_INVALID;
nico@207: 00142    }
nico@207: 00143 }
nico@207: 00144 
nico@207: 00145 UNS32 getODentry( CO_Data* d, 
nico@207: 00146                   UNS16 wIndex,
nico@207: 00147                   UNS8 bSubindex,
nico@207: 00148                   void * pDestData,
nico@207: 00149                   UNS8 * pExpectedSize,
nico@207: 00150                   UNS8 * pDataType,
nico@207: 00151                   UNS8 checkAccess)
nico@207: 00152 {
nico@207: 00153         return _getODentry( d, 
nico@207: 00154                   wIndex,
nico@207: 00155                   bSubindex,
nico@207: 00156                   pDestData,
nico@207: 00157                   pExpectedSize,
nico@207: 00158                   pDataType,
nico@207: 00159                   checkAccess,
nico@207: 00160                   1);//endianize
nico@207: 00161 }
nico@207: 00162 
nico@207: 00163 UNS32 readLocalDict( CO_Data* d, 
nico@207: 00164                   UNS16 wIndex,
nico@207: 00165                   UNS8 bSubindex,
nico@207: 00166                   void * pDestData,
nico@207: 00167                   UNS8 * pExpectedSize,
nico@207: 00168                   UNS8 * pDataType,
nico@207: 00169                   UNS8 checkAccess)
nico@207: 00170 {
nico@207: 00171                 return _getODentry( d, 
nico@207: 00172                   wIndex,
nico@207: 00173                   bSubindex,
nico@207: 00174                   pDestData,
nico@207: 00175                   pExpectedSize,
nico@207: 00176                   pDataType,
nico@207: 00177                   checkAccess,
nico@207: 00178                   0);//do not endianize
nico@207: 00179 }
nico@207: 00180 
nico@207: 00181 UNS32 _setODentry( CO_Data* d, 
nico@207: 00182                   UNS16 wIndex,
nico@207: 00183                   UNS8 bSubindex, 
nico@207: 00184                   void * pSourceData, 
nico@207: 00185                   UNS8 * pExpectedSize, 
nico@207: 00186                   UNS8 checkAccess,
nico@207: 00187                   UNS8 endianize)
nico@207: 00188 {
nico@207: 00189   UNS8 szData;
nico@207: 00190   UNS8 dataType;
nico@207: 00191   UNS32 errorCode;
nico@207: 00192   const indextable *ptrTable;
nico@207: 00193   ODCallback_t *Callback;
nico@207: 00194 
nico@207: 00195   ptrTable =(*d->scanIndexOD)(wIndex, &errorCode, &Callback);
nico@207: 00196   if (errorCode != OD_SUCCESSFUL)
nico@207: 00197     return errorCode;
nico@207: 00198 
nico@207: 00199   if( ptrTable->bSubCount <= bSubindex ) {
nico@207: 00200     /* Subindex not found */
nico@207: 00201     accessDictionaryError(wIndex, bSubindex, 0, *pExpectedSize, OD_NO_SUCH_SUBINDEX);
nico@207: 00202     return OD_NO_SUCH_SUBINDEX;
nico@207: 00203   }
nico@207: 00204   if (checkAccess && (ptrTable->pSubindex[bSubindex].bAccessType == RO)) {
nico@207: 00205         MSG_WAR(0x2B25, "Access Type : ", ptrTable->pSubindex[bSubindex].bAccessType);
nico@207: 00206     accessDictionaryError(wIndex, bSubindex, 0, *pExpectedSize, OD_WRITE_NOT_ALLOWED);
nico@207: 00207     return OD_WRITE_NOT_ALLOWED;
nico@207: 00208   }
nico@207: 00209 
nico@207: 00210 
nico@207: 00211    dataType = ptrTable->pSubindex[bSubindex].bDataType;
nico@207: 00212    szData = ptrTable->pSubindex[bSubindex].size;
nico@207: 00213 
nico@207: 00214   if( *pExpectedSize == 0 ||
nico@207: 00215         *pExpectedSize == szData ||
nico@207: 00216         (dataType == visible_string && *pExpectedSize < szData)) /* We allow to store a shorter string than entry size */
nico@207: 00217   {
nico@207: 00218       #ifdef CANOPEN_BIG_ENDIAN
nico@207: 00219               if(endianize && dataType > boolean && dataType < visible_string)
nico@207: 00220               {
nico@207: 00221                         /* we invert the data source directly. This let us do range testing without */
nico@207: 00222                         /* additional temp variable */
nico@207: 00223                         UNS8 i;
nico@207: 00224                         for ( i = 0 ; i < ( ptrTable->pSubindex[bSubindex].size >> 1)  ; i++) 
nico@207: 00225                         {
nico@207: 00226                         UNS8 tmp =((UNS8 *)pSourceData) [(ptrTable->pSubindex[bSubindex].size - 1) - i];
nico@207: 00227                         ((UNS8 *)pSourceData) [(ptrTable->pSubindex[bSubindex].size - 1) - i] = ((UNS8 *)pSourceData)[i];
nico@207: 00228                         ((UNS8 *)pSourceData)[i] = tmp;
nico@207: 00229                 }
nico@207: 00230                 }  
nico@207: 00231       #endif
nico@207: 00232       errorCode = (*d->valueRangeTest)(dataType, pSourceData);
nico@207: 00233       if (errorCode) {
nico@207: 00234         accessDictionaryError(wIndex, bSubindex, szData, *pExpectedSize, errorCode);
nico@207: 00235         return errorCode;
nico@207: 00236       }
nico@207: 00237       memcpy(ptrTable->pSubindex[bSubindex].pObject,pSourceData, *pExpectedSize);
nico@207: 00238       *pExpectedSize = szData;
nico@207: 00239       
nico@207: 00240       /* Callbacks */
nico@207: 00241       if(Callback && Callback[bSubindex]){
nico@207: 00242          (*Callback[bSubindex])(d, ptrTable, bSubindex);
nico@207: 00243       }
nico@207: 00244       
nico@207: 00245       /* TODO : Store dans NVRAM */     
nico@207: 00246       if (ptrTable->pSubindex[bSubindex].bAccessType & TO_BE_SAVE){
nico@207: 00247         (*d->storeODSubIndex)(wIndex, bSubindex);
nico@207: 00248       }
nico@207: 00249       return OD_SUCCESSFUL;
nico@207: 00250   }else{
nico@207: 00251       *pExpectedSize = szData;
nico@207: 00252       accessDictionaryError(wIndex, bSubindex, szData, *pExpectedSize, OD_LENGTH_DATA_INVALID);
nico@207: 00253       return OD_LENGTH_DATA_INVALID;
nico@207: 00254   }
nico@207: 00255 }
nico@207: 00256 
nico@207: 00257 UNS32 setODentry( CO_Data* d, 
nico@207: 00258                   UNS16 wIndex,
nico@207: 00259                   UNS8 bSubindex, 
nico@207: 00260                   void * pSourceData, 
nico@207: 00261                   UNS8 * pExpectedSize, 
nico@207: 00262                   UNS8 checkAccess)
nico@207: 00263 {
nico@207: 00264         return _setODentry( d, 
nico@207: 00265                   wIndex,
nico@207: 00266                   bSubindex, 
nico@207: 00267                   pSourceData, 
nico@207: 00268                   pExpectedSize, 
nico@207: 00269                   checkAccess,
nico@207: 00270                   1);//endianize
nico@207: 00271 }
nico@207: 00272 
nico@207: 00273 UNS32 writeLocalDict( CO_Data* d, 
nico@207: 00274                   UNS16 wIndex,
nico@207: 00275                   UNS8 bSubindex, 
nico@207: 00276                   void * pSourceData, 
nico@207: 00277                   UNS8 * pExpectedSize, 
nico@207: 00278                   UNS8 checkAccess)
nico@207: 00279 {
nico@207: 00280         return _setODentry( d, 
nico@207: 00281                   wIndex,
nico@207: 00282                   bSubindex, 
nico@207: 00283                   pSourceData, 
nico@207: 00284                   pExpectedSize, 
nico@207: 00285                   checkAccess,
nico@207: 00286                   0);//do not endianize
nico@207: 00287 }
nico@207: 00288 
nico@207: 00289 
nico@207: 00290 
nico@207: 00291 
nico@207: 00292 const indextable * scanIndexOD (CO_Data* d, UNS16 wIndex, UNS32 *errorCode, ODCallback_t **Callback)
nico@207: 00293 {
nico@207: 00294   return (*d->scanIndexOD)(wIndex, errorCode, Callback);
nico@207: 00295 }
nico@207: 00296 
nico@207: 00297 UNS32 RegisterSetODentryCallBack(CO_Data* d, UNS16 wIndex, UNS8 bSubindex, ODCallback_t Callback)
nico@207: 00298 {
nico@207: 00299         UNS32 errorCode;
nico@207: 00300         ODCallback_t *CallbackList;
nico@207: 00301 
nico@207: 00302         scanIndexOD (d, wIndex, &errorCode, &CallbackList);
nico@207: 00303         if(errorCode == OD_SUCCESSFUL && CallbackList) 
nico@207: 00304                 CallbackList[bSubindex] = Callback;
nico@207: 00305         return errorCode;
nico@207: 00306 }
nico@207: 00307 
nico@207: 00308 void _storeODSubIndex (UNS16 wIndex, UNS8 bSubindex){}
nico@207: