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

*/

    40

**

    41

** @param d

    44

**

    45

** @return

    46

**/

    95

    96

/*!

    97

**

    98

**

    99

** @param d

   100

** @param cobId

   101

**

   102

** @return

   103

**/

   104

UNS8 sendPDOrequest( CO_Data* d, UNS32 cobId )

   105

{

   106

  UNS32 * pwCobId;

   107

  UNS16          offset;

   108

  UNS16          lastIndex;

   109

  UNS8           err;

   110

   111

  MSG_WAR(0x3930, "sendPDOrequest ",0);

   112

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

   113

     receive */

   114

  /* part dictionary */

   115

  offset = d->firstIndex->PDO_RCV;

   116

  lastIndex = d->lastIndex->PDO_RCV;

   117

  if (offset)

   118

    while (offset <= lastIndex) {

   119

      /* get the CobId*/

   120

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

   121

   122

      if ( *pwCobId  == cobId ) {

   125

      }

   126

      offset++;

   127

    }

   128

  MSG_WAR(0x1931, "sendPDOrequest : COBID not found : ", cobId);

   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;

   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. */

   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 */

   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++;

   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 (after request?) */

   287

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

   290

          status = state5;

   291

          break;

   303

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

   304

            nothing. */

   305

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

   306

          return 0xFF;

   307

        }

   308

   318

        return 0;

   320

      }/* end switch status */

   321

    }/* end while */

   322

  }/* end if Requete */

   323

   324

  return 0;

   325

}

   326

   327

/*!

   328

**

   329

**

   330

** @param NbBits

   331

** @param SrcByteIndex

   332

** @param SrcBitIndex

   333

** @param SrcBigEndian

   334

** @param DestByteIndex

   335

** @param DestBitIndex

   336

** @param DestBigEndian

   337

**/

   338

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

   339

{

   340

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

   341

  /* successively bytes*/

   342

  // boundaries from LSB to MSB.

   343

  while(NbBits > 0)

   344

    {

   345

      /* Bit missalignement between src and dest*/

   346

      INTEGER8 Vect = DestBitIndex - SrcBitIndex;

   347

   348

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

   349

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

   350

   351

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

   352

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

   353

      UNS8 BitsToCopy = BoudaryLimit > NbBits ? NbBits : BoudaryLimit;

   354

   355

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

   356

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

   357

   358

      /* - Filtering src*/

   359

      UNS8 Filtered = Aligned & ~Mask;

   360

   361

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

   362

      *DestByteIndex &= Mask;

   363

   364

      /* Then write.*/

   365

      *DestByteIndex |= Filtered ;

   366

   367

      /*Compute next time cursors for src*/

   368

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

   369

        {

   370

          SrcBitIndex = 0;/* First bit*/

   371

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

   372

        }

   373

   374

   375

      /*Compute next time cursors for dest*/

   376

      if((DestBitIndex += BitsToCopy)>7)

   377

        {

   378

          DestBitIndex = 0;/* First bit*/

   379

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

   380

        }

   381

   382

      /*And decrement counter.*/

   383

      NbBits -= BitsToCopy;

   384

    }

   385

   386

}

   387

/*!

   388

**

   389

**

   390

** @param d

   391

**

   392

** @return

   393

**/

   472

      }