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

    50

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

    51

{

    52

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

    53

  const indextable *TPDO_map =

    54

    d->objdict + d->firstIndex->PDO_TRS_MAP + numPdo;

    55

    56

  UNS8 prp_j = 0x00;

    57

  UNS8 offset = 0x00;

    58

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

    59

    60

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

    61

  pdo->rtr = NOT_A_REQUEST;

    62

    63

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

    64

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

    65

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

    66

    67

  do

    68

    {

    69

      UNS8 dataType;            /* Unused */

    70

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

    71

    72

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

    73

      UNS32 *pMappingParameter =

    74

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

    75

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

    76

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

    77

    78

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

    79

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

    80

        {

    81

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

    82

          UNS8 subIndex =

    83

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

    84

    85

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

    86

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

    87

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

    88

    89

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

    90

              OD_SUCCESSFUL)

    91

            {

    92

              MSG_ERR (0x1013,

    93

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

    94

                       (UNS16) (*pMappingParameter));

    95

              return 0xFF;

    96

            }

    97

          /* copy bit per bit in little endian */

    98

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

    99

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

   100

   101

          offset += Size;

   102

        }

   103

      prp_j++;

   104

    }

   105

  while (prp_j < *pMappingCount);

   106

   107

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

   108

   109

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

   110

   111

  return 0;

   112

}

   113

   114

/*!

   115

**

   116

**

   117

** @param d

   118

** @param cobId

   119

**

   120

** @return

   121

**/

   122

UNS8

   123

sendPDOrequest (CO_Data * d, UNS16 RPDOIndex)

   124

{

   125

  UNS16 *pwCobId;

   126

  UNS16 offset = d->firstIndex->PDO_RCV;

   127

  UNS16 lastIndex = d->lastIndex->PDO_RCV;

   128

   129

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

   130

     receive */

   131

  /* part dictionary */

   132

   133

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

   134

   135

  if (offset && RPDOIndex >= 0x1400)

   136

    {

   137

      offset += RPDOIndex - 0x1400;

   138

      if (offset <= lastIndex)

   139

        {

   140

          /* get the CobId */

   141

          pwCobId = (UNS16 *) d->objdict[offset].pSubindex[1].pObject;

   142

   143

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

   144

          {

   145

            Message pdo;

   146

            pdo.cob_id = *pwCobId;

   147

            pdo.rtr = REQUEST;

   148

            pdo.len = 0;

   149

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

   150

          }

   151

        }

   152

    }

   153

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

   154

  return 0xFF;

   155

}

   156

   157

   158

/*!

   159

**

   160

**

   161

** @param d

   162

** @param m

   163

**

   164

** @return

   165

**/

   166

UNS8

   167

proceedPDO (CO_Data * d, Message * m)

   168

{

   169

  UNS8 numPdo;

   170

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

   171

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

   172

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

   173

  /*  void *     pMappedAppObject = NULL;   */

   174

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

   175

     mapping entry */

   176

  UNS32 *pMappingParameter = NULL;

   177

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

   178

                                           type */

   179

  UNS16 *pwCobId = NULL;

   180

  UNS8 Size;

   181

  UNS8 offset;

   182

  UNS8 status;

   183

  UNS32 objDict;

   184

  UNS16 offsetObjdict;

   185

  UNS16 lastIndex;

   186

   187

  status = state2;

   188

   189

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

   190

  offset = 0x00;

   191

  numPdo = 0;

   192

  numMap = 0;

   193

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

   194

    {                           /* The PDO received is not a

   195

                                   request. */

   196

   197

      offsetObjdict = d->firstIndex->PDO_RCV;

   198

      lastIndex = d->lastIndex->PDO_RCV;

   199

   200

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

   201

      if (offsetObjdict)

   202

        while (offsetObjdict <= lastIndex)

   203

          {

   204

   205

            switch (status)

   206

              {

   207

   208

              case state2:

   209

                /* get CobId of the dictionary correspondant to the received

   210

                   PDO */

   211

                pwCobId =

   212

                  (UNS16 *) d->objdict[offsetObjdict].pSubindex[1].pObject;

   213

                /* check the CobId coherance */

   214

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

   215

                 */

   216

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

   217

                  {

   218

                    /* The cobId is recognized */

   219

                    status = state4;

   220

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

   221

                             0x1400 + numPdo);

   222

                    break;

   223

                  }

   224

                else

   225

                  {

   226

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

   227

                       dictionnary */

   228

                    numPdo++;

   229

                    offsetObjdict++;

   230

                    status = state2;

   231

                    break;

   232

                  }

   233

   234

              case state4:     /* Get Mapped Objects Number */

   235

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

   236

                   dictionnary. */

   237

                offsetObjdict = d->firstIndex->PDO_RCV_MAP;

   238

                lastIndex = d->lastIndex->PDO_RCV_MAP;

   239

                pMappingCount =

   240

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

   241

                            numPdo)->pSubindex[0].pObject;

   242

                numMap = 0;

   243

                while (numMap < *pMappingCount)

   244

                  {

   245

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

   246

                    UNS8 ByteSize;

   247

                    pMappingParameter =

   248

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

   249

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

   250

                    if (pMappingParameter == NULL)

   251

                      {

   252

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

   253

                                 numMap + 1);

   254

                        return 0xFF;

   255

                      }

   256

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

   257

                    /* detail of *pMappingParameter : */

   258

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

   259

                       contains the subindex, */

   260

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

   261

                       variable. */

   262

   263

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

   264

   265

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

   266

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

   267

                      {

   268

                        /* copy bit per bit in little endian */

   269

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

   270

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

   271

   272

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

   273

                                                                   2, ... */

   274

   275

                        objDict =

   276

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

   277

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

   278

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

   279

   280

                        if (objDict != OD_SUCCESSFUL)

   281

                          {

   282

                            MSG_ERR (0x1938,

   283

                                     "error accessing to the mapped var : ",

   284

                                     numMap + 1);

   285

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

   286

                                     (*pMappingParameter) >> 16);

   287

                            MSG_WAR (0x2940, "                subindex : ",

   288

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

   289

                            return 0xFF;

   290

                          }

   291

   292

                        MSG_WAR (0x3942,

   293

                                 "Variable updated with value received by PDO cobid : ",

   294

                                 m->cob_id);

   295

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

   296

                                 (*pMappingParameter) >> 16);

   297

                        MSG_WAR (0x3944, "                subindex : ",

   298

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

   299

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

   300

                        offset += Size;

   301

                      }

   302

                    numMap++;

   303

                  }             /* end loop while on mapped variables */

   304

   305

                offset = 0x00;

   306

                numMap = 0;

   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 : ", 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

                  (UNS16 *) (d->objdict +

   331

                             offsetObjdict)->pSubindex[1].pObject;

   332

                if (*pwCobId == (*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

                    /* RTR_SYNC mean data is prepared at SYNC, and transmitted on RTR */

   355

                  }

   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, force emission with 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

                                 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, this will trigger emission et reset timers */

   388

                    PDOEventTimerAlarm (d, numPdo);

   389

                    return 0;

   390

                  }

   391

                else

   392

                  {

   393

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

   394

                       nothing. */

   395

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

   396

                             m->cob_id);

   397

                    return 0xFF;

   398

                  }

   399

   400

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

   401

                {

   402

                  Message pdo;

   403

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

   404

                    {

   405

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

   406

                      return 0xFF;

   407

                    }

   408

                  canSend (d->canHandle, &pdo);

   409

                  return 0;

   410

                }

   411

              }                 /* end switch status */

   412

          }                     /* end while */

   413

    }                           /* end if Requete */

   414

   415

  return 0;

   416

}

   417

   418

/*!

   419

**

   420

**

   421

** @param NbBits

   422

** @param SrcByteIndex

   423

** @param SrcBitIndex

   424

** @param SrcBigEndian

   425

** @param DestByteIndex

   426

** @param DestBitIndex

   427

** @param DestBigEndian

   428

**/

   429

void

   430

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

   431

          UNS8 SrcBigEndian, UNS8 * DestByteIndex, UNS8 DestBitIndex,

   432

          UNS8 DestBigEndian)

   433

{

   434

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

   435

  /* successively bytes */

   436

  // boundaries from LSB to MSB.

   437

  while (NbBits > 0)

   438

    {

   439

      /* Bit missalignement between src and dest */

   440

      INTEGER8 Vect = DestBitIndex - SrcBitIndex;

   441

   442

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

   443

      UNS8 Aligned =

   444

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

   445

   446

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

   447

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

   448

      UNS8 BitsToCopy = BoudaryLimit > NbBits ? NbBits : BoudaryLimit;

   449

   450

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

   451

      UNS8 Mask =

   452

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

   453

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

   454

   455

      /* - Filtering src */

   456

      UNS8 Filtered = Aligned & ~Mask;

   457

   458

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

   459

      *DestByteIndex &= Mask;

   460

   461

      /* Then write. */

   462

      *DestByteIndex |= Filtered;

   463

   464

      /*Compute next time cursors for src */

   465

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

   466

        {

   467

          SrcBitIndex = 0;      /* First bit */

   468

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

   469

        }

   470

   471

   472

      /*Compute next time cursors for dest */

   473

      if ((DestBitIndex += BitsToCopy) > 7)

   474

        {

   475

          DestBitIndex = 0;     /* First bit */

   476

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

   477

        }

   478

   479

      /*And decrement counter. */

   480

      NbBits -= BitsToCopy;

   481

    }

   482

   483

}

   484

   485

/*!

   486

**

   487

**

   488

** @param d

   489

**

   490

** @return

   491

**/

   492

   493

UNS8