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

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

   129

  return 0xFF;

   130

}

   131

   132

   133

/*!

   134

**

   135

**

   136

** @param d

   137

** @param m

   138

**

   139

** @return

   140

**/

   141

UNS8 proceedPDO(CO_Data* d, Message *m)

   142

{

   143

  UNS8   numPdo;

   144

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

   145

  UNS8 i;

   146

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

   147

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

   148

  /*  void *     pMappedAppObject = NULL;   */

   149

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

   150

     mapping entry */

   151

  UNS32 *    pMappingParameter = NULL;

   152

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

   153

                                         type */

   154

  UNS32 *    pwCobId = NULL;

   155

  UNS8       Size;

   156

  UNS8       dataType;

   157

  UNS8       offset;

   158

  UNS8       status;

   159

  UNS32      objDict;

   160

  UNS16      offsetObjdict;

   161

  UNS16      lastIndex;

   162

   163

  status = state2;

   164

   165

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

   166

  offset = 0x00;

   167

  numPdo = 0;

   168

  numMap = 0;

   169

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

   170

                                     request. */

   171

   172

    offsetObjdict = d->firstIndex->PDO_RCV;

   173

    lastIndex = d->lastIndex->PDO_RCV;

   174

   175

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

   176

    if(offsetObjdict)

   177

      while (offsetObjdict <= lastIndex) {

   178

   179

        switch( status ) {

   180

   181

        case state2:

   182

          /* get CobId of the dictionary correspondant to the received

   183

             PDO */

   184

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

   185

          /* check the CobId coherance */

   186

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

   187

            */

   188

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

   189

            /* The cobId is recognized */

   190

            status = state4;

   191

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

   192

            break;

   193

          }

   194

          else {

   195

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

   196

              dictionnary */

   197

            numPdo++;

   198

            offsetObjdict++;

   199

            status = state2;

   200

            break;

   201

          }

   202

   203

            case state4:/* Get Mapped Objects Number */

   204

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

   205

                 dictionnary. */

   206

               offsetObjdict = d->firstIndex->PDO_RCV_MAP;

   207

             lastIndex = d->lastIndex->PDO_RCV_MAP;

   208

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

   209

             numMap = 0;

   210

             while (numMap < *pMappingCount) {

   211

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

   212

               UNS8 ByteSize;

   213

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

   214

               if (pMappingParameter == NULL) {

   215

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

   216

                 return 0xFF;

   217

               }

   218

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

   219

               /* detail of *pMappingParameter : */

   220

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

   221

                 contains the subindex, */

   222

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

   223

                 variable. */

   224

   225

               Size = (UNS8)(*pMappingParameter);

   226

   227

               /* copy bit per bit in little endian */

   228

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

   229

   230

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

   231

                                                   2, ... */

   232

   233

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

   234

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

   235

                                 tmp, &ByteSize, 0 );

   236

   237

               if(objDict != OD_SUCCESSFUL) {

   238

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

   239

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

   240

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

   241

                 return 0xFF;

   242

               }

   243

   244

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

   245

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

   246

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

   247

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

   248

               offset += Size;

   249

               numMap++;

   250

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

   251

             } /* end loop while on mapped variables */

   252

   253

             offset=0x00;

   254

             numMap = 0;

   255

             return 0;

   256

   257

        }/* end switch status*/

   258

      }/* end while*/

   259

  }/* end if Donnees */

   260

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

   261

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

   262

    status = state1;

   263

    offsetObjdict = d->firstIndex->PDO_TRS;

   264

    lastIndex = d->lastIndex->PDO_TRS;

   265

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

   266

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

   267

   268

      switch( status ){

   269

   270

      case state1:/* check the CobId */

   271

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

   272

         */

   273

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

   274

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

   275

          status = state4;

   276

          break;

   277

        }

   278

        else {

   279

          numPdo++;

   280

          offsetObjdict++;

   281

        }

   282

        status = state1;

   283

        break;

   284

   285

   286

      case state4:/* check transmission type */

   287

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

   288

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

   289

        if ( (*pTransmissionType == TRANS_RTR)) {

   290

          status = state5;

   291

          break;

   292

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

   293

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

   294

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

   295

            /*Data ready, just send*/

   296

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

   297

            return 0;

   298

    	  }else{

   299

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

   300

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

   301

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

   302

            status = state5;

   303

    	  }

   304

          break;

   305

        }else if(

   306

             (*pTransmissionType == TRANS_EVENT_PROFILE) ||

   307

             (*pTransmissionType == TRANS_EVENT_SPECIFIC) ) {

   308

	  /* Zap all timers and inhibit flag */

   309

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

   310

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

   311

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

   312

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

   313

          PDOEventTimerAlarm(d, numPdo);

   314

          return 0;

   315

        }else {

   316

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

   317

            nothing. */

   318

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

   319

          return 0xFF;

   320

        }

   321

   322

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

   323

      {

   324

      	Message pdo;

   325

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

   326

    	{

   327

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

   328

          return 0xFF;

   329

    	}

   330

    	canSend(d->canHandle,&pdo);

   331

        return 0;

   332

      }

   333

      }/* end switch status */

   334

    }/* end while */

   335

  }/* end if Requete */

   336

   337

  return 0;

   338

}

   339

   340

/*!

   341

**

   342

**

   343

** @param NbBits

   344

** @param SrcByteIndex

   345

** @param SrcBitIndex

   346

** @param SrcBigEndian

   347

** @param DestByteIndex

   348

** @param DestBitIndex

   349

** @param DestBigEndian

   350

**/

   351

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

   352

{

   353

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

   354

  /* successively bytes*/

   355

  // boundaries from LSB to MSB.

   356

  while(NbBits > 0)

   357

    {

   358

      /* Bit missalignement between src and dest*/

   359

      INTEGER8 Vect = DestBitIndex - SrcBitIndex;

   360

   361

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

   362

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

   363

   364

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

   365

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

   366

      UNS8 BitsToCopy = BoudaryLimit > NbBits ? NbBits : BoudaryLimit;

   367

   368

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

   369

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

   370

   371

      /* - Filtering src*/

   372

      UNS8 Filtered = Aligned & ~Mask;

   373

   374

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

   375

      *DestByteIndex &= Mask;

   376

   377

      /* Then write.*/

   378

      *DestByteIndex |= Filtered ;

   379

   380

      /*Compute next time cursors for src*/

   381

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

   382

        {

   383

          SrcBitIndex = 0;/* First bit*/

   384

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

   385

        }

   386

   387

   388

      /*Compute next time cursors for dest*/

   389

      if((DestBitIndex += BitsToCopy)>7)

   390

        {

   391

          DestBitIndex = 0;/* First bit*/

   392

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

   393

        }

   394

   395

      /*And decrement counter.*/

   396

      NbBits -= BitsToCopy;

   397

    }

   398

   399

}

   400

/*!

   401

**

   402

**

   403

** @param d

   404

**

   405

** @return

   406

**/

   407

   408

UNS8 sendPDOevent( CO_Data* d)

   409

{

   410

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

   411

  return _sendPDOevent(d, 0);

   412

}

   413

   414

   415

void PDOEventTimerAlarm(CO_Data* d, UNS32 pdoNum)

   416

{

   417

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

   418

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

   419

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

   420

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

   421

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

   422

}

   423

   424

void PDOInhibitTimerAlarm(CO_Data* d, UNS32 pdoNum)

   425

{

   426

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

   427

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

   428

	/* Remove inhibit flag */

   429

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

   430

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

   431

}

   432

   433

/*!

   434

**

   435

**

   436

** @param d

   437

** @param isSyncEvent

   438

**

   439

** @return

   440

**/

   441

   442

UNS8 _sendPDOevent( CO_Data* d, UNS8 isSyncEvent )

   443

{ 

   444

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

   445

  UNS8* pTransmissionType = NULL;  

   446

  UNS8 status = state3;

   447

  UNS16 offsetObjdict = d->firstIndex->PDO_TRS;

   448

  UNS16 offsetObjdictMap = d->firstIndex->PDO_TRS_MAP;

   449

  UNS16 lastIndex = d->lastIndex->PDO_TRS;  

   450

   451

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

   452

  if(offsetObjdict){

   453

   Message pdo = Message_Initializer;

   454

   while( offsetObjdict <= lastIndex) {  

   455

    switch( status ) {

   456

    case state3:

   457

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

   458

          /* check if TPDO is not valid */ 

   459

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

   460

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

   461

	  /*Go next TPDO*/

   462

	  status = state11;

   463

	  break;

   464

	}

   465

      /* get the PDO transmission type */

   466

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

   467

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

   468

   469

      /* check if transmission type is SYNCRONOUS */

   470

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

   471

      if( isSyncEvent && 

   472

      	  (*pTransmissionType >= TRANS_SYNC_MIN) &&

   473

      	  (*pTransmissionType <= TRANS_SYNC_MAX) &&

   474

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

   475

        /*Reset count of SYNC*/	

   476

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

   477

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

   478

	pdo = (Message)Message_Initializer;

   479

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

   480

        {

   481

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

   482

	    status = state11;

   483

	    break;

   484

        }

   485

	status = state5;

   486

      /* If transmission RTR, with data sampled on SYNC */

   487

      }else if( isSyncEvent && 

   488

      	  (*pTransmissionType == TRANS_RTR_SYNC)) {

   489

          if(buildPDO(d, pdoNum, &d->PDO_status[pdoNum].last_message))