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

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