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

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

   142

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

   143

   144

#  ifdef CANOPEN_BIG_ENDIAN

   148

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

   149

      UNS8 i, j = 0;

   150

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

   151

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

   152

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

   153

        MSG_WAR(i," ", j);

   154

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

   155

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

   156

      }

   157

    }

   158

    else /* no endianisation change */

   159

#  endif

   160

    if(*pDataType != visible_string) {

   161

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

   162

        *pExpectedSize = szData;

   163

    }else{

   164

        /* TODO : CONFORM TO DS-301 : 

   165

         *  - stop using NULL terminated strings

   166

         *  - store string size in td_subindex 

   167

         * */

   168

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

   169

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

   170

        UNS8 *ptr_start = ptr;

   171

        UNS8 *ptr_end = ptr + *pExpectedSize; /* *pExpectedSize IS < szData */ 

   172

        UNS8 *ptr_dest = (UNS8*)pDestData;

   173

        while( *ptr && ptr < ptr_end){

   174

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

   175

        } 

   176

   177

        *pExpectedSize = ptr - ptr_start;

   178

        /* terminate string if not maximum length */

   179

        if (*pExpectedSize < szData) 

   180

            *(ptr) = 0; 

   181

    }

   182

   183

    return OD_SUCCESSFUL;

   184

  }

   185

  else { /* Error ! */

   186

    *pExpectedSize = szData;

   187

    accessDictionaryError(wIndex, bSubindex, szData,

   188

                          *pExpectedSize, OD_LENGTH_DATA_INVALID);

   189

    return OD_LENGTH_DATA_INVALID;

   190

  }

   191

}

   192

   193

/*!

   194

**

   195

**

   196

** @param d

   197

** @param wIndex

   198

** @param bSubindex

   199

** @param pDestData

   200

** @param pExpectedSize

   201

** @param pDataType

   202

** @param checkAccess

   203

**

   204

** @return

   205

**/

   206

UNS32 getODentry( CO_Data* d,

   207

                  UNS16 wIndex,

   208

                  UNS8 bSubindex,

   209

                  void * pDestData,

   210

                  UNS8 * pExpectedSize,

   211

                  UNS8 * pDataType,

   212

                  UNS8 checkAccess)

   213

{

   214

  return _getODentry( d,

   215

                      wIndex,

   216

                      bSubindex,

   217

                      pDestData,

   218

                      pExpectedSize,

   219

                      pDataType,

   220

                      checkAccess,

   221

                      1);//endianize

   222

}

   223

   224

/*!

   225

**

   226

**

   227

** @param d

   228

** @param wIndex

   229

** @param bSubindex

   230

** @param pDestData

   231

** @param pExpectedSize

   232

** @param pDataType

   233

** @param checkAccess

   234

**

   235

** @return

   236

**/

   237

UNS32 readLocalDict( CO_Data* d,

   238

                     UNS16 wIndex,

   239

                     UNS8 bSubindex,

   240

                     void * pDestData,

   241

                     UNS8 * pExpectedSize,

   242

                     UNS8 * pDataType,

   243

                     UNS8 checkAccess)

   244

{

   245

  return _getODentry( d,

   246

                      wIndex,

   247

                      bSubindex,

   248

                      pDestData,

   249

                      pExpectedSize,

   250

                      pDataType,

   251

                      checkAccess,

   252

                      0);//do not endianize

   253

}

   254

   255

/*!

   256

**

   257

**

   258

** @param d

   259

** @param wIndex

   260

** @param bSubindex

   261

** @param pSourceData

   262

** @param pExpectedSize

   263

** @param checkAccess

   264

** @param endianize

   265

**

   266

** @return

   267

**/

   268

UNS32 _setODentry( CO_Data* d,

   269

                   UNS16 wIndex,

   270

                   UNS8 bSubindex,

   271

                   void * pSourceData,

   272

                   UNS8 * pExpectedSize,

   273

                   UNS8 checkAccess,

   274

                   UNS8 endianize)

   275

{

   276

  UNS8 szData;

   277

  UNS8 dataType;

   278

  UNS32 errorCode;

   279

  const indextable *ptrTable;

   280

  ODCallback_t *Callback;

   281

   282

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

   283

  if (errorCode != OD_SUCCESSFUL)

   284

    return errorCode;

   285

   286

  if( ptrTable->bSubCount <= bSubindex ) {

   287

    /* Subindex not found */

   288

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

   289

    return OD_NO_SUCH_SUBINDEX;

   290

  }

   291

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

   292

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

   293

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

   294

    return OD_WRITE_NOT_ALLOWED;

   295

  }

   296

   297

   298

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

   299

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

   300

   301

  if( *pExpectedSize == 0 ||

   302

      *pExpectedSize == szData ||

   303

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

   304

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

   305

    {

   306

#ifdef CANOPEN_BIG_ENDIAN

   307

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

   308

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

   309

            dataType >= visible_string && 

   310

            domain <= dataType))

   311

        {

   312

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

   313

            testing without */

   314

          /* additional temp variable */

   315

          UNS8 i;

   316

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

   317

            {

   318

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

   319

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

   320

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

   321

            }

   322

        }

   323

#endif

   324

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

   325

      if (errorCode) {

   326

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

   327

        return errorCode;

   328

      }

   329

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

   330

     /* TODO : CONFORM TO DS-301 : 

   331

      *  - stop using NULL terminated strings

   332

      *  - store string size in td_subindex 

   333

      * */

   334

      /* terminate visible_string with '\0' */

   335

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

   336

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

   337

   338

      *pExpectedSize = szData;

   339

   340

      /* Callbacks */

   341

      if(Callback && Callback[bSubindex]){

   342

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

   343

      }

   344

   345

      /* TODO : Store dans NVRAM */

   346

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

   347

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

   348

      }

   349

      return OD_SUCCESSFUL;

   350

    }else{

   351

      *pExpectedSize = szData;

   352

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

   353

      return OD_LENGTH_DATA_INVALID;

   354

    }

   355

}

   356

   357

/*!

   358

**

   359

**

   360

** @param d

   361

** @param wIndex

   362

** @param bSubindex

   363

** @param pSourceData

   364

** @param pExpectedSize

   365

** @param checkAccess

   366

**

   367

** @return

   368

**/

   369

UNS32 setODentry( CO_Data* d,

   370

                  UNS16 wIndex,

   371

                  UNS8 bSubindex,

   372

                  void * pSourceData,

   373

                  UNS8 * pExpectedSize,

   374

                  UNS8 checkAccess)

   375

{

   376

  return _setODentry( d,

   377

                      wIndex,

   378

                      bSubindex,

   379

                      pSourceData,

   380

                      pExpectedSize,

   381

                      checkAccess,

   382

                      1);//endianize

   383

}

   384

   385

/*!

   386

**

   387

**

   388

** @param d

   389

** @param wIndex

   390

** @param bSubindex

   391

** @param pSourceData

   392

** @param pExpectedSize

   393

** @param checkAccess

   394

**

   395

** @return

   396

**/

   397

UNS32 writeLocalDict( CO_Data* d,

   398

                      UNS16 wIndex,

   399

                      UNS8 bSubindex,

   400

                      void * pSourceData,

   401

                      UNS8 * pExpectedSize,

   402

                      UNS8 checkAccess)

   403

{

   404

  return _setODentry( d,

   405

                      wIndex,

   406

                      bSubindex,

   407

                      pSourceData,

   408

                      pExpectedSize,

   409

                      checkAccess,

   410

                      0);//do not endianize

   411

}

   412

   413

/*!

   414

**

   415

**

   416

** @param d

   417

** @param wIndex

   418

** @param errorCode

   419

** @param Callback

   420

**

   421

** @return

   422

**/

   423

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

   424

{

   425

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

   426

}

   427

   428

/*!

   429

**

   430

**

   431

** @param d

   432

** @param wIndex

   433

** @param bSubindex

   434

** @param Callback

   435

**

   436

** @return

   437

**/

   438

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

   439

{

   440

  UNS32 errorCode;

   441

  ODCallback_t *CallbackList;

   442

   443

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

   444

  if(errorCode == OD_SUCCESSFUL && CallbackList)

   445

    CallbackList[bSubindex] = Callback;

   446

  return errorCode;

   447

}

   448

   449

/*!

   450

**

   451

**

   452

** @param wIndex

   453

** @param bSubindex

   454

**/

   455

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