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

    31

UNS8 accessDictionaryError(UNS16 index, UNS8 subIndex, 

    32

			     UNS8 sizeDataDict, UNS8 sizeDataGiven, UNS32 code)

    33

{

    34

#ifdef DEBUG_WAR_CONSOLE_ON

    35

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

    36

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

    37

  switch (code) {

    38

    case  OD_NO_SUCH_OBJECT: 

    39

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

    40

      break;

    41

    case OD_NO_SUCH_SUBINDEX :

    42

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

    43

      break;   

    44

    case OD_WRITE_NOT_ALLOWED :

    45

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

    46

      break;         

    47

    case OD_LENGTH_DATA_INVALID :    

    48

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

    49

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

    50

      break;

    51

    case OD_NOT_MAPPABLE :

    52

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

    53

      break;

    54

    case OD_VALUE_TOO_LOW :

    55

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

    56

      break;

    57

    case OD_VALUE_TOO_HIGH :

    58

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

    59

      break;

    60

  default :

    61

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

    62

  }

    63

#endif

    64

  return 0; 

    65

}	

    66

    67

    68

UNS32 getODentry( CO_Data* d, 

    69

                  UNS16 wIndex,

    70

		  UNS8 bSubindex,

    71

		  void * pDestData,

    72

		  UNS8 * pExpectedSize,

    73

		  UNS8 * pDataType,

    74

		  UNS8 checkAccess)

    75

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

    76

  UNS32 errorCode;

    77

  UNS8 szData;

    78

  const indextable *ptrTable;

    79

  ODCallback_t *Callback;

    80

    81

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

    82

    83

  if (errorCode != OD_SUCCESSFUL)

    84

    return errorCode;

    85

  if( ptrTable->bSubCount <= bSubindex ) {

    86

    /* Subindex not found */

    87

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

    88

    return OD_NO_SUCH_SUBINDEX;

    89

  }

    90

    91

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

    92

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

    93

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

    94

    return OD_READ_NOT_ALLOWED;

    95

  }

    96

    97

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

    98

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

    99

   100

   if(	*pExpectedSize == 0 ||

   101

  	*pExpectedSize == szData ||

   102

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

   103

   104

#  ifdef CANOPEN_BIG_ENDIAN

   105

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

   106

       /* data must be transmited with low byte first */

   107

       UNS8 i, j = 0;

   108

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

   109

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

   110

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

   111

         MSG_WAR(i," ", j);

   112

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

   113

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

   114

       }

   115

     }

   116

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

   117

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

   118

#  else

   119

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

   120

#  endif

   121

   122

     *pExpectedSize = szData;

   123

#if 0

   124

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

   125

     {

   126

       UNS8 i;

   127

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

   128

	 MSG_WAR(*pExpectedSize, "dic data= ",

   129

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

   130

       }

   131

   132

     }

   133

#endif

   134

     return OD_SUCCESSFUL;

   135

   }

   136

   else { /* Error ! */

   137

     *pExpectedSize = szData;

   138

     accessDictionaryError(wIndex, bSubindex, szData, 

   139

			   *pExpectedSize, OD_LENGTH_DATA_INVALID);

   140

     return OD_LENGTH_DATA_INVALID;

   141

   }

   142

}

   143

   144

UNS32 setODentry( CO_Data* d, 

   145

                  UNS16 wIndex,

   146

		  UNS8 bSubindex, 

   147

		  void * pSourceData, 

   148

		  UNS8 * pExpectedSize, 

   149

		  UNS8 checkAccess)

   150

{

   151

  UNS8 szData;

   152

  UNS8 dataType;

   153

  UNS32 errorCode;

   154

  const indextable *ptrTable;

   155

  ODCallback_t *Callback;

   156

   157

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

   158

  if (errorCode != OD_SUCCESSFUL)

   159

    return errorCode;

   160

   161

  if( ptrTable->bSubCount <= bSubindex ) {

   162

    /* Subindex not found */

   163

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

   164

    return OD_NO_SUCH_SUBINDEX;

   165

  }

   166

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

   167

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

   168

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

   169

    return OD_WRITE_NOT_ALLOWED;

   170

  }

   171

   172

   173

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

   174

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

   175

   176

  if( *pExpectedSize == 0 ||

   177

  	*pExpectedSize == szData ||

   178

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

   179

  {

   180

      #ifdef CANOPEN_BIG_ENDIAN

   181

	      if(dataType > boolean && dataType < visible_string)

   182

	      {

   183

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

   184

			/* additional temp variable */

   185

		 	UNS8 i;

   186

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

   187

	  		{

   188

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

   189

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

   190

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

   191

	    	}

   192

		}  

   193

      #endif

   194

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

   195

      if (errorCode) {

   196

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

   197

	return errorCode;

   198

      }

   199

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

   200

      *pExpectedSize = szData;

   201

   202

      /* Callbacks */

   203

      if(Callback && Callback[bSubindex]){

   204

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

   205

      }

   206

   207

      /* TODO : Store dans NVRAM */     

   208

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

   209

      return OD_SUCCESSFUL;

   210

  }else{

   211

      *pExpectedSize = szData;

   212

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

   213

      return OD_LENGTH_DATA_INVALID;

   214

  }

   215

}

   216

   217

   218

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

   219

{

   220

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

   221

}

   222

   223

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

   224

{

   225

	UNS32 errorCode;

   226

	ODCallback_t *CallbackList;

   227

   228

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

   229

	if(errorCode == OD_SUCCESSFUL && CallbackList) 

   230

		CallbackList[bSubindex] = Callback;

   231

	return errorCode;

   232

}

   233

   234