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_LE(*(UNS32*)TPDO_com->pSubindex[1].pObject & 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

  UNS32 *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 = d->objdict[offset].pSubindex[1].pObject;

   142

   143

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

   144

          {

   145

            Message pdo;

   146

            pdo.cob_id = UNS16_LE(*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

  UNS32 *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 : ", (UNS16_LE(m->cob_id) & 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

                  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 == UNS16_LE(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

                                 UNS16_LE(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

                return 0;

   306

   307

              }                 /* end switch status */

   308

          }                     /* end while */

   309

    }                           /* end if Donnees */

   310

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

   311

    {

   312

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

   313

      status = state1;

   314

      offsetObjdict = d->firstIndex->PDO_TRS;

   315

      lastIndex = d->lastIndex->PDO_TRS;

   316

      if (offsetObjdict)

   317

        while (offsetObjdict <= lastIndex)

   318

          {

   319

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

   320

   321

            switch (status)

   322

              {

   323

   324

              case state1:     /* check the CobId */

   325

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

   326

                 */

   327

                pwCobId =

   328

                   (d->objdict +

   329

                             offsetObjdict)->pSubindex[1].pObject;

   330

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

   331

                  {

   332

                    status = state4;

   333

                    break;

   334

                  }

   335

                else

   336

                  {

   337

                    numPdo++;

   338

                    offsetObjdict++;

   339

                  }

   340

                status = state1;

   341

                break;

   342

   343

   344

              case state4:     /* check transmission type */

   345

                pTransmissionType =

   346

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

   347

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

   348

                if ((*pTransmissionType == TRANS_RTR))

   349

                  {

   350

                    status = state5;

   351

                    break;

   352

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

   353

                  }

   354

                else if ((*pTransmissionType == TRANS_RTR_SYNC))

   355

                  {

   356

                    if (d->PDO_status[numPdo].

   357

                        transmit_type_parameter & PDO_RTR_SYNC_READY)

   358

                      {

   359

                        /*Data ready, just send */

   360

                        canSend (d->canHandle,

   361

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

   362

                        return 0;

   363

                      }

   364

                    else

   365

                      {

   366

                        /* if SYNC did never occur, force emission with current data */

   367

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

   368

                        MSG_ERR (0x1947,

   369

                                 "Not ready RTR_SYNC TPDO send current data : ",

   370

                                 UNS16_LE(m->cob_id));

   371

                        status = state5;

   372

                      }

   373

                    break;

   374

                  }

   375

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

   376

                         (*pTransmissionType == TRANS_EVENT_SPECIFIC))

   377

                  {

   378

                    /* Zap all timers and inhibit flag */

   379

                    d->PDO_status[numPdo].event_timer =

   380

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

   381

                    d->PDO_status[numPdo].inhibit_timer =

   382

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

   383

                    d->PDO_status[numPdo].transmit_type_parameter &=

   384

                      ~PDO_INHIBITED;

   385

                    /* Call  PDOEventTimerAlarm for this TPDO, 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 n�", 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

/*!

   484

**

   485

**

   486

** @param d

   487

**

   488

** @return

   489

**/

   490

   491

UNS8

   492