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

    44

/*!

    45

**

    46

**

    47

** @param index

    48

** @param subIndex

    49

** @param sizeDataDict

    50

** @param sizeDataGiven

    51

** @param code

    52

**

    53

** @return

    54

**/

    55

#ifdef DEBUG_WAR_CONSOLE_ON

    56

UNS8 accessDictionaryError(UNS16 index, UNS8 subIndex,

    57

                           UNS32 sizeDataDict, UNS32 sizeDataGiven, UNS32 code)

    58

{

    59

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

    60

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

    61

  switch (code) {

    62

  case  OD_NO_SUCH_OBJECT:

    63

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

    64

    break;

    65

  case OD_NO_SUCH_SUBINDEX :

    66

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

    67

    break;

    68

  case OD_WRITE_NOT_ALLOWED :

    69

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

    70

    break;

    71

  case OD_LENGTH_DATA_INVALID :

    72

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

    73

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

    74

    break;

    75

  case OD_NOT_MAPPABLE :

    76

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

    77

    break;

    78

  case OD_VALUE_TOO_LOW :

    79

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

    80

    break;

    81

  case OD_VALUE_TOO_HIGH :

    82

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

    83

    break;

    84

  default :

    85

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

    86

  }

    87

  return 0;

    88

}

    89

#else

    90

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

    91

#endif

    92

    93

/*!

    94

**

    95

**

    96

** @param d

    97

** @param wIndex

    98

** @param bSubindex

    99

** @param pDestData

   100

** @param pExpectedSize

   101

** @param pDataType

   102

** @param checkAccess

   103

** @param endianize

   104

**

   105

** @return

   106

**/

   107

UNS32 _getODentry( CO_Data* d,

   108

                   UNS16 wIndex,

   109

                   UNS8 bSubindex,

   110

                   void * pDestData,

   111

                   UNS32 * pExpectedSize,

   112

                   UNS8 * pDataType,

   113

                   UNS8 checkAccess,

   114

                   UNS8 endianize)

   115

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

   116

    loop if it fails. */

   117

  UNS32 errorCode;

   118

  UNS32 szData;

   119

  const indextable *ptrTable;

   120

  ODCallback_t *Callback;

   121

   122

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

   123

   124

  if (errorCode != OD_SUCCESSFUL)

   125

    return errorCode;

   126

  if( ptrTable->bSubCount <= bSubindex ) {

   127

    /* Subindex not found */

   128

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

   129

    return OD_NO_SUCH_SUBINDEX;

   130

  }

   131

   132

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

   133

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

   134

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

   135

    return OD_READ_NOT_ALLOWED;

   136

  }

   137

   138

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

   139

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

   140

   141

  if(*pExpectedSize == 0 ||

   142

     *pExpectedSize == szData ||

   143

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

   144

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

   145

   146

#  ifdef CANOPEN_BIG_ENDIAN

   147

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

   148

            *pDataType >= visible_string && 

   149

            *pDataType <= domain)) {

   150

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

   151

      UNS8 i, j = 0;

   152

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

   153

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

   154

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

   155

        MSG_WAR(i," ", j);

   156

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

   157

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

   158

      }

   159

      *pExpectedSize = szData;

   160

    }

   161

    else /* no endianisation change */

   162

#  endif

   163

    if(*pDataType != visible_string) {

   164

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

   165

        *pExpectedSize = szData;

   166

    }else{

   167

        /* TODO : CONFORM TO DS-301 : 

   168

         *  - stop using NULL terminated strings

   169

         *  - store string size in td_subindex 

   170

         * */

   171

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

   172

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

   173

        UNS8 *ptr_start = ptr;

   174

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

   175

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

   176

        UNS8 *ptr_dest = (UNS8*)pDestData;

   177

        while( *ptr && ptr < ptr_end){

   178

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

   179

        } 

   180

   181

        *pExpectedSize = ptr - ptr_start;

   182

        /* terminate string if not maximum length */

   183

        if (*pExpectedSize < szData) 

   184

            *(ptr) = 0; 

   185

    }

   186

   187

    return OD_SUCCESSFUL;

   188

  }

   189

  else { /* Error ! */

   190

    *pExpectedSize = szData;

   191

    accessDictionaryError(wIndex, bSubindex, szData,

   192

                          *pExpectedSize, OD_LENGTH_DATA_INVALID);

   193

    return OD_LENGTH_DATA_INVALID;

   194

  }

   195

}

   196

   197

/*!

   198

**

   199

**

   200

** @param d

   201

** @param wIndex

   202

** @param bSubindex

   203

** @param pSourceData

   204

** @param pExpectedSize

   205

** @param checkAccess

   206

** @param endianize

   207

**

   208

** @return

   209

**/

   210

UNS32 _setODentry( CO_Data* d,

   211

                   UNS16 wIndex,

   212

                   UNS8 bSubindex,

   213

                   void * pSourceData,

   214

                   UNS32 * pExpectedSize,

   215

                   UNS8 checkAccess,

   216

                   UNS8 endianize)

   217

{

   218

  UNS32 szData;

   219

  UNS8 dataType;

   220

  UNS32 errorCode;

   221

  const indextable *ptrTable;

   222

  ODCallback_t *Callback;

   223

   224

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

   225

  if (errorCode != OD_SUCCESSFUL)

   226

    return errorCode;

   227

   228

  if( ptrTable->bSubCount <= bSubindex ) {

   229

    /* Subindex not found */

   230

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

   231

    return OD_NO_SUCH_SUBINDEX;

   232

  }

   233

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

   234

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

   235

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

   236

    return OD_WRITE_NOT_ALLOWED;

   237

  }

   238

   239

   240

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

   241

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

   242

   243

  if( *pExpectedSize == 0 ||

   244

      *pExpectedSize == szData ||

   245

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

   246

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

   247

    {

   248

#ifdef CANOPEN_BIG_ENDIAN

   249

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

   250

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

   251

            dataType >= visible_string && 

   252

            dataType <= domain))

   253

        {

   254

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

   255

            testing without */

   256

          /* additional temp variable */

   257

          UNS8 i;

   258

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

   259

            {

   260

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

   261

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

   262

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

   263

            }

   264

        }

   265

#endif

   266

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

   267

      if (errorCode) {

   268

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

   269

        return errorCode;

   270

      }

   271

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

   272

     /* TODO : CONFORM TO DS-301 : 

   273

      *  - stop using NULL terminated strings

   274

      *  - store string size in td_subindex 

   275

      * */

   276

      /* terminate visible_string with '\0' */

   277

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

   278

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

   279

   280

      *pExpectedSize = szData;

   281

   282

      /* Callbacks */

   283

      if(Callback && Callback[bSubindex]){

   284

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

   285

        if(errorCode != OD_SUCCESSFUL)

   286

        {

   287

            return errorCode;

   288

        }

   289

       }

   290

   291

      /* TODO : Store dans NVRAM */

   292

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

   293

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

   294

      }

   295

      return OD_SUCCESSFUL;

   296

    }else{

   297

      *pExpectedSize = szData;

   298

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

   299

      return OD_LENGTH_DATA_INVALID;

   300

    }

   301

}

   302

   303

/*!

   304

**

   305

**

   306

** @param d

   307

** @param wIndex

   308

** @param errorCode

   309

** @param Callback

   310

**

   311

** @return

   312

**/

   313

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

   314

{

   315

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

   316

}

   317

   318

/*!

   319

**

   320

**

   321

** @param d

   322

** @param wIndex

   323

** @param bSubindex

   324

** @param Callback

   325

**

   326

** @return

   327

**/

   328

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

   329

{

   330

UNS32 errorCode;

   331

ODCallback_t *CallbackList;

   332

const indextable *odentry;

   333

   334

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

   335

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

   336

    CallbackList[bSubindex] = Callback;

   337

  return errorCode;

   338

}

   339

   340

/*!

   341

**

   342

**

   343

** @param wIndex

   344

** @param bSubindex

   345

**/

   346

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