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

   145

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

   146

            *pDataType >= visible_string && 

   147

            *pDataType <= domain)) {

   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

      *pExpectedSize = szData;

   158

    }

   159

    else /* no endianisation change */

   160

#  endif

   161

    if(*pDataType != visible_string) {

   162

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

   163

        *pExpectedSize = szData;

   164

    }else{

   165

        /* TODO : CONFORM TO DS-301 : 

   166

         *  - stop using NULL terminated strings

   167

         *  - store string size in td_subindex 

   168

         * */

   169

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

   170

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

   171

        UNS8 *ptr_start = ptr;

   172

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

   173

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

   174

        UNS8 *ptr_dest = (UNS8*)pDestData;

   175

        while( *ptr && ptr < ptr_end){

   176

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

   177

        } 

   178

   179

        *pExpectedSize = ptr - ptr_start;

   180

        /* terminate string if not maximum length */

   181

        if (*pExpectedSize < szData) 

   182

            *(ptr) = 0; 

   183

    }

   184

   185

    return OD_SUCCESSFUL;

   186

  }

   187

  else { /* Error ! */

   188

    *pExpectedSize = szData;

   189

    accessDictionaryError(wIndex, bSubindex, szData,

   190

                          *pExpectedSize, OD_LENGTH_DATA_INVALID);

   191

    return OD_LENGTH_DATA_INVALID;

   192

  }

   193

}

   194

   195

/*!

   196

**

   197

**

   198

** @param d

   199

** @param wIndex

   200

** @param bSubindex

   201

** @param pDestData

   202

** @param pExpectedSize

   203

** @param pDataType

   204

** @param checkAccess

   205

**

   206

** @return

   207

**/

   208

UNS32 getODentry( CO_Data* d,

   209

                  UNS16 wIndex,

   210

                  UNS8 bSubindex,

   211

                  void * pDestData,

   212

                  UNS8 * pExpectedSize,

   213

                  UNS8 * pDataType,

   214

                  UNS8 checkAccess)

   215

{

   216

  return _getODentry( d,

   217

                      wIndex,

   218

                      bSubindex,

   219

                      pDestData,

   220

                      pExpectedSize,

   221

                      pDataType,

   222

                      checkAccess,

   223

                      1);//endianize

   224

}

   225

   226

/*!

   227

**

   228

**

   229

** @param d

   230

** @param wIndex

   231

** @param bSubindex

   232

** @param pDestData

   233

** @param pExpectedSize

   234

** @param pDataType

   235

** @param checkAccess

   236

**

   237

** @return

   238

**/

   239

UNS32 readLocalDict( CO_Data* d,

   240

                     UNS16 wIndex,

   241

                     UNS8 bSubindex,

   242

                     void * pDestData,

   243

                     UNS8 * pExpectedSize,

   244

                     UNS8 * pDataType,

   245

                     UNS8 checkAccess)

   246

{

   247

  return _getODentry( d,

   248

                      wIndex,

   249

                      bSubindex,

   250

                      pDestData,

   251

                      pExpectedSize,

   252

                      pDataType,

   253

                      checkAccess,

   254

                      0);//do not endianize

   255

}

   256

   257

/*!

   258

**

   259

**

   260

** @param d

   261

** @param wIndex

   262

** @param bSubindex

   263

** @param pSourceData

   264

** @param pExpectedSize

   265

** @param checkAccess

   266

** @param endianize

   267

**

   268

** @return

   269

**/

   270

UNS32 _setODentry( CO_Data* d,

   271

                   UNS16 wIndex,

   272

                   UNS8 bSubindex,

   273

                   void * pSourceData,

   274

                   UNS8 * pExpectedSize,

   275

                   UNS8 checkAccess,

   276

                   UNS8 endianize)

   277

{

   278

  UNS8 szData;

   279

  UNS8 dataType;

   280

  UNS32 errorCode;

   281

  const indextable *ptrTable;

   282

  ODCallback_t *Callback;

   283

   284

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

   285

  if (errorCode != OD_SUCCESSFUL)

   286

    return errorCode;

   287

   288

  if( ptrTable->bSubCount <= bSubindex ) {

   289

    /* Subindex not found */

   290

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

   291

    return OD_NO_SUCH_SUBINDEX;

   292

  }

   293

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

   294

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

   295

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

   296

    return OD_WRITE_NOT_ALLOWED;

   297

  }

   298

   299

   300

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

   301

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

   302

   303

  if( *pExpectedSize == 0 ||

   304

      *pExpectedSize == szData ||

   305

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

   306

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

   307

    {

   308

#ifdef CANOPEN_BIG_ENDIAN

   309

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

   310

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

   311

            dataType >= visible_string && 

   312

            dataType <= domain))

   313

        {

   314

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

   315

            testing without */

   316

          /* additional temp variable */

   317

          UNS8 i;

   318

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

   319

            {

   320

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

   321

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

   322

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

   323

            }

   324

        }

   325

#endif

   326

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

   327

      if (errorCode) {

   328

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

   329

        return errorCode;

   330

      }

   331

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

   332

     /* TODO : CONFORM TO DS-301 : 

   333

      *  - stop using NULL terminated strings

   334

      *  - store string size in td_subindex 

   335

      * */

   336

      /* terminate visible_string with '\0' */

   337

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

   338

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

   339

   340

      *pExpectedSize = szData;

   341

   342

      /* Callbacks */

   343

      if(Callback && Callback[bSubindex]){

   344

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

   345

        if(errorCode != OD_SUCCESSFUL)

   346

        {

   347

            return errorCode;

   348

        }

   349

       }

   350

   351

      /* TODO : Store dans NVRAM */

   352

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

   353

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

   354

      }

   355

      return OD_SUCCESSFUL;

   356

    }else{

   357

      *pExpectedSize = szData;

   358

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

   359

      return OD_LENGTH_DATA_INVALID;

   360

    }

   361

}

   362

   363

/*!

   364

**

   365

**

   366

** @param d

   367

** @param wIndex

   368

** @param bSubindex

   369

** @param pSourceData

   370

** @param pExpectedSize

   371

** @param checkAccess

   372

**

   373

** @return

   374

**/

   375

UNS32 setODentry( CO_Data* d,

   376

                  UNS16 wIndex,

   377

                  UNS8 bSubindex,

   378

                  void * pSourceData,

   379

                  UNS8 * pExpectedSize,

   380

                  UNS8 checkAccess)

   381

{

   382

  return _setODentry( d,

   383

                      wIndex,

   384

                      bSubindex,

   385

                      pSourceData,

   386

                      pExpectedSize,

   387

                      checkAccess,

   388

                      1);//endianize

   389

}

   390

   391

/*!

   392

**

   393

**

   394

** @param d

   395

** @param wIndex

   396

** @param bSubindex

   397

** @param pSourceData

   398

** @param pExpectedSize

   399

** @param checkAccess

   400

**

   401

** @return

   402

**/

   403

UNS32 writeLocalDict( CO_Data* d,

   404

                      UNS16 wIndex,

   405

                      UNS8 bSubindex,

   406

                      void * pSourceData,

   407

                      UNS8 * pExpectedSize,

   408

                      UNS8 checkAccess)

   409

{

   410

  return _setODentry( d,

   411

                      wIndex,

   412

                      bSubindex,

   413

                      pSourceData,

   414

                      pExpectedSize,

   415

                      checkAccess,

   416

                      0);//do not endianize

   417

}

   418

   419

/*!

   420

**

   421

**

   422

** @param d

   423

** @param wIndex

   424

** @param errorCode

   425

** @param Callback

   426

**

   427

** @return

   428

**/

   429

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

   430

{

   431

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

   432

}

   433

   434

/*!

   435

**

   436

**

   437

** @param d

   438

** @param wIndex

   439

** @param bSubindex

   440

** @param Callback

   441

**

   442

** @return

   443

**/

   444

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

   445

{

   446

  UNS32 errorCode;

   447

  ODCallback_t *CallbackList;

   448

   449

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

   450

  if(errorCode == OD_SUCCESSFUL && CallbackList)

   451

    CallbackList[bSubindex] = Callback;

   452

  return errorCode;

   453

}

   454

   455

/*!

   456

**

   457

**

   458

** @param wIndex

   459

** @param bSubindex

   460

**/

   461

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