18 You should have received a copy of the GNU Lesser General Public |
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 |
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 |
20 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA |
21 */ |
21 */ |
22 |
22 |
23 //#define DEBUG_WAR_CONSOLE_ON |
23 /* #define DEBUG_WAR_CONSOLE_ON */ |
24 //#define DEBUG_ERR_CONSOLE_ON |
24 /* #define DEBUG_ERR_CONSOLE_ON */ |
25 |
25 |
26 |
26 |
27 #include "objacces.h" |
27 #include "objacces.h" |
28 |
28 |
29 |
29 |
68 UNS8 bSubindex, |
68 UNS8 bSubindex, |
69 void * pDestData, |
69 void * pDestData, |
70 UNS8 * pExpectedSize, |
70 UNS8 * pExpectedSize, |
71 UNS8 * pDataType, |
71 UNS8 * pDataType, |
72 UNS8 checkAccess) |
72 UNS8 checkAccess) |
73 { // DO NOT USE MSG_ERR because the macro may send a PDO -> infinite loop if it fails. |
73 { /* DO NOT USE MSG_ERR because the macro may send a PDO -> infinite loop if it fails. */ |
74 UNS32 errorCode; |
74 UNS32 errorCode; |
75 UNS8 szData; |
75 UNS8 szData; |
76 const indextable *ptrTable; |
76 const indextable *ptrTable; |
77 ODCallback_t *Callback; |
77 ODCallback_t *Callback; |
78 |
78 |
79 ptrTable = (*d->scanIndexOD)(wIndex, &errorCode, &Callback); |
79 ptrTable = (*d->scanIndexOD)(wIndex, &errorCode, &Callback); |
80 |
80 |
81 if (errorCode != OD_SUCCESSFUL) |
81 if (errorCode != OD_SUCCESSFUL) |
82 return errorCode; |
82 return errorCode; |
83 if( ptrTable->bSubCount <= bSubindex ) { |
83 if( ptrTable->bSubCount <= bSubindex ) { |
84 // Subindex not found |
84 /* Subindex not found */ |
85 accessDictionaryError(wIndex, bSubindex, 0, 0, OD_NO_SUCH_SUBINDEX); |
85 accessDictionaryError(wIndex, bSubindex, 0, 0, OD_NO_SUCH_SUBINDEX); |
86 return OD_NO_SUCH_SUBINDEX; |
86 return OD_NO_SUCH_SUBINDEX; |
87 } |
87 } |
88 |
88 |
89 if (checkAccess && !(ptrTable->pSubindex[bSubindex].bAccessType & WO)) { |
89 if (checkAccess && !(ptrTable->pSubindex[bSubindex].bAccessType & WO)) { |
95 *pDataType = ptrTable->pSubindex[bSubindex].bDataType; |
95 *pDataType = ptrTable->pSubindex[bSubindex].bDataType; |
96 szData = ptrTable->pSubindex[bSubindex].size; |
96 szData = ptrTable->pSubindex[bSubindex].size; |
97 |
97 |
98 if( *pExpectedSize == 0 || |
98 if( *pExpectedSize == 0 || |
99 *pExpectedSize == szData || |
99 *pExpectedSize == szData || |
100 (*pDataType == visible_string && *pExpectedSize < szData)) {// We allow to fetch a shorter string than expected |
100 (*pDataType == visible_string && *pExpectedSize < szData)) {/* We allow to fetch a shorter string than expected */ |
101 |
101 |
102 # ifdef CANOPEN_BIG_ENDIAN |
102 # ifdef CANOPEN_BIG_ENDIAN |
103 if(*pDataType > boolean && *pDataType < visible_string) { |
103 if(*pDataType > boolean && *pDataType < visible_string) { |
104 // data must be transmited with low byte first |
104 /* data must be transmited with low byte first */ |
105 UNS8 i, j = 0; |
105 UNS8 i, j = 0; |
106 MSG_WAR(boolean, "data type ", *pDataType); |
106 MSG_WAR(boolean, "data type ", *pDataType); |
107 MSG_WAR(visible_string, "data type ", *pDataType); |
107 MSG_WAR(visible_string, "data type ", *pDataType); |
108 for ( i = szData ; i > 0 ; i--) { |
108 for ( i = szData ; i > 0 ; i--) { |
109 MSG_WAR(i," ", j); |
109 MSG_WAR(i," ", j); |
110 ((UNS8*)pDestData)[j++] = |
110 ((UNS8*)pDestData)[j++] = |
111 ((UNS8*)ptrTable->pSubindex[bSubindex].pObject)[i-1]; |
111 ((UNS8*)ptrTable->pSubindex[bSubindex].pObject)[i-1]; |
112 } |
112 } |
113 } |
113 } |
114 else // It it is a visible string no endianisation to perform |
114 else /* It it is a visible string no endianisation to perform */ |
115 memcpy(pDestData, ptrTable->pSubindex[bSubindex].pObject,szData); |
115 memcpy(pDestData, ptrTable->pSubindex[bSubindex].pObject,szData); |
116 # else |
116 # else |
117 memcpy(pDestData, ptrTable->pSubindex[bSubindex].pObject,szData); |
117 memcpy(pDestData, ptrTable->pSubindex[bSubindex].pObject,szData); |
118 # endif |
118 # endif |
119 |
119 |
120 *pExpectedSize = szData; |
120 *pExpectedSize = szData; |
121 #if 0 |
121 #if 0 |
122 // Me laisser ça, please ! (FD) |
122 /* Me laisser ça, please ! (FD) */ |
123 { |
123 { |
124 UNS8 i; |
124 UNS8 i; |
125 for (i = 0 ; i < 10 ; i++) { |
125 for (i = 0 ; i < 10 ; i++) { |
126 MSG_WAR(*pExpectedSize, "dic data= ", |
126 MSG_WAR(*pExpectedSize, "dic data= ", |
127 *(UNS8 *)(ptrTable->pSubindex[bSubindex].pObject + i)); |
127 *(UNS8 *)(ptrTable->pSubindex[bSubindex].pObject + i)); |
129 |
129 |
130 } |
130 } |
131 #endif |
131 #endif |
132 return OD_SUCCESSFUL; |
132 return OD_SUCCESSFUL; |
133 } |
133 } |
134 else { // Error ! |
134 else { /* Error ! */ |
135 *pExpectedSize = szData; |
135 *pExpectedSize = szData; |
136 accessDictionaryError(wIndex, bSubindex, szData, |
136 accessDictionaryError(wIndex, bSubindex, szData, |
137 *pExpectedSize, OD_LENGTH_DATA_INVALID); |
137 *pExpectedSize, OD_LENGTH_DATA_INVALID); |
138 return OD_LENGTH_DATA_INVALID; |
138 return OD_LENGTH_DATA_INVALID; |
139 } |
139 } |
155 ptrTable =(*d->scanIndexOD)(wIndex, &errorCode, &Callback); |
155 ptrTable =(*d->scanIndexOD)(wIndex, &errorCode, &Callback); |
156 if (errorCode != OD_SUCCESSFUL) |
156 if (errorCode != OD_SUCCESSFUL) |
157 return errorCode; |
157 return errorCode; |
158 |
158 |
159 if( ptrTable->bSubCount <= bSubindex ) { |
159 if( ptrTable->bSubCount <= bSubindex ) { |
160 // Subindex not found |
160 /* Subindex not found */ |
161 accessDictionaryError(wIndex, bSubindex, 0, *pExpectedSize, OD_NO_SUCH_SUBINDEX); |
161 accessDictionaryError(wIndex, bSubindex, 0, *pExpectedSize, OD_NO_SUCH_SUBINDEX); |
162 return OD_NO_SUCH_SUBINDEX; |
162 return OD_NO_SUCH_SUBINDEX; |
163 } |
163 } |
164 if (checkAccess && (ptrTable->pSubindex[bSubindex].bAccessType == RO)) { |
164 if (checkAccess && (ptrTable->pSubindex[bSubindex].bAccessType == RO)) { |
165 MSG_WAR(0x2B25, "Access Type : ", ptrTable->pSubindex[bSubindex].bAccessType); |
165 MSG_WAR(0x2B25, "Access Type : ", ptrTable->pSubindex[bSubindex].bAccessType); |
171 dataType = ptrTable->pSubindex[bSubindex].bDataType; |
171 dataType = ptrTable->pSubindex[bSubindex].bDataType; |
172 szData = ptrTable->pSubindex[bSubindex].size; |
172 szData = ptrTable->pSubindex[bSubindex].size; |
173 |
173 |
174 if( *pExpectedSize == 0 || |
174 if( *pExpectedSize == 0 || |
175 *pExpectedSize == szData || |
175 *pExpectedSize == szData || |
176 (dataType == visible_string && *pExpectedSize < szData)) // We allow to store a shorter string than entry size |
176 (dataType == visible_string && *pExpectedSize < szData)) /* We allow to store a shorter string than entry size */ |
177 { |
177 { |
178 #ifdef CANOPEN_BIG_ENDIAN |
178 #ifdef CANOPEN_BIG_ENDIAN |
179 if(dataType > boolean && dataType < visible_string) |
179 if(dataType > boolean && dataType < visible_string) |
180 { |
180 { |
181 // we invert the data source directly. This let us do range testing without |
181 /* we invert the data source directly. This let us do range testing without */ |
182 // additional temp variable |
182 /* additional temp variable */ |
183 UNS8 i; |
183 UNS8 i; |
184 for ( i = 0 ; i < ( ptrTable->pSubindex[bSubindex].size >> 1) ; i++) |
184 for ( i = 0 ; i < ( ptrTable->pSubindex[bSubindex].size >> 1) ; i++) |
185 { |
185 { |
186 UNS8 tmp =((UNS8 *)pSourceData) [(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]; |
187 ((UNS8 *)pSourceData) [(ptrTable->pSubindex[bSubindex].size - 1) - i] = ((UNS8 *)pSourceData)[i]; |
195 return errorCode; |
195 return errorCode; |
196 } |
196 } |
197 memcpy(ptrTable->pSubindex[bSubindex].pObject,pSourceData, *pExpectedSize); |
197 memcpy(ptrTable->pSubindex[bSubindex].pObject,pSourceData, *pExpectedSize); |
198 *pExpectedSize = szData; |
198 *pExpectedSize = szData; |
199 |
199 |
200 // Callbacks |
200 /* Callbacks */ |
201 if(Callback && Callback[bSubindex]){ |
201 if(Callback && Callback[bSubindex]){ |
202 (*Callback[bSubindex])(d, ptrTable, bSubindex); |
202 (*Callback[bSubindex])(d, ptrTable, bSubindex); |
203 } |
203 } |
204 |
204 |
205 // TODO : Store dans NVRAM |
205 /* TODO : Store dans NVRAM */ |
206 // if (ptrTable->pSubindex[bSubindex].bAccessType & TO_BE_SAVED) |
206 /* if (ptrTable->pSubindex[bSubindex].bAccessType & TO_BE_SAVED) */ |
207 return OD_SUCCESSFUL; |
207 return OD_SUCCESSFUL; |
208 }else{ |
208 }else{ |
209 *pExpectedSize = szData; |
209 *pExpectedSize = szData; |
210 accessDictionaryError(wIndex, bSubindex, szData, *pExpectedSize, OD_LENGTH_DATA_INVALID); |
210 accessDictionaryError(wIndex, bSubindex, szData, *pExpectedSize, OD_LENGTH_DATA_INVALID); |
211 return OD_LENGTH_DATA_INVALID; |
211 return OD_LENGTH_DATA_INVALID; |