|
0
|
1 |
/*
|
|
|
2 |
This file is part of CanFestival, a library implementing CanOpen Stack.
|
|
|
3 |
|
|
|
4 |
Copyright (C): Edouard TISSERANT and Francis DUPIN
|
|
|
5 |
|
|
|
6 |
See COPYING file for copyrights details.
|
|
|
7 |
|
|
|
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.
|
|
|
12 |
|
|
|
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.
|
|
|
17 |
|
|
|
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
|
|
|
21 |
*/
|
|
|
22 |
|
|
|
23 |
#include "objacces.h"
|
|
|
24 |
|
|
|
25 |
#ifdef DEBUG_WAR_CONSOLE_ON
|
|
|
26 |
UNS8 accessDictionaryError(UNS16 index, UNS8 subIndex,
|
|
|
27 |
UNS8 sizeDataDict, UNS8 sizeDataGiven, UNS32 code)
|
|
|
28 |
{
|
|
|
29 |
MSG_WAR(0x2B09,"Dictionary index : ", index);
|
|
|
30 |
MSG_WAR(0X2B10," subindex : ", subIndex);
|
|
|
31 |
switch (code) {
|
|
|
32 |
case OD_NO_SUCH_OBJECT:
|
|
|
33 |
MSG_WAR(0x2B11,"Index not found ", index);
|
|
|
34 |
break;
|
|
|
35 |
case OD_NO_SUCH_SUBINDEX :
|
|
|
36 |
MSG_WAR(0x2B12,"SubIndex not found ", subIndex);
|
|
|
37 |
break;
|
|
|
38 |
case OD_WRITE_NOT_ALLOWED :
|
|
|
39 |
MSG_WAR(0x2B13,"Write not allowed, data is read only ", index);
|
|
|
40 |
break;
|
|
|
41 |
case OD_LENGTH_DATA_INVALID :
|
|
|
42 |
MSG_WAR(0x2B14,"Conflict size data. Should be (bytes) : ", sizeDataDict);
|
|
|
43 |
MSG_WAR(0x2B15,"But you have given the size : ", sizeDataGiven);
|
|
|
44 |
break;
|
|
|
45 |
case OD_NOT_MAPPABLE :
|
|
|
46 |
MSG_WAR(0x2B16,"Not mappable data in a PDO at index : ", index);
|
|
|
47 |
break;
|
|
|
48 |
case OD_VALUE_TOO_LOW :
|
|
|
49 |
MSG_WAR(0x2B17,"Value range error : value too low. SDOabort : ", code);
|
|
|
50 |
break;
|
|
|
51 |
case OD_VALUE_TOO_HIGH :
|
|
|
52 |
MSG_WAR(0x2B18,"Value range error : value too high. SDOabort : ", code);
|
|
|
53 |
break;
|
|
|
54 |
default :
|
|
|
55 |
MSG_WAR(0x2B20, "Unknown error code : ", code);
|
|
|
56 |
}
|
|
|
57 |
return 0;
|
|
|
58 |
}
|
|
|
59 |
#endif
|
|
|
60 |
|
|
|
61 |
UNS32 getODentry( CO_Data* d,
|
|
|
62 |
UNS16 wIndex,
|
|
|
63 |
UNS8 bSubindex,
|
|
|
64 |
void * pDestData,
|
|
|
65 |
UNS8 * pExpectedSize,
|
|
|
66 |
UNS8 * pDataType,
|
|
|
67 |
UNS8 checkAccess)
|
|
|
68 |
{ // DO NOT USE MSG_ERR because the macro may send a PDO -> infinite loop if it fails.
|
|
|
69 |
UNS32 errorCode;
|
|
|
70 |
UNS8 szData;
|
|
|
71 |
const indextable *ptrTable;
|
|
|
72 |
ODCallback_t *Callback;
|
|
|
73 |
|
|
|
74 |
ptrTable = (*d->scanIndexOD)(wIndex, &errorCode, &Callback);
|
|
|
75 |
|
|
|
76 |
if (errorCode != OD_SUCCESSFUL)
|
|
|
77 |
return errorCode;
|
|
|
78 |
if( ptrTable->bSubCount <= bSubindex ) {
|
|
|
79 |
// Subindex not found
|
|
|
80 |
accessDictionaryError(wIndex, bSubindex, 0, 0, OD_NO_SUCH_SUBINDEX);
|
|
|
81 |
return OD_NO_SUCH_SUBINDEX;
|
|
|
82 |
}
|
|
|
83 |
|
|
|
84 |
if (checkAccess && !(ptrTable->pSubindex[bSubindex].bAccessType & WO)) {
|
|
|
85 |
accessDictionaryError(wIndex, bSubindex, 0, 0, OD_WRITE_NOT_ALLOWED);
|
|
|
86 |
return OD_READ_NOT_ALLOWED;
|
|
|
87 |
}
|
|
|
88 |
|
|
|
89 |
*pDataType = ptrTable->pSubindex[bSubindex].bDataType;
|
|
|
90 |
szData = ptrTable->pSubindex[bSubindex].size;
|
|
|
91 |
|
|
|
92 |
if( *pExpectedSize == 0 ||
|
|
|
93 |
*pExpectedSize == szData ||
|
|
|
94 |
(*pDataType == visible_string && *pExpectedSize > szData)) // We allow to fetch a shorter string than expected
|
|
|
95 |
{
|
|
|
96 |
#ifdef CANOPEN_BIG_ENDIAN
|
|
|
97 |
{
|
|
|
98 |
// data must be transmited with low byte first
|
|
|
99 |
UNS8 i, j = 0;
|
|
|
100 |
for ( i = ptrTable->pSubindex[bSubindex].size - 1 ; i >= 0 ; i--) {
|
|
|
101 |
((char*)pDestData)[j++] = ((char*)ptrTable->pSubindex[bSubindex].pObject)[i];
|
|
|
102 |
}
|
|
|
103 |
}
|
|
|
104 |
#else
|
|
|
105 |
memcpy(pDestData, ptrTable->pSubindex[bSubindex].pObject,*pExpectedSize);
|
|
|
106 |
#endif
|
|
|
107 |
*pExpectedSize = szData;
|
|
|
108 |
return OD_SUCCESSFUL;
|
|
|
109 |
}else{
|
|
|
110 |
*pExpectedSize = szData;
|
|
|
111 |
accessDictionaryError(wIndex, bSubindex, szData, *pExpectedSize, OD_LENGTH_DATA_INVALID);
|
|
|
112 |
return OD_LENGTH_DATA_INVALID;
|
|
|
113 |
}
|
|
|
114 |
}
|
|
|
115 |
|
|
|
116 |
UNS32 setODentry( CO_Data* d,
|
|
|
117 |
UNS16 wIndex,
|
|
|
118 |
UNS8 bSubindex,
|
|
|
119 |
void * pSourceData,
|
|
|
120 |
UNS8 * pExpectedSize,
|
|
|
121 |
UNS8 checkAccess)
|
|
|
122 |
{
|
|
|
123 |
UNS8 szData;
|
|
|
124 |
UNS8 dataType;
|
|
|
125 |
UNS32 errorCode;
|
|
|
126 |
const indextable *ptrTable;
|
|
|
127 |
ODCallback_t *Callback;
|
|
|
128 |
|
|
|
129 |
ptrTable =(*d->scanIndexOD)(wIndex, &errorCode, &Callback);
|
|
|
130 |
if (errorCode != OD_SUCCESSFUL)
|
|
|
131 |
return errorCode;
|
|
|
132 |
|
|
|
133 |
if( ptrTable->bSubCount <= bSubindex ) {
|
|
|
134 |
// Subindex not found
|
|
|
135 |
accessDictionaryError(wIndex, bSubindex, 0, *pExpectedSize, OD_NO_SUCH_SUBINDEX);
|
|
|
136 |
return OD_NO_SUCH_SUBINDEX;
|
|
|
137 |
}
|
|
|
138 |
if (checkAccess && !(ptrTable->pSubindex[bSubindex].bAccessType & RO)) {
|
|
|
139 |
accessDictionaryError(wIndex, bSubindex, 0, *pExpectedSize, OD_WRITE_NOT_ALLOWED);
|
|
|
140 |
return OD_WRITE_NOT_ALLOWED;
|
|
|
141 |
}
|
|
|
142 |
|
|
|
143 |
|
|
|
144 |
dataType = ptrTable->pSubindex[bSubindex].bDataType;
|
|
|
145 |
szData = ptrTable->pSubindex[bSubindex].size;
|
|
|
146 |
|
|
|
147 |
if( *pExpectedSize == 0 ||
|
|
|
148 |
*pExpectedSize == szData ||
|
|
|
149 |
(dataType == visible_string && *pExpectedSize < szData)) // We allow to store a shorter string than entry size
|
|
|
150 |
{
|
|
|
151 |
errorCode = (*d->valueRangeTest)(dataType, pSourceData);
|
|
|
152 |
if (errorCode) {
|
|
|
153 |
accessDictionaryError(wIndex, bSubindex, szData, *pExpectedSize, errorCode);
|
|
|
154 |
return errorCode;
|
|
|
155 |
}
|
|
|
156 |
#ifdef CANOPEN_BIG_ENDIAN
|
|
|
157 |
{
|
|
|
158 |
// data must be transmited with low byte first
|
|
|
159 |
UNS8 i, j = 0;
|
|
|
160 |
for ( i = ptrTable->pSubindex[bSubindex].size - 1 ; i >= 0 ; i--) {
|
|
|
161 |
((char*)ptrTable->pSubindex[bSubindex].pObject)[i] = ((char*)pSourceData)[j++];
|
|
|
162 |
}
|
|
|
163 |
}
|
|
|
164 |
#else
|
|
|
165 |
memcpy(ptrTable->pSubindex[bSubindex].pObject,pSourceData, *pExpectedSize);
|
|
|
166 |
#endif
|
|
|
167 |
*pExpectedSize = szData;
|
|
|
168 |
|
|
|
169 |
// Callbacks
|
|
|
170 |
if(Callback && Callback[bSubindex]){
|
|
|
171 |
(*Callback[bSubindex])(d, ptrTable, bSubindex);
|
|
|
172 |
}
|
|
|
173 |
|
|
|
174 |
// TODO : Store dans NVRAM
|
|
|
175 |
// if (ptrTable->pSubindex[bSubindex].bAccessType & TO_BE_SAVED)
|
|
|
176 |
return OD_SUCCESSFUL;
|
|
|
177 |
}else{
|
|
|
178 |
*pExpectedSize = szData;
|
|
|
179 |
accessDictionaryError(wIndex, bSubindex, szData, *pExpectedSize, OD_LENGTH_DATA_INVALID);
|
|
|
180 |
return OD_LENGTH_DATA_INVALID;
|
|
|
181 |
}
|
|
|
182 |
}
|
|
|
183 |
|
|
|
184 |
|
|
|
185 |
const indextable * scanIndexOD (CO_Data* d, UNS16 wIndex, UNS32 *errorCode, ODCallback_t **Callback)
|
|
|
186 |
{
|
|
|
187 |
return (*d->scanIndexOD)(wIndex, errorCode, Callback);
|
|
|
188 |
}
|
|
|
189 |
|
|
|
190 |
UNS32 RegisterSetODentryCallBack(CO_Data* d, UNS16 wIndex, UNS8 bSubindex, ODCallback_t Callback)
|
|
|
191 |
{
|
|
|
192 |
UNS32 errorCode;
|
|
|
193 |
ODCallback_t *CallbackList;
|
|
|
194 |
|
|
|
195 |
scanIndexOD (d, wIndex, &errorCode, &CallbackList);
|
|
|
196 |
if(errorCode == OD_SUCCESSFUL && CallbackList)
|
|
|
197 |
CallbackList[bSubindex] = Callback;
|
|
|
198 |
return errorCode;
|
|
|
199 |
}
|
|
|
200 |
|