2 This file is part of CanFestival, a library implementing CanOpen Stack.
4 Copyright (C): Edouard TISSERANT and Francis DUPIN
6 See COPYING file for copyrights details.
8 This library is free software; you can redistribute it and/or
9 modify it under the terms of the GNU Lesser General Public
10 License as published by the Free Software Foundation; either
11 version 2.1 of the License, or (at your option) any later version.
13 This library is distributed in the hope that it will be useful,
14 but WITHOUT ANY WARRANTY; without even the implied warranty of
15 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
16 Lesser General Public License for more details.
18 You should have received a copy of the GNU Lesser General Public
19 License along with this library; if not, write to the Free Software
20 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
23 /* #define DEBUG_WAR_CONSOLE_ON */
24 /* #define DEBUG_ERR_CONSOLE_ON */
30 #ifdef DEBUG_WAR_CONSOLE_ON
31 UNS8 accessDictionaryError(UNS16 index, UNS8 subIndex,
32 UNS8 sizeDataDict, UNS8 sizeDataGiven, UNS32 code)
34 MSG_WAR(0x2B09,"Dictionary index : ", index);
35 MSG_WAR(0X2B10," subindex : ", subIndex);
37 case OD_NO_SUCH_OBJECT:
38 MSG_WAR(0x2B11,"Index not found ", index);
40 case OD_NO_SUCH_SUBINDEX :
41 MSG_WAR(0x2B12,"SubIndex not found ", subIndex);
43 case OD_WRITE_NOT_ALLOWED :
44 MSG_WAR(0x2B13,"Write not allowed, data is read only ", index);
46 case OD_LENGTH_DATA_INVALID :
47 MSG_WAR(0x2B14,"Conflict size data. Should be (bytes) : ", sizeDataDict);
48 MSG_WAR(0x2B15,"But you have given the size : ", sizeDataGiven);
50 case OD_NOT_MAPPABLE :
51 MSG_WAR(0x2B16,"Not mappable data in a PDO at index : ", index);
53 case OD_VALUE_TOO_LOW :
54 MSG_WAR(0x2B17,"Value range error : value too low. SDOabort : ", code);
56 case OD_VALUE_TOO_HIGH :
57 MSG_WAR(0x2B18,"Value range error : value too high. SDOabort : ", code);
60 MSG_WAR(0x2B20, "Unknown error code : ", code);
66 UNS32 getODentry( CO_Data* d,
73 { /* DO NOT USE MSG_ERR because the macro may send a PDO -> infinite loop if it fails. */
76 const indextable *ptrTable;
77 ODCallback_t *Callback;
79 ptrTable = (*d->scanIndexOD)(wIndex, &errorCode, &Callback);
81 if (errorCode != OD_SUCCESSFUL)
83 if( ptrTable->bSubCount <= bSubindex ) {
84 /* Subindex not found */
85 accessDictionaryError(wIndex, bSubindex, 0, 0, OD_NO_SUCH_SUBINDEX);
86 return OD_NO_SUCH_SUBINDEX;
89 if (checkAccess && !(ptrTable->pSubindex[bSubindex].bAccessType & WO)) {
90 MSG_WAR(0x2B30, "Access Type : ", ptrTable->pSubindex[bSubindex].bAccessType);
91 accessDictionaryError(wIndex, bSubindex, 0, 0, OD_WRITE_NOT_ALLOWED);
92 return OD_READ_NOT_ALLOWED;
95 *pDataType = ptrTable->pSubindex[bSubindex].bDataType;
96 szData = ptrTable->pSubindex[bSubindex].size;
98 if( *pExpectedSize == 0 ||
99 *pExpectedSize == szData ||
100 (*pDataType == visible_string && *pExpectedSize < szData)) {/* We allow to fetch a shorter string than expected */
102 # ifdef CANOPEN_BIG_ENDIAN
103 if(*pDataType > boolean && *pDataType < visible_string) {
104 /* data must be transmited with low byte first */
106 MSG_WAR(boolean, "data type ", *pDataType);
107 MSG_WAR(visible_string, "data type ", *pDataType);
108 for ( i = szData ; i > 0 ; i--) {
110 ((UNS8*)pDestData)[j++] =
111 ((UNS8*)ptrTable->pSubindex[bSubindex].pObject)[i-1];
114 else /* It it is a visible string no endianisation to perform */
115 memcpy(pDestData, ptrTable->pSubindex[bSubindex].pObject,szData);
117 memcpy(pDestData, ptrTable->pSubindex[bSubindex].pObject,szData);
120 *pExpectedSize = szData;
122 /* Me laisser ça, please ! (FD) */
125 for (i = 0 ; i < 10 ; i++) {
126 MSG_WAR(*pExpectedSize, "dic data= ",
127 *(UNS8 *)(ptrTable->pSubindex[bSubindex].pObject + i));
132 return OD_SUCCESSFUL;
135 *pExpectedSize = szData;
136 accessDictionaryError(wIndex, bSubindex, szData,
137 *pExpectedSize, OD_LENGTH_DATA_INVALID);
138 return OD_LENGTH_DATA_INVALID;
142 UNS32 setODentry( CO_Data* d,
146 UNS8 * pExpectedSize,
152 const indextable *ptrTable;
153 ODCallback_t *Callback;
155 ptrTable =(*d->scanIndexOD)(wIndex, &errorCode, &Callback);
156 if (errorCode != OD_SUCCESSFUL)
159 if( ptrTable->bSubCount <= bSubindex ) {
160 /* Subindex not found */
161 accessDictionaryError(wIndex, bSubindex, 0, *pExpectedSize, OD_NO_SUCH_SUBINDEX);
162 return OD_NO_SUCH_SUBINDEX;
164 if (checkAccess && (ptrTable->pSubindex[bSubindex].bAccessType == RO)) {
165 MSG_WAR(0x2B25, "Access Type : ", ptrTable->pSubindex[bSubindex].bAccessType);
166 accessDictionaryError(wIndex, bSubindex, 0, *pExpectedSize, OD_WRITE_NOT_ALLOWED);
167 return OD_WRITE_NOT_ALLOWED;
171 dataType = ptrTable->pSubindex[bSubindex].bDataType;
172 szData = ptrTable->pSubindex[bSubindex].size;
174 if( *pExpectedSize == 0 ||
175 *pExpectedSize == szData ||
176 (dataType == visible_string && *pExpectedSize < szData)) /* We allow to store a shorter string than entry size */
178 #ifdef CANOPEN_BIG_ENDIAN
179 if(dataType > boolean && dataType < visible_string)
181 /* we invert the data source directly. This let us do range testing without */
182 /* additional temp variable */
184 for ( i = 0 ; i < ( ptrTable->pSubindex[bSubindex].size >> 1) ; i++)
186 UNS8 tmp =((UNS8 *)pSourceData) [(ptrTable->pSubindex[bSubindex].size - 1) - i];
187 ((UNS8 *)pSourceData) [(ptrTable->pSubindex[bSubindex].size - 1) - i] = ((UNS8 *)pSourceData)[i];
188 ((UNS8 *)pSourceData)[i] = tmp;
192 errorCode = (*d->valueRangeTest)(dataType, pSourceData);
194 accessDictionaryError(wIndex, bSubindex, szData, *pExpectedSize, errorCode);
197 memcpy(ptrTable->pSubindex[bSubindex].pObject,pSourceData, *pExpectedSize);
198 *pExpectedSize = szData;
201 if(Callback && Callback[bSubindex]){
202 (*Callback[bSubindex])(d, ptrTable, bSubindex);
205 /* TODO : Store dans NVRAM */
206 /* if (ptrTable->pSubindex[bSubindex].bAccessType & TO_BE_SAVED) */
207 return OD_SUCCESSFUL;
209 *pExpectedSize = szData;
210 accessDictionaryError(wIndex, bSubindex, szData, *pExpectedSize, OD_LENGTH_DATA_INVALID);
211 return OD_LENGTH_DATA_INVALID;
216 const indextable * scanIndexOD (CO_Data* d, UNS16 wIndex, UNS32 *errorCode, ODCallback_t **Callback)
218 return (*d->scanIndexOD)(wIndex, errorCode, Callback);
221 UNS32 RegisterSetODentryCallBack(CO_Data* d, UNS16 wIndex, UNS8 bSubindex, ODCallback_t Callback)
224 ODCallback_t *CallbackList;
226 scanIndexOD (d, wIndex, &errorCode, &CallbackList);
227 if(errorCode == OD_SUCCESSFUL && CallbackList)
228 CallbackList[bSubindex] = Callback;