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

                       (UNS16) (*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

   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

    {                           /* The PDO received is not a

   198

                                   request. */

   199

   200

      offsetObjdict = d->firstIndex->PDO_RCV;

   201

      lastIndex = d->lastIndex->PDO_RCV;

   202

   203

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

   204

      if (offsetObjdict)

   205

        while (offsetObjdict <= lastIndex)

   206

          {

   207

   208

            switch (status)

   209

              {

   210

   211

              case state2:

   212

                /* get CobId of the dictionary correspondant to the received

   213

                   PDO */

   214

                pwCobId =

   215

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

   216

                /* check the CobId coherance */

   217

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

   218

                 */

   219

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

   220

                  {

   221

                    /* The cobId is recognized */

   222

                    status = state4;

   223

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

   224

                             0x1400 + numPdo);

   225

                    break;

   226

                  }

   227

                else

   228

                  {

   229

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

   230

                       dictionnary */

   231

                    numPdo++;

   232

                    offsetObjdict++;

   233

                    status = state2;

   234

                    break;

   235

                  }

   236

   237

              case state4:     /* Get Mapped Objects Number */

   238

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

   239

                   dictionnary. */

   240

                offsetObjdict = d->firstIndex->PDO_RCV_MAP;

   241

                lastIndex = d->lastIndex->PDO_RCV_MAP;

   242

                pMappingCount =

   243

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

   244

                            numPdo)->pSubindex[0].pObject;

   245

                numMap = 0;

   246

                while (numMap < *pMappingCount)

   247

                  {

   248

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

   249

                    UNS32 ByteSize;

   250

                    pMappingParameter =

   251

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

   252

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

   253

                    if (pMappingParameter == NULL)

   254

                      {

   255

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

   256

                                 numMap + 1);

   257

                        return 0xFF;

   258

                      }

   259

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

   260

                    /* detail of *pMappingParameter : */

   261

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

   262

                       contains the subindex, */

   263

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

   264

                       variable. */

   265

   266

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

   267

   268

                    /* set variable only if Size != 0 and 

   269

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

   270

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

   271

                      {

   272

                        /* copy bit per bit in little endian */

   273

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

   274

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

   275

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

   276

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

   277

   278

                        objDict =

   279

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

   280

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

   281

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

   282

   283

                        if (objDict != OD_SUCCESSFUL)

   284

                          {

   285

                            MSG_ERR (0x1938,

   286

                                     "error accessing to the mapped var : ",

   287

                                     numMap + 1);

   288

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

   289

                                     (*pMappingParameter) >> 16);

   290

                            MSG_WAR (0x2940, "                subindex : ",

   291

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

   292

                            return 0xFF;

   293

                          }

   294

   295

                        MSG_WAR (0x3942,

   296

                                 "Variable updated by PDO cobid : ",

   297

                                 UNS16_LE(m->cob_id));

   298

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

   299

                                 (*pMappingParameter) >> 16);

   300

                        MSG_WAR (0x3944, "                subindex : ",

   301

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

   302

                        offset += Size;

   303

                      }

   304

                    numMap++;

   305

                  }             /* end loop while on mapped variables */

   306

   307

                return 0;

   308

   309

              }                 /* end switch status */

   310

          }                     /* end while */

   311

    }                           /* end if Donnees */

   312

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

   313

    {

   314

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

   315

      status = state1;

   316

      offsetObjdict = d->firstIndex->PDO_TRS;

   317

      lastIndex = d->lastIndex->PDO_TRS;

   318

      if (offsetObjdict)

   319

        while (offsetObjdict <= lastIndex)

   320

          {

   321

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

   322

   323

            switch (status)

   324

              {

   325

   326

              case state1:     /* check the CobId */

   327

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

   328

                 */

   329

                pwCobId =

   330

                   (d->objdict +

   331

                             offsetObjdict)->pSubindex[1].pObject;

   332

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

   333

                  {

   334

                    status = state4;

   335

                    break;

   336

                  }

   337

                else

   338

                  {

   339

                    numPdo++;

   340

                    offsetObjdict++;

   341

                  }

   342

                status = state1;

   343

                break;

   344

   345

   346

              case state4:     /* check transmission type */

   347

                pTransmissionType =

   348

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

   349

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

   350

                if ((*pTransmissionType == TRANS_RTR))

   351

                  {

   352

                    status = state5;

   353

                    break;

   354

                  }

   355

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

   356

                else if ((*pTransmissionType == TRANS_RTR_SYNC))

   357

                  {

   358

                    if (d->PDO_status[numPdo].

   359

                        transmit_type_parameter & PDO_RTR_SYNC_READY)

   360

                      {

   361

                        /*Data ready, just send */

   362

                        canSend (d->canHandle,

   363

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

   364

                        return 0;

   365

                      }

   366

                    else

   367

                      {

   368

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

   369

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

   370

                        MSG_ERR (0x1947,

   371

                                 "Not ready RTR_SYNC TPDO send current data : ",

   372

                                 UNS16_LE(m->cob_id));

   373

                        status = state5;

   374

                      }

   375

                    break;

   376

                  }

   377

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

   378

                         (*pTransmissionType == TRANS_EVENT_SPECIFIC))

   379

                  {

   380

                    /* Zap all timers and inhibit flag */

   381

                    d->PDO_status[numPdo].event_timer =

   382

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

   383

                    d->PDO_status[numPdo].inhibit_timer =

   384

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

   385

                    d->PDO_status[numPdo].transmit_type_parameter &=

   386

                      ~PDO_INHIBITED;

   387

                    /* Call  PDOEventTimerAlarm for this TPDO, 

   388

                     * this will trigger emission et reset timers */

   389

                    PDOEventTimerAlarm (d, numPdo);

   390

                    return 0;

   391

                  }

   392

                else

   393

                  {

   394

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

   395

                       nothing. */

   396

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

   397

                             UNS16_LE(m->cob_id));

   398

                    return 0xFF;

   399

                  }

   400

   401

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

   402

                {

   403

                  Message pdo;

   404

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

   405

                    {

   406

                      MSG_ERR (0x1948, " Couldn't build TPDO n�", numPdo);

   407

                      return 0xFF;

   408

                    }

   409

                  canSend (d->canHandle, &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

   431

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

   432

          UNS8 SrcBigEndian, UNS8 * DestByteIndex, UNS8 DestBitIndex,

   433

          UNS8 DestBigEndian)

   434

{

   435

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

   436

  /* successively bytes */

   437

  // boundaries from LSB to MSB.

   438

  while (NbBits > 0)

   439

    {

   440

      /* Bit missalignement between src and dest */

   441

      INTEGER8 Vect = DestBitIndex - SrcBitIndex;

   442

   443

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

   444

      UNS8 Aligned =

   445

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

   446

   447

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

   448

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

   449

      UNS8 BitsToCopy = BoudaryLimit > NbBits ? NbBits : BoudaryLimit;

   450

   451

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

   452

      UNS8 Mask =

   453

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

   454

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

   455

   456

      /* - Filtering src */

   457

      UNS8 Filtered = Aligned & ~Mask;

   458

   459

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

   460

      *DestByteIndex &= Mask;

   461

   462

      /* Then write. */

   463

      *DestByteIndex |= Filtered;

   464

   465

      /*Compute next time cursors for src */

   466

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

   467

        {

   468

          SrcBitIndex = 0;      /* First bit */

   469

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

   470

        }

   471

   472

   473

      /*Compute next time cursors for dest */

   474

      if ((DestBitIndex += BitsToCopy) > 7)

   475

        {

   476

          DestBitIndex = 0;     /* First bit */

   477

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

   478

        }

   479

   480

      /*And decrement counter. */

   481

      NbBits -= BitsToCopy;

   482

    }

   483

   484

}

   485

   486

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

   487

{

   488

  /*store_as_last_message */

   489

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

   490

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