Sam Grove / Mbed 2 deprecated canopen_slavenode

CANfestival - an open source CANopen framework

Dependencies:   mbed

framework/src/objacces.c@0:6219434a0cb5, 2011-05-30 (annotated)

Committer:
sam_grove
Date:
Mon May 30 07:14:41 2011 +0000
Revision:
0:6219434a0cb5
Initial public release of slave node framework port

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sam_grove 0:6219434a0cb5 1 /*
sam_grove 0:6219434a0cb5 2 This file is part of CanFestival, a library implementing CanOpen Stack.
sam_grove 0:6219434a0cb5 3
sam_grove 0:6219434a0cb5 4 Copyright (C): Edouard TISSERANT and Francis DUPIN
sam_grove 0:6219434a0cb5 5
sam_grove 0:6219434a0cb5 6 See COPYING file for copyrights details.
sam_grove 0:6219434a0cb5 7
sam_grove 0:6219434a0cb5 8 This library is free software; you can redistribute it and/or
sam_grove 0:6219434a0cb5 9 modify it under the terms of the GNU Lesser General Public
sam_grove 0:6219434a0cb5 10 License as published by the Free Software Foundation; either
sam_grove 0:6219434a0cb5 11 version 2.1 of the License, or (at your option) any later version.
sam_grove 0:6219434a0cb5 12
sam_grove 0:6219434a0cb5 13 This library is distributed in the hope that it will be useful,
sam_grove 0:6219434a0cb5 14 but WITHOUT ANY WARRANTY; without even the implied warranty of
sam_grove 0:6219434a0cb5 15 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
sam_grove 0:6219434a0cb5 16 Lesser General Public License for more details.
sam_grove 0:6219434a0cb5 17
sam_grove 0:6219434a0cb5 18 You should have received a copy of the GNU Lesser General Public
sam_grove 0:6219434a0cb5 19 License along with this library; if not, write to the Free Software
sam_grove 0:6219434a0cb5 20 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307
sam_grove 0:6219434a0cb5 21 USA
sam_grove 0:6219434a0cb5 22 */
sam_grove 0:6219434a0cb5 23 /*!
sam_grove 0:6219434a0cb5 24 ** @file objacces.c
sam_grove 0:6219434a0cb5 25 ** @author Edouard TISSERANT and Francis DUPIN
sam_grove 0:6219434a0cb5 26 ** @date Tue Jun 5 08:55:23 2007
sam_grove 0:6219434a0cb5 27 **
sam_grove 0:6219434a0cb5 28 ** @brief
sam_grove 0:6219434a0cb5 29 **
sam_grove 0:6219434a0cb5 30 **
sam_grove 0:6219434a0cb5 31 */
sam_grove 0:6219434a0cb5 32
sam_grove 0:6219434a0cb5 33
sam_grove 0:6219434a0cb5 34
sam_grove 0:6219434a0cb5 35
sam_grove 0:6219434a0cb5 36 /* #define DEBUG_WAR_CONSOLE_ON */
sam_grove 0:6219434a0cb5 37 /* #define DEBUG_ERR_CONSOLE_ON */
sam_grove 0:6219434a0cb5 38
sam_grove 0:6219434a0cb5 39
sam_grove 0:6219434a0cb5 40 #include "data.h"
sam_grove 0:6219434a0cb5 41
sam_grove 0:6219434a0cb5 42 //We need the function implementation for linking
sam_grove 0:6219434a0cb5 43 //Only a placeholder with a define isnt enough!
sam_grove 0:6219434a0cb5 44 UNS8 accessDictionaryError(UNS16 index, UNS8 subIndex,
sam_grove 0:6219434a0cb5 45 UNS32 sizeDataDict, UNS32 sizeDataGiven, UNS32 code)
sam_grove 0:6219434a0cb5 46 {
sam_grove 0:6219434a0cb5 47 #ifdef DEBUG_WAR_CONSOLE_ON
sam_grove 0:6219434a0cb5 48 MSG_WAR(0x2B09,"Dictionary index : ", index);
sam_grove 0:6219434a0cb5 49 MSG_WAR(0X2B10," subindex : ", subIndex);
sam_grove 0:6219434a0cb5 50 switch (code) {
sam_grove 0:6219434a0cb5 51 case OD_NO_SUCH_OBJECT:
sam_grove 0:6219434a0cb5 52 MSG_WAR(0x2B11,"Index not found ", index);
sam_grove 0:6219434a0cb5 53 break;
sam_grove 0:6219434a0cb5 54 case OD_NO_SUCH_SUBINDEX :
sam_grove 0:6219434a0cb5 55 MSG_WAR(0x2B12,"SubIndex not found ", subIndex);
sam_grove 0:6219434a0cb5 56 break;
sam_grove 0:6219434a0cb5 57 case OD_WRITE_NOT_ALLOWED :
sam_grove 0:6219434a0cb5 58 MSG_WAR(0x2B13,"Write not allowed, data is read only ", index);
sam_grove 0:6219434a0cb5 59 break;
sam_grove 0:6219434a0cb5 60 case OD_LENGTH_DATA_INVALID :
sam_grove 0:6219434a0cb5 61 MSG_WAR(0x2B14,"Conflict size data. Should be (bytes) : ", sizeDataDict);
sam_grove 0:6219434a0cb5 62 MSG_WAR(0x2B15,"But you have given the size : ", sizeDataGiven);
sam_grove 0:6219434a0cb5 63 break;
sam_grove 0:6219434a0cb5 64 case OD_NOT_MAPPABLE :
sam_grove 0:6219434a0cb5 65 MSG_WAR(0x2B16,"Not mappable data in a PDO at index : ", index);
sam_grove 0:6219434a0cb5 66 break;
sam_grove 0:6219434a0cb5 67 case OD_VALUE_TOO_LOW :
sam_grove 0:6219434a0cb5 68 MSG_WAR(0x2B17,"Value range error : value too low. SDOabort : ", code);
sam_grove 0:6219434a0cb5 69 break;
sam_grove 0:6219434a0cb5 70 case OD_VALUE_TOO_HIGH :
sam_grove 0:6219434a0cb5 71 MSG_WAR(0x2B18,"Value range error : value too high. SDOabort : ", code);
sam_grove 0:6219434a0cb5 72 break;
sam_grove 0:6219434a0cb5 73 default :
sam_grove 0:6219434a0cb5 74 MSG_WAR(0x2B20, "Unknown error code : ", code);
sam_grove 0:6219434a0cb5 75 }
sam_grove 0:6219434a0cb5 76 #endif
sam_grove 0:6219434a0cb5 77
sam_grove 0:6219434a0cb5 78 return 0;
sam_grove 0:6219434a0cb5 79 }
sam_grove 0:6219434a0cb5 80
sam_grove 0:6219434a0cb5 81 UNS32 _getODentry( CO_Data* d,
sam_grove 0:6219434a0cb5 82 UNS16 wIndex,
sam_grove 0:6219434a0cb5 83 UNS8 bSubindex,
sam_grove 0:6219434a0cb5 84 void * pDestData,
sam_grove 0:6219434a0cb5 85 UNS32 * pExpectedSize,
sam_grove 0:6219434a0cb5 86 UNS8 * pDataType,
sam_grove 0:6219434a0cb5 87 UNS8 checkAccess,
sam_grove 0:6219434a0cb5 88 UNS8 endianize)
sam_grove 0:6219434a0cb5 89 { /* DO NOT USE MSG_ERR because the macro may send a PDO -> infinite
sam_grove 0:6219434a0cb5 90 loop if it fails. */
sam_grove 0:6219434a0cb5 91 UNS32 errorCode;
sam_grove 0:6219434a0cb5 92 UNS32 szData;
sam_grove 0:6219434a0cb5 93 const indextable *ptrTable;
sam_grove 0:6219434a0cb5 94 ODCallback_t *Callback;
sam_grove 0:6219434a0cb5 95
sam_grove 0:6219434a0cb5 96 ptrTable = (*d->scanIndexOD)(wIndex, &errorCode, &Callback);
sam_grove 0:6219434a0cb5 97
sam_grove 0:6219434a0cb5 98 if (errorCode != OD_SUCCESSFUL)
sam_grove 0:6219434a0cb5 99 return errorCode;
sam_grove 0:6219434a0cb5 100 if( ptrTable->bSubCount <= bSubindex ) {
sam_grove 0:6219434a0cb5 101 /* Subindex not found */
sam_grove 0:6219434a0cb5 102 accessDictionaryError(wIndex, bSubindex, 0, 0, OD_NO_SUCH_SUBINDEX);
sam_grove 0:6219434a0cb5 103 return OD_NO_SUCH_SUBINDEX;
sam_grove 0:6219434a0cb5 104 }
sam_grove 0:6219434a0cb5 105
sam_grove 0:6219434a0cb5 106 if (checkAccess && (ptrTable->pSubindex[bSubindex].bAccessType & WO)) {
sam_grove 0:6219434a0cb5 107 MSG_WAR(0x2B30, "Access Type : ", ptrTable->pSubindex[bSubindex].bAccessType);
sam_grove 0:6219434a0cb5 108 accessDictionaryError(wIndex, bSubindex, 0, 0, OD_READ_NOT_ALLOWED);
sam_grove 0:6219434a0cb5 109 return OD_READ_NOT_ALLOWED;
sam_grove 0:6219434a0cb5 110 }
sam_grove 0:6219434a0cb5 111
sam_grove 0:6219434a0cb5 112 *pDataType = ptrTable->pSubindex[bSubindex].bDataType;
sam_grove 0:6219434a0cb5 113 szData = ptrTable->pSubindex[bSubindex].size;
sam_grove 0:6219434a0cb5 114
sam_grove 0:6219434a0cb5 115 if(*pExpectedSize == 0 ||
sam_grove 0:6219434a0cb5 116 *pExpectedSize == szData ||
sam_grove 0:6219434a0cb5 117 /* allow to fetch a shorter string than expected */
sam_grove 0:6219434a0cb5 118 (*pDataType >= visible_string && *pExpectedSize < szData)) {
sam_grove 0:6219434a0cb5 119
sam_grove 0:6219434a0cb5 120 # ifdef CANOPEN_BIG_ENDIAN
sam_grove 0:6219434a0cb5 121 if(endianize && *pDataType > boolean && !(
sam_grove 0:6219434a0cb5 122 *pDataType >= visible_string &&
sam_grove 0:6219434a0cb5 123 *pDataType <= domain)) {
sam_grove 0:6219434a0cb5 124 /* data must be transmited with low byte first */
sam_grove 0:6219434a0cb5 125 UNS8 i, j = 0;
sam_grove 0:6219434a0cb5 126 MSG_WAR(boolean, "data type ", *pDataType);
sam_grove 0:6219434a0cb5 127 MSG_WAR(visible_string, "data type ", *pDataType);
sam_grove 0:6219434a0cb5 128 for ( i = szData ; i > 0 ; i--) {
sam_grove 0:6219434a0cb5 129 MSG_WAR(i," ", j);
sam_grove 0:6219434a0cb5 130 ((UNS8*)pDestData)[j++] =
sam_grove 0:6219434a0cb5 131 ((UNS8*)ptrTable->pSubindex[bSubindex].pObject)[i-1];
sam_grove 0:6219434a0cb5 132 }
sam_grove 0:6219434a0cb5 133 *pExpectedSize = szData;
sam_grove 0:6219434a0cb5 134 }
sam_grove 0:6219434a0cb5 135 else /* no endianisation change */
sam_grove 0:6219434a0cb5 136 # endif
sam_grove 0:6219434a0cb5 137 if(*pDataType != visible_string) {
sam_grove 0:6219434a0cb5 138 memcpy(pDestData, ptrTable->pSubindex[bSubindex].pObject,szData);
sam_grove 0:6219434a0cb5 139 *pExpectedSize = szData;
sam_grove 0:6219434a0cb5 140 }else{
sam_grove 0:6219434a0cb5 141 /* TODO : CONFORM TO DS-301 :
sam_grove 0:6219434a0cb5 142 * - stop using NULL terminated strings
sam_grove 0:6219434a0cb5 143 * - store string size in td_subindex
sam_grove 0:6219434a0cb5 144 * */
sam_grove 0:6219434a0cb5 145 /* Copy null terminated string to user, and return discovered size */
sam_grove 0:6219434a0cb5 146 UNS8 *ptr = (UNS8*)ptrTable->pSubindex[bSubindex].pObject;
sam_grove 0:6219434a0cb5 147 UNS8 *ptr_start = ptr;
sam_grove 0:6219434a0cb5 148 /* *pExpectedSize IS < szData . if null, use szData */
sam_grove 0:6219434a0cb5 149 UNS8 *ptr_end = ptr + (*pExpectedSize ? *pExpectedSize : szData) ;
sam_grove 0:6219434a0cb5 150 UNS8 *ptr_dest = (UNS8*)pDestData;
sam_grove 0:6219434a0cb5 151 while( *ptr && ptr < ptr_end){
sam_grove 0:6219434a0cb5 152 *(ptr_dest++) = *(ptr++);
sam_grove 0:6219434a0cb5 153 }
sam_grove 0:6219434a0cb5 154
sam_grove 0:6219434a0cb5 155 *pExpectedSize = (UNS32) (ptr - ptr_start);
sam_grove 0:6219434a0cb5 156 /* terminate string if not maximum length */
sam_grove 0:6219434a0cb5 157 if (*pExpectedSize < szData)
sam_grove 0:6219434a0cb5 158 *(ptr) = 0;
sam_grove 0:6219434a0cb5 159 }
sam_grove 0:6219434a0cb5 160
sam_grove 0:6219434a0cb5 161 return OD_SUCCESSFUL;
sam_grove 0:6219434a0cb5 162 }
sam_grove 0:6219434a0cb5 163 else { /* Error ! */
sam_grove 0:6219434a0cb5 164 *pExpectedSize = szData;
sam_grove 0:6219434a0cb5 165 accessDictionaryError(wIndex, bSubindex, szData,
sam_grove 0:6219434a0cb5 166 *pExpectedSize, OD_LENGTH_DATA_INVALID);
sam_grove 0:6219434a0cb5 167 return OD_LENGTH_DATA_INVALID;
sam_grove 0:6219434a0cb5 168 }
sam_grove 0:6219434a0cb5 169 }
sam_grove 0:6219434a0cb5 170
sam_grove 0:6219434a0cb5 171 UNS32 _setODentry( CO_Data* d,
sam_grove 0:6219434a0cb5 172 UNS16 wIndex,
sam_grove 0:6219434a0cb5 173 UNS8 bSubindex,
sam_grove 0:6219434a0cb5 174 void * pSourceData,
sam_grove 0:6219434a0cb5 175 UNS32 * pExpectedSize,
sam_grove 0:6219434a0cb5 176 UNS8 checkAccess,
sam_grove 0:6219434a0cb5 177 UNS8 endianize)
sam_grove 0:6219434a0cb5 178 {
sam_grove 0:6219434a0cb5 179 UNS32 szData;
sam_grove 0:6219434a0cb5 180 UNS8 dataType;
sam_grove 0:6219434a0cb5 181 UNS32 errorCode;
sam_grove 0:6219434a0cb5 182 const indextable *ptrTable;
sam_grove 0:6219434a0cb5 183 ODCallback_t *Callback;
sam_grove 0:6219434a0cb5 184
sam_grove 0:6219434a0cb5 185 ptrTable =(*d->scanIndexOD)(wIndex, &errorCode, &Callback);
sam_grove 0:6219434a0cb5 186 if (errorCode != OD_SUCCESSFUL)
sam_grove 0:6219434a0cb5 187 return errorCode;
sam_grove 0:6219434a0cb5 188
sam_grove 0:6219434a0cb5 189 if( ptrTable->bSubCount <= bSubindex ) {
sam_grove 0:6219434a0cb5 190 /* Subindex not found */
sam_grove 0:6219434a0cb5 191 accessDictionaryError(wIndex, bSubindex, 0, *pExpectedSize, OD_NO_SUCH_SUBINDEX);
sam_grove 0:6219434a0cb5 192 return OD_NO_SUCH_SUBINDEX;
sam_grove 0:6219434a0cb5 193 }
sam_grove 0:6219434a0cb5 194 if (checkAccess && (ptrTable->pSubindex[bSubindex].bAccessType == RO)) {
sam_grove 0:6219434a0cb5 195 MSG_WAR(0x2B25, "Access Type : ", ptrTable->pSubindex[bSubindex].bAccessType);
sam_grove 0:6219434a0cb5 196 accessDictionaryError(wIndex, bSubindex, 0, *pExpectedSize, OD_WRITE_NOT_ALLOWED);
sam_grove 0:6219434a0cb5 197 return OD_WRITE_NOT_ALLOWED;
sam_grove 0:6219434a0cb5 198 }
sam_grove 0:6219434a0cb5 199
sam_grove 0:6219434a0cb5 200
sam_grove 0:6219434a0cb5 201 dataType = ptrTable->pSubindex[bSubindex].bDataType;
sam_grove 0:6219434a0cb5 202 szData = ptrTable->pSubindex[bSubindex].size;
sam_grove 0:6219434a0cb5 203
sam_grove 0:6219434a0cb5 204 if( *pExpectedSize == 0 ||
sam_grove 0:6219434a0cb5 205 *pExpectedSize == szData ||
sam_grove 0:6219434a0cb5 206 /* allow to store a shorter string than entry size */
sam_grove 0:6219434a0cb5 207 (dataType == visible_string && *pExpectedSize < szData))
sam_grove 0:6219434a0cb5 208 {
sam_grove 0:6219434a0cb5 209 #ifdef CANOPEN_BIG_ENDIAN
sam_grove 0:6219434a0cb5 210 /* re-endianize do not occur for bool, strings time and domains */
sam_grove 0:6219434a0cb5 211 if(endianize && dataType > boolean && !(
sam_grove 0:6219434a0cb5 212 dataType >= visible_string &&
sam_grove 0:6219434a0cb5 213 dataType <= domain))
sam_grove 0:6219434a0cb5 214 {
sam_grove 0:6219434a0cb5 215 /* we invert the data source directly. This let us do range
sam_grove 0:6219434a0cb5 216 testing without */
sam_grove 0:6219434a0cb5 217 /* additional temp variable */
sam_grove 0:6219434a0cb5 218 UNS8 i;
sam_grove 0:6219434a0cb5 219 for ( i = 0 ; i < ( ptrTable->pSubindex[bSubindex].size >> 1) ; i++)
sam_grove 0:6219434a0cb5 220 {
sam_grove 0:6219434a0cb5 221 UNS8 tmp =((UNS8 *)pSourceData) [(ptrTable->pSubindex[bSubindex].size - 1) - i];
sam_grove 0:6219434a0cb5 222 ((UNS8 *)pSourceData) [(ptrTable->pSubindex[bSubindex].size - 1) - i] = ((UNS8 *)pSourceData)[i];
sam_grove 0:6219434a0cb5 223 ((UNS8 *)pSourceData)[i] = tmp;
sam_grove 0:6219434a0cb5 224 }
sam_grove 0:6219434a0cb5 225 }
sam_grove 0:6219434a0cb5 226 #endif
sam_grove 0:6219434a0cb5 227 errorCode = (*d->valueRangeTest)(dataType, pSourceData);
sam_grove 0:6219434a0cb5 228 if (errorCode) {
sam_grove 0:6219434a0cb5 229 accessDictionaryError(wIndex, bSubindex, szData, *pExpectedSize, errorCode);
sam_grove 0:6219434a0cb5 230 return errorCode;
sam_grove 0:6219434a0cb5 231 }
sam_grove 0:6219434a0cb5 232 memcpy(ptrTable->pSubindex[bSubindex].pObject,pSourceData, *pExpectedSize);
sam_grove 0:6219434a0cb5 233 /* TODO : CONFORM TO DS-301 :
sam_grove 0:6219434a0cb5 234 * - stop using NULL terminated strings
sam_grove 0:6219434a0cb5 235 * - store string size in td_subindex
sam_grove 0:6219434a0cb5 236 * */
sam_grove 0:6219434a0cb5 237 /* terminate visible_string with '\0' */
sam_grove 0:6219434a0cb5 238 if(dataType == visible_string && *pExpectedSize < szData)
sam_grove 0:6219434a0cb5 239 ((UNS8*)ptrTable->pSubindex[bSubindex].pObject)[*pExpectedSize] = 0;
sam_grove 0:6219434a0cb5 240
sam_grove 0:6219434a0cb5 241 *pExpectedSize = szData;
sam_grove 0:6219434a0cb5 242
sam_grove 0:6219434a0cb5 243 /* Callbacks */
sam_grove 0:6219434a0cb5 244 if(Callback && Callback[bSubindex]){
sam_grove 0:6219434a0cb5 245 errorCode = (Callback[bSubindex])(d, ptrTable, bSubindex);
sam_grove 0:6219434a0cb5 246 if(errorCode != OD_SUCCESSFUL)
sam_grove 0:6219434a0cb5 247 {
sam_grove 0:6219434a0cb5 248 return errorCode;
sam_grove 0:6219434a0cb5 249 }
sam_grove 0:6219434a0cb5 250 }
sam_grove 0:6219434a0cb5 251
sam_grove 0:6219434a0cb5 252 /* TODO : Store dans NVRAM */
sam_grove 0:6219434a0cb5 253 if (ptrTable->pSubindex[bSubindex].bAccessType & TO_BE_SAVE){
sam_grove 0:6219434a0cb5 254 (*d->storeODSubIndex)(d, wIndex, bSubindex);
sam_grove 0:6219434a0cb5 255 }
sam_grove 0:6219434a0cb5 256 return OD_SUCCESSFUL;
sam_grove 0:6219434a0cb5 257 }else{
sam_grove 0:6219434a0cb5 258 *pExpectedSize = szData;
sam_grove 0:6219434a0cb5 259 accessDictionaryError(wIndex, bSubindex, szData, *pExpectedSize, OD_LENGTH_DATA_INVALID);
sam_grove 0:6219434a0cb5 260 return OD_LENGTH_DATA_INVALID;
sam_grove 0:6219434a0cb5 261 }
sam_grove 0:6219434a0cb5 262 }
sam_grove 0:6219434a0cb5 263
sam_grove 0:6219434a0cb5 264 const indextable * scanIndexOD (CO_Data* d, UNS16 wIndex, UNS32 *errorCode, ODCallback_t **Callback)
sam_grove 0:6219434a0cb5 265 {
sam_grove 0:6219434a0cb5 266 return (*d->scanIndexOD)(wIndex, errorCode, Callback);
sam_grove 0:6219434a0cb5 267 }
sam_grove 0:6219434a0cb5 268
sam_grove 0:6219434a0cb5 269 UNS32 RegisterSetODentryCallBack(CO_Data* d, UNS16 wIndex, UNS8 bSubindex, ODCallback_t Callback)
sam_grove 0:6219434a0cb5 270 {
sam_grove 0:6219434a0cb5 271 UNS32 errorCode;
sam_grove 0:6219434a0cb5 272 ODCallback_t *CallbackList;
sam_grove 0:6219434a0cb5 273 const indextable *odentry;
sam_grove 0:6219434a0cb5 274
sam_grove 0:6219434a0cb5 275 odentry = scanIndexOD (d, wIndex, &errorCode, &CallbackList);
sam_grove 0:6219434a0cb5 276 if(errorCode == OD_SUCCESSFUL && CallbackList && bSubindex < odentry->bSubCount)
sam_grove 0:6219434a0cb5 277 CallbackList[bSubindex] = Callback;
sam_grove 0:6219434a0cb5 278 return errorCode;
sam_grove 0:6219434a0cb5 279 }
sam_grove 0:6219434a0cb5 280
sam_grove 0:6219434a0cb5 281 void _storeODSubIndex (CO_Data* d, UNS16 wIndex, UNS8 bSubindex){}