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 *     pMappingCount = NULL;    /* count of mapped objects... */

   150

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

   151

  /*  void *     pMappedAppObject = NULL;   */

   152

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

   153

     mapping entry */

   154

  UNS32 *    pMappingParameter = NULL;

   155

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

   156

                                         type */

   157

  UNS32 *    pwCobId = NULL;

   158

  UNS8       Size;

   159

  UNS8       offset;

   160

  UNS8       status;

   161

  UNS32      objDict;

   162

  UNS16      offsetObjdict;

   163

  UNS16      lastIndex;

   164

   165

  status = state2;

   166

   167

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

   168

  offset = 0x00;

   169

  numPdo = 0;

   170

  numMap = 0;

   171

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

   172

                                     request. */

   173

   174

    offsetObjdict = d->firstIndex->PDO_RCV;

   175

    lastIndex = d->lastIndex->PDO_RCV;

   176

   177

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

   178

    if(offsetObjdict)

   179

      while (offsetObjdict <= lastIndex) {

   180

   181

        switch( status ) {

   182

   183

        case state2:

   184

          /* get CobId of the dictionary correspondant to the received

   185

             PDO */

   186

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

   187

          /* check the CobId coherance */

   188

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

   189

            */

   190

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

   191

            /* The cobId is recognized */

   192

            status = state4;

   193

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

   194

            break;

   195

          }

   196

          else {

   197

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

   198

              dictionnary */

   199

            numPdo++;

   200

            offsetObjdict++;

   201

            status = state2;

   202

            break;

   203

          }

   204

   205

            case state4:/* Get Mapped Objects Number */

   206

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

   207

                 dictionnary. */

   208

               offsetObjdict = d->firstIndex->PDO_RCV_MAP;

   209

             lastIndex = d->lastIndex->PDO_RCV_MAP;

   210

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

   211

             numMap = 0;

   212

             while (numMap < *pMappingCount) {

   213

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

   214

               UNS8 ByteSize;

   215

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

   216

               if (pMappingParameter == NULL) {

   217

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

   218

                 return 0xFF;

   219

               }

   220

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

   221

               /* detail of *pMappingParameter : */

   222

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

   223

                 contains the subindex, */

   224

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

   225

                 variable. */

   226

   227

               Size = (UNS8)(*pMappingParameter);

   228

   229

               /* copy bit per bit in little endian */

   230

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

   231

   232

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

   233

                                                   2, ... */

   234

   235

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

   236

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

   237

                                 tmp, &ByteSize, 0 );

   238

   239

               if(objDict != OD_SUCCESSFUL) {

   240

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

   241

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

   242

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

   243

                 return 0xFF;

   244

               }

   245

   246

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

   247

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

   248

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

   249

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

   250

               offset += Size;

   251

               numMap++;

   252

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

   253

             } /* end loop while on mapped variables */

   254

   255

             offset=0x00;

   256

             numMap = 0;

   257

             return 0;

   258

   259

        }/* end switch status*/

   260

      }/* end while*/

   261

  }/* end if Donnees */

   262

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

   263

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

   264

    status = state1;

   265

    offsetObjdict = d->firstIndex->PDO_TRS;

   266

    lastIndex = d->lastIndex->PDO_TRS;

   267

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

   268

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

   269

   270

      switch( status ){

   271

   272

      case state1:/* check the CobId */

   273

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

   274

         */

   275

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

   276

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

   277

          status = state4;

   278

          break;

   279

        }

   280

        else {

   281

          numPdo++;

   282

          offsetObjdict++;

   283

        }

   284

        status = state1;

   285

        break;

   286

   287

   288

      case state4:/* check transmission type */

   289

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

   290

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

   291

        if ( (*pTransmissionType == TRANS_RTR)) {

   292

          status = state5;

   293

          break;

   294

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

   295

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

   296

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

   297

            /*Data ready, just send*/

   298

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

   299

            return 0;

   300

    	  }else{

   301

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

   302

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

   303

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

   304

            status = state5;

   305

    	  }

   306

          break;

   307

        }else if(

   308

             (*pTransmissionType == TRANS_EVENT_PROFILE) ||

   309

             (*pTransmissionType == TRANS_EVENT_SPECIFIC) ) {

   310

	  /* Zap all timers and inhibit flag */

   311

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

   312

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

   313

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

   314

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

   315

          PDOEventTimerAlarm(d, numPdo);

   316

          return 0;

   317

        }else {

   318

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

   319

            nothing. */

   320

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

   321

          return 0xFF;

   322

        }

   323

   324

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

   325

      {

   326

      	Message pdo;

   327

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

   328

    	{

   329

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

   330

          return 0xFF;

   331

    	}

   332

    	canSend(d->canHandle,&pdo);

   333

        return 0;

   334

      }

   335

      }/* end switch status */

   336

    }/* end while */

   337

  }/* end if Requete */

   338

   339

  return 0;

   340

}

   341

   342

/*!

   343

**

   344

**

   345

** @param NbBits

   346

** @param SrcByteIndex

   347

** @param SrcBitIndex

   348

** @param SrcBigEndian

   349

** @param DestByteIndex

   350

** @param DestBitIndex

   351

** @param DestBigEndian

   352

**/

   353

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

   354

{

   355

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

   356

  /* successively bytes*/

   357

  // boundaries from LSB to MSB.

   358

  while(NbBits > 0)

   359

    {

   360

      /* Bit missalignement between src and dest*/

   361

      INTEGER8 Vect = DestBitIndex - SrcBitIndex;

   362

   363

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

   364

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

   365

   366

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

   367

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

   368

      UNS8 BitsToCopy = BoudaryLimit > NbBits ? NbBits : BoudaryLimit;

   369

   370

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

   371

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

   372

   373

      /* - Filtering src*/

   374

      UNS8 Filtered = Aligned & ~Mask;

   375

   376

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

   377

      *DestByteIndex &= Mask;

   378

   379

      /* Then write.*/

   380

      *DestByteIndex |= Filtered ;

   381

   382

      /*Compute next time cursors for src*/

   383

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

   384

        {

   385

          SrcBitIndex = 0;/* First bit*/

   386

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

   387

        }

   388

   389

   390

      /*Compute next time cursors for dest*/

   391

      if((DestBitIndex += BitsToCopy)>7)

   392

        {

   393

          DestBitIndex = 0;/* First bit*/

   394

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

   395

        }

   396

   397

      /*And decrement counter.*/

   398

      NbBits -= BitsToCopy;

   399

    }

   400

   401

}

   402

/*!

   403

**

   404

**

   405

** @param d

   406

**

   407

** @return

   408

**/

   409

   410

UNS8 sendPDOevent( CO_Data* d)

   411

{

   412

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

   413

  return _sendPDOevent(d, 0);

   414

}

   415

   416

   417

void PDOEventTimerAlarm(CO_Data* d, UNS32 pdoNum)

   418

{

   419

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

   420

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

   421

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

   422

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

   423

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

   424

}

   425

   426

void PDOInhibitTimerAlarm(CO_Data* d, UNS32 pdoNum)

   427

{

   428

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

   429

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

   430

	/* Remove inhibit flag */

   431

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

   432

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

   433

}

   434

   435

/*!

   436

**

   437

**

   438

** @param d

   439

** @param isSyncEvent

   440

**

   441

** @return

   442

**/

   443

   444

UNS8 _sendPDOevent( CO_Data* d, UNS8 isSyncEvent )

   445

{ 

   446

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

   447

  UNS8* pTransmissionType = NULL;  

   448

  UNS8 status = state3;

   449

  UNS16 offsetObjdict = d->firstIndex->PDO_TRS;

   450

  UNS16 offsetObjdictMap = d->firstIndex->PDO_TRS_MAP;

   451

  UNS16 lastIndex = d->lastIndex->PDO_TRS;  

   452

   453

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

   454

  if(offsetObjdict){

   455

   Message pdo = Message_Initializer;

   456

   while( offsetObjdict <= lastIndex) {  

   457

    switch( status ) {

   458

    case state3:

   459

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

   460

          /* check if TPDO is not valid */ 

   461

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

   462

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

   463

	  /*Go next TPDO*/

   464

	  status = state11;

   465

	  break;

   466

	}

   467

      /* get the PDO transmission type */

   468

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

   469

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

   470

   471

      /* check if transmission type is SYNCRONOUS */

   472

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

   473

      if( isSyncEvent && 

   474

      	  (*pTransmissionType >= TRANS_SYNC_MIN) &&

   475

      	  (*pTransmissionType <= TRANS_SYNC_MAX) &&

   476

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

   477

        /*Reset count of SYNC*/	

   478

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

   479

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

   480

 {

   481

 Message msg_init = Message_Initializer;

   482

 pdo = msg_init;

   483

 }	

   484

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

   485

        {

   486

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

   487

	    status = state11;

   488

	    break;

   489

        }