CanFestival: src/objacces.c Source File

00001 /*
nico@210: 00002   This file is part of CanFestival, a library implementing CanOpen
nico@210: 00003   Stack.
nico@210: 00004 
nico@210: 00005   Copyright (C): Edouard TISSERANT and Francis DUPIN
nico@210: 00006 
nico@210: 00007   See COPYING file for copyrights details.
nico@210: 00008 
nico@210: 00009   This library is free software; you can redistribute it and/or
nico@210: 00010   modify it under the terms of the GNU Lesser General Public
nico@210: 00011   License as published by the Free Software Foundation; either
nico@210: 00012   version 2.1 of the License, or (at your option) any later version.
nico@210: 00013 
nico@210: 00014   This library is distributed in the hope that it will be useful,
nico@210: 00015   but WITHOUT ANY WARRANTY; without even the implied warranty of
nico@210: 00016   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
nico@210: 00017   Lesser General Public License for more details.
nico@210: 00018 
nico@210: 00019   You should have received a copy of the GNU Lesser General Public
nico@210: 00020   License along with this library; if not, write to the Free Software
nico@210: 00021   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307
nico@210: 00022   USA
nico@210: 00023 */
nico@210: 00037 /* #define DEBUG_WAR_CONSOLE_ON */
nico@210: 00038 /* #define DEBUG_ERR_CONSOLE_ON */
nico@210: 00039 
nico@210: 00040 
nico@215: 00041 #include "objacces.h"
nico@210: 00042 
nico@210: 00043 
etisserant@240: 00055 UNS8 accessDictionaryError(UNS16 index, UNS8 subIndex,
etisserant@240: 00056                            UNS8 sizeDataDict, UNS8 sizeDataGiven, UNS32 code)
nico@210: 00057 {
nico@210: 00058 #ifdef DEBUG_WAR_CONSOLE_ON
etisserant@240: 00059   MSG_WAR(0x2B09,"Dictionary index : ", index);
etisserant@240: 00060   MSG_WAR(0X2B10,"           subindex : ", subIndex);
nico@210: 00061   switch (code) {
etisserant@240: 00062   case  OD_NO_SUCH_OBJECT:
etisserant@240: 00063     MSG_WAR(0x2B11,"Index not found ", index);
nico@210: 00064     break;
etisserant@240: 00065   case OD_NO_SUCH_SUBINDEX :
etisserant@240: 00066     MSG_WAR(0x2B12,"SubIndex not found ", subIndex);
nico@210: 00067     break;
etisserant@240: 00068   case OD_WRITE_NOT_ALLOWED :
etisserant@240: 00069     MSG_WAR(0x2B13,"Write not allowed, data is read only ", index);
nico@210: 00070     break;
etisserant@240: 00071   case OD_LENGTH_DATA_INVALID :
etisserant@240: 00072     MSG_WAR(0x2B14,"Conflict size data. Should be (bytes)  : ", sizeDataDict);
etisserant@240: 00073     MSG_WAR(0x2B15,"But you have given the size  : ", sizeDataGiven);
nico@210: 00074     break;
etisserant@240: 00075   case OD_NOT_MAPPABLE :
etisserant@240: 00076     MSG_WAR(0x2B16,"Not mappable data in a PDO at index    : ", index);
nico@210: 00077     break;
etisserant@240: 00078   case OD_VALUE_TOO_LOW :
etisserant@240: 00079     MSG_WAR(0x2B17,"Value range error : value too low. SDOabort : ", code);
nico@210: 00080     break;
etisserant@240: 00081   case OD_VALUE_TOO_HIGH :
etisserant@240: 00082     MSG_WAR(0x2B18,"Value range error : value too high. SDOabort : ", code);
nico@210: 00083     break;
nico@210: 00084   default :
etisserant@240: 00085     MSG_WAR(0x2B20, "Unknown error code : ", code);
nico@210: 00086   }
nico@210: 00087 #endif
nico@210: 00088   return 0;
nico@210: 00089 }
nico@210: 00090 
etisserant@240: 00105 UNS32 _getODentry( CO_Data* d,
etisserant@240: 00106                    UNS16 wIndex,
etisserant@240: 00107                    UNS8 bSubindex,
nico@210: 00108                    void * pDestData,
etisserant@240: 00109                    UNS8 * pExpectedSize,
etisserant@240: 00110                    UNS8 * pDataType,
etisserant@240: 00111                    UNS8 checkAccess,
etisserant@240: 00112                    UNS8 endianize)
nico@215: 00113 { /* DO NOT USE MSG_ERR because the macro may send a PDO -> infinite
nico@215: 00114     loop if it fails. */
etisserant@240: 00115   UNS32 errorCode;
etisserant@240: 00116   UNS8 szData;
etisserant@240: 00117   const indextable *ptrTable;
etisserant@240: 00118   ODCallback_t *Callback;
nico@210: 00119 
etisserant@240: 00120   ptrTable = (*d->scanIndexOD)(wIndex, &errorCode, &Callback);
nico@210: 00121 
etisserant@240: 00122   if (errorCode != OD_SUCCESSFUL)
nico@210: 00123     return errorCode;
etisserant@240: 00124   if( ptrTable->bSubCount <= bSubindex ) {
nico@210: 00125     /* Subindex not found */
etisserant@240: 00126     accessDictionaryError(wIndex, bSubindex, 0, 0, OD_NO_SUCH_SUBINDEX);
etisserant@240: 00127     return OD_NO_SUCH_SUBINDEX;
nico@210: 00128   }
nico@210: 00129 
etisserant@240: 00130   if (checkAccess && !(ptrTable->pSubindex[bSubindex].bAccessType & WO)) {
etisserant@240: 00131     MSG_WAR(0x2B30, "Access Type : ", ptrTable->pSubindex[bSubindex].bAccessType);
etisserant@240: 00132     accessDictionaryError(wIndex, bSubindex, 0, 0, OD_WRITE_NOT_ALLOWED);
etisserant@240: 00133     return OD_READ_NOT_ALLOWED;
nico@210: 00134   }
nico@210: 00135 
etisserant@240: 00136   *pDataType = ptrTable->pSubindex[bSubindex].bDataType;
etisserant@240: 00137   szData = ptrTable->pSubindex[bSubindex].size;
nico@210: 00138 
nico@210: 00139   if(*pExpectedSize == 0 ||
nico@210: 00140      *pExpectedSize == szData ||
etisserant@240: 00141      (*pDataType == visible_string && *pExpectedSize < szData)) {
nico@215: 00142     /* We
nico@215: 00143       allow to fetch a shorter string than expected */
nico@215: 00144 
nico@210: 00145 #  ifdef CANOPEN_BIG_ENDIAN
etisserant@240: 00146     if(endianize && *pDataType > boolean && *pDataType < visible_string) {
nico@215: 00147       /* data must be transmited with low byte first */
etisserant@240: 00148       UNS8 i, j = 0;
etisserant@240: 00149       MSG_WAR(boolean, "data type ", *pDataType);
etisserant@240: 00150       MSG_WAR(visible_string, "data type ", *pDataType);
nico@210: 00151       for ( i = szData ; i > 0 ; i--) {
etisserant@240: 00152         MSG_WAR(i," ", j);
etisserant@240: 00153         ((UNS8*)pDestData)[j++] =
etisserant@240: 00154           ((UNS8*)ptrTable->pSubindex[bSubindex].pObject)[i-1];
nico@210: 00155       }
nico@210: 00156     }
nico@215: 00157     else /* It it is a visible string no endianisation to perform */
etisserant@240: 00158       memcpy(pDestData, ptrTable->pSubindex[bSubindex].pObject,szData);
nico@210: 00159 #  else
etisserant@240: 00160     memcpy(pDestData, ptrTable->pSubindex[bSubindex].pObject,szData);
nico@210: 00161 #  endif
nico@210: 00162 
nico@210: 00163     *pExpectedSize = szData;
nico@210: 00164 #if 0
nico@215: 00165     /* Me laisser a, please ! (FD) */
nico@210: 00166     {
etisserant@240: 00167       UNS8 i;
nico@210: 00168       for (i = 0 ; i < 10 ; i++) {
etisserant@240: 00169         MSG_WAR(*pExpectedSize, "dic data= ",
etisserant@240: 00170                 *(UNS8 *)(ptrTable->pSubindex[bSubindex].pObject + i));
nico@210: 00171       }
nico@210: 00172 
nico@210: 00173     }
nico@210: 00174 #endif
etisserant@240: 00175     return OD_SUCCESSFUL;
nico@210: 00176   }
nico@215: 00177   else { /* Error ! */
nico@210: 00178     *pExpectedSize = szData;
etisserant@240: 00179     accessDictionaryError(wIndex, bSubindex, szData,
etisserant@240: 00180                           *pExpectedSize, OD_LENGTH_DATA_INVALID);
etisserant@240: 00181     return OD_LENGTH_DATA_INVALID;
nico@210: 00182   }
nico@210: 00183 }
nico@210: 00184 
etisserant@240: 00198 UNS32 getODentry( CO_Data* d,
etisserant@240: 00199                   UNS16 wIndex,
etisserant@240: 00200                   UNS8 bSubindex,
nico@210: 00201                   void * pDestData,
etisserant@240: 00202                   UNS8 * pExpectedSize,
etisserant@240: 00203                   UNS8 * pDataType,
etisserant@240: 00204                   UNS8 checkAccess)
nico@210: 00205 {
etisserant@240: 00206   return _getODentry( d,
nico@210: 00207                       wIndex,
nico@210: 00208                       bSubindex,
nico@210: 00209                       pDestData,
nico@210: 00210                       pExpectedSize,
nico@210: 00211                       pDataType,
nico@210: 00212                       checkAccess,
nico@210: 00213                       1);//endianize
nico@210: 00214 }
nico@210: 00215 
etisserant@240: 00229 UNS32 readLocalDict( CO_Data* d,
etisserant@240: 00230                      UNS16 wIndex,
etisserant@240: 00231                      UNS8 bSubindex,
nico@210: 00232                      void * pDestData,
etisserant@240: 00233                      UNS8 * pExpectedSize,
etisserant@240: 00234                      UNS8 * pDataType,
etisserant@240: 00235                      UNS8 checkAccess)
nico@210: 00236 {
etisserant@240: 00237   return _getODentry( d,
nico@210: 00238                       wIndex,
nico@210: 00239                       bSubindex,
nico@210: 00240                       pDestData,
nico@210: 00241                       pExpectedSize,
nico@210: 00242                       pDataType,
nico@210: 00243                       checkAccess,
nico@210: 00244                       0);//do not endianize
nico@210: 00245 }
nico@210: 00246 
etisserant@240: 00260 UNS32 _setODentry( CO_Data* d,
etisserant@240: 00261                    UNS16 wIndex,
etisserant@240: 00262                    UNS8 bSubindex,
nico@210: 00263                    void * pSourceData,
etisserant@240: 00264                    UNS8 * pExpectedSize,
etisserant@240: 00265                    UNS8 checkAccess,
etisserant@240: 00266                    UNS8 endianize)
nico@210: 00267 {
etisserant@240: 00268   UNS8 szData;
etisserant@240: 00269   UNS8 dataType;
etisserant@240: 00270   UNS32 errorCode;
etisserant@240: 00271   const indextable *ptrTable;
etisserant@240: 00272   ODCallback_t *Callback;
nico@210: 00273 
etisserant@240: 00274   ptrTable =(*d->scanIndexOD)(wIndex, &errorCode, &Callback);
etisserant@240: 00275   if (errorCode != OD_SUCCESSFUL)
nico@210: 00276     return errorCode;
nico@210: 00277 
etisserant@240: 00278   if( ptrTable->bSubCount <= bSubindex ) {
nico@215: 00279     /* Subindex not found */
etisserant@240: 00280     accessDictionaryError(wIndex, bSubindex, 0, *pExpectedSize, OD_NO_SUCH_SUBINDEX);
etisserant@240: 00281     return OD_NO_SUCH_SUBINDEX;
nico@210: 00282   }
etisserant@240: 00283   if (checkAccess && (ptrTable->pSubindex[bSubindex].bAccessType == RO)) {
etisserant@240: 00284     MSG_WAR(0x2B25, "Access Type : ", ptrTable->pSubindex[bSubindex].bAccessType);
etisserant@240: 00285     accessDictionaryError(wIndex, bSubindex, 0, *pExpectedSize, OD_WRITE_NOT_ALLOWED);
etisserant@240: 00286     return OD_WRITE_NOT_ALLOWED;
nico@210: 00287   }
nico@207: 00288 
nico@207: 00289 
etisserant@240: 00290   dataType = ptrTable->pSubindex[bSubindex].bDataType;
etisserant@240: 00291   szData = ptrTable->pSubindex[bSubindex].size;
nico@210: 00292 
nico@210: 00293   if( *pExpectedSize == 0 ||
nico@210: 00294       *pExpectedSize == szData ||
etisserant@240: 00295       (dataType == visible_string && *pExpectedSize < szData)) /* We
nico@210: 00296                                                                   allow to store a shorter string than entry size */
nico@210: 00297     {
nico@210: 00298 #ifdef CANOPEN_BIG_ENDIAN
etisserant@240: 00299       if(endianize && dataType > boolean && dataType < visible_string)
nico@210: 00300         {
nico@215: 00301           /* we invert the data source directly. This let us do range
nico@215: 00302             testing without */
nico@215: 00303           /* additional temp variable */
etisserant@240: 00304           UNS8 i;
etisserant@240: 00305           for ( i = 0 ; i < ( ptrTable->pSubindex[bSubindex].size >> 1)  ; i++)
nico@210: 00306             {
etisserant@240: 00307               UNS8 tmp =((UNS8 *)pSourceData) [(ptrTable->pSubindex[bSubindex].size - 1) - i];
etisserant@240: 00308               ((UNS8 *)pSourceData) [(ptrTable->pSubindex[bSubindex].size - 1) - i] = ((UNS8 *)pSourceData)[i];
nico@210: 00309               ((UNS8 *)pSourceData)[i] = tmp;
nico@210: 00310             }
nico@210: 00311         }
nico@210: 00312 #endif
etisserant@240: 00313       errorCode = (*d->valueRangeTest)(dataType, pSourceData);
nico@210: 00314       if (errorCode) {
etisserant@240: 00315         accessDictionaryError(wIndex, bSubindex, szData, *pExpectedSize, errorCode);
nico@210: 00316         return errorCode;
nico@210: 00317       }
etisserant@240: 00318       memcpy(ptrTable->pSubindex[bSubindex].pObject,pSourceData, *pExpectedSize);
nico@210: 00319       *pExpectedSize = szData;
nico@210: 00320 
nico@215: 00321       /* Callbacks */
nico@210: 00322       if(Callback && Callback[bSubindex]){
etisserant@240: 00323         (*Callback[bSubindex])(d, ptrTable, bSubindex);
nico@210: 00324       }
nico@210: 00325 
nico@215: 00326       /* TODO : Store dans NVRAM */
etisserant@240: 00327       if (ptrTable->pSubindex[bSubindex].bAccessType & TO_BE_SAVE){
etisserant@240: 00328         (*d->storeODSubIndex)(wIndex, bSubindex);
nico@210: 00329       }
etisserant@240: 00330       return OD_SUCCESSFUL;
nico@210: 00331     }else{
nico@210: 00332       *pExpectedSize = szData;
etisserant@240: 00333       accessDictionaryError(wIndex, bSubindex, szData, *pExpectedSize, OD_LENGTH_DATA_INVALID);
etisserant@240: 00334       return OD_LENGTH_DATA_INVALID;
nico@210: 00335     }
nico@210: 00336 }
nico@210: 00337 
etisserant@240: 00350 UNS32 setODentry( CO_Data* d,
etisserant@240: 00351                   UNS16 wIndex,
etisserant@240: 00352                   UNS8 bSubindex,
nico@210: 00353                   void * pSourceData,
etisserant@240: 00354                   UNS8 * pExpectedSize,
etisserant@240: 00355                   UNS8 checkAccess)
nico@210: 00356 {
etisserant@240: 00357   return _setODentry( d,
nico@210: 00358                       wIndex,
nico@210: 00359                       bSubindex,
nico@210: 00360                       pSourceData,
nico@210: 00361                       pExpectedSize,
nico@210: 00362                       checkAccess,
nico@210: 00363                       1);//endianize
nico@210: 00364 }
nico@210: 00365 
etisserant@240: 00378 UNS32 writeLocalDict( CO_Data* d,
etisserant@240: 00379                       UNS16 wIndex,
etisserant@240: 00380                       UNS8 bSubindex,
nico@210: 00381                       void * pSourceData,
etisserant@240: 00382                       UNS8 * pExpectedSize,
etisserant@240: 00383                       UNS8 checkAccess)
nico@210: 00384 {
etisserant@240: 00385   return _setODentry( d,
nico@210: 00386                       wIndex,
nico@210: 00387                       bSubindex,
nico@210: 00388                       pSourceData,
nico@210: 00389                       pExpectedSize,
nico@210: 00390                       checkAccess,
nico@210: 00391                       0);//do not endianize
nico@210: 00392 }
nico@210: 00393 
etisserant@240: 00404 const indextable * scanIndexOD (CO_Data* d, UNS16 wIndex, UNS32 *errorCode, ODCallback_t **Callback)
nico@210: 00405 {
etisserant@240: 00406   return (*d->scanIndexOD)(wIndex, errorCode, Callback);
nico@210: 00407 }
nico@210: 00408 
etisserant@240: 00419 UNS32 RegisterSetODentryCallBack(CO_Data* d, UNS16 wIndex, UNS8 bSubindex, ODCallback_t Callback)
nico@210: 00420 {
etisserant@240: 00421   UNS32 errorCode;
etisserant@240: 00422   ODCallback_t *CallbackList;
nico@210: 00423 
etisserant@240: 00424   scanIndexOD (d, wIndex, &errorCode, &CallbackList);
etisserant@240: 00425   if(errorCode == OD_SUCCESSFUL && CallbackList)
nico@210: 00426     CallbackList[bSubindex] = Callback;
nico@210: 00427   return errorCode;
nico@210: 00428 }
nico@210: 00429 
etisserant@240: 00436 void _storeODSubIndex (UNS16 wIndex, UNS8 bSubindex){}
etisserant@240: