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

//We need the function implementation for linking

    44

//Only a placeholder with a define isnt enough!

    45

UNS8 accessDictionaryError(UNS16 index, UNS8 subIndex,

    46

                           UNS32 sizeDataDict, UNS32 sizeDataGiven, UNS32 code)

    47

{

    48

#ifdef DEBUG_WAR_CONSOLE_ON

    49

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

    50

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

    51

  switch (code) {

    52

  case  OD_NO_SUCH_OBJECT:

    53

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

    54

    break;

    55

  case OD_NO_SUCH_SUBINDEX :

    56

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

    57

    break;

    58

  case OD_WRITE_NOT_ALLOWED :

    59

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

    60

    break;

    61

  case OD_LENGTH_DATA_INVALID :

    62

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

    63

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

    64

    break;

    65

  case OD_NOT_MAPPABLE :

    66

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

    67

    break;

    68

  case OD_VALUE_TOO_LOW :

    69

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

    70

    break;

    71

  case OD_VALUE_TOO_HIGH :

    72

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

    73

    break;

    74

  default :

    75

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

    76

  }

    77

  #endif

    78

    79

  return 0;

    80

}

    81

    82

UNS32 _getODentry( CO_Data* d,

    83

                   UNS16 wIndex,

    84

                   UNS8 bSubindex,

    85

                   void * pDestData,

    86

                   UNS32 * pExpectedSize,

    87

                   UNS8 * pDataType,

    88

                   UNS8 checkAccess,

    89

                   UNS8 endianize)

    90

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

    91

    loop if it fails. */

    92

  UNS32 errorCode;

    93

  UNS32 szData;

    94

  const indextable *ptrTable;

    95

  ODCallback_t *Callback;

    96

    97

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

    98

    99

  if (errorCode != OD_SUCCESSFUL)

   100

    return errorCode;

   101

  if( ptrTable->bSubCount <= bSubindex ) {

   102

    /* Subindex not found */

   103

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

   104

    return OD_NO_SUCH_SUBINDEX;

   105

  }

   106

   107

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

   108

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

   109

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

   110

    return OD_READ_NOT_ALLOWED;

   111

  }

   112

   113

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

   114

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

   115

   116

  if(*pExpectedSize == 0 ||

   117

     *pExpectedSize == szData ||

   118

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

   119

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

   120

   121

#  ifdef CANOPEN_BIG_ENDIAN

   122

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

   123

            *pDataType >= visible_string && 

   124

            *pDataType <= domain)) {

   125

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

   126

      UNS8 i, j = 0;

   127

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

   128

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

   129

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

   130

        MSG_WAR(i," ", j);

   131

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

   132

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

   133

      }

   134

      *pExpectedSize = szData;

   135

    }

   136

    else /* no endianisation change */

   137

#  endif

   138

    if(*pDataType != visible_string) {

   139

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

   140

        *pExpectedSize = szData;

   141

    }else{

   142

        /* TODO : CONFORM TO DS-301 : 

   143

         *  - stop using NULL terminated strings

   144

         *  - store string size in td_subindex 

   145

         * */

   146

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

   147

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

   148

        UNS8 *ptr_start = ptr;

   149

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

   150

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

   151

        UNS8 *ptr_dest = (UNS8*)pDestData;

   152

        while( *ptr && ptr < ptr_end){

   153

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

   154

        } 

   155

   156

        *pExpectedSize = (UNS32) (ptr - ptr_start);

   157

        /* terminate string if not maximum length */

   158

        if (*pExpectedSize < szData) 

   159

            *(ptr) = 0; 

   160

    }

   161

   162

    return OD_SUCCESSFUL;

   163

  }

   164

  else { /* Error ! */

   165

    *pExpectedSize = szData;

   166

    accessDictionaryError(wIndex, bSubindex, szData,

   167

                          *pExpectedSize, OD_LENGTH_DATA_INVALID);

   168

    return OD_LENGTH_DATA_INVALID;

   169

  }

   170

}

   171

   172

UNS32 _setODentry( CO_Data* d,

   173

                   UNS16 wIndex,

   174

                   UNS8 bSubindex,

   175

                   void * pSourceData,

   176

                   UNS32 * pExpectedSize,

   177

                   UNS8 checkAccess,

   178

                   UNS8 endianize)

   179

{

   180

  UNS32 szData;

   181

  UNS8 dataType;

   182

  UNS32 errorCode;

   183

  const indextable *ptrTable;

   184

  ODCallback_t *Callback;

   185

   186

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

   187

  if (errorCode != OD_SUCCESSFUL)

   188

    return errorCode;

   189

   190

  if( ptrTable->bSubCount <= bSubindex ) {

   191

    /* Subindex not found */

   192

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

   193

    return OD_NO_SUCH_SUBINDEX;

   194

  }

   195

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

   196

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

   197

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

   198

    return OD_WRITE_NOT_ALLOWED;

   199

  }

   200

   201

   202

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

   203

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

   204

   205

  if( *pExpectedSize == 0 ||

   206

      *pExpectedSize == szData ||

   207

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

   208

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

   209

    {

   210

#ifdef CANOPEN_BIG_ENDIAN

   211

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

   212

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

   213

            dataType >= visible_string && 

   214

            dataType <= domain))

   215

        {

   216

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

   217

            testing without */

   218

          /* additional temp variable */

   219

          UNS8 i;

   220

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

   221

            {

   222

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

   223

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

   224

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

   225

            }

   226

        }

   227

#endif

   228

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

   229

      if (errorCode) {

   230

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

   231

        return errorCode;

   232

      }

   233

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

   234

     /* TODO : CONFORM TO DS-301 : 

   235

      *  - stop using NULL terminated strings

   236

      *  - store string size in td_subindex 

   237

      * */

   238

      /* terminate visible_string with '\0' */

   239

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

   240

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

   241

   242

      *pExpectedSize = szData;

   243

   244

      /* Callbacks */

   245

      if(Callback && Callback[bSubindex]){

   246

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

   247

        if(errorCode != OD_SUCCESSFUL)

   248

        {

   249

            return errorCode;

   250

        }

   251

       }

   252

   253

      /* TODO : Store dans NVRAM */

   254

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

   255

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

   256

      }

   257

      return OD_SUCCESSFUL;

   258

    }else{

   259

      *pExpectedSize = szData;

   260

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

   261

      return OD_LENGTH_DATA_INVALID;

   262

    }

   263

}

   264

   265

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

   266

{

   267

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

   268

}

   269

   270

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

   271

{

   272

UNS32 errorCode;

   273

ODCallback_t *CallbackList;

   274

const indextable *odentry;

   275

   276

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

   277

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

   278

    CallbackList[bSubindex] = Callback;

   279

  return errorCode;

   280

}

   281

   282

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