objacces.c

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