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 getODentry( CO_Data* OD,

   172

                  UNS16 wIndex,

   173

                  UNS8 bSubindex,

   174

                  void * pDestData,

   175

                  UNS32 * pExpectedSize,

   176

                  UNS8 * pDataType,

   177

                  UNS8 checkAccess)

   178

{

   179

	return _getODentry( OD, wIndex, bSubindex, pDestData, pExpectedSize,

   180

                        pDataType,  checkAccess, 1);

   181

}

   182

   183

UNS32 readLocalDict( CO_Data* OD,

   184

                     UNS16 wIndex,

   185

                     UNS8 bSubindex,

   186

                     void * pDestData,

   187

                     UNS32 * pExpectedSize,

   188

                     UNS8 * pDataType,

   189

                     UNS8 checkAccess)

   190

{

   191

  return _getODentry( OD, wIndex, bSubindex, pDestData, pExpectedSize,

   192

                      pDataType,  checkAccess, 0);

   193

}

   194

   195

UNS32 _setODentry( CO_Data* d,

   196

                   UNS16 wIndex,

   197

                   UNS8 bSubindex,

   198

                   void * pSourceData,

   199

                   UNS32 * pExpectedSize,

   200

                   UNS8 checkAccess,

   201

                   UNS8 endianize)

   202

{

   203

  UNS32 szData;

   204

  UNS8 dataType;

   205

  UNS32 errorCode;

   206

  const indextable *ptrTable;

   207

  ODCallback_t *Callback;

   208

   209

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

   210

  if (errorCode != OD_SUCCESSFUL)

   211

    return errorCode;

   212

   213

  if( ptrTable->bSubCount <= bSubindex ) {

   214

    /* Subindex not found */

   215

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

   216

    return OD_NO_SUCH_SUBINDEX;

   217

  }

   218

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

   219

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

   220

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

   221

    return OD_WRITE_NOT_ALLOWED;

   222

  }

   223

   224

   225

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

   226

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

   227

   228

  if( *pExpectedSize == 0 ||

   229

      *pExpectedSize == szData ||

   230

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

   231

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

   232

    {

   233

#ifdef CANOPEN_BIG_ENDIAN

   234

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

   235

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

   236

            dataType >= visible_string && 

   237

            dataType <= domain))

   238

        {

   239

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

   240

            testing without */

   241

          /* additional temp variable */

   242

          UNS8 i;

   243

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

   244

            {

   245

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

   246

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

   247

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

   248

            }

   249

        }

   250

#endif

   251

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

   252

      if (errorCode) {

   253

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

   254

        return errorCode;

   255

      }

   256

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

   257

     /* TODO : CONFORM TO DS-301 : 

   258

      *  - stop using NULL terminated strings

   259

      *  - store string size in td_subindex 

   260

      * */

   261

      /* terminate visible_string with '\0' */

   262

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

   263

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

   264

   265

      *pExpectedSize = szData;

   266

   267

      /* Callbacks */

   268

      if(Callback && Callback[bSubindex]){

   269

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

   270

        if(errorCode != OD_SUCCESSFUL)

   271

        {

   272

            return errorCode;

   273

        }

   274

       }

   275

   276

      /* TODO : Store dans NVRAM */

   277

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

   278

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

   279

      }

   280

      return OD_SUCCESSFUL;

   281

    }else{

   282

      *pExpectedSize = szData;

   283

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

   284

      return OD_LENGTH_DATA_INVALID;

   285

    }

   286

}

   287

   288

UNS32 setODentry( CO_Data* d,

   289

                  UNS16 wIndex,

   290

                  UNS8 bSubindex,

   291

                  void * pSourceData,

   292

                  UNS32 * pExpectedSize,

   293

                  UNS8 checkAccess)

   294

{

   295

  return _setODentry( d, wIndex, bSubindex, pSourceData, pExpectedSize,

   296

                      checkAccess, 1);

   297

}

   298

   299

UNS32 writeLocalDict( CO_Data* d,

   300

                     UNS16 wIndex,

   301

                     UNS8 bSubindex,

   302

                     void * pSourceData,

   303

                     UNS32 * pExpectedSize,

   304

                     UNS8 checkAccess)

   305

{

   306

  return _setODentry( d, wIndex, bSubindex, pSourceData, pExpectedSize, checkAccess, 0);

   307

}

   308

   309

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

   310

{

   311

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

   312

}

   313

   314

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

   315

{

   316

UNS32 errorCode;

   317

ODCallback_t *CallbackList;

   318

const indextable *odentry;

   319

   320

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

   321

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

   322

    CallbackList[bSubindex] = Callback;

   323

  return errorCode;

   324

}

   325

   326

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