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

    27

#include "objacces.h"

    28

    29

    30

#ifdef DEBUG_WAR_CONSOLE_ON

    31

UNS8 accessDictionaryError(UNS16 index, UNS8 subIndex, 

    32

			     UNS8 sizeDataDict, UNS8 sizeDataGiven, UNS32 code)

    33

{

    34

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

    35

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

    36

  switch (code) {

    37

    case  OD_NO_SUCH_OBJECT: 

    38

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

    39

      break;

    40

    case OD_NO_SUCH_SUBINDEX :

    41

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

    42

      break;   

    43

    case OD_WRITE_NOT_ALLOWED :

    44

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

    45

      break;         

    46

    case OD_LENGTH_DATA_INVALID :    

    47

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

    48

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

    49

      break;

    50

    case OD_NOT_MAPPABLE :

    51

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

    52

      break;

    53

    case OD_VALUE_TOO_LOW :

    54

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

    55

      break;

    56

    case OD_VALUE_TOO_HIGH :

    57

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

    58

      break;

    59

  default :

    60

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

    61

  }

    62

  return 0; 

    63

}	

    64

#endif

    65

    66

UNS32 getODentry( CO_Data* d, 

    67

                  UNS16 wIndex,

    68

		  UNS8 bSubindex,

    69

		  void * pDestData,

    70

		  UNS8 * pExpectedSize,

    71

		  UNS8 * pDataType,

    72

		  UNS8 checkAccess)

    73

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

    74

  UNS32 errorCode;

    75

  UNS8 szData;

    76

  const indextable *ptrTable;

    77

  ODCallback_t *Callback;

    78

    79

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

    80

    81

  if (errorCode != OD_SUCCESSFUL)

    82

    return errorCode;

    83

  if( ptrTable->bSubCount <= bSubindex ) {

    84

    // Subindex not found

    85

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

    86

    return OD_NO_SUCH_SUBINDEX;

    87

  }

    88

    89

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

    90

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

    91

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

    92

    return OD_READ_NOT_ALLOWED;

    93

  }

    94

    95

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

    96

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

    97

    98

   if(	*pExpectedSize == 0 ||

    99

  	*pExpectedSize == szData ||

   100

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

   101

   102

#  ifdef CANOPEN_BIG_ENDIAN

   103

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

   104

       // data must be transmited with low byte first

   105

       UNS8 i, j = 0;

   106

       MSG_WAR(boolean, "data type ", *pDataType);

   107

       MSG_WAR(visible_string, "data type ", *pDataType);

   108

       for ( i = szData ; i > 0 ; i--) {

   109

         MSG_WAR(i," ", j);

   110

	 ((UNS8*)pDestData)[j++] = 

   111

	   ((UNS8*)ptrTable->pSubindex[bSubindex].pObject)[i-1];

   112

       }

   113

     }

   114

     else // It it is a visible string no endianisation to perform

   115

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

   116

#  else

   117

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

   118

#  endif

   119

   120

     *pExpectedSize = szData;

   121

#if 0

   122

     // Me laisser ça, please ! (FD)

   123

     {

   124

       UNS8 i;

   125

       for (i = 0 ; i < 10 ; i++) {

   126

	 MSG_WAR(*pExpectedSize, "dic data= ",

   127

		 *(UNS8 *)(ptrTable->pSubindex[bSubindex].pObject + i));

   128

       }

   129

   130

     }

   131

#endif

   132

     return OD_SUCCESSFUL;

   133

   }

   134

   else { // Error !

   135

     *pExpectedSize = szData;

   136

     accessDictionaryError(wIndex, bSubindex, szData, 

   137

			   *pExpectedSize, OD_LENGTH_DATA_INVALID);

   138

     return OD_LENGTH_DATA_INVALID;

   139

   }

   140

}

   141

   142

UNS32 setODentry( CO_Data* d, 

   143

                  UNS16 wIndex,

   144

		  UNS8 bSubindex, 

   145

		  void * pSourceData, 

   146

		  UNS8 * pExpectedSize, 

   147

		  UNS8 checkAccess)

   148

{

   149

  UNS8 szData;

   150

  UNS8 dataType;

   151

  UNS32 errorCode;

   152

  const indextable *ptrTable;

   153

  ODCallback_t *Callback;

   154

   155

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

   156

  if (errorCode != OD_SUCCESSFUL)

   157

    return errorCode;

   158

   159

  if( ptrTable->bSubCount <= bSubindex ) {

   160

    // Subindex not found

   161

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

   162

    return OD_NO_SUCH_SUBINDEX;

   163

  }

   164

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

   165

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

   166

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

   167

    return OD_WRITE_NOT_ALLOWED;

   168

  }

   169

   170

   171

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

   172

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

   173

   174

  if( *pExpectedSize == 0 ||

   175

  	*pExpectedSize == szData ||

   176

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

   177

  {

   178

      #ifdef CANOPEN_BIG_ENDIAN

   179

	      if(dataType > boolean && dataType < visible_string)

   180

	      {

   181

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

   182

			// additional temp variable

   183

		 	UNS8 i;

   184

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

   185

	  		{

   186

	    		UNS8 tmp =((UNS8 *)pSourceData) [(ptrTable->pSubindex[bSubindex].size - 1) - i];

   187

	    		((UNS8 *)pSourceData) [(ptrTable->pSubindex[bSubindex].size - 1) - i] = ((UNS8 *)pSourceData)[i];

   188

	    		((UNS8 *)pSourceData)[i] = tmp;

   189

	    	}

   190

		}  

   191

      #endif

   192

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

   193

      if (errorCode) {

   194

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

   195

	return errorCode;

   196

      }

   197

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

   198

      *pExpectedSize = szData;

   199

   200

      // Callbacks

   201

      if(Callback && Callback[bSubindex]){

   202

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

   203

      }

   204

   205

      // TODO : Store dans NVRAM      

   206

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

   207

      return OD_SUCCESSFUL;

   208

  }else{

   209

      *pExpectedSize = szData;

   210

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

   211

      return OD_LENGTH_DATA_INVALID;

   212

  }

   213

}

   214

   215

   216

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

   217

{

   218

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

   219

}

   220

   221

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

   222

{

   223

	UNS32 errorCode;

   224

	ODCallback_t *CallbackList;

   225

   226

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

   227

	if(errorCode == OD_SUCCESSFUL && CallbackList) 

   228

		CallbackList[bSubindex] = Callback;

   229

	return errorCode;

   230

}

   231

   232