108 MSG_WAR(0x2B30, "Access Type : ", ptrTable->pSubindex[bSubindex].bAccessType); |
108 MSG_WAR(0x2B30, "Access Type : ", ptrTable->pSubindex[bSubindex].bAccessType); |
109 accessDictionaryError(wIndex, bSubindex, 0, 0, OD_READ_NOT_ALLOWED); |
109 accessDictionaryError(wIndex, bSubindex, 0, 0, OD_READ_NOT_ALLOWED); |
110 return OD_READ_NOT_ALLOWED; |
110 return OD_READ_NOT_ALLOWED; |
111 } |
111 } |
112 |
112 |
|
113 if (pDestData == 0) { |
|
114 return SDOABT_GENERAL_ERROR; |
|
115 } |
|
116 |
|
117 if (ptrTable->pSubindex[bSubindex].size > *pExpectedSize) { |
|
118 /* Requested variable is too large to fit into a transfer line, inform * |
|
119 * the caller about the real size of the requested variable. */ |
|
120 *pExpectedSize = ptrTable->pSubindex[bSubindex].size; |
|
121 return SDOABT_OUT_OF_MEMORY; |
|
122 } |
|
123 |
113 *pDataType = ptrTable->pSubindex[bSubindex].bDataType; |
124 *pDataType = ptrTable->pSubindex[bSubindex].bDataType; |
114 szData = ptrTable->pSubindex[bSubindex].size; |
125 szData = ptrTable->pSubindex[bSubindex].size; |
115 |
126 |
116 if(*pExpectedSize == 0 || |
|
117 *pExpectedSize == szData || |
|
118 /* allow to fetch a shorter string than expected */ |
|
119 (*pDataType >= visible_string && *pExpectedSize < szData)) { |
|
120 |
|
121 # ifdef CANOPEN_BIG_ENDIAN |
127 # ifdef CANOPEN_BIG_ENDIAN |
122 if(endianize && *pDataType > boolean && !( |
128 if(endianize && *pDataType > boolean && !( |
123 *pDataType >= visible_string && |
129 *pDataType >= visible_string && |
124 *pDataType <= domain)) { |
130 *pDataType <= domain)) { |
125 /* data must be transmited with low byte first */ |
131 /* data must be transmited with low byte first */ |
126 UNS8 i, j = 0; |
132 UNS8 i, j = 0; |
127 MSG_WAR(boolean, "data type ", *pDataType); |
133 MSG_WAR(boolean, "data type ", *pDataType); |
128 MSG_WAR(visible_string, "data type ", *pDataType); |
134 MSG_WAR(visible_string, "data type ", *pDataType); |
129 for ( i = szData ; i > 0 ; i--) { |
135 for ( i = szData ; i > 0 ; i--) { |
130 MSG_WAR(i," ", j); |
136 MSG_WAR(i," ", j); |
131 ((UNS8*)pDestData)[j++] = |
137 ((UNS8*)pDestData)[j++] = |
132 ((UNS8*)ptrTable->pSubindex[bSubindex].pObject)[i-1]; |
138 ((UNS8*)ptrTable->pSubindex[bSubindex].pObject)[i-1]; |
|
139 } |
|
140 *pExpectedSize = szData; |
|
141 } |
|
142 else /* no endianisation change */ |
|
143 # endif |
|
144 |
|
145 if(*pDataType != visible_string) { |
|
146 memcpy(pDestData, ptrTable->pSubindex[bSubindex].pObject,szData); |
|
147 *pExpectedSize = szData; |
|
148 }else{ |
|
149 /* TODO : CONFORM TO DS-301 : |
|
150 * - stop using NULL terminated strings |
|
151 * - store string size in td_subindex |
|
152 * */ |
|
153 /* Copy null terminated string to user, and return discovered size */ |
|
154 UNS8 *ptr = (UNS8*)ptrTable->pSubindex[bSubindex].pObject; |
|
155 UNS8 *ptr_start = ptr; |
|
156 /* *pExpectedSize IS < szData . if null, use szData */ |
|
157 UNS8 *ptr_end = ptr + (*pExpectedSize ? *pExpectedSize : szData) ; |
|
158 UNS8 *ptr_dest = (UNS8*)pDestData; |
|
159 while( *ptr && ptr < ptr_end){ |
|
160 *(ptr_dest++) = *(ptr++); |
133 } |
161 } |
134 *pExpectedSize = szData; |
162 |
135 } |
163 *pExpectedSize = (UNS32) (ptr - ptr_start); |
136 else /* no endianisation change */ |
164 /* terminate string if not maximum length */ |
137 # endif |
165 if (*pExpectedSize < szData) |
138 if(*pDataType != visible_string) { |
166 *(ptr) = 0; |
139 memcpy(pDestData, ptrTable->pSubindex[bSubindex].pObject,szData); |
167 } |
140 *pExpectedSize = szData; |
168 |
141 }else{ |
169 return OD_SUCCESSFUL; |
142 /* TODO : CONFORM TO DS-301 : |
|
143 * - stop using NULL terminated strings |
|
144 * - store string size in td_subindex |
|
145 * */ |
|
146 /* Copy null terminated string to user, and return discovered size */ |
|
147 UNS8 *ptr = (UNS8*)ptrTable->pSubindex[bSubindex].pObject; |
|
148 UNS8 *ptr_start = ptr; |
|
149 /* *pExpectedSize IS < szData . if null, use szData */ |
|
150 UNS8 *ptr_end = ptr + (*pExpectedSize ? *pExpectedSize : szData) ; |
|
151 UNS8 *ptr_dest = (UNS8*)pDestData; |
|
152 while( *ptr && ptr < ptr_end){ |
|
153 *(ptr_dest++) = *(ptr++); |
|
154 } |
|
155 |
|
156 *pExpectedSize = (UNS32) (ptr - ptr_start); |
|
157 /* terminate string if not maximum length */ |
|
158 if (*pExpectedSize < szData) |
|
159 *(ptr) = 0; |
|
160 } |
|
161 |
|
162 return OD_SUCCESSFUL; |
|
163 } |
|
164 else { /* Error ! */ |
|
165 *pExpectedSize = szData; |
|
166 accessDictionaryError(wIndex, bSubindex, szData, |
|
167 *pExpectedSize, OD_LENGTH_DATA_INVALID); |
|
168 return OD_LENGTH_DATA_INVALID; |
|
169 } |
|
170 } |
170 } |
171 |
171 |
172 UNS32 _setODentry( CO_Data* d, |
172 UNS32 _setODentry( CO_Data* d, |
173 UNS16 wIndex, |
173 UNS16 wIndex, |
174 UNS8 bSubindex, |
174 UNS8 bSubindex, |