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

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

   128

     receive */

   129

  /* part dictionary */

   130

   131

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

   132

   133

  if (offset && RPDOIndex >= 0x1400)

   134

    {

   135

      offset += RPDOIndex - 0x1400;

   136

      if (offset <= lastIndex)

   137

        {

   138

          /* get the CobId */

   139

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

   140

   141

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

   142

          {

   143

            Message pdo;

   144

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

   145

            pdo.rtr = REQUEST;

   146

            pdo.len = 0;

   147

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

   148

          }

   149

        }

   150

    }

   151

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

   152

  return 0xFF;

   153

}

   154

   155

   156

/*!

   157

**

   158

**

   159

** @param d

   160

** @param m

   161

**

   162

** @return

   163

**/

   164

UNS8

   165

proceedPDO (CO_Data * d, Message * m)

   166

{

   167

  UNS8 numPdo;

   168

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

   169

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

   170

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

   171

  /*  void *     pMappedAppObject = NULL;   */

   172

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

   173

     mapping entry */

   174

  UNS32 *pMappingParameter = NULL;

   175

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

   176

                                           type */

   177

  UNS32 *pwCobId = NULL;

   178

  UNS8 Size;

   179

  UNS8 offset;

   180

  UNS8 status;

   181

  UNS32 objDict;

   182

  UNS16 offsetObjdict;

   183

  UNS16 lastIndex;

   184

   185

  status = state2;

   186

   187

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

   188

  offset = 0x00;

   189

  numPdo = 0;

   190

  numMap = 0;

   191

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

   192

    {                           /* The PDO received is not a

   193

                                   request. */

   194

   195

      offsetObjdict = d->firstIndex->PDO_RCV;

   196

      lastIndex = d->lastIndex->PDO_RCV;

   197

   198

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

   199

      if (offsetObjdict)

   200

        while (offsetObjdict <= lastIndex)

   201

          {

   202

   203

            switch (status)

   204

              {

   205

   206

              case state2:

   207

                /* get CobId of the dictionary correspondant to the received

   208

                   PDO */

   209

                pwCobId =

   210

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

   211

                /* check the CobId coherance */

   212

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

   213

                 */

   214

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

   215

                  {

   216

                    /* The cobId is recognized */

   217

                    status = state4;

   218

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

   219

                             0x1400 + numPdo);

   220

                    break;

   221

                  }

   222

                else

   223

                  {

   224

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

   225

                       dictionnary */

   226

                    numPdo++;

   227

                    offsetObjdict++;

   228

                    status = state2;

   229

                    break;

   230

                  }

   231

   232

              case state4:     /* Get Mapped Objects Number */

   233

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

   234

                   dictionnary. */

   235

                offsetObjdict = d->firstIndex->PDO_RCV_MAP;

   236

                lastIndex = d->lastIndex->PDO_RCV_MAP;

   237

                pMappingCount =

   238

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

   239

                            numPdo)->pSubindex[0].pObject;

   240

                numMap = 0;

   241

                while (numMap < *pMappingCount)

   242

                  {

   243

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

   244

                    UNS32 ByteSize;

   245

                    pMappingParameter =

   246

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

   247

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

   248

                    if (pMappingParameter == NULL)

   249

                      {

   250

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

   251

                                 numMap + 1);

   252

                        return 0xFF;

   253

                      }

   254

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

   255

                    /* detail of *pMappingParameter : */

   256

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

   257

                       contains the subindex, */

   258

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

   259

                       variable. */

   260

   261

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

   262

   263

                    /* set variable only if Size != 0 and 

   264

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

   265

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

   266

                      {

   267

                        /* copy bit per bit in little endian */

   268

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

   269

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

   270

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

   271

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

   272

   273

                        objDict =

   274

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

   275

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

   276

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

   277

   278

                        if (objDict != OD_SUCCESSFUL)

   279

                          {

   280

                            MSG_ERR (0x1938,

   281

                                     "error accessing to the mapped var : ",

   282

                                     numMap + 1);

   283

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

   284

                                     (*pMappingParameter) >> 16);

   285

                            MSG_WAR (0x2940, "                subindex : ",

   286

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

   287

                            return 0xFF;

   288

                          }

   289

   290

                        MSG_WAR (0x3942,

   291

                                 "Variable updated by PDO cobid : ",

   292

                                 UNS16_LE(m->cob_id));

   293

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

   294

                                 (*pMappingParameter) >> 16);

   295

                        MSG_WAR (0x3944, "                subindex : ",

   296

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

   297

                        offset += Size;

   298

                      }

   299

                    numMap++;

   300

                  }             /* end loop while on mapped variables */

   301

   302

                return 0;

   303

   304

              }                 /* end switch status */

   305

          }                     /* end while */

   306

    }                           /* end if Donnees */

   307

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

   308

    {

   309

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

   310

      status = state1;

   311

      offsetObjdict = d->firstIndex->PDO_TRS;

   312

      lastIndex = d->lastIndex->PDO_TRS;

   313

      if (offsetObjdict)

   314

        while (offsetObjdict <= lastIndex)

   315

          {

   316

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

   317

   318

            switch (status)

   319

              {

   320

   321

              case state1:     /* check the CobId */

   322

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

   323

                 */

   324

                pwCobId =

   325

                   (d->objdict +

   326

                             offsetObjdict)->pSubindex[1].pObject;

   327

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

   328

                  {

   329

                    status = state4;

   330

                    break;

   331

                  }

   332

                else

   333

                  {

   334

                    numPdo++;

   335

                    offsetObjdict++;

   336

                  }

   337

                status = state1;

   338

                break;

   339

   340

   341

              case state4:     /* check transmission type */

   342

                pTransmissionType =

   343

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

   344

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

   345

                if ((*pTransmissionType == TRANS_RTR))

   346

                  {

   347

                    status = state5;

   348

                    break;

   349

                  }

   350

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

   351

                else if ((*pTransmissionType == TRANS_RTR_SYNC))

   352

                  {

   353

                    if (d->PDO_status[numPdo].

   354

                        transmit_type_parameter & PDO_RTR_SYNC_READY)

   355

                      {

   356

                        /*Data ready, just send */

   357

                        canSend (d->canHandle,

   358

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

   359

                        return 0;

   360

                      }

   361

                    else

   362

                      {

   363

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

   364

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

   365

                        MSG_ERR (0x1947,

   366

                                 "Not ready RTR_SYNC TPDO send current data : ",

   367

                                 UNS16_LE(m->cob_id));

   368

                        status = state5;

   369

                      }

   370

                    break;

   371

                  }

   372

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

   373

                         (*pTransmissionType == TRANS_EVENT_SPECIFIC))

   374

                  {

   375

                    /* Zap all timers and inhibit flag */

   376

                    d->PDO_status[numPdo].event_timer =

   377

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

   378

                    d->PDO_status[numPdo].inhibit_timer =

   379

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

   380

                    d->PDO_status[numPdo].transmit_type_parameter &=

   381

                      ~PDO_INHIBITED;

   382

                    /* Call  PDOEventTimerAlarm for this TPDO, 

   383

                     * this will trigger emission et reset timers */

   384

                    PDOEventTimerAlarm (d, numPdo);

   385

                    return 0;

   386

                  }

   387

                else

   388

                  {

   389

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

   390

                       nothing. */

   391

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

   392

                             UNS16_LE(m->cob_id));

   393

                    return 0xFF;

   394

                  }

   395

   396

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

   397

                {

   398

                  Message pdo;

   399

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

   400

                    {

   401

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

   402

                      return 0xFF;

   403

                    }

   404

                  canSend (d->canHandle, &pdo);

   405

                  return 0;

   406

                }

   407

              }                 /* end switch status */

   408

          }                     /* end while */

   409

    }                           /* end if Requete */

   410

   411

  return 0;

   412

}

   413

   414

/*!

   415

**

   416

**

   417

** @param NbBits

   418

** @param SrcByteIndex

   419

** @param SrcBitIndex

   420

** @param SrcBigEndian

   421

** @param DestByteIndex

   422

** @param DestBitIndex

   423

** @param DestBigEndian

   424

**/

   425

void

   426

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

   427

          UNS8 SrcBigEndian, UNS8 * DestByteIndex, UNS8 DestBitIndex,

   428

          UNS8 DestBigEndian)

   429

{

   430

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

   431

  /* successively bytes */

   432

  // boundaries from LSB to MSB.

   433

  while (NbBits > 0)

   434

    {

   435

      /* Bit missalignement between src and dest */

   436

      INTEGER8 Vect = DestBitIndex - SrcBitIndex;

   437

   438

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

   439

      UNS8 Aligned =

   440

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

   441

   442

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

   443

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

   444

      UNS8 BitsToCopy = BoudaryLimit > NbBits ? NbBits : BoudaryLimit;

   445

   446

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

   447

      UNS8 Mask =

   448

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

   449

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

   450

   451

      /* - Filtering src */

   452

      UNS8 Filtered = Aligned & ~Mask;

   453

   454

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

   455

      *DestByteIndex &= Mask;

   456

   457

      /* Then write. */

   458

      *DestByteIndex |= Filtered;

   459

   460

      /*Compute next time cursors for src */

   461

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

   462

        {

   463

          SrcBitIndex = 0;      /* First bit */

   464

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

   465

        }

   466

   467

   468

      /*Compute next time cursors for dest */

   469

      if ((DestBitIndex += BitsToCopy) > 7)

   470

        {

   471

          DestBitIndex = 0;     /* First bit */

   472

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

   473

        }

   474

   475

      /*And decrement counter. */

   476

      NbBits -= BitsToCopy;

   477

    }

   478

   479

}

   480

   481

/*!

   482

**

   483

**

   484

** @param d

   485

**

   486

** @return

   487

**/

   488

   489

UNS8

   490

sendPDOevent (CO_Data * d)