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

	UNS8 offset = 0x00;

    56

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

    57

    58

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

    59

	pdo->rtr = NOT_A_REQUEST;

    60

    61

	MSG_WAR(0x3009, "  PDO CobId is : ", *(UNS32*)TPDO_com->pSubindex[1].pObject);

    62

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

    63

    64

	do{

    65

		UNS8 dataType; /* Unused */

    66

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

    67

    68

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

    69

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

    70

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

    71

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

    72

    73

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

    74

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

    75

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

    76

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

    77

    78

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

    79

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

    80

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

    81

    82

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

    83

				MSG_ERR(0x1013, " Couldn't find mapped variable at index-subindex-size : ", (UNS16)(*pMappingParameter));

    84

				return 0xFF;

    85

			}

    86

			/* copy bit per bit in little endian*/

    87

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

    88

    89

			offset += Size ;

    90

		}

    91

		prp_j++;

    92

	}while( prp_j < *pMappingCount );

    93

    94

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

    95

    96

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

    97

    98

	return 0;

    99

}

   100

   101

/*!

   102

**

   103

**

   104

** @param d

   105

** @param cobId

   106

**

   107

** @return

   108

**/

   109

UNS8 sendPDOrequest( CO_Data* d, UNS16 RPDOIndex )

   110

{

   111

  UNS16 * pwCobId;

   112

  UNS16 offset = d->firstIndex->PDO_RCV;

   113

  UNS16 lastIndex = d->lastIndex->PDO_RCV;

   114

   115

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

   116

     receive */

   117

  /* part dictionary */

   118

   119

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

   120

   121

  if (offset && RPDOIndex >= 0x1400){

   122

    offset += RPDOIndex - 0x1400;  

   123

    if (offset <= lastIndex) {

   124

      /* get the CobId*/

   125

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

   126

   127

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

   128

      {

   129

          Message pdo;

   130

          pdo.cob_id = *pwCobId;

   131

          pdo.rtr = REQUEST;

   132

          pdo.len = 0;      

   133

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

   134

      }

   135

    }

   136

  }

   137

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

   138

  return 0xFF;

   139

}

   140

   141

   142

/*!

   143

**

   144

**

   145

** @param d

   146

** @param m

   147

**

   148

** @return

   149

**/

   150

UNS8 proceedPDO(CO_Data* d, Message *m)

   151

{

   152

  UNS8   numPdo;

   153

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

   154

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

   155

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

   156

  /*  void *     pMappedAppObject = NULL;   */

   157

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

   158

     mapping entry */

   159

  UNS32 *    pMappingParameter = NULL;

   160

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

   161

                                         type */

   162

  UNS16 *    pwCobId = NULL;

   163

  UNS8       Size;

   164

  UNS8       offset;

   165

  UNS8       status;

   166

  UNS32      objDict;

   167

  UNS16      offsetObjdict;

   168

  UNS16      lastIndex;

   169

   170

  status = state2;

   171

   172

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

   173

  offset = 0x00;

   174

  numPdo = 0;

   175

  numMap = 0;

   176

  if((*m).rtr == NOT_A_REQUEST ) { /* The PDO received is not a

   177

                                     request. */

   178

   179

    offsetObjdict = d->firstIndex->PDO_RCV;

   180

    lastIndex = d->lastIndex->PDO_RCV;

   181

   182

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

   183

    if(offsetObjdict)

   184

      while (offsetObjdict <= lastIndex) {

   185

   186

        switch( status ) {

   187

   188

        case state2:

   189

          /* get CobId of the dictionary correspondant to the received

   190

             PDO */

   191

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

   192

          /* check the CobId coherance */

   193

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

   194

            */

   195

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

   196

            /* The cobId is recognized */

   197

            status = state4;

   198

            MSG_WAR(0x3936, "cobId found at index ", 0x1400 + numPdo);

   199

            break;

   200

          }

   201

          else {

   202

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

   203

              dictionnary */

   204

            numPdo++;

   205

            offsetObjdict++;

   206

            status = state2;

   207

            break;

   208

          }

   209

   210

            case state4:/* Get Mapped Objects Number */

   211

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

   212

                 dictionnary. */

   213

               offsetObjdict = d->firstIndex->PDO_RCV_MAP;

   214

             lastIndex = d->lastIndex->PDO_RCV_MAP;

   215

             pMappingCount = (UNS8*) (d->objdict + offsetObjdict + numPdo)->pSubindex[0].pObject;

   216

             numMap = 0;

   217

             while (numMap < *pMappingCount) {

   218

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

   219

               UNS8 ByteSize;

   220

               pMappingParameter = (UNS32*) (d->objdict + offsetObjdict + numPdo)->pSubindex[numMap + 1].pObject;

   221

               if (pMappingParameter == NULL) {

   222

                 MSG_ERR(0x1937, "Couldn't get mapping parameter : ", numMap + 1);

   223

                 return 0xFF;

   224

               }

   225

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

   226

               /* detail of *pMappingParameter : */

   227

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

   228

                 contains the subindex, */

   229

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

   230

                 variable. */

   231

   232

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

   233

   234

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

   235

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

   236

                 /* copy bit per bit in little endian */

   237

                 CopyBits(Size, (UNS8*)&m->data[offset>>3], offset%8, 0, ((UNS8*)tmp), 0, 0);

   238

   239

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

   240

                                                     2, ... */

   241

   242

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

   243

                                      (UNS8)(((*pMappingParameter) >> 8 ) & 0xFF),

   244

                                   tmp, &ByteSize, 0 );

   245

   246

                 if(objDict != OD_SUCCESSFUL) {

   247

                   MSG_ERR(0x1938, "error accessing to the mapped var : ", numMap + 1);

   248

                   MSG_WAR(0x2939, "         Mapped at index : ", (*pMappingParameter) >> 16);

   249

                   MSG_WAR(0x2940, "                subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF);

   250

                   return 0xFF;

   251

                 }

   252

   253

                 MSG_WAR(0x3942, "Variable updated with value received by PDO cobid : ", m->cob_id);

   254

                 MSG_WAR(0x3943, "         Mapped at index : ", (*pMappingParameter) >> 16);

   255

                 MSG_WAR(0x3944, "                subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF);

   256

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

   257

                 offset += Size;

   258

					}

   259

               numMap++;

   260

             } /* end loop while on mapped variables */

   261

   262

             offset=0x00;

   263

             numMap = 0;

   264

             return 0;

   265

   266

        }/* end switch status*/

   267

      }/* end while*/

   268

  }/* end if Donnees */

   269

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

   270

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

   271

    status = state1;

   272

    offsetObjdict = d->firstIndex->PDO_TRS;

   273

    lastIndex = d->lastIndex->PDO_TRS;

   274

    if(offsetObjdict) while( offsetObjdict  <= lastIndex ){

   275

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

   276

   277

      switch( status ){

   278

   279

      case state1:/* check the CobId */

   280

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

   281

         */

   282

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

   283

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

   284

          status = state4;

   285

          break;

   286

        }

   287

        else {

   288

          numPdo++;

   289

          offsetObjdict++;

   290

        }

   291

        status = state1;

   292

        break;

   293

   294

   295

      case state4:/* check transmission type */

   296

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

   297

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

   298

        if ( (*pTransmissionType == TRANS_RTR)) {

   299

          status = state5;

   300

          break;

   301

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

   302

        }else if ((*pTransmissionType == TRANS_RTR_SYNC )) {

   303

    	  if(d->PDO_status[numPdo].transmit_type_parameter & PDO_RTR_SYNC_READY){

   304

            /*Data ready, just send*/

   305

    	    canSend(d->canHandle,&d->PDO_status[numPdo].last_message);

   306

            return 0;

   307

    	  }else{

   308

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

   309

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

   310

            MSG_ERR(0x1947, "Not ready RTR_SYNC TPDO send current data : ", m->cob_id);

   311

            status = state5;

   312

    	  }

   313

          break;

   314

        }else if(

   315

             (*pTransmissionType == TRANS_EVENT_PROFILE) ||

   316

             (*pTransmissionType == TRANS_EVENT_SPECIFIC) ) {

   317

	  /* Zap all timers and inhibit flag */

   318

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

   319

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

   320

  	  d->PDO_status[numPdo].transmit_type_parameter &= ~PDO_INHIBITED;

   321

  	  /* Call  PDOEventTimerAlarm for this TPDO, this will trigger emission et reset timers */

   322

          PDOEventTimerAlarm(d, numPdo);

   323

          return 0;

   324

        }else {

   325

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

   326

            nothing. */

   327

          MSG_WAR(0x2947, "PDO is not to send on request : ", m->cob_id);

   328

          return 0xFF;

   329

        }

   330

   331

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

   332

      {

   333

      	Message pdo;

   334

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

   335

    	{

   336

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

   337

          return 0xFF;

   338

    	}

   339

    	canSend(d->canHandle,&pdo);

   340

        return 0;

   341

      }

   342

      }/* end switch status */

   343

    }/* end while */

   344

  }/* end if Requete */

   345

   346

  return 0;

   347

}

   348

   349

/*!

   350

**

   351

**

   352

** @param NbBits

   353

** @param SrcByteIndex

   354

** @param SrcBitIndex

   355

** @param SrcBigEndian

   356

** @param DestByteIndex

   357

** @param DestBitIndex

   358

** @param DestBigEndian

   359

**/

   360

void CopyBits(UNS8 NbBits, UNS8* SrcByteIndex, UNS8 SrcBitIndex, UNS8 SrcBigEndian, UNS8* DestByteIndex, UNS8 DestBitIndex, UNS8 DestBigEndian)

   361

{

   362

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

   363

  /* successively bytes*/

   364

  // boundaries from LSB to MSB.

   365

  while(NbBits > 0)

   366

    {

   367

      /* Bit missalignement between src and dest*/

   368

      INTEGER8 Vect = DestBitIndex - SrcBitIndex;

   369

   370

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

   371

      UNS8 Aligned = Vect>0 ? *SrcByteIndex << Vect : *SrcByteIndex >> -Vect;

   372

   373

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

   374

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

   375

      UNS8 BitsToCopy = BoudaryLimit > NbBits ? NbBits : BoudaryLimit;

   376

   377

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

   378

      UNS8 Mask = ((0xff << (DestBitIndex + BitsToCopy)) | (0xff >> (8 - DestBitIndex)));

   379

   380

      /* - Filtering src*/

   381

      UNS8 Filtered = Aligned & ~Mask;

   382

   383

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

   384

      *DestByteIndex &= Mask;

   385

   386

      /* Then write.*/

   387

      *DestByteIndex |= Filtered ;

   388

   389

      /*Compute next time cursors for src*/

   390

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

   391

        {

   392

          SrcBitIndex = 0;/* First bit*/

   393

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

   394

        }

   395

   396

   397

      /*Compute next time cursors for dest*/

   398

      if((DestBitIndex += BitsToCopy)>7)

   399

        {

   400

          DestBitIndex = 0;/* First bit*/

   401

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

   402

        }

   403

   404

      /*And decrement counter.*/

   405

      NbBits -= BitsToCopy;

   406

    }

   407

   408

}

   409

/*!

   410

**

   411

**

   412

** @param d

   413

**

   414

** @return

   415

**/

   416

   417

UNS8 sendPDOevent( CO_Data* d)

   418

{

   419

  /* Calls _sendPDOevent specifying it is not a sync event */

   420

  return _sendPDOevent(d, 0);

   421

}

   422

   423

   424

void PDOEventTimerAlarm(CO_Data* d, UNS32 pdoNum)

   425

{

   426

	/* This is needed to avoid deletion of re-attribuated timer */

   427

	d->PDO_status[pdoNum].event_timer = TIMER_NONE;

   428

	/* force emission of PDO by artificially changing last emitted*/

   429

	d->PDO_status[pdoNum].last_message.cob_id = 0;

   430

	_sendPDOevent( d, 0 ); /* not a Sync Event*/	

   431

}

   432

   433

void PDOInhibitTimerAlarm(CO_Data* d, UNS32 pdoNum)

   434

{

   435

	/* This is needed to avoid deletion of re-attribuated timer */

   436

	d->PDO_status[pdoNum].inhibit_timer = TIMER_NONE;

   437

	/* Remove inhibit flag */

   438

	d->PDO_status[pdoNum].transmit_type_parameter &= ~PDO_INHIBITED;

   439

	_sendPDOevent( d, 0 ); /* not a Sync Event*/

   440

}

   441

   442

/*!

   443

**

   444

**

   445

** @param d

   446

** @param isSyncEvent

   447

**

   448

** @return

   449

**/

   450

   451

UNS8 _sendPDOevent( CO_Data* d, UNS8 isSyncEvent )

   452

{ 

   453

  UNS8 	pdoNum = 0x00;       /* number of the actual processed pdo-nr. */

   454

  UNS8* pTransmissionType = NULL;  

   455

  UNS8 status = state3;

   456

  UNS16 offsetObjdict = d->firstIndex->PDO_TRS;

   457

  UNS16 offsetObjdictMap = d->firstIndex->PDO_TRS_MAP;

   458

  UNS16 lastIndex = d->lastIndex->PDO_TRS;  

   459

   460

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

   461

  if(offsetObjdict){

   462

   Message pdo = Message_Initializer;

   463

   while( offsetObjdict <= lastIndex) {  

   464

    switch( status ) {

   465

    case state3:

   466

      if (/*d->objdict[offsetObjdict].bSubCount < 5 || not necessary with objdictedit (always 5)*/

   467

          /* check if TPDO is not valid */ 

   468

          *(UNS32*)d->objdict[offsetObjdict].pSubindex[1].pObject & 0x80000000) {

   469

	  MSG_WAR(0x3960, "Not a valid PDO ", 0x1800 + pdoNum);

   470

	  /*Go next TPDO*/

   471

	  status = state11;

   472

	  break;

   473

	}

   474

      /* get the PDO transmission type */

   475

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

   476

      MSG_WAR(0x3962, "Reading PDO at index : ", 0x1800 + pdoNum);

   477

   478

      /* check if transmission type is SYNCRONOUS */

   479

      /* The message is transmited every n SYNC with n=TransmissionType */      

   480

      if( isSyncEvent && 

   481

      	  (*pTransmissionType >= TRANS_SYNC_MIN) &&

   482

      	  (*pTransmissionType <= TRANS_SYNC_MAX) &&

   483

          (++d->PDO_status[pdoNum].transmit_type_parameter == *pTransmissionType) ) {

   484

        /*Reset count of SYNC*/	

   485

	d->PDO_status[pdoNum].transmit_type_parameter = 0;

   486

	MSG_WAR(0x3964, "  PDO is on SYNCHRO. Trans type : ", *pTransmissionType);

   487

 {

   488

 Message msg_init = Message_Initializer;