nico@207: nico@207:
nico@215: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: