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

#include "sysdep.h"

    28

    29

/*!

    30

** @file   pdo.c

    31

** @author Edouard TISSERANT and Francis DUPIN

    32

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

    33

**

    34

** @brief

    35

**

    36

**

    37

*/

    38

    39

/*!

    40

**

    41

**

    42

** @param d

    43

** @param TPDO_com TPDO communication parameters OD entry

    44

** @param TPDO_map TPDO mapping parameters OD entry

    45

**

    46

** @return

    47

**/

    48

    49

UNS8 buildPDO (CO_Data * d, UNS8 numPdo, Message * pdo)

    50

{

    51

  const indextable *TPDO_com = d->objdict + d->firstIndex->PDO_TRS + numPdo;

    52

  const indextable *TPDO_map = d->objdict + d->firstIndex->PDO_TRS_MAP + numPdo;

    53

    54

  UNS8 prp_j = 0x00;

    55

  UNS32 offset = 0x00000000;

    56

  const UNS8 *pMappingCount = (UNS8 *) TPDO_map->pSubindex[0].pObject;

    57

    58

  pdo->cob_id = (UNS16) UNS16_LE(*(UNS32*)TPDO_com->pSubindex[1].pObject & 0x7FF);

    59

  pdo->rtr = NOT_A_REQUEST;

    60

    61

  MSG_WAR (0x3009, "  PDO CobId is : ",

    62

           *(UNS32 *) TPDO_com->pSubindex[1].pObject);

    63

  MSG_WAR (0x300D, "  Number of objects mapped : ", *pMappingCount);

    64

    65

  do

    66

    {

    67

      UNS8 dataType;            /* Unused */

    68

      UNS8 tmp[] = { 0, 0, 0, 0, 0, 0, 0, 0 };  /* temporary space to hold bits */

    69

    70

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

    71

      UNS32 *pMappingParameter =

    72

        (UNS32 *) TPDO_map->pSubindex[prp_j + 1].pObject;

    73

      UNS16 index = (UNS16) ((*pMappingParameter) >> 16);

    74

      UNS32 Size = (UNS32) (*pMappingParameter & (UNS32) 0x000000FF);     /* Size in bits */

    75

    76

      /* get variable only if Size != 0 and Size is lower than remaining bits in the PDO */

    77

      if (Size && ((offset + Size) <= 64))

    78

        {

    79

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

    80

          UNS8 subIndex =

    81

            (UNS8) (((*pMappingParameter) >> (UNS8) 8) & (UNS32) 0x000000FF);

    82

    83

          MSG_WAR (0x300F, "  got mapping parameter : ", *pMappingParameter);

    84

          MSG_WAR (0x3050, "    at index : ", TPDO_map->index);

    85

          MSG_WAR (0x3051, "    sub-index : ", prp_j + 1);

    86

    87

          if (getODentry (d, index, subIndex, tmp, &ByteSize, &dataType, 0) !=

    88

              OD_SUCCESSFUL)

    89

            {

    90

              MSG_ERR (0x1013,

    91

                       " Couldn't find mapped variable at index-subindex-size : ",

    92

                       (UNS32) (*pMappingParameter));

    93

              return 0xFF;

    94

            }

    95

          /* copy bit per bit in little endian */

    96

          CopyBits ((UNS8) Size, ((UNS8 *) tmp), 0, 0,

    97

                    (UNS8 *) & pdo->data[offset >> 3], (UNS8)(offset % 8), 0);

    98

    99

          offset += Size;

   100

        }

   101

      prp_j++;

   102

    }

   103

  while (prp_j < *pMappingCount);

   104

   105

  pdo->len = (UNS8)(1 + ((offset - 1) >> 3));

   106

   107

  MSG_WAR (0x3015, "  End scan mapped variable", 0);

   108

   109

  return 0;

   110

}

   111

   112

/*!

   113

**

   114

**

   115

** @param d

   116

** @param cobId

   117

**

   118

** @return

   119

**/

   120

UNS8

   121

sendPDOrequest (CO_Data * d, UNS16 RPDOIndex)

   122

{

   123

  UNS32 *pwCobId;

   124

  UNS16 offset = d->firstIndex->PDO_RCV;

   125

  UNS16 lastIndex = d->lastIndex->PDO_RCV;

   126

   127

  if (!d->CurrentCommunicationState.csPDO)

   128

    {

   129

      return 0;

   130

    }

   131

   132

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

   133

     receive */

   134

  /* part dictionary */

   135

   136

  MSG_WAR (0x3930, "sendPDOrequest RPDO Index : ", RPDOIndex);

   137

   138

  if (offset && RPDOIndex >= 0x1400)

   139

    {

   140

      offset += RPDOIndex - 0x1400;

   141

      if (offset <= lastIndex)

   142

        {

   143

          /* get the CobId */

   144

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

   145

   146

          MSG_WAR (0x3930, "sendPDOrequest cobId is : ", *pwCobId);

   147

          {

   148

            Message pdo;

   149

            pdo.cob_id = (UNS16)UNS16_LE(*pwCobId);

   150

            pdo.rtr = REQUEST;

   151

            pdo.len = 0;

   152

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

   153

          }

   154

        }

   155

    }

   156

  MSG_ERR (0x1931, "sendPDOrequest : RPDO Index not found : ", RPDOIndex);

   157

  return 0xFF;

   158

}

   159

   160

   161

/*!

   162

**

   163

**

   164

** @param d

   165

** @param m

   166

**

   167

** @return

   168

**/

   169

UNS8

   170

proceedPDO (CO_Data * d, Message * m)

   171

{

   172

  UNS8 numPdo;

   173

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

   174

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

   175

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

   176

  /*  void *     pMappedAppObject = NULL;   */

   177

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

   178

     mapping entry */

   179

  UNS32 *pMappingParameter = NULL;

   180

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

   181

                                           type */

   182

  UNS32 *pwCobId = NULL;

   183

  UNS8 Size;

   184

  UNS8 offset;

   185

  UNS8 status;

   186

  UNS32 objDict;

   187

  UNS16 offsetObjdict;

   188

  UNS16 lastIndex;

   189

  TIMEVAL EventTimerDuration = 0;

   190

   191

  status = state2;

   192

   193

  MSG_WAR (0x3935, "proceedPDO, cobID : ", (UNS16_LE(m->cob_id) & 0x7ff));

   194

  offset = 0x00;

   195

  numPdo = 0;

   196

  numMap = 0;

   197

  if ((*m).rtr == NOT_A_REQUEST)

   198

    {                           /* The PDO received is not a

   199

                                   request. */

   200

   201

      offsetObjdict = d->firstIndex->PDO_RCV;

   202

      lastIndex = d->lastIndex->PDO_RCV;

   203

   204

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

   205

      if (offsetObjdict)

   206

        while (offsetObjdict <= lastIndex)

   207

          {

   208

   209

            switch (status)

   210

              {

   211

   212

              case state2:

   213

                /* get CobId of the dictionary correspondant to the received

   214

                   PDO */

   215

                pwCobId =

   216

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

   217

                /* check the CobId coherance */

   218

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

   219

                 */

   220

                if (*pwCobId == UNS16_LE(m->cob_id))

   221

                  {

   222

                    /* The cobId is recognized */

   223

                    status = state4;

   224

                    EventTimerDuration = *(UNS16 *)d->objdict[offsetObjdict].pSubindex[5].pObject;

   225

                    MSG_WAR (0x3936, "cobId found at index ",

   226

                             0x1400 + numPdo);

   227

                    break;

   228

                  }

   229

                else

   230

                  {

   231

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

   232

                       dictionnary */

   233

                    numPdo++;

   234

                    offsetObjdict++;

   235

                    status = state2;

   236

                    break;

   237

                  }

   238

   239

              case state4:     /* Get Mapped Objects Number */

   240

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

   241

                   dictionnary. */

   242

                offsetObjdict = d->firstIndex->PDO_RCV_MAP;

   243

                lastIndex = d->lastIndex->PDO_RCV_MAP;

   244

                pMappingCount =

   245

                  (UNS8 *) (d->objdict + offsetObjdict +

   246

                            numPdo)->pSubindex[0].pObject;

   247

                numMap = 0;

   248

                while (numMap < *pMappingCount)

   249

                  {

   250

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

   251

                    UNS32 ByteSize;

   252

                    pMappingParameter =

   253

                      (UNS32 *) (d->objdict + offsetObjdict +

   254

                                 numPdo)->pSubindex[numMap + 1].pObject;

   255

                    if (pMappingParameter == NULL)

   256

                      {

   257

                        MSG_ERR (0x1937, "Couldn't get mapping parameter : ",

   258

                                 numMap + 1);

   259

                        return 0xFF;

   260

                      }

   261

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

   262

                    /* detail of *pMappingParameter : */

   263

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

   264

                       contains the subindex, */

   265

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

   266

                       variable. */

   267

   268

                    Size = (UNS8) (*pMappingParameter & (UNS32) 0x000000FF);

   269

   270

                    /* set variable only if Size != 0 and 

   271

                     * Size is lower than remaining bits in the PDO */

   272

                    if (Size && ((offset + Size) <= (m->len << 3)))

   273

                      {

   274

                        /* copy bit per bit in little endian */

   275

                        CopyBits (Size, (UNS8 *) & m->data[offset >> 3],

   276

                                  offset % 8, 0, ((UNS8 *) tmp), 0, 0);

   277

                        /*1->8 => 1 ; 9->16 =>2, ... */

   278

                        ByteSize = (UNS32)(1 + ((Size - 1) >> 3));

   279

   280

                        objDict =

   281

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

   282

                                      (UNS8) (((*pMappingParameter) >> 8) &

   283

                                              0xFF), tmp, &ByteSize, 0);

   284

   285

                        if (objDict != OD_SUCCESSFUL)

   286

                          {

   287

                            MSG_ERR (0x1938,

   288

                                     "error accessing to the mapped var : ",

   289

                                     numMap + 1);

   290

                            MSG_WAR (0x2939, "         Mapped at index : ",

   291

                                     (*pMappingParameter) >> 16);

   292

                            MSG_WAR (0x2940, "                subindex : ",

   293

                                     ((*pMappingParameter) >> 8) & 0xFF);

   294

                            return 0xFF;

   295

                          }

   296

   297

                        MSG_WAR (0x3942,

   298

                                 "Variable updated by PDO cobid : ",

   299

                                 UNS16_LE(m->cob_id));

   300

                        MSG_WAR (0x3943, "         Mapped at index : ",

   301

                                 (*pMappingParameter) >> 16);

   302

                        MSG_WAR (0x3944, "                subindex : ",

   303

                                 ((*pMappingParameter) >> 8) & 0xFF);

   304

                        offset += Size;

   305

                      }

   306

                    numMap++;

   307

                  }             /* end loop while on mapped variables */

   308

                if (EventTimerDuration && d->RxPDO_EventTimers)

   309

                {

   310

                    DelAlarm (d->RxPDO_EventTimers[numPdo]);

   311

                    d->RxPDO_EventTimers[numPdo] = SetAlarm (d, numPdo, d->RxPDO_EventTimers_Handler,

   312

                    MS_TO_TIMEVAL (EventTimerDuration), 0);

   313

                }

   314

                return 0;

   315

   316

              }                 /* end switch status */

   317

          }                     /* end while */

   318

    }                           /* end if Donnees */

   319

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

   320

    {

   321

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

   322

      status = state1;

   323

      offsetObjdict = d->firstIndex->PDO_TRS;

   324

      lastIndex = d->lastIndex->PDO_TRS;

   325

      if (offsetObjdict)

   326

        while (offsetObjdict <= lastIndex)

   327

          {

   328

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

   329

   330

            switch (status)

   331

              {

   332

   333

              case state1:     /* check the CobId */

   334

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

   335

                 */

   336

                pwCobId =

   337

                   (d->objdict +

   338

                             offsetObjdict)->pSubindex[1].pObject;

   339

                if (*pwCobId == UNS16_LE(m->cob_id))

   340

                  {

   341

                    status = state4;

   342

                    break;

   343

                  }

   344

                else

   345

                  {

   346

                    numPdo++;

   347

                    offsetObjdict++;

   348

                  }

   349

                status = state1;

   350

                break;

   351

   352

   353

              case state4:     /* check transmission type */

   354

                pTransmissionType =

   355

                  (UNS8 *) d->objdict[offsetObjdict].pSubindex[2].pObject;

   356

                /* If PDO is to be sampled and send on RTR, do it */

   357

                if ((*pTransmissionType == TRANS_RTR))

   358

                  {

   359

                    status = state5;

   360

                    break;

   361

                  }

   362

                /* RTR_SYNC means data prepared at SYNC, transmitted on RTR */

   363

                else if ((*pTransmissionType == TRANS_RTR_SYNC))

   364

                  {

   365

                    if (d->PDO_status[numPdo].

   366

                        transmit_type_parameter & PDO_RTR_SYNC_READY)

   367

                      {

   368

                        /*Data ready, just send */

   369

                        canSend (d->canHandle,

   370

                                 &d->PDO_status[numPdo].last_message);

   371

                        return 0;

   372

                      }

   373

                    else

   374

                      {

   375

                        /* if SYNC did never occur, transmit current data */

   376

                        /* DS301 do not tell what to do in such a case... */

   377

                        MSG_ERR (0x1947,

   378

                                 "Not ready RTR_SYNC TPDO send current data : ",

   379

                                 UNS16_LE(m->cob_id));

   380

                        status = state5;

   381

                      }

   382

                    break;

   383

                  }

   384

                else if ((*pTransmissionType == TRANS_EVENT_PROFILE) ||

   385

                         (*pTransmissionType == TRANS_EVENT_SPECIFIC))

   386

                  {

   387

                    /* Zap all timers and inhibit flag */

   388

                    d->PDO_status[numPdo].event_timer =

   389

                      DelAlarm (d->PDO_status[numPdo].event_timer);

   390

                    d->PDO_status[numPdo].inhibit_timer =

   391

                      DelAlarm (d->PDO_status[numPdo].inhibit_timer);

   392

                    d->PDO_status[numPdo].transmit_type_parameter &=

   393

                      ~PDO_INHIBITED;

   394

                    /* Call  PDOEventTimerAlarm for this TPDO, 

   395

                     * this will trigger emission et reset timers */

   396

                    PDOEventTimerAlarm (d, numPdo);

   397

                    return 0;

   398

                  }

   399

                else

   400

                  {

   401

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

   402

                       nothing. */

   403

                    MSG_WAR (0x2947, "PDO is not to send on request : ",

   404

                             UNS16_LE(m->cob_id));

   405

                    return 0xFF;

   406

                  }

   407

   408

              case state5:     /* build and send requested PDO */

   409

                {

   410

                  Message pdo;

   411

                  if (buildPDO (d, numPdo, &pdo))

   412

                    {

   413

                      MSG_ERR (0x1948, " Couldn't build TPDO number : ", numPdo);

   414

                      return 0xFF;

   415

                    }

   416

                  canSend (d->canHandle, &pdo);

   417

                  return 0;

   418

                }

   419

              }                 /* end switch status */

   420

          }                     /* end while */

   421

    }                           /* end if Requete */

   422

   423

  return 0;

   424

}

   425

   426

/*!

   427

**

   428

**

   429

** @param NbBits

   430

** @param SrcByteIndex

   431

** @param SrcBitIndex

   432

** @param SrcBigEndian

   433

** @param DestByteIndex

   434

** @param DestBitIndex

   435

** @param DestBigEndian

   436

**/

   437

void

   438

CopyBits (UNS8 NbBits, UNS8 * SrcByteIndex, UNS8 SrcBitIndex,

   439

          UNS8 SrcBigEndian, UNS8 * DestByteIndex, UNS8 DestBitIndex,

   440

          UNS8 DestBigEndian)

   441

{

   442

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

   443

  /* successively bytes */

   444

  // boundaries from LSB to MSB.

   445

  while (NbBits > 0)

   446

    {

   447

      /* Bit missalignement between src and dest */

   448

      INTEGER8 Vect = DestBitIndex - SrcBitIndex;

   449

   450

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

   451

      UNS8 Aligned =

   452

        Vect > 0 ? *SrcByteIndex << Vect : *SrcByteIndex >> -Vect;

   453

   454

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

   455

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

   456

      UNS8 BitsToCopy = BoudaryLimit > NbBits ? NbBits : BoudaryLimit;

   457

   458

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

   459

      UNS8 Mask =

   460

        ((0xff << (DestBitIndex + BitsToCopy)) |

   461

         (0xff >> (8 - DestBitIndex)));

   462

   463

      /* - Filtering src */

   464

      UNS8 Filtered = Aligned & ~Mask;

   465

   466

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

   467

      *DestByteIndex &= Mask;

   468

   469

      /* Then write. */

   470

      *DestByteIndex |= Filtered;

   471

   472

      /*Compute next time cursors for src */

   473

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

   474

        {

   475

          SrcBitIndex = 0;      /* First bit */

   476

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

   477

        }

   478

   479

   480

      /*Compute next time cursors for dest */

   481

      if ((DestBitIndex += BitsToCopy) > 7)

   482

        {

   483

          DestBitIndex = 0;     /* First bit */

   484

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

   485

        }

   486

   487

      /*And decrement counter. */

   488

      NbBits -= BitsToCopy;

   489

    }

   490