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

  UNS16 *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_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

  UNS16 *pwCobId = NULL;

   183

  UNS8 Size;

   184

  UNS8 offset;

   185

  UNS8 status;

   186

  UNS32 objDict;

   187

  UNS16 offsetObjdict;

   188

  UNS16 lastIndex;

   189

   190

  status = state2;

   191

   192

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

   193

  offset = 0x00;

   194

  numPdo = 0;

   195

  numMap = 0;

   196

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

   197

    { 

   198

      offsetObjdict = d->firstIndex->PDO_RCV;

   199

      lastIndex = d->lastIndex->PDO_RCV;

   200

   201

      if (offsetObjdict)

   202

        while (offsetObjdict <= lastIndex)

   203

          {

   204

            switch (status)

   205

              {

   206

   207

              case state2:

   208

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

   209

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

   210

                  {

   211

                    /* The cobId is recognized */

   212

                    status = state4;

   213

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

   214

                             0x1400 + numPdo);

   215

                    break;

   216

                  }

   217

                else

   218

                  {

   219

                    /* received cobId does not match */

   220

                    numPdo++;

   221

                    offsetObjdict++;

   222

                    status = state2;

   223

                    break;

   224

                  }

   225

   226

              case state4:     /* Get Mapped Objects Number */

   227

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

   228

                   dictionnary. */

   229

                offsetObjdict = d->firstIndex->PDO_RCV_MAP;

   230

                lastIndex = d->lastIndex->PDO_RCV_MAP;

   231

                pMappingCount =

   232

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

   233

                            numPdo)->pSubindex[0].pObject;

   234

                numMap = 0;

   235

                while (numMap < *pMappingCount)

   236

                  {

   237

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

   238

                    UNS32 ByteSize;

   239

                    pMappingParameter =

   240

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

   241

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

   242

                    if (pMappingParameter == NULL)

   243

                      {

   244

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

   245

                                 numMap + 1);

   246

                        return 0xFF;

   247

                      }

   248

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

   249

                    /* detail of *pMappingParameter : */

   250

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

   251

                       contains the subindex, */

   252

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

   253

                       variable. */

   254

   255

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

   256

   257

                    /* set variable only if Size != 0 and 

   258

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

   259

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

   260

                      {

   261

                        /* copy bit per bit in little endian */

   262

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

   263

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

   264

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

   265

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

   266

   267

                        objDict =

   268

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

   269

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

   270

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

   271

   272

                        if (objDict != OD_SUCCESSFUL)

   273

                          {

   274

                            MSG_ERR (0x1938,

   275

                                     "error accessing to the mapped var : ",

   276

                                     numMap + 1);

   277

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

   278

                                     (*pMappingParameter) >> 16);

   279

                            MSG_WAR (0x2940, "                subindex : ",

   280

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

   281

                            return 0xFF;

   282

                          }

   283

   284

                        MSG_WAR (0x3942,

   285

                                 "Variable updated by PDO cobid : ",

   286

                                 UNS16_LE(m->cob_id));

   287

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

   288

                                 (*pMappingParameter) >> 16);

   289

                        MSG_WAR (0x3944, "                subindex : ",

   290

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

   291

                        offset += Size;

   292

                      }

   293

                    numMap++;

   294

                  }             /* end loop while on mapped variables */

   295

                if (d->RxPDO_EventTimers)

   296

                {

   297

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

   298

                    if(EventTimerDuration){

   299

                        DelAlarm (d->RxPDO_EventTimers[numPdo]);

   300

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

   301

                        MS_TO_TIMEVAL (EventTimerDuration), 0);

   302

                    }

   303

                }

   304

                return 0;

   305

   306

              }                 /* end switch status */

   307

          }                     /* end while */

   308

    }                           /* end if Donnees */

   309

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

   310

    {

   311

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

   312

      status = state1;

   313

      offsetObjdict = d->firstIndex->PDO_TRS;

   314

      lastIndex = d->lastIndex->PDO_TRS;

   315

      if (offsetObjdict)

   316

        while (offsetObjdict <= lastIndex)

   317

          {

   318

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

   319

   320

            switch (status)

   321

              {

   322

   323

              case state1:     /* check the CobId */

   324

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

   325

                 */

   326

                pwCobId =

   327

                   (d->objdict +

   328

                             offsetObjdict)->pSubindex[1].pObject;

   329

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

   330

                  {

   331

                    status = state4;

   332

                    break;

   333

                  }

   334

                else

   335

                  {

   336

                    numPdo++;

   337

                    offsetObjdict++;

   338

                  }

   339

                status = state1;

   340

                break;

   341

   342

   343

              case state4:     /* check transmission type */

   344

                pTransmissionType =

   345

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

   346

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

   347

                if ((*pTransmissionType == TRANS_RTR))

   348

                  {

   349

                    status = state5;

   350

                    break;

   351

                  }

   352

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

   353

                else if ((*pTransmissionType == TRANS_RTR_SYNC))

   354

                  {

   355

                    if (d->PDO_status[numPdo].

   356

                        transmit_type_parameter & PDO_RTR_SYNC_READY)

   357

                      {

   358

                        /*Data ready, just send */

   359

                        canSend (d->canHandle,

   360

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

   361

                        return 0;

   362

                      }

   363

                    else

   364

                      {

   365

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

   366

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

   367

                        MSG_ERR (0x1947,

   368

                                 "Not ready RTR_SYNC TPDO send current data : ",

   369

                                 UNS16_LE(m->cob_id));

   370

                        status = state5;

   371

                      }

   372

                    break;

   373

                  }

   374

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

   375

                         (*pTransmissionType == TRANS_EVENT_SPECIFIC))

   376

                  {

   377

                    /* Zap all timers and inhibit flag */

   378

                    d->PDO_status[numPdo].event_timer =

   379

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

   380

                    d->PDO_status[numPdo].inhibit_timer =

   381

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

   382

                    d->PDO_status[numPdo].transmit_type_parameter &=

   383

                      ~PDO_INHIBITED;

   384

                    /* Call  PDOEventTimerAlarm for this TPDO, 

   385

                     * this will trigger emission et reset timers */

   386

                    PDOEventTimerAlarm (d, numPdo);

   387

                    return 0;

   388

                  }

   389

                else

   390

                  {

   391

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

   392

                       nothing. */

   393

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

   394

                             UNS16_LE(m->cob_id));

   395

                    return 0xFF;

   396

                  }

   397

   398

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

   399

                {

   400

                  Message pdo;

   401

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

   402

                    {

   403

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

   404

                      return 0xFF;

   405

                    }

   406

                  canSend (d->canHandle, &pdo);

   407

                  return 0;

   408

                }

   409

              }                 /* end switch status */

   410

          }                     /* end while */

   411

    }                           /* end if Requete */

   412

   413

  return 0;

   414

}

   415

   416

/*!

   417

**

   418

**

   419

** @param NbBits

   420

** @param SrcByteIndex

   421

** @param SrcBitIndex

   422

** @param SrcBigEndian

   423

** @param DestByteIndex

   424

** @param DestBitIndex

   425

** @param DestBigEndian

   426

**/

   427

void

   428

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

   429

          UNS8 SrcBigEndian, UNS8 * DestByteIndex, UNS8 DestBitIndex,

   430

          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 =

   442

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

   443

   444

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

   445

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

   446

      UNS8 BitsToCopy = BoudaryLimit > NbBits ? NbBits : BoudaryLimit;

   447

   448

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

   449

      UNS8 Mask =

   450

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

   451

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

   452

   453

      /* - Filtering src */

   454

      UNS8 Filtered = Aligned & ~Mask;

   455

   456

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

   457

      *DestByteIndex &= Mask;

   458

   459

      /* Then write. */

   460

      *DestByteIndex |= Filtered;

   461

   462

      /*Compute next time cursors for src */

   463

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

   464

        {

   465

          SrcBitIndex = 0;      /* First bit */

   466

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

   467

        }

   468

   469

   470

      /*Compute next time cursors for dest */

   471

      if ((DestBitIndex += BitsToCopy) > 7)

   472

        {

   473

          DestBitIndex = 0;     /* First bit */

   474

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

   475

        }

   476

   477

      /*And decrement counter. */

   478

      NbBits -= BitsToCopy;

   479

    }

   480

   481

}

   482

   483

static void sendPdo(CO_Data * d, UNS32 pdoNum, Message * pdo)

   484

{

   485

  /*store_as_last_message */

   486

  d->PDO_status[pdoNum].last_message = *pdo;

   487

  MSG_WAR (0x396D, "sendPDO cobId :", UNS16_LE(pdo->cob_id));

   488

  MSG_WAR (0x396E, "     Nb octets  : ", pdo->len);

   489