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

//#define DEBUG_WAR_CONSOLE_ON

    24

//#define DEBUG_ERR_CONSOLE_ON

    25

    26

#include "objacces.h"

    27

    28

#ifdef DEBUG_WAR_CONSOLE_ON

    29

UNS8 accessDictionaryError(UNS16 index, UNS8 subIndex, 

    30

			     UNS8 sizeDataDict, UNS8 sizeDataGiven, UNS32 code)

    31

{

    32

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

    33

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

    34

  switch (code) {

    35

    case  OD_NO_SUCH_OBJECT: 

    36

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

    37

      break;

    38

    case OD_NO_SUCH_SUBINDEX :

    39

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

    40

      break;   

    41

    case OD_WRITE_NOT_ALLOWED :

    42

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

    43

      break;         

    44

    case OD_LENGTH_DATA_INVALID :    

    45

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

    46

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

    47

      break;

    48

    case OD_NOT_MAPPABLE :

    49

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

    50

      break;

    51

    case OD_VALUE_TOO_LOW :

    52

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

    53

      break;

    54

    case OD_VALUE_TOO_HIGH :

    55

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

    56

      break;

    57

  default :

    58

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

    59

  }

    60

  return 0; 

    61

}	

    62

#endif

    63

    64

UNS32 getODentry( CO_Data* d, 

    65

                  UNS16 wIndex,

    66

		  UNS8 bSubindex,

    67

		  void * pDestData,

    68

		  UNS8 * pExpectedSize,

    69

		  UNS8 * pDataType,

    70

		  UNS8 checkAccess)

    71

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

    72

  UNS32 errorCode;

    73

  UNS8 szData;

    74

  const indextable *ptrTable;

    75

  ODCallback_t *Callback;

    76

    77

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

    78

    79

  if (errorCode != OD_SUCCESSFUL)

    80

    return errorCode;

    81

  if( ptrTable->bSubCount <= bSubindex ) {

    82

    // Subindex not found

    83

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

    84

    return OD_NO_SUCH_SUBINDEX;

    85

  }

    86

    87

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

    88

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

    89

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

    90

    return OD_READ_NOT_ALLOWED;

    91

  }

    92

    93

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

    94

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

    95

    96

  if(	*pExpectedSize == 0 ||

    97

  	*pExpectedSize == szData ||

    98

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

    99

  {

   100

	#ifdef CANOPEN_BIG_ENDIAN

   101

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

   102

	      {

   103

		// data must be transmited with low byte first

   104

		UNS8 i, j = 0;

   105

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

   106

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

   107

		}

   108

	      }

   109

	#else  	

   110

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

   111

	#endif

   112

      *pExpectedSize = szData;

   113

      return OD_SUCCESSFUL;

   114

  }else{

   115

      *pExpectedSize = szData;

   116

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

   117

      return OD_LENGTH_DATA_INVALID;

   118

  }

   119

}

   120

   121

UNS32 setODentry( CO_Data* d, 

   122

                  UNS16 wIndex,

   123

		  UNS8 bSubindex, 

   124

		  void * pSourceData, 

   125

		  UNS8 * pExpectedSize, 

   126

		  UNS8 checkAccess)

   127

{

   128

  UNS8 szData;

   129

  UNS8 dataType;

   130

  UNS32 errorCode;

   131

  const indextable *ptrTable;

   132

  ODCallback_t *Callback;

   133

   134

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

   135

  if (errorCode != OD_SUCCESSFUL)

   136

    return errorCode;

   137

   138

  if( ptrTable->bSubCount <= bSubindex ) {

   139

    // Subindex not found

   140

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

   141

    return OD_NO_SUCH_SUBINDEX;

   142

  }

   143

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

   144

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

   145

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

   146

    return OD_WRITE_NOT_ALLOWED;

   147

  }

   148

   149

   150

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

   151

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

   152

   153

  if( *pExpectedSize == 0 ||

   154

  	*pExpectedSize == szData ||

   155

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

   156

  {

   157

      #ifdef CANOPEN_BIG_ENDIAN

   158

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

   159

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

   160

		// additional temp variable

   161

		UNS8 i, j = 0;

   162

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

   163

			char tmp = ((char*)pSourceData)[i - 1];

   164

			((char*)pSourceData)[i - 1] = ((char*)pSourceData)[j];

   165

			((char*)pSourceData)[j++] = tmp;

   166

		}

   167

	      }

   168

      #endif

   169

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

   170

      if (errorCode) {

   171

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

   172

	return errorCode;

   173

      }

   174

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

   175

      *pExpectedSize = szData;

   176

   177

      // Callbacks

   178

      if(Callback && Callback[bSubindex]){

   179

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

   180

      }

   181

   182

      // TODO : Store dans NVRAM      

   183

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

   184

      return OD_SUCCESSFUL;

   185

  }else{

   186

      *pExpectedSize = szData;

   187

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

   188

      return OD_LENGTH_DATA_INVALID;

   189

  }

   190

}

   191

   192

   193

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

   194

{

   195

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

   196

}

   197

   198

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

   199

{

   200

	UNS32 errorCode;

   201

	ODCallback_t *CallbackList;

   202

   203

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

   204

	if(errorCode == OD_SUCCESSFUL && CallbackList) 

   205

		CallbackList[bSubindex] = Callback;

   206

	return errorCode;

   207

}

   208

   209