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

#include "pdo.h"

    25

#include "objacces.h"

    26

#include "canfestival.h"

    27

    28

/*!

    29

** @file   pdo.c

    30

** @author Edouard TISSERANT and Francis DUPIN

    31

** @date   Tue Jun  5 09:32:32 2007

    32

**

    33

** @brief

    34

**

    35

**

    36

*/

    37

UNS8 sendPDO(CO_Data* d, s_PDO pdo, UNS8 req)

    38

{

    39

  UNS8 i;

    40

  if( d->nodeState == Operational ) {

    41

    Message m;

    42

    43

    /*! Message copy for sending */

    44

    m.cob_id.w = pdo.cobId & 0x7FF; /*! Because the cobId is 11 bytes

    45

                                      length */

    46

    if ( req == NOT_A_REQUEST ) {

    47

      UNS8 i;

    48

      m.rtr = NOT_A_REQUEST;

    49

      m.len = pdo.len;

    50

      /*! memcpy(&m.data, &pdo.data, m.len); */

    51

      /*! This Memcpy depends on packing structure. Avoid */

    52

      for (i = 0 ; i < pdo.len ; i++)

    53

        m.data[i] = pdo.data[i];

    54

    }

    55

    else {

    56

      m.rtr = REQUEST;

    57

      m.len = 0;

    58

    }

    59

    60

    MSG_WAR(0x3901, "sendPDO cobId :", m.cob_id.w);

    61

    MSG_WAR(0x3902,  "     Nb octets  : ",  m.len);

    62

    for (i = 0 ; i < m.len ; i++) {

    63

      MSG_WAR(0x3903,"           data : ", m.data[i]);

    64

    }

    65

    66

    return canSend(d->canHandle,&m);

    67

  } /*! end if */

    68

  return 0xFF;

    69

}

    70

    71

/*!

    72

** PDO Manager

    73

**

    74

** @param d

    75

** @param cobId

    76

**

    77

** @return

    78

**/

    79

UNS8 PDOmGR(CO_Data* d, UNS32 cobId)

    80

{

    81

  UNS8 res;

    82

  UNS8 i;

    83

  s_PDO pdo;

    84

    85

  MSG_WAR(0x3905, "PDOmGR",0);

    86

    87

  /*! if PDO is waiting for transmission,

    88

    preparation of the message to send */

    89

  pdo.cobId = cobId;

    90

  pdo.len =  d->process_var.count;

    91

  /*! memcpy(&(pdo.data), &(process_var.data), pdo.len); */

    92

     /* Ce memcpy devrait tre portable */

    93

    for ( i = 0 ; i < pdo.len ; i++) 

    94

      pdo.data[i] = d->process_var.data[i];

    95

    96

    res = sendPDO(d, pdo, NOT_A_REQUEST);

    97

    98

    return res;

    99

}

   100

   101

#if 0

   102

/*********************************************************************/

   103

/* TODO : implement bit mapping                          			 */

   104

/*********************************************************************/

   105

   106

UNS8 buildPDO(CO_Data* d, UNS16 index)

   107

{ /* DO NOT USE MSG_ERR because the macro may send a PDO -> infinite loop if it fails. */	

   108

  UNS16 ind;

   109

  UNS8      subInd;

   110

   111

  UNS8 *     pMappingCount = NULL;      /* count of mapped objects... */

   112

  /* pointer to the var which is mapped to a pdo */

   113

/*  void *     pMappedAppObject = NULL;  */

   114

  /* pointer fo the var which holds the mapping parameter of an mapping entry  */ 

   115

   116

  UNS32 *    pMappingParameter = NULL;

   117

   118

  UNS8      Size;

   119

  UNS8      dataType;

   120

  UNS8      offset;

   121

  UNS16     offsetObjdict;

   122

  UNS16     offsetObjdictPrm;

   123

  UNS32     objDict;

   124

   125

  subInd=(UNS8)0x00;

   126

  offset = 0x00;

   127

  ind = index - 0x1800;

   128

   129

  MSG_WAR(0x3910,"Prepare PDO to send index :", index);

   130

   131

  /*! only operational state allows PDO transmission */

   132

  if( d->nodeState != Operational ) {

   133

    MSG_WAR(0x2911, "Unable to send the PDO (node not in OPERATIONAL mode). Node : ", index);

   134

    return 0xFF;

   135

  }

   136

  offsetObjdictPrm = d->firstIndex->PDO_TRS;

   137

  offsetObjdict = d->firstIndex->PDO_TRS_MAP;

   138

   139

  if (offsetObjdictPrm && offsetObjdict)

   140

    {

   141

      /*! get mapped objects number to transmit with this PDO */

   142

      pMappingCount = (d->objdict + offsetObjdict + ind)->pSubindex[0].pObject;

   143

      MSG_WAR(0x3912, "Nb maped objects : ",* pMappingCount);

   144

      MSG_WAR(0x3913, "        at index : ", 0x1A00 + ind);

   145

      while (subInd < *pMappingCount) { /*! Loop on mapped variables */

   146

        /*! get mapping parameters */

   147

        pMappingParameter = (d->objdict + offsetObjdict + ind)->pSubindex[subInd + 1].pObject;

   148

        MSG_WAR(0x3914, "Get the mapping      at index : ", (UNS16)0x1A00 + ind);

   149

        MSG_WAR(0x3915, "                     subIndex : ", subInd + 1);

   150

        MSG_WAR(0x3916, "                     value    : ", *(UNS32 *)pMappingParameter);

   151

        /*! Get the mapped variable */

   152

        Size = ((UNS8)(((*pMappingParameter) & 0xFF) >> 3));

   153

        objDict = getODentry(d, (UNS16)((*pMappingParameter) >> 16),

   154

                             (UNS8)(((*pMappingParameter) >> 8 ) & 0x000000FF),

   155

                             (void *)&d->process_var.data[offset], &Size, &dataType, 0 );

   156

   157

        if (objDict != OD_SUCCESSFUL) {

   158

          MSG_WAR(0x2919, "error accessing to the mapped var : ", subInd + 1);

   159

          MSG_WAR(0x2920, "         Mapped at index : ", (*pMappingParameter) >> 16);

   160

          MSG_WAR(0x2921, "                subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF);

   161

          return 0xFF;

   162

        }

   163

   164

        offset += Size;

   165

        d->process_var.count = offset;

   166

        subInd++;

   167

      }/*! end Loop on mapped variables  */

   168

    }

   169

  return 0;

   170

}

   171

#endif

   172

   173

/*!

   174

**

   175

**

   176

** @param d

   177

** @param cobId

   178

**

   179

** @return

   180

**/

   181

UNS8 sendPDOrequest( CO_Data* d, UNS32 cobId )

   182

{

   183

  UNS32 * pwCobId;

   184

  UNS16          offset;

   185

  UNS16          lastIndex;

   186

  UNS8           err;

   187

   188

  MSG_WAR(0x3930, "sendPDOrequest ",0);

   189

  /*! Sending the request only if the cobid have been found on the PDO

   190

     receive */

   191

  /*! part dictionary */

   192

  offset = d->firstIndex->PDO_RCV;

   193

  lastIndex = d->lastIndex->PDO_RCV;

   194

  if (offset)

   195

    while (offset <= lastIndex) {

   196

      /*! get the CobId*/

   197

      pwCobId = d->objdict[offset].pSubindex[1].pObject;

   198

   199

      if ( *pwCobId  == cobId ) {

   200

        s_PDO pdo;

   201

        pdo.cobId = *pwCobId;

   202

        pdo.len = 0;

   203

        err  = sendPDO(d, pdo, REQUEST);

   204

        return err;

   205

      }

   206

      offset++;

   207

    }

   208

  MSG_WAR(0x1931, "sendPDOrequest : COBID not found : ", cobId);

   209

  return 0xFF;

   210

}

   211

   212

   213

/*!

   214

**

   215

**

   216

** @param d

   217

** @param m

   218

**

   219

** @return

   220

**/

   221

UNS8 proceedPDO(CO_Data* d, Message *m)

   222

{

   223

  UNS8   numPdo;

   224

  UNS8   numMap;  /*! Number of the mapped varable */

   225

  UNS8 i;

   226

  UNS8 *     pMappingCount = NULL;    /*! count of mapped objects... */

   227

  /*! pointer to the var which is mapped to a pdo... */

   228

  /*!  void *     pMappedAppObject = NULL;   */

   229

  /*! pointer fo the var which holds the mapping parameter of an

   230

     mapping entry */

   231

  UNS32 *    pMappingParameter = NULL;

   232

  UNS8  *    pTransmissionType = NULL; /*! pointer to the transmission

   233

                                         type */

   234

  UNS32 *    pwCobId = NULL;

   235

  UNS8       Size;

   236

  UNS8       dataType;

   237

  UNS8       offset;

   238

  UNS8       status;

   239

  UNS32      objDict;

   240

  UNS16      offsetObjdict;

   241

  UNS16      lastIndex;

   242

  status = state1;

   243

   244

  MSG_WAR(0x3935, "proceedPDO, cobID : ", ((*m).cob_id.w & 0x7ff));

   245

  offset = 0x00;

   246

  numPdo = 0;

   247

  numMap = 0;

   248

  if((*m).rtr == NOT_A_REQUEST ) { /*! The PDO received is not a

   249

                                     request. */

   250

    offsetObjdict = d->firstIndex->PDO_RCV;

   251

    lastIndex = d->lastIndex->PDO_RCV;

   252

   253

    /*! study of all the PDO stored in the dictionary */

   254

    if(offsetObjdict)

   255

      while (offsetObjdict <= lastIndex) {

   256

   257

        switch( status ) {

   258

   259

        case state1:/*! data are stored in process_var array */

   260

          /*! memcpy(&(process_var.data), &m->data, (*m).len); */

   261

          /*! Ce memcpy devrait etre portable */

   262

          for ( i = 0 ; i < m->len ; i++)

   263

            d->process_var.data[i] = m->data[i];

   264

          d->process_var.count = (*m).len;

   265

   266

          status = state2;

   267

          break;

   268

   269

        case state2:

   270

          /*! get CobId of the dictionary correspondant to the received

   271

             PDO */

   272

          pwCobId = d->objdict[offsetObjdict].pSubindex[1].pObject;

   273

          /*! check the CobId coherance */

   274

          /*!pwCobId is the cobId read in the dictionary at the state 3

   275

            */

   276

          if ( *pwCobId == (*m).cob_id.w ){

   277

            /*! The cobId is recognized */

   278

            status = state4;

   279

            MSG_WAR(0x3936, "cobId found at index ", 0x1400 + numPdo);

   280

            break;

   281

          }

   282

          else {

   283

            /*! cobId received does not match with those write in the

   284

              dictionnary */

   285

            numPdo++;

   286

            offsetObjdict++;

   287

            status = state2;

   288

            break;

   289

          }

   290

   291

            case state4:/*! Get Mapped Objects Number */

   292

               /*! The cobId of the message received has been found in the

   293

                 dictionnary. */

   294

               offsetObjdict = d->firstIndex->PDO_RCV_MAP;

   295

             lastIndex = d->lastIndex->PDO_RCV_MAP;

   296

             pMappingCount = (d->objdict + offsetObjdict + numPdo)->pSubindex[0].pObject;

   297

             numMap = 0;

   298

             while (numMap < *pMappingCount) {

   299

               UNS8 tmp[]= {0,0,0,0,0,0,0,0};

   300

               UNS8 ByteSize;

   301

               pMappingParameter = (d->objdict + offsetObjdict + numPdo)->pSubindex[numMap + 1].pObject;

   302

               if (pMappingParameter == NULL) {

   303

                 MSG_ERR(0x1937, "Couldn't get mapping parameter : ", numMap + 1);

   304

                 return 0xFF;

   305

               }

   306

               /*! Get the addresse of the mapped variable. */

   307

               /*! detail of *pMappingParameter : */

   308

               /*! The 16 hight bits contains the index, the medium 8 bits

   309

                 contains the subindex, */

   310

               /*! and the lower 8 bits contains the size of the mapped

   311

                 variable. */

   312

   313

               Size = (UNS8)(*pMappingParameter);

   314

   315

               /*! copy bit per bit in little endian */

   316

               CopyBits(Size, (UNS8*)&d->process_var.data[offset>>3], offset%8, 0, ((UNS8*)tmp), 0, 0);

   317

   318

               ByteSize = 1 + ((Size - 1) >> 3); /*1->8 => 1 ; 9->16 =>

   319

                                                   2, ... */

   320

   321

               objDict = setODentry(d, (UNS16)((*pMappingParameter) >> 16),

   322

                                    (UNS8)(((*pMappingParameter) >> 8 ) & 0xFF),

   323

                                 tmp, &ByteSize, 0 );

   324

   325

               if(objDict != OD_SUCCESSFUL) {

   326

                 MSG_ERR(0x1938, "error accessing to the mapped var : ", numMap + 1);

   327

                 MSG_WAR(0x2939, "         Mapped at index : ", (*pMappingParameter) >> 16);

   328

                 MSG_WAR(0x2940, "                subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF);

   329

                 return 0xFF;

   330

               }

   331

   332

               MSG_WAR(0x3942, "Variable updated with value received by PDO cobid : ", m->cob_id.w);

   333

               MSG_WAR(0x3943, "         Mapped at index : ", (*pMappingParameter) >> 16);

   334

               MSG_WAR(0x3944, "                subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF);

   335

               /*! MSG_WAR(0x3945, "                data : ",*((UNS32*)pMappedAppObject)); */

   336

               offset += Size;

   337

               numMap++;

   338

             } /*! end loop while on mapped variables */

   339

   340

             offset=0x00;

   341

             numMap = 0;

   342

             return 0;

   343

   344

        }/*! end switch status*/

   345

      }/*! end while*/

   346

  }/*! end if Donnees */

   347

  else if ((*m).rtr == REQUEST ){

   348

    MSG_WAR(0x3946, "Receive a PDO request cobId : ", m->cob_id.w);

   349

    status = state1;

   350

    offsetObjdict = d->firstIndex->PDO_TRS;

   351

    lastIndex = d->lastIndex->PDO_TRS;

   352

    if(offsetObjdict) while( offsetObjdict  <= lastIndex ){

   353

      /*! study of all PDO stored in the objects dictionary */

   354

   355

      switch( status ){

   356

   357

      case state1:/*! check the CobId */

   358

        /*! get CobId of the dictionary which match to the received PDO

   359

         */

   360

        pwCobId = (d->objdict + offsetObjdict)->pSubindex[1].pObject;

   361

        if ( *pwCobId == (*m).cob_id.w ) {

   362

          status = state4;

   363

          break;

   364

        }

   365

        else {

   366

          numPdo++;

   367

          offsetObjdict++;

   368

        }

   369

        status = state1;

   370

        break;

   371

   372

   373

      case state4:/*! check transmission type (after request?) */

   374

        pTransmissionType = d->objdict[offsetObjdict].pSubindex[2].pObject;

   375

        if ( (*pTransmissionType == TRANS_RTR) || (*pTransmissionType == TRANS_RTR_SYNC ) || (*pTransmissionType == TRANS_EVENT) ) {

   376

          status = state5;

   377

          break;

   378

        }

   379

        else {

   380

          /*! The requested PDO is not to send on request. So, does

   381

            nothing. */

   382

          MSG_WAR(0x2947, "PDO is not to send on request : ", m->cob_id.w);

   383

          return 0xFF;

   384

        }

   385

   386

      case state5:/*! get mapped objects number */

   387

        offsetObjdict = d->firstIndex->PDO_TRS_MAP;

   388

        lastIndex = d->lastIndex->PDO_TRS_MAP;

   389

        pMappingCount = (d->objdict + offsetObjdict + numPdo)->pSubindex[0].pObject;

   390

        numMap = 0;

   391

        while (numMap < *pMappingCount) {

   392

          pMappingParameter = (d->objdict + offsetObjdict + numPdo)->pSubindex[numMap + 1].pObject;

   393

          /*! Get the mapped variable */

   394

          Size = ((UNS8)(((*pMappingParameter) & 0xFF) >> 3));

   395

          objDict = getODentry( d, (UNS16)((*pMappingParameter) >> (UNS8)16),

   396

                                (UNS8)(( (*pMappingParameter) >> (UNS8)8 ) & 0xFF),

   397

                                (void *)&d->process_var.data[offset], &Size, &dataType, 0 );

   398

          if (objDict != OD_SUCCESSFUL) {

   399

            MSG_ERR(0x1948, "error accessing to the mapped var : ", numMap + 1);

   400

            MSG_WAR(0x2949, "         Mapped at index : ", (*pMappingParameter) >> 16);

   401

            MSG_WAR(0x2950, "                subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF);

   402

            return 0xFF;

   403

          }

   404

          offset += (UNS8) (((*pMappingParameter) & 0xFF) >> 3);

   405

          d->process_var.count = offset;

   406

          numMap++;

   407

   408

        } /*! end while */

   409

        PDOmGR( d, *pwCobId ); /*! Transmit the PDO */

   410

        return 0;

   411

   412

      }/*! end switch status */

   413

    }/*! end while */

   414

  }/*! end if Requete */

   415

   416

  return 0;

   417

}

   418

   419

/*!

   420

**

   421

**

   422

** @param NbBits

   423

** @param SrcByteIndex

   424

** @param SrcBitIndex

   425

** @param SrcBigEndian

   426

** @param DestByteIndex

   427

** @param DestBitIndex

   428

** @param DestBigEndian

   429

**/

   430

void CopyBits(UNS8 NbBits, UNS8* SrcByteIndex, UNS8 SrcBitIndex, UNS8 SrcBigEndian, UNS8* DestByteIndex, UNS8 DestBitIndex, UNS8 DestBigEndian)

   431

{

   432

  /*! This loop copy as many bits that it can each time, crossing*/

   433

  /*! successively bytes*/

   434

  // boundaries from LSB to MSB.

   435

  while(NbBits > 0)

   436

    {

   437

      /*! Bit missalignement between src and dest*/

   438

      INTEGER8 Vect = DestBitIndex - SrcBitIndex;

   439

   440

      /*! We can now get src and align it to dest*/

   441

      UNS8 Aligned = Vect>0 ? *SrcByteIndex << Vect : *SrcByteIndex >> -Vect;

   442

   443

      /*! Compute the nb of bit we will be able to copy*/

   444

      UNS8 BoudaryLimit = (Vect>0 ? 8 - DestBitIndex :  8 - SrcBitIndex );

   445

      UNS8 BitsToCopy = BoudaryLimit > NbBits ? NbBits : BoudaryLimit;

   446

   447

      /*! Create a mask that will serve in:*/

   448

      UNS8 Mask = ((0xff << (DestBitIndex + BitsToCopy)) | (0xff >> (8 - DestBitIndex)));

   449

   450

      /*! - Filtering src*/

   451

      UNS8 Filtered = Aligned & ~Mask;

   452

   453

      /*! - and erase bits where we write, preserve where we don't*/

   454

      *DestByteIndex &= Mask;

   455

   456

      /*! Then write.*/

   457

      *DestByteIndex |= Filtered ;

   458

   459

      /*!Compute next time cursors for src*/

   460

      if((SrcBitIndex += BitsToCopy)>7)/*! cross boundary ?*/

   461

        {

   462

          SrcBitIndex = 0;/*! First bit*/

   463

          SrcByteIndex += (SrcBigEndian ? -1 : 1);/*! Next byte*/

   464

        }

   465

   466

   467

      /*!Compute next time cursors for dest*/

   468

      if((DestBitIndex += BitsToCopy)>7)

   469

        {

   470

          DestBitIndex = 0;/*! First bit*/

   471

          DestByteIndex += (DestBigEndian ? -1 : 1);/*! Next byte*/

   472

        }

   473

   474

      /*!And decrement counter.*/

   475

      NbBits -= BitsToCopy;

   476

    }

   477

   478

}

   479

   480

#if 0

   481

   482

/*********************************************************************/

   483

/* TODO : reimplement this using CallBacks

   484

 */

   485

/*********************************************************************/

   486

   487

/*!

   488

**

   489

**

   490

** @param d

   491

** @param variable

   492

**

   493

** @return

   494

**/

   495

UNS8 sendPDOevent( CO_Data* d, void * variable )

   496

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

   497

    loop if it fails.*/

   498

  UNS32           objDict = 0;

   499