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 "objacces.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

#if 0

   165

    /* Me laisser a, please ! (FD) */

   166

    {

   167

      UNS8 i;

   168

      for (i = 0 ; i < 10 ; i++) {

   169

        MSG_WAR(*pExpectedSize, "dic data= ",

   170

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

   171

      }

   172

   173

    }

   174

#endif

   175

    return OD_SUCCESSFUL;

   176

  }

   177

  else { /* Error ! */

   178

    *pExpectedSize = szData;

   179

    accessDictionaryError(wIndex, bSubindex, szData,

   180

                          *pExpectedSize, OD_LENGTH_DATA_INVALID);

   181

    return OD_LENGTH_DATA_INVALID;

   182

  }

   183

}

   184

   185

/*!

   186

**

   187

**

   188

** @param d

   189

** @param wIndex

   190

** @param bSubindex

   191

** @param pDestData

   192

** @param pExpectedSize

   193

** @param pDataType

   194

** @param checkAccess

   195

**

   196

** @return

   197

**/

   198

UNS32 getODentry( CO_Data* d,

   199

                  UNS16 wIndex,

   200

                  UNS8 bSubindex,

   201

                  void * pDestData,

   202

                  UNS8 * pExpectedSize,

   203

                  UNS8 * pDataType,

   204

                  UNS8 checkAccess)

   205

{

   206

  return _getODentry( d,

   207

                      wIndex,

   208

                      bSubindex,

   209

                      pDestData,

   210

                      pExpectedSize,

   211

                      pDataType,

   212

                      checkAccess,

   213

                      1);//endianize

   214

}

   215

   216

/*!

   217

**

   218

**

   219

** @param d

   220

** @param wIndex

   221

** @param bSubindex

   222

** @param pDestData

   223

** @param pExpectedSize

   224

** @param pDataType

   225

** @param checkAccess

   226

**

   227

** @return

   228

**/

   229

UNS32 readLocalDict( CO_Data* d,

   230

                     UNS16 wIndex,

   231

                     UNS8 bSubindex,

   232

                     void * pDestData,

   233

                     UNS8 * pExpectedSize,

   234

                     UNS8 * pDataType,

   235

                     UNS8 checkAccess)

   236

{

   237

  return _getODentry( d,

   238

                      wIndex,

   239

                      bSubindex,

   240

                      pDestData,

   241

                      pExpectedSize,

   242

                      pDataType,

   243

                      checkAccess,

   244

                      0);//do not endianize

   245

}

   246

   247

/*!

   248

**

   249

**

   250

** @param d

   251

** @param wIndex

   252

** @param bSubindex

   253

** @param pSourceData

   254

** @param pExpectedSize

   255

** @param checkAccess

   256

** @param endianize

   257

**

   258

** @return

   259

**/

   260

UNS32 _setODentry( CO_Data* d,

   261

                   UNS16 wIndex,

   262

                   UNS8 bSubindex,

   263

                   void * pSourceData,

   264

                   UNS8 * pExpectedSize,

   265

                   UNS8 checkAccess,

   266

                   UNS8 endianize)

   267

{

   268

  UNS8 szData;

   269

  UNS8 dataType;

   270

  UNS32 errorCode;

   271

  const indextable *ptrTable;

   272

  ODCallback_t *Callback;

   273

   274

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

   275

  if (errorCode != OD_SUCCESSFUL)

   276

    return errorCode;

   277

   278

  if( ptrTable->bSubCount <= bSubindex ) {

   279

    /* Subindex not found */

   280

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

   281

    return OD_NO_SUCH_SUBINDEX;

   282

  }

   283

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

   284

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

   285

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

   286

    return OD_WRITE_NOT_ALLOWED;

   287

  }

   288

   289

   290

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

   291

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

   292

   293

  if( *pExpectedSize == 0 ||

   294

      *pExpectedSize == szData ||

   295

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

   296

                                                                  allow to store a shorter string than entry size */

   297

    {

   298

#ifdef CANOPEN_BIG_ENDIAN

   299

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

   300

        {

   301

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

   302

            testing without */

   303

          /* additional temp variable */

   304

          UNS8 i;

   305

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

   306

            {

   307

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

   308

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

   309

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

   310

            }

   311

        }

   312

#endif

   313

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

   314

      if (errorCode) {

   315

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

   316

        return errorCode;

   317

      }

   318

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

   319

      *pExpectedSize = szData;

   320

   321

      /* Callbacks */

   322

      if(Callback && Callback[bSubindex]){

   323

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

   324

      }

   325

   326

      /* TODO : Store dans NVRAM */

   327

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

   328

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

   329

      }

   330

      return OD_SUCCESSFUL;

   331

    }else{

   332

      *pExpectedSize = szData;

   333

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

   334

      return OD_LENGTH_DATA_INVALID;

   335

    }

   336

}

   337

   338

/*!

   339

**

   340

**

   341

** @param d

   342

** @param wIndex

   343

** @param bSubindex

   344

** @param pSourceData

   345

** @param pExpectedSize

   346

** @param checkAccess

   347

**

   348

** @return

   349

**/

   350

UNS32 setODentry( CO_Data* d,

   351

                  UNS16 wIndex,

   352

                  UNS8 bSubindex,

   353

                  void * pSourceData,

   354

                  UNS8 * pExpectedSize,

   355

                  UNS8 checkAccess)

   356

{

   357

  return _setODentry( d,

   358

                      wIndex,

   359

                      bSubindex,

   360

                      pSourceData,

   361

                      pExpectedSize,

   362

                      checkAccess,

   363

                      1);//endianize

   364

}

   365

   366

/*!

   367

**

   368

**

   369

** @param d

   370

** @param wIndex

   371

** @param bSubindex

   372

** @param pSourceData

   373

** @param pExpectedSize

   374

** @param checkAccess

   375

**

   376

** @return

   377

**/

   378

UNS32 writeLocalDict( CO_Data* d,

   379

                      UNS16 wIndex,

   380

                      UNS8 bSubindex,

   381

                      void * pSourceData,

   382

                      UNS8 * pExpectedSize,

   383

                      UNS8 checkAccess)

   384

{

   385

  return _setODentry( d,

   386

                      wIndex,

   387

                      bSubindex,

   388

                      pSourceData,

   389

                      pExpectedSize,

   390

                      checkAccess,

   391

                      0);//do not endianize

   392

}

   393

   394

/*!

   395

**

   396

**

   397

** @param d

   398

** @param wIndex

   399

** @param errorCode

   400

** @param Callback

   401

**

   402

** @return

   403

**/

   404

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

   405

{

   406

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

   407

}

   408

   409

/*!

   410

**

   411

**

   412

** @param d

   413

** @param wIndex

   414

** @param bSubindex

   415

** @param Callback

   416

**

   417

** @return

   418

**/

   419

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

   420

{

   421

  UNS32 errorCode;

   422

  ODCallback_t *CallbackList;

   423

   424

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

   425

  if(errorCode == OD_SUCCESSFUL && CallbackList)

   426

    CallbackList[bSubindex] = Callback;

   427

  return errorCode;

   428

}

   429

   430

/*!

   431

**

   432

**

   433

** @param wIndex

   434

** @param bSubindex

   435

**/

   436

void _storeODSubIndex (UNS16 wIndex, UNS8 bSubindex){}