|
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 |