objacces.c

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