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 = {*pwCobId, REQUEST, 0};

   125

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

   126

      }

   127

    }

   128

  }

   129

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

   130

  return 0xFF;

   131

}

   132

   133

   134

/*!

   135

**

   136

**

   137

** @param d

   138

** @param m

   139

**

   140

** @return

   141

**/

   142

UNS8 proceedPDO(CO_Data* d, Message *m)

   143

{

   144

  UNS8   numPdo;

   145

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

   146

  UNS8 i;

   147

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

   148

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

   149

  /*  void *     pMappedAppObject = NULL;   */

   150

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

   151

     mapping entry */

   152

  UNS32 *    pMappingParameter = NULL;

   153

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

   154

                                         type */

   155

  UNS32 *    pwCobId = NULL;

   156

  UNS8       Size;

   157

  UNS8       dataType;

   158

  UNS8       offset;

   159

  UNS8       status;

   160

  UNS32      objDict;

   161

  UNS16      offsetObjdict;

   162

  UNS16      lastIndex;

   163

   164

  status = state2;

   165

   166

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

   167

  offset = 0x00;

   168

  numPdo = 0;

   169

  numMap = 0;

   170

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

   171

                                     request. */

   172

   173

    offsetObjdict = d->firstIndex->PDO_RCV;

   174

    lastIndex = d->lastIndex->PDO_RCV;

   175

   176

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

   177

    if(offsetObjdict)

   178

      while (offsetObjdict <= lastIndex) {

   179

   180

        switch( status ) {

   181

   182

        case state2:

   183

          /* get CobId of the dictionary correspondant to the received

   184

             PDO */

   185

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

   186

          /* check the CobId coherance */

   187

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

   188

            */

   189

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

   190

            /* The cobId is recognized */

   191

            status = state4;

   192

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

   193

            break;

   194

          }

   195

          else {

   196

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

   197

              dictionnary */

   198

            numPdo++;

   199

            offsetObjdict++;

   200

            status = state2;

   201

            break;

   202

          }

   203

   204

            case state4:/* Get Mapped Objects Number */

   205

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

   206

                 dictionnary. */

   207

               offsetObjdict = d->firstIndex->PDO_RCV_MAP;

   208

             lastIndex = d->lastIndex->PDO_RCV_MAP;

   209

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

   210

             numMap = 0;

   211

             while (numMap < *pMappingCount) {

   212

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

   213

               UNS8 ByteSize;

   214

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

   215

               if (pMappingParameter == NULL) {

   216

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

   217

                 return 0xFF;

   218

               }

   219

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

   220

               /* detail of *pMappingParameter : */

   221

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

   222

                 contains the subindex, */

   223

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

   224

                 variable. */

   225

   226

               Size = (UNS8)(*pMappingParameter);

   227

   228

               /* copy bit per bit in little endian */

   229

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

   230

   231

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

   232

                                                   2, ... */

   233

   234

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

   235

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

   236

                                 tmp, &ByteSize, 0 );

   237

   238

               if(objDict != OD_SUCCESSFUL) {

   239

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

   240

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

   241

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

   242

                 return 0xFF;

   243

               }

   244

   245

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

   246

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

   247

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

   248

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

   249

               offset += Size;

   250

               numMap++;

   251

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

   252

             } /* end loop while on mapped variables */

   253

   254

             offset=0x00;

   255

             numMap = 0;

   256

             return 0;

   257

   258

        }/* end switch status*/

   259

      }/* end while*/

   260

  }/* end if Donnees */

   261

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

   262

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

   263

    status = state1;

   264

    offsetObjdict = d->firstIndex->PDO_TRS;

   265

    lastIndex = d->lastIndex->PDO_TRS;

   266

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

   267

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

   268

   269

      switch( status ){

   270

   271

      case state1:/* check the CobId */

   272

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

   273

         */

   274

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

   275

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

   276

          status = state4;

   277

          break;

   278

        }

   279

        else {

   280

          numPdo++;

   281

          offsetObjdict++;

   282

        }

   283

        status = state1;

   284

        break;

   285

   286

   287

      case state4:/* check transmission type */

   288

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

   289

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

   290

        if ( (*pTransmissionType == TRANS_RTR)) {

   291

          status = state5;

   292

          break;

   293

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

   294

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

   295

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

   296

            /*Data ready, just send*/

   297

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

   298

            return 0;

   299

    	  }else{

   300

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

   301

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

   302

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

   303

            status = state5;

   304

    	  }

   305

          break;

   306

        }else if(

   307

             (*pTransmissionType == TRANS_EVENT_PROFILE) ||

   308

             (*pTransmissionType == TRANS_EVENT_SPECIFIC) ) {

   309

	  /* Zap all timers and inhibit flag */

   310

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

   311

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

   312

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

   313

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

   314

          PDOEventTimerAlarm(d, numPdo);

   315

          return 0;

   316

        }else {

   317

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

   318

            nothing. */

   319

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

   320

          return 0xFF;

   321

        }

   322

   323

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

   324

      {

   325

      	Message pdo;

   326

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

   327

    	{

   328

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

   329

          return 0xFF;

   330

    	}

   331

    	canSend(d->canHandle,&pdo);

   332

        return 0;

   333

      }

   334

      }/* end switch status */

   335

    }/* end while */

   336

  }/* end if Requete */

   337

   338

  return 0;

   339

}

   340

   341

/*!

   342

**

   343

**

   344

** @param NbBits

   345

** @param SrcByteIndex

   346

** @param SrcBitIndex

   347

** @param SrcBigEndian

   348

** @param DestByteIndex

   349

** @param DestBitIndex

   350

** @param DestBigEndian

   351

**/

   352

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

   353

{

   354

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

   355

  /* successively bytes*/

   356

  // boundaries from LSB to MSB.

   357

  while(NbBits > 0)

   358

    {

   359

      /* Bit missalignement between src and dest*/

   360

      INTEGER8 Vect = DestBitIndex - SrcBitIndex;

   361

   362

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

   363

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

   364

   365

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

   366

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

   367

      UNS8 BitsToCopy = BoudaryLimit > NbBits ? NbBits : BoudaryLimit;

   368

   369

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

   370

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

   371

   372

      /* - Filtering src*/

   373

      UNS8 Filtered = Aligned & ~Mask;

   374

   375

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

   376

      *DestByteIndex &= Mask;

   377

   378

      /* Then write.*/

   379

      *DestByteIndex |= Filtered ;

   380

   381

      /*Compute next time cursors for src*/

   382

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

   383

        {

   384

          SrcBitIndex = 0;/* First bit*/

   385

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

   386

        }

   387

   388

   389

      /*Compute next time cursors for dest*/

   390

      if((DestBitIndex += BitsToCopy)>7)

   391

        {

   392

          DestBitIndex = 0;/* First bit*/

   393

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

   394

        }

   395

   396

      /*And decrement counter.*/

   397

      NbBits -= BitsToCopy;

   398

    }

   399

   400

}

   401

/*!

   402

**

   403

**

   404

** @param d

   405

**

   406

** @return

   407

**/

   408

   409

UNS8 sendPDOevent( CO_Data* d)

   410

{

   411

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

   412

  return _sendPDOevent(d, 0);

   413

}

   414

   415

   416

void PDOEventTimerAlarm(CO_Data* d, UNS32 pdoNum)

   417

{

   418

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

   419

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

   420

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

   421

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

   422

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

   423

}

   424

   425

void PDOInhibitTimerAlarm(CO_Data* d, UNS32 pdoNum)

   426

{

   427

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

   428

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

   429

	/* Remove inhibit flag */

   430

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

   431

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

   432

}

   433

   434

/*!

   435

**

   436

**

   437

** @param d

   438

** @param isSyncEvent

   439

**

   440

** @return

   441

**/

   442

   443

UNS8 _sendPDOevent( CO_Data* d, UNS8 isSyncEvent )

   444

{ 

   445

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

   446

  UNS8* pTransmissionType = NULL;  

   447

  UNS8 status = state3;

   448

  UNS16 offsetObjdict = d->firstIndex->PDO_TRS;

   449

  UNS16 offsetObjdictMap = d->firstIndex->PDO_TRS_MAP;

   450

  UNS16 lastIndex = d->lastIndex->PDO_TRS;  

   451

   452

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

   453

  if(offsetObjdict){

   454

   Message pdo = Message_Initializer;

   455

   while( offsetObjdict <= lastIndex) {  

   456

    switch( status ) {

   457

    case state3:

   458

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

   459

          /* check if TPDO is not valid */ 

   460

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

   461

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

   462

	  /*Go next TPDO*/

   463

	  status = state11;

   464

	  break;

   465

	}

   466

      /* get the PDO transmission type */

   467

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

   468

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

   469

   470

      /* check if transmission type is SYNCRONOUS */

   471

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

   472

      if( isSyncEvent && 

   473

      	  (*pTransmissionType >= TRANS_SYNC_MIN) &&

   474

      	  (*pTransmissionType <= TRANS_SYNC_MAX) &&

   475

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

   476

        /*Reset count of SYNC*/	

   477

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

   478

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

   479

 {

   480

 Message msg_init = Message_Initializer;

   481

 pdo = msg_init;

   482

 }	

   483

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

   484

        {

   485

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

   486

	    status = state11;

   487

	    break;

   488

        }

   489

	status = state5;