1

/*

     2

  This file is part of CanFestival, a library implementing CanOpen

     3

  Stack.

     4

     5

  Copyright (C): Edouard TISSERANT and Francis DUPIN

     6

     7

  See COPYING file for copyrights details.

     8

     9

  This library is free software; you can redistribute it and/or

    10

  modify it under the terms of the GNU Lesser General Public

    11

  License as published by the Free Software Foundation; either

    12

  version 2.1 of the License, or (at your option) any later version.

    13

    14

  This library is distributed in the hope that it will be useful,

    15

  but WITHOUT ANY WARRANTY; without even the implied warranty of

    16

  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU

    17

  Lesser General Public License for more details.

    18

    19

  You should have received a copy of the GNU Lesser General Public

    20

  License along with this library; if not, write to the Free Software

    21

  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307

    22

  USA

    23

*/

    24

/*!

    25

** @file   objacces.c

    26

** @author Edouard TISSERANT and Francis DUPIN

    27

** @date   Tue Jun  5 08:55:23 2007

    28

**

    29

** @brief

    30

**

    31

**

    32

*/

    33

    34

    35

    36

    37

/* #define DEBUG_WAR_CONSOLE_ON */

    38

/* #define DEBUG_ERR_CONSOLE_ON */

    39

    40

    41

#include "data.h"

    42

    43

#ifdef DEBUG_WAR_CONSOLE_ON

    44

UNS8 accessDictionaryError(UNS16 index, UNS8 subIndex,

    45

                           UNS32 sizeDataDict, UNS32 sizeDataGiven, UNS32 code)

    46

{

    47

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

    48

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

    49

  switch (code) {

    50

  case  OD_NO_SUCH_OBJECT:

    51

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

    52

    break;

    53

  case OD_NO_SUCH_SUBINDEX :

    54

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

    55

    break;

    56

  case OD_WRITE_NOT_ALLOWED :

    57

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

    58

    break;

    59

  case OD_LENGTH_DATA_INVALID :

    60

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

    61

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

    62

    break;

    63

  case OD_NOT_MAPPABLE :

    64

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

    65

    break;

    66

  case OD_VALUE_TOO_LOW :

    67

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

    68

    break;

    69

  case OD_VALUE_TOO_HIGH :

    70

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

    71

    break;

    72

  default :

    73

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

    74

  }

    75

  return 0;

    76

}

    77

#else

    78

#define accessDictionaryError(index, subIndex, sizeDataDict, sizeDataGiven, code)

    79

#endif

    80

    81

UNS32 _getODentry( CO_Data* d,

    82

                   UNS16 wIndex,

    83

                   UNS8 bSubindex,

    84

                   void * pDestData,

    85

                   UNS32 * pExpectedSize,

    86

                   UNS8 * pDataType,

    87

                   UNS8 checkAccess,

    88

                   UNS8 endianize)

    89

{ /* DO NOT USE MSG_ERR because the macro may send a PDO -> infinite

    90

    loop if it fails. */

    91

  UNS32 errorCode;

    92

  UNS32 szData;

    93

  const indextable *ptrTable;

    94

  ODCallback_t *Callback;

    95

    96

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

    97

    98

  if (errorCode != OD_SUCCESSFUL)

    99

    return errorCode;

   100

  if( ptrTable->bSubCount <= bSubindex ) {

   101

    /* Subindex not found */

   102

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

   103

    return OD_NO_SUCH_SUBINDEX;

   104

  }

   105

   106

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

   107

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

   108

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

   109

    return OD_READ_NOT_ALLOWED;

   110

  }

   111

   112

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

   113

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

   114

   115

  if(*pExpectedSize == 0 ||

   116

     *pExpectedSize == szData ||

   117

     /* allow to fetch a shorter string than expected */

   118

     (*pDataType >= visible_string && *pExpectedSize < szData)) { 

   119

   120

#  ifdef CANOPEN_BIG_ENDIAN

   121

     if(endianize && *pDataType > boolean && !(

   122

            *pDataType >= visible_string && 

   123

            *pDataType <= domain)) {

   124

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

   125

      UNS8 i, j = 0;

   126

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

   127

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

   128

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

   129

        MSG_WAR(i," ", j);

   130

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

   131

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

   132

      }

   133

      *pExpectedSize = szData;

   134

    }

   135

    else /* no endianisation change */

   136

#  endif

   137

    if(*pDataType != visible_string) {

   138

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

   139

        *pExpectedSize = szData;

   140

    }else{

   141

        /* TODO : CONFORM TO DS-301 : 

   142

         *  - stop using NULL terminated strings

   143

         *  - store string size in td_subindex 

   144

         * */

   145

        /* Copy null terminated string to user, and return discovered size */

   146

        UNS8 *ptr = (UNS8*)ptrTable->pSubindex[bSubindex].pObject;

   147

        UNS8 *ptr_start = ptr;

   148

        /* *pExpectedSize IS < szData . if null, use szData */

   149

        UNS8 *ptr_end = ptr + (*pExpectedSize ? *pExpectedSize : szData) ; 

   150

        UNS8 *ptr_dest = (UNS8*)pDestData;

   151

        while( *ptr && ptr < ptr_end){

   152

            *(ptr_dest++) = *(ptr++);

   153

        } 

   154

   155

        *pExpectedSize = ptr - ptr_start;

   156

        /* terminate string if not maximum length */

   157

        if (*pExpectedSize < szData) 

   158

            *(ptr) = 0; 

   159

    }

   160

   161

    return OD_SUCCESSFUL;

   162

  }

   163

  else { /* Error ! */

   164

    *pExpectedSize = szData;

   165

    accessDictionaryError(wIndex, bSubindex, szData,

   166

                          *pExpectedSize, OD_LENGTH_DATA_INVALID);

   167

    return OD_LENGTH_DATA_INVALID;

   168

  }

   169

}

   170

   171

UNS32 _setODentry( CO_Data* d,

   172

                   UNS16 wIndex,

   173

                   UNS8 bSubindex,

   174

                   void * pSourceData,

   175

                   UNS32 * pExpectedSize,

   176

                   UNS8 checkAccess,

   177

                   UNS8 endianize)

   178

{

   179

  UNS32 szData;

   180

  UNS8 dataType;

   181

  UNS32 errorCode;

   182

  const indextable *ptrTable;

   183

  ODCallback_t *Callback;

   184

   185

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

   186

  if (errorCode != OD_SUCCESSFUL)

   187

    return errorCode;

   188

   189

  if( ptrTable->bSubCount <= bSubindex ) {

   190

    /* Subindex not found */

   191

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

   192

    return OD_NO_SUCH_SUBINDEX;

   193

  }

   194

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

   195

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

   196

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

   197

    return OD_WRITE_NOT_ALLOWED;

   198

  }

   199

   200

   201

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

   202

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

   203

   204

  if( *pExpectedSize == 0 ||

   205

      *pExpectedSize == szData ||

   206

      /* allow to store a shorter string than entry size */

   207

      (dataType == visible_string && *pExpectedSize < szData))

   208

    {

   209

#ifdef CANOPEN_BIG_ENDIAN

   210

      /* re-endianize do not occur for bool, strings time and domains */

   211

      if(endianize && dataType > boolean && !(

   212

            dataType >= visible_string && 

   213

            dataType <= domain))

   214

        {

   215

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

   216

            testing without */

   217

          /* additional temp variable */

   218

          UNS8 i;

   219

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

   220

            {

   221

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

   222

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

   223

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

   224

            }

   225

        }

   226

#endif

   227

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

   228

      if (errorCode) {

   229

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

   230

        return errorCode;

   231

      }

   232

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

   233

     /* TODO : CONFORM TO DS-301 : 

   234

      *  - stop using NULL terminated strings

   235

      *  - store string size in td_subindex 

   236

      * */

   237

      /* terminate visible_string with '\0' */

   238

      if(dataType == visible_string && *pExpectedSize < szData)

   239

        ((UNS8*)ptrTable->pSubindex[bSubindex].pObject)[*pExpectedSize] = 0;

   240

   241

      *pExpectedSize = szData;

   242

   243

      /* Callbacks */

   244

      if(Callback && Callback[bSubindex]){

   245

        errorCode = (Callback[bSubindex])(d, ptrTable, bSubindex);

   246

        if(errorCode != OD_SUCCESSFUL)

   247

        {

   248

            return errorCode;

   249

        }

   250

       }

   251

   252

      /* TODO : Store dans NVRAM */

   253

      if (ptrTable->pSubindex[bSubindex].bAccessType & TO_BE_SAVE){

   254

        (*d->storeODSubIndex)(d, wIndex, bSubindex);

   255

      }

   256

      return OD_SUCCESSFUL;

   257

    }else{

   258

      *pExpectedSize = szData;

   259

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

   260

      return OD_LENGTH_DATA_INVALID;

   261

    }

   262

}

   263

   264

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

   265

{

   266

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

   267

}

   268

   269

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

   270

{

   271

UNS32 errorCode;

   272

ODCallback_t *CallbackList;

   273

const indextable *odentry;

   274

   275

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

   276

  if(errorCode == OD_SUCCESSFUL  &&  CallbackList  &&  bSubindex < odentry->bSubCount) 

   277

    CallbackList[bSubindex] = Callback;

   278

  return errorCode;

   279

}

   280

   281

void _storeODSubIndex (CO_Data* d, UNS16 wIndex, UNS8 bSubindex){}