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

    28

/*!

    29

** @file   pdo.c

    30

** @author Edouard TISSERANT and Francis DUPIN

    31

** @date   Tue Jun  5 09:32:32 2007

    32

**

    33

** @brief

    34

**

    35

**

    36

*/

    37

    38

/*!

    39

**

    40

**

    41

** @param d

    42

** @param TPDO_com TPDO communication parameters OD entry

    43

** @param TPDO_map TPDO mapping parameters OD entry

    44

**

    45

** @return

    46

**/

    47

    48

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

    49

{

    50

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

    51

	const indextable* TPDO_map = d->objdict + d->firstIndex->PDO_TRS_MAP + numPdo;

    52

    53

	UNS8 prp_j = 0x00;

    54

	UNS8 offset = 0x00;

    55

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

    56

    57

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

    58

	pdo->rtr = NOT_A_REQUEST;

    59

    60

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

    61

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

    62

    63

	do{

    64

		UNS8 dataType; /* Unused */

    65

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

    66

    67

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

    68

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

    69

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

    70

		UNS8 Size = (UNS8)(*pMappingParameter); /* Size in bits */

    71

    72

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

    73

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

    74

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

    75

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

    76

    77

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

    78

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

    79

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

    80

    81

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

    82

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

    83

				return 0xFF;

    84

			}

    85

			/* copy bit per bit in little endian*/

    86

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

    87

    88

			offset += Size ;

    89

		}

    90

		prp_j++;

    91

	}while( prp_j < *pMappingCount );

    92

    93

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

    94

    95

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

    96

    97

	return 0;

    98

}

    99

   100

/*!

   101

**

   102

**

   103

** @param d

   104

** @param cobId

   105

**

   106

** @return

   107

**/

   108

UNS8 sendPDOrequest( CO_Data* d, UNS16 RPDOIndex )

   109

{

   110

  UNS32 * pwCobId;

   111

  UNS16 offset = d->firstIndex->PDO_RCV;

   112

  UNS16 lastIndex = d->lastIndex->PDO_RCV;

   113

   114

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

   115

     receive */

   116

  /* part dictionary */

   117

   118

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

   119

   120

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

   121

    offset += RPDOIndex - 0x1400;  

   122

    if (offset <= lastIndex) {

   123

      /* get the CobId*/

   124

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

   125

   126

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

   127

      {

   128

          Message pdo;

   129

          pdo.cob_id.w = *pwCobId;

   130

          pdo.rtr = REQUEST;

   131

          pdo.len = 0;      

   132

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

   133

      }

   134

    }

   135

  }

   136

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

   137

  return 0xFF;

   138

}

   139

   140

   141

/*!

   142

**

   143

**

   144

** @param d

   145

** @param m

   146

**

   147

** @return

   148

**/

   149

UNS8 proceedPDO(CO_Data* d, Message *m)

   150

{

   151

  UNS8   numPdo;

   152

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

   153

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

   154

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

   155

  /*  void *     pMappedAppObject = NULL;   */

   156

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

   157

     mapping entry */

   158

  UNS32 *    pMappingParameter = NULL;

   159

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

   160

                                         type */

   161

  UNS32 *    pwCobId = NULL;

   162

  UNS8       Size;

   163

  UNS8       offset;

   164

  UNS8       status;

   165

  UNS32      objDict;

   166

  UNS16      offsetObjdict;

   167

  UNS16      lastIndex;

   168

   169

  status = state2;

   170

   171

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

   172

  offset = 0x00;

   173

  numPdo = 0;

   174

  numMap = 0;

   175

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

   176

                                     request. */

   177

   178

    offsetObjdict = d->firstIndex->PDO_RCV;

   179

    lastIndex = d->lastIndex->PDO_RCV;

   180

   181

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

   182

    if(offsetObjdict)

   183

      while (offsetObjdict <= lastIndex) {

   184

   185

        switch( status ) {

   186

   187

        case state2:

   188

          /* get CobId of the dictionary correspondant to the received

   189

             PDO */

   190

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

   191

          /* check the CobId coherance */

   192

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

   193

            */

   194

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

   195

            /* The cobId is recognized */

   196

            status = state4;

   197

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

   198

            break;

   199

          }

   200

          else {

   201

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

   202

              dictionnary */

   203

            numPdo++;

   204

            offsetObjdict++;

   205

            status = state2;

   206

            break;

   207

          }

   208

   209

            case state4:/* Get Mapped Objects Number */

   210

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

   211

                 dictionnary. */

   212

               offsetObjdict = d->firstIndex->PDO_RCV_MAP;

   213

             lastIndex = d->lastIndex->PDO_RCV_MAP;

   214

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

   215

             numMap = 0;

   216

             while (numMap < *pMappingCount) {

   217

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

   218

               UNS8 ByteSize;

   219

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

   220

               if (pMappingParameter == NULL) {

   221

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

   222

                 return 0xFF;

   223

               }

   224

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

   225

               /* detail of *pMappingParameter : */

   226

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

   227

                 contains the subindex, */

   228

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

   229

                 variable. */

   230

   231

               Size = (UNS8)(*pMappingParameter);

   232

   233

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

   234

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

   235

                 /* copy bit per bit in little endian */

   236

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

   237

   238

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

   239

                                                     2, ... */

   240

   241

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

   242

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

   243

                                   tmp, &ByteSize, 0 );

   244

   245

                 if(objDict != OD_SUCCESSFUL) {

   246

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

   247

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

   248

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

   249

                   return 0xFF;

   250

                 }

   251

   252

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

   253

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

   254

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

   255

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

   256

                 offset += Size;

   257

					}

   258

               numMap++;

   259

             } /* end loop while on mapped variables */

   260

   261

             offset=0x00;

   262

             numMap = 0;

   263

             return 0;

   264

   265

        }/* end switch status*/

   266

      }/* end while*/

   267

  }/* end if Donnees */

   268

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

   269

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

   270

    status = state1;

   271

    offsetObjdict = d->firstIndex->PDO_TRS;

   272

    lastIndex = d->lastIndex->PDO_TRS;

   273

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

   274

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

   275

   276

      switch( status ){

   277

   278

      case state1:/* check the CobId */

   279

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

   280

         */

   281

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

   282

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

   283

          status = state4;

   284

          break;

   285

        }

   286

        else {

   287

          numPdo++;

   288

          offsetObjdict++;

   289

        }

   290

        status = state1;

   291

        break;

   292

   293

   294

      case state4:/* check transmission type */

   295

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

   296

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

   297

        if ( (*pTransmissionType == TRANS_RTR)) {

   298

          status = state5;

   299

          break;

   300

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

   301

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

   302

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

   303

            /*Data ready, just send*/

   304

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

   305

            return 0;

   306

    	  }else{

   307

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

   308

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

   309

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

   310

            status = state5;

   311

    	  }

   312

          break;

   313

        }else if(

   314

             (*pTransmissionType == TRANS_EVENT_PROFILE) ||

   315

             (*pTransmissionType == TRANS_EVENT_SPECIFIC) ) {

   316

	  /* Zap all timers and inhibit flag */

   317

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

   318

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

   319

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

   320

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

   321

          PDOEventTimerAlarm(d, numPdo);

   322

          return 0;

   323

        }else {

   324

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

   325

            nothing. */

   326

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

   327

          return 0xFF;

   328

        }

   329

   330

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

   331

      {

   332

      	Message pdo;

   333

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

   334

    	{

   335

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

   336

          return 0xFF;

   337

    	}

   338

    	canSend(d->canHandle,&pdo);

   339

        return 0;

   340

      }

   341

      }/* end switch status */

   342

    }/* end while */

   343

  }/* end if Requete */

   344

   345

  return 0;

   346

}

   347

   348

/*!

   349

**

   350

**

   351

** @param NbBits

   352

** @param SrcByteIndex

   353

** @param SrcBitIndex

   354

** @param SrcBigEndian

   355

** @param DestByteIndex

   356

** @param DestBitIndex

   357

** @param DestBigEndian

   358

**/

   359

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

   360

{

   361

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

   362

  /* successively bytes*/

   363

  // boundaries from LSB to MSB.

   364

  while(NbBits > 0)

   365

    {

   366

      /* Bit missalignement between src and dest*/

   367

      INTEGER8 Vect = DestBitIndex - SrcBitIndex;

   368

   369

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

   370

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

   371

   372

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

   373

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

   374

      UNS8 BitsToCopy = BoudaryLimit > NbBits ? NbBits : BoudaryLimit;

   375

   376

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

   377

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

   378

   379

      /* - Filtering src*/

   380

      UNS8 Filtered = Aligned & ~Mask;

   381

   382

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

   383

      *DestByteIndex &= Mask;

   384

   385

      /* Then write.*/

   386

      *DestByteIndex |= Filtered ;

   387

   388

      /*Compute next time cursors for src*/

   389

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

   390

        {

   391

          SrcBitIndex = 0;/* First bit*/

   392

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

   393

        }

   394

   395

   396

      /*Compute next time cursors for dest*/

   397

      if((DestBitIndex += BitsToCopy)>7)

   398

        {

   399

          DestBitIndex = 0;/* First bit*/

   400

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

   401

        }

   402

   403

      /*And decrement counter.*/

   404

      NbBits -= BitsToCopy;

   405

    }

   406

   407

}

   408

/*!

   409

**

   410

**

   411

** @param d

   412

**

   413

** @return

   414

**/

   415

   416

UNS8 sendPDOevent( CO_Data* d)

   417

{

   418

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

   419

  return _sendPDOevent(d, 0);

   420

}

   421

   422

   423

void PDOEventTimerAlarm(CO_Data* d, UNS32 pdoNum)

   424

{

   425

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

   426

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

   427

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

   428

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

   429

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

   430

}

   431

   432

void PDOInhibitTimerAlarm(CO_Data* d, UNS32 pdoNum)

   433

{

   434

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

   435

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

   436

	/* Remove inhibit flag */

   437

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

   438

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

   439

}

   440

   441

/*!

   442

**

   443

**

   444

** @param d

   445

** @param isSyncEvent

   446

**

   447

** @return

   448

**/

   449

   450

UNS8 _sendPDOevent( CO_Data* d, UNS8 isSyncEvent )

   451

{ 

   452

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

   453

  UNS8* pTransmissionType = NULL;  

   454

  UNS8 status = state3;

   455

  UNS16 offsetObjdict = d->firstIndex->PDO_TRS;

   456

  UNS16 offsetObjdictMap = d->firstIndex->PDO_TRS_MAP;

   457

  UNS16 lastIndex = d->lastIndex->PDO_TRS;  

   458

   459

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

   460

  if(offsetObjdict){

   461

   Message pdo = Message_Initializer;

   462

   while( offsetObjdict <= lastIndex) {  

   463

    switch( status ) {

   464

    case state3:

   465

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

   466

          /* check if TPDO is not valid */ 

   467

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

   468

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

   469

	  /*Go next TPDO*/

   470

	  status = state11;

   471

	  break;

   472

	}

   473

      /* get the PDO transmission type */

   474

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

   475

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

   476

   477

      /* check if transmission type is SYNCRONOUS */

   478

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

   479

      if( isSyncEvent && 

   480

      	  (*pTransmissionType >= TRANS_SYNC_MIN) &&

   481

      	  (*pTransmissionType <= TRANS_SYNC_MAX) &&

   482

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

   483

        /*Reset count of SYNC*/	

   484

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

   485

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

   486

 {

   487

 Message msg_init = Message_Initializer;

   488

 pdo = msg_init;

   489

 }