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

UNS8 accessDictionaryError(UNS16 index, UNS8 subIndex,

    56

                           UNS8 sizeDataDict, UNS8 sizeDataGiven, UNS32 code)

    57

{

    58

#ifdef DEBUG_WAR_CONSOLE_ON

    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

#endif

    88

  return 0;

    89

}

    90

    91

/*!

    92

**

    93

**

    94

** @param d

    95

** @param wIndex

    96

** @param bSubindex

    97

** @param pDestData

    98

** @param pExpectedSize

    99

** @param pDataType

   100

** @param checkAccess

   101

** @param endianize

   102

**

   103

** @return

   104

**/

   105

UNS32 _getODentry( CO_Data* d,

   106

                   UNS16 wIndex,

   107

                   UNS8 bSubindex,

   108

                   void * pDestData,

   109

                   UNS8 * pExpectedSize,

   110

                   UNS8 * pDataType,

   111

                   UNS8 checkAccess,

   112

                   UNS8 endianize)

   113

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

   114

    loop if it fails. */

   115

  UNS32 errorCode;

   116

  UNS8 szData;

   117

  const indextable *ptrTable;

   118

  ODCallback_t *Callback;

   119

   120

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

   121

   122

  if (errorCode != OD_SUCCESSFUL)

   123

    return errorCode;

   124

  if( ptrTable->bSubCount <= bSubindex ) {

   125

    /* Subindex not found */

   126

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

   127

    return OD_NO_SUCH_SUBINDEX;

   128

  }

   129

   130

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

   131

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

   132

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

   133

    return OD_READ_NOT_ALLOWED;

   134

  }

   135

   136

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

   137

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

   138

   139

  if(*pExpectedSize == 0 ||

   140

     *pExpectedSize == szData ||

   141

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

   142

    /* We

   143

      allow to fetch a shorter string than expected */

   144

   145

#  ifdef CANOPEN_BIG_ENDIAN

   146

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

   147

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

   148

      UNS8 i, j = 0;

   149

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

   150

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

   151

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

   152

        MSG_WAR(i," ", j);

   153

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

   154

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

   155

      }

   156

    }

   157

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

   158

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

   159

#  else

   160

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

   161

#  endif

   162

   163

    *pExpectedSize = szData;

   164

   165

    return OD_SUCCESSFUL;

   166

  }

   167

  else { /* Error ! */

   168

    *pExpectedSize = szData;

   169

    accessDictionaryError(wIndex, bSubindex, szData,

   170

                          *pExpectedSize, OD_LENGTH_DATA_INVALID);

   171

    return OD_LENGTH_DATA_INVALID;

   172

  }

   173

}

   174

   175

/*!

   176

**

   177

**

   178

** @param d

   179

** @param wIndex

   180

** @param bSubindex

   181

** @param pDestData

   182

** @param pExpectedSize

   183

** @param pDataType

   184

** @param checkAccess

   185

**

   186

** @return

   187

**/

   188

UNS32 getODentry( CO_Data* d,

   189

                  UNS16 wIndex,

   190

                  UNS8 bSubindex,

   191

                  void * pDestData,

   192

                  UNS8 * pExpectedSize,

   193

                  UNS8 * pDataType,

   194

                  UNS8 checkAccess)

   195

{

   196

  return _getODentry( d,

   197

                      wIndex,

   198

                      bSubindex,

   199

                      pDestData,

   200

                      pExpectedSize,

   201

                      pDataType,

   202

                      checkAccess,

   203

                      1);//endianize

   204

}

   205

   206

/*!

   207

**

   208

**

   209

** @param d

   210

** @param wIndex

   211

** @param bSubindex

   212

** @param pDestData

   213

** @param pExpectedSize

   214

** @param pDataType

   215

** @param checkAccess

   216

**

   217

** @return

   218

**/

   219

UNS32 readLocalDict( CO_Data* d,

   220

                     UNS16 wIndex,

   221

                     UNS8 bSubindex,

   222

                     void * pDestData,

   223

                     UNS8 * pExpectedSize,

   224

                     UNS8 * pDataType,

   225

                     UNS8 checkAccess)

   226

{

   227

  return _getODentry( d,

   228

                      wIndex,

   229

                      bSubindex,

   230

                      pDestData,

   231

                      pExpectedSize,

   232

                      pDataType,

   233

                      checkAccess,

   234

                      0);//do not endianize

   235

}

   236

   237

/*!

   238

**

   239

**

   240

** @param d

   241

** @param wIndex

   242

** @param bSubindex

   243

** @param pSourceData

   244

** @param pExpectedSize

   245

** @param checkAccess

   246

** @param endianize

   247

**

   248

** @return

   249

**/

   250

UNS32 _setODentry( CO_Data* d,

   251

                   UNS16 wIndex,

   252

                   UNS8 bSubindex,

   253

                   void * pSourceData,

   254

                   UNS8 * pExpectedSize,

   255

                   UNS8 checkAccess,

   256

                   UNS8 endianize)

   257

{

   258

  UNS8 szData;

   259

  UNS8 dataType;

   260

  UNS32 errorCode;

   261

  const indextable *ptrTable;

   262

  ODCallback_t *Callback;

   263

   264

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

   265

  if (errorCode != OD_SUCCESSFUL)

   266

    return errorCode;

   267

   268

  if( ptrTable->bSubCount <= bSubindex ) {

   269

    /* Subindex not found */

   270

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

   271

    return OD_NO_SUCH_SUBINDEX;

   272

  }

   273

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

   274

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

   275

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

   276

    return OD_WRITE_NOT_ALLOWED;

   277

  }

   278

   279

   280

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

   281

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

   282

   283

  if( *pExpectedSize == 0 ||

   284

      *pExpectedSize == szData ||

   285

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

   286

                                                                  allow to store a shorter string than entry size */

   287

    {

   288

#ifdef CANOPEN_BIG_ENDIAN

   289

      if(endianize && dataType > boolean && dataType < visible_string)

   290

        {

   291

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

   292

            testing without */

   293

          /* additional temp variable */

   294

          UNS8 i;

   295

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

   296

            {

   297

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

   298

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

   299

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

   300

            }

   301

        }

   302

#endif

   303

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

   304

      if (errorCode) {

   305

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

   306

        return errorCode;

   307

      }

   308

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

   309

      *pExpectedSize = szData;

   310

   311

      /* Callbacks */

   312

      if(Callback && Callback[bSubindex]){

   313

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

   314

      }

   315

   316

      /* TODO : Store dans NVRAM */

   317

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

   318

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

   319

      }

   320

      return OD_SUCCESSFUL;

   321

    }else{

   322

      *pExpectedSize = szData;

   323

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

   324

      return OD_LENGTH_DATA_INVALID;

   325

    }

   326

}

   327

   328

/*!

   329

**

   330

**

   331

** @param d

   332

** @param wIndex

   333

** @param bSubindex

   334

** @param pSourceData

   335

** @param pExpectedSize

   336

** @param checkAccess

   337

**

   338

** @return

   339

**/

   340

UNS32 setODentry( CO_Data* d,

   341

                  UNS16 wIndex,

   342

                  UNS8 bSubindex,

   343

                  void * pSourceData,

   344

                  UNS8 * pExpectedSize,

   345

                  UNS8 checkAccess)

   346

{

   347

  return _setODentry( d,

   348

                      wIndex,

   349

                      bSubindex,

   350

                      pSourceData,

   351

                      pExpectedSize,

   352

                      checkAccess,

   353

                      1);//endianize

   354

}

   355

   356

/*!

   357

**

   358

**

   359

** @param d

   360

** @param wIndex

   361

** @param bSubindex

   362

** @param pSourceData

   363

** @param pExpectedSize

   364

** @param checkAccess

   365

**

   366

** @return

   367

**/

   368

UNS32 writeLocalDict( CO_Data* d,

   369

                      UNS16 wIndex,

   370

                      UNS8 bSubindex,

   371

                      void * pSourceData,

   372

                      UNS8 * pExpectedSize,

   373

                      UNS8 checkAccess)

   374

{

   375

  return _setODentry( d,

   376

                      wIndex,

   377

                      bSubindex,

   378

                      pSourceData,

   379

                      pExpectedSize,

   380

                      checkAccess,

   381

                      0);//do not endianize

   382

}

   383

   384

/*!

   385

**

   386

**

   387

** @param d

   388

** @param wIndex

   389

** @param errorCode

   390

** @param Callback

   391

**

   392

** @return

   393

**/

   394

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

   395

{

   396

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

   397

}

   398

   399

/*!

   400

**

   401

**

   402

** @param d

   403

** @param wIndex

   404

** @param bSubindex

   405

** @param Callback

   406

**

   407

** @return

   408

**/

   409

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

   410

{

   411

  UNS32 errorCode;

   412

  ODCallback_t *CallbackList;

   413

   414

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

   415

  if(errorCode == OD_SUCCESSFUL && CallbackList)

   416

    CallbackList[bSubindex] = Callback;

   417

  return errorCode;

   418

}

   419

   420

/*!

   421

**

   422

**

   423

** @param wIndex

   424

** @param bSubindex

   425

**/

   426

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