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

    25

    26

#include "objacces.h"

    27

    28

    29

#ifdef DEBUG_WAR_CONSOLE_ON

    30

UNS8 accessDictionaryError(UNS16 index, UNS8 subIndex, 

    31

			     UNS8 sizeDataDict, UNS8 sizeDataGiven, UNS32 code)

    32

{

    33

  MSG_WAR(0x2B09,"Dictionary index : ", index);

    34

  MSG_WAR(0X2B10,"           subindex : ", subIndex);

    35

  switch (code) {

    36

    case  OD_NO_SUCH_OBJECT: 

    37

      MSG_WAR(0x2B11,"Index not found ", index);

    38

      break;

    39

    case OD_NO_SUCH_SUBINDEX :

    40

      MSG_WAR(0x2B12,"SubIndex not found ", subIndex);

    41

      break;   

    42

    case OD_WRITE_NOT_ALLOWED :

    43

      MSG_WAR(0x2B13,"Write not allowed, data is read only ", index);

    44

      break;         

    45

    case OD_LENGTH_DATA_INVALID :    

    46

      MSG_WAR(0x2B14,"Conflict size data. Should be (bytes)  : ", sizeDataDict);

    47

      MSG_WAR(0x2B15,"But you have given the size  : ", sizeDataGiven);

    48

      break;

    49

    case OD_NOT_MAPPABLE :

    50

      MSG_WAR(0x2B16,"Not mappable data in a PDO at index    : ", index);

    51

      break;

    52

    case OD_VALUE_TOO_LOW :

    53

      MSG_WAR(0x2B17,"Value range error : value too low. SDOabort : ", code);

    54

      break;

    55

    case OD_VALUE_TOO_HIGH :

    56

      MSG_WAR(0x2B18,"Value range error : value too high. SDOabort : ", code);

    57

      break;

    58

  default :

    59

    MSG_WAR(0x2B20, "Unknown error code : ", code);

    60

  }

    61

  return 0; 

    62

}	

    63

#endif

    64

    65

UNS32 getODentry( CO_Data* d, 

    66

                  UNS16 wIndex,

    67

		  UNS8 bSubindex,

    68

		  void * pDestData,

    69

		  UNS8 * pExpectedSize,

    70

		  UNS8 * pDataType,

    71

		  UNS8 checkAccess)

    72

{ // DO NOT USE MSG_ERR because the macro may send a PDO -> infinite loop if it fails.

    73

  UNS32 errorCode;

    74

  UNS8 szData;

    75

  const indextable *ptrTable;

    76

  ODCallback_t *Callback;

    77

    78

  ptrTable = (*d->scanIndexOD)(wIndex, &errorCode, &Callback);

    79

    80

  if (errorCode != OD_SUCCESSFUL)

    81

    return errorCode;

    82

  if( ptrTable->bSubCount <= bSubindex ) {

    83

    // Subindex not found

    84

    accessDictionaryError(wIndex, bSubindex, 0, 0, OD_NO_SUCH_SUBINDEX);

    85

    return OD_NO_SUCH_SUBINDEX;

    86

  }

    87

    88

  if (checkAccess && !(ptrTable->pSubindex[bSubindex].bAccessType & WO)) {

    89

  	MSG_WAR(0x2B30, "Access Type : ", ptrTable->pSubindex[bSubindex].bAccessType);

    90

    accessDictionaryError(wIndex, bSubindex, 0, 0, OD_WRITE_NOT_ALLOWED);

    91

    return OD_READ_NOT_ALLOWED;

    92

  }

    93

    94

  *pDataType = ptrTable->pSubindex[bSubindex].bDataType;

    95

   szData = ptrTable->pSubindex[bSubindex].size;

    96

    97

  if(	*pExpectedSize == 0 ||

    98

  	*pExpectedSize == szData ||

    99

  	(*pDataType == visible_string && *pExpectedSize > szData)) // We allow to fetch a shorter string than expected

   100

  {

   101

	#ifdef CANOPEN_BIG_ENDIAN

   103

		// data must be transmited with low byte first

   105

		UNS8 i, j = 0;

   106

		for ( i = ptrTable->pSubindex[bSubindex].size ; i > 0 ; i--) {

   107

			((char*)pDestData)[j++] = ((char*)ptrTable->pSubindex[bSubindex].pObject)[i-1];

   108

		}

   109

	      }

   113

	#else  	

   114

  	      memcpy(pDestData, ptrTable->pSubindex[bSubindex].pObject,*pExpectedSize);

   115

	#endif

   116

      *pExpectedSize = szData;

   117

      return OD_SUCCESSFUL;

   118

  }else{

   119

      *pExpectedSize = szData;

   120

      accessDictionaryError(wIndex, bSubindex, szData, *pExpectedSize, OD_LENGTH_DATA_INVALID);

   121

      return OD_LENGTH_DATA_INVALID;

   122

  }

   123

}

   124

   125

UNS32 setODentry( CO_Data* d, 

   126

                  UNS16 wIndex,

   127

		  UNS8 bSubindex, 

   128

		  void * pSourceData, 

   129

		  UNS8 * pExpectedSize, 

   130

		  UNS8 checkAccess)

   131

{

   132

  UNS8 szData;

   133

  UNS8 dataType;

   134

  UNS32 errorCode;

   135

  const indextable *ptrTable;

   136

  ODCallback_t *Callback;

   137

   138

  ptrTable =(*d->scanIndexOD)(wIndex, &errorCode, &Callback);

   139

  if (errorCode != OD_SUCCESSFUL)

   140

    return errorCode;

   141

   142

  if( ptrTable->bSubCount <= bSubindex ) {

   143

    // Subindex not found

   144

    accessDictionaryError(wIndex, bSubindex, 0, *pExpectedSize, OD_NO_SUCH_SUBINDEX);

   145

    return OD_NO_SUCH_SUBINDEX;

   146

  }

   147

  if (checkAccess && (ptrTable->pSubindex[bSubindex].bAccessType == RO)) {

   148

  	MSG_WAR(0x2B25, "Access Type : ", ptrTable->pSubindex[bSubindex].bAccessType);

   149

    accessDictionaryError(wIndex, bSubindex, 0, *pExpectedSize, OD_WRITE_NOT_ALLOWED);

   150

    return OD_WRITE_NOT_ALLOWED;

   151

  }

   152

   153

   154

   dataType = ptrTable->pSubindex[bSubindex].bDataType;

   155

   szData = ptrTable->pSubindex[bSubindex].size;

   156

   157

  if( *pExpectedSize == 0 ||

   158

  	*pExpectedSize == szData ||

   159

  	(dataType == visible_string && *pExpectedSize < szData)) // We allow to store a shorter string than entry size

   160

  {

   161

      #ifdef CANOPEN_BIG_ENDIAN

   162

	      if(dataType > boolean && dataType < visible_string){

   163

		// we invert the data source directly. This let us do range testing without

   164

		// additional temp variable

   166

		UNS8 i, j = 0;

   169

		}

   178

	      }

   179

      #endif

   180

      errorCode = (*d->valueRangeTest)(dataType, pSourceData);

   181

      if (errorCode) {

   182

	accessDictionaryError(wIndex, bSubindex, szData, *pExpectedSize, errorCode);

   183

	return errorCode;

   184

      }

   185

      memcpy(ptrTable->pSubindex[bSubindex].pObject,pSourceData, *pExpectedSize);

   186

      *pExpectedSize = szData;

   187

   188

      // Callbacks

   189

      if(Callback && Callback[bSubindex]){

   190

      	 (*Callback[bSubindex])(d, ptrTable, bSubindex);

   191

      }

   192

   193

      // TODO : Store dans NVRAM      

   194

      // if (ptrTable->pSubindex[bSubindex].bAccessType & TO_BE_SAVED)

   195

      return OD_SUCCESSFUL;

   196

  }else{

   197

      *pExpectedSize = szData;

   198

      accessDictionaryError(wIndex, bSubindex, szData, *pExpectedSize, OD_LENGTH_DATA_INVALID);

   199

      return OD_LENGTH_DATA_INVALID;

   200

  }

   201

}

   202

   203

   204

const indextable * scanIndexOD (CO_Data* d, UNS16 wIndex, UNS32 *errorCode, ODCallback_t **Callback)

   205

{

   206

  return (*d->scanIndexOD)(wIndex, errorCode, Callback);

   207

}

   208

   209

UNS32 RegisterSetODentryCallBack(CO_Data* d, UNS16 wIndex, UNS8 bSubindex, ODCallback_t Callback)

   210

{

   211

	UNS32 errorCode;

   212

	ODCallback_t *CallbackList;

   213

   214

	scanIndexOD (d, wIndex, &errorCode, &CallbackList);

   215

	if(errorCode == OD_SUCCESSFUL && CallbackList) 

   216

		CallbackList[bSubindex] = Callback;

   217

	return errorCode;

   218

}

   219

   220