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

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

    72

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

    73

    74

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

    75

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

    76

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

    77

    78

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

    79

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

    80

			return 0xFF;

    81

		}

    82

		/* copy bit per bit in little endian*/

    83

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

    84

    85

		offset += Size ;

    86

		prp_j++;

    87

	}while( prp_j < *pMappingCount );

    88

    89

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

    90

    91

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

    92

    93

	return 0;

    94

}

    95

    96

/*!

    97

**

    98

**

    99

** @param d

   100

** @param cobId

   101

**

   102

** @return

   103

**/

   104

UNS8 sendPDOrequest( CO_Data* d, UNS16 RPDOIndex )

   105

{

   106

  UNS32 * pwCobId;

   107

  UNS16 offset = d->firstIndex->PDO_RCV;

   108

  UNS16 lastIndex = d->lastIndex->PDO_RCV;

   109

   110

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

   111

     receive */

   112

  /* part dictionary */

   113

   114

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

   115

   116

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

   117

    offset += RPDOIndex - 0x1400;  

   118

    if (offset <= lastIndex) {

   119

      /* get the CobId*/

   120

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

   121

   122

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

   123

      {

   124

          Message pdo;

   125

          pdo.cob_id.w = *pwCobId;

   126

          pdo.rtr = REQUEST;

   127

          pdo.len = 0;      

   128

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

   129

      }

   130

    }

   131

  }

   132

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

   133

  return 0xFF;

   134

}

   135

   136

   137

/*!

   138

**

   139

**

   140

** @param d

   141

** @param m

   142

**

   143

** @return

   144

**/

   145

UNS8 proceedPDO(CO_Data* d, Message *m)

   146

{

   147

  UNS8   numPdo;

   148

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

   149

  UNS8 i;

   150

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

   151

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

   152

  /*  void *     pMappedAppObject = NULL;   */

   153

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

   154

     mapping entry */

   155

  UNS32 *    pMappingParameter = NULL;

   156

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

   157

                                         type */

   158

  UNS32 *    pwCobId = NULL;

   159

  UNS8       Size;

   160

  UNS8       dataType;

   161

  UNS8       offset;

   162

  UNS8       status;

   163

  UNS32      objDict;

   164

  UNS16      offsetObjdict;

   165

  UNS16      lastIndex;

   166

   167

  status = state2;

   168

   169

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

   170

  offset = 0x00;

   171

  numPdo = 0;

   172

  numMap = 0;

   173

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

   174

                                     request. */

   175

   176

    offsetObjdict = d->firstIndex->PDO_RCV;

   177

    lastIndex = d->lastIndex->PDO_RCV;

   178

   179

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

   180

    if(offsetObjdict)

   181

      while (offsetObjdict <= lastIndex) {

   182

   183

        switch( status ) {

   184

   185

        case state2:

   186

          /* get CobId of the dictionary correspondant to the received

   187

             PDO */

   188

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

   189

          /* check the CobId coherance */

   190

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

   191

            */

   192

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

   193

            /* The cobId is recognized */

   194

            status = state4;

   195

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

   196

            break;

   197

          }

   198

          else {

   199

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

   200

              dictionnary */

   201

            numPdo++;

   202

            offsetObjdict++;

   203

            status = state2;

   204

            break;

   205

          }

   206

   207

            case state4:/* Get Mapped Objects Number */

   208

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

   209

                 dictionnary. */

   210

               offsetObjdict = d->firstIndex->PDO_RCV_MAP;

   211

             lastIndex = d->lastIndex->PDO_RCV_MAP;

   212

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

   213

             numMap = 0;

   214

             while (numMap < *pMappingCount) {

   215

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

   216

               UNS8 ByteSize;

   217

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

   218

               if (pMappingParameter == NULL) {

   219

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

   220

                 return 0xFF;

   221

               }

   222

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

   223

               /* detail of *pMappingParameter : */

   224

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

   225

                 contains the subindex, */

   226

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

   227

                 variable. */

   228

   229

               Size = (UNS8)(*pMappingParameter);

   230

   231

               /* copy bit per bit in little endian */

   232

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

   233

   234

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

   235

                                                   2, ... */

   236

   237

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

   238

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

   239

                                 tmp, &ByteSize, 0 );

   240

   241

               if(objDict != OD_SUCCESSFUL) {

   242

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

   243

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

   244

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

   245

                 return 0xFF;

   246

               }

   247

   248

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

   249

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

   250

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

   251

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

   252

               offset += Size;

   253

               numMap++;

   254

               /*TODO :  check that offset is not not greater that message size (in bit) */

   255

             } /* end loop while on mapped variables */

   256

   257

             offset=0x00;

   258

             numMap = 0;

   259

             return 0;

   260

   261

        }/* end switch status*/

   262

      }/* end while*/

   263

  }/* end if Donnees */

   264

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

   265

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

   266

    status = state1;

   267

    offsetObjdict = d->firstIndex->PDO_TRS;

   268

    lastIndex = d->lastIndex->PDO_TRS;

   269

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

   270

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

   271

   272

      switch( status ){

   273

   274

      case state1:/* check the CobId */

   275

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

   276

         */

   277

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

   278

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

   279

          status = state4;

   280

          break;

   281

        }

   282

        else {

   283

          numPdo++;

   284

          offsetObjdict++;

   285

        }

   286

        status = state1;

   287

        break;

   288

   289

   290

      case state4:/* check transmission type */

   291

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

   292

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

   293

        if ( (*pTransmissionType == TRANS_RTR)) {

   294

          status = state5;

   295

          break;

   296

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

   297

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

   298

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

   299

            /*Data ready, just send*/

   300

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

   301

            return 0;

   302

    	  }else{

   303

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

   304

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

   305

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

   306

            status = state5;

   307

    	  }

   308

          break;

   309

        }else if(

   310

             (*pTransmissionType == TRANS_EVENT_PROFILE) ||

   311

             (*pTransmissionType == TRANS_EVENT_SPECIFIC) ) {

   312

	  /* Zap all timers and inhibit flag */

   313

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

   314

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

   315

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

   316

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

   317

          PDOEventTimerAlarm(d, numPdo);

   318

          return 0;

   319

        }else {

   320

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

   321

            nothing. */

   322

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

   323

          return 0xFF;

   324

        }

   325

   326

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

   327

      {

   328

      	Message pdo;

   329

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

   330

    	{

   331

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

   332

          return 0xFF;

   333

    	}

   334

    	canSend(d->canHandle,&pdo);

   335

        return 0;

   336

      }

   337

      }/* end switch status */

   338

    }/* end while */

   339

  }/* end if Requete */

   340

   341

  return 0;

   342

}

   343

   344

/*!

   345

**

   346

**

   347

** @param NbBits

   348

** @param SrcByteIndex

   349

** @param SrcBitIndex

   350

** @param SrcBigEndian

   351

** @param DestByteIndex

   352

** @param DestBitIndex

   353

** @param DestBigEndian

   354

**/

   355

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

   356

{

   357

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

   358

  /* successively bytes*/

   359

  // boundaries from LSB to MSB.

   360

  while(NbBits > 0)

   361

    {

   362

      /* Bit missalignement between src and dest*/

   363

      INTEGER8 Vect = DestBitIndex - SrcBitIndex;

   364

   365

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

   366

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

   367

   368

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

   369

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

   370

      UNS8 BitsToCopy = BoudaryLimit > NbBits ? NbBits : BoudaryLimit;

   371

   372

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

   373

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

   374

   375

      /* - Filtering src*/

   376

      UNS8 Filtered = Aligned & ~Mask;

   377

   378

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

   379

      *DestByteIndex &= Mask;

   380

   381

      /* Then write.*/

   382

      *DestByteIndex |= Filtered ;

   383

   384

      /*Compute next time cursors for src*/

   385

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

   386

        {

   387

          SrcBitIndex = 0;/* First bit*/

   388

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

   389

        }

   390

   391

   392

      /*Compute next time cursors for dest*/

   393

      if((DestBitIndex += BitsToCopy)>7)

   394

        {

   395

          DestBitIndex = 0;/* First bit*/

   396

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

   397

        }

   398

   399

      /*And decrement counter.*/

   400

      NbBits -= BitsToCopy;

   401

    }

   402

   403

}

   404

/*!

   405

**

   406

**

   407

** @param d

   408

**

   409

** @return

   410

**/

   411

   412

UNS8 sendPDOevent( CO_Data* d)

   413

{

   414

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

   415

  return _sendPDOevent(d, 0);

   416

}

   417

   418

   419

void PDOEventTimerAlarm(CO_Data* d, UNS32 pdoNum)

   420

{

   421

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

   422

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

   423

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

   424

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

   425

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

   426

}

   427

   428

void PDOInhibitTimerAlarm(CO_Data* d, UNS32 pdoNum)

   429

{

   430

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

   431

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

   432

	/* Remove inhibit flag */

   433

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

   434

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

   435

}

   436

   437

/*!

   438

**

   439

**

   440

** @param d

   441

** @param isSyncEvent

   442

**

   443

** @return

   444

**/

   445

   446

UNS8 _sendPDOevent( CO_Data* d, UNS8 isSyncEvent )

   447

{ 

   448

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

   449

  UNS8* pTransmissionType = NULL;  

   450

  UNS8 status = state3;

   451

  UNS16 offsetObjdict = d->firstIndex->PDO_TRS;

   452

  UNS16 offsetObjdictMap = d->firstIndex->PDO_TRS_MAP;

   453

  UNS16 lastIndex = d->lastIndex->PDO_TRS;  

   454

   455

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

   456

  if(offsetObjdict){

   457

   Message pdo = Message_Initializer;

   458

   while( offsetObjdict <= lastIndex) {  

   459

    switch( status ) {

   460

    case state3:

   461

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

   462

          /* check if TPDO is not valid */ 

   463

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

   464

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

   465

	  /*Go next TPDO*/

   466

	  status = state11;

   467

	  break;

   468

	}

   469

      /* get the PDO transmission type */

   470

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

   471

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

   472

   473

      /* check if transmission type is SYNCRONOUS */

   474

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

   475

      if( isSyncEvent && 

   476

      	  (*pTransmissionType >= TRANS_SYNC_MIN) &&

   477

      	  (*pTransmissionType <= TRANS_SYNC_MAX) &&

   478

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

   479

        /*Reset count of SYNC*/	

   480

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

   481

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

   482

 {

   483

 Message msg_init = Message_Initializer;

   484

 pdo = msg_init;

   485

 }	

   486

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

   487

        {

   488

            MSG_ERR(0x1906, " Couldn't build TPDO number : ", pdoNum);

   489

	    status = state11;