src/pdo.c

   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 #include "sysdep.h"
  28
  29 /*!
  30 ** @file   pdo.c
  31 ** @author Edouard TISSERANT and Francis DUPIN
  32 ** @date   Tue Jun  5 09:32:32 2007
  33 **
  34 ** @brief
  35 **
  36 **
  37 */
  38
  39 /*!
  40 **
  41 **
  42 ** @param d
  43 ** @param TPDO_com TPDO communication parameters OD entry
  44 ** @param TPDO_map TPDO mapping parameters OD entry
  45 **
  46 ** @return
  47 **/
  48
  49 UNS8
  50 buildPDO (CO_Data * d, UNS8 numPdo, Message * pdo)
  51 {
  52   const indextable *TPDO_com = d->objdict + d->firstIndex->PDO_TRS + numPdo;
  53   const indextable *TPDO_map =
  54     d->objdict + d->firstIndex->PDO_TRS_MAP + numPdo;
  55
  56   UNS8 prp_j = 0x00;
  57   UNS8 offset = 0x00;
  58   const UNS8 *pMappingCount = (UNS8 *) TPDO_map->pSubindex[0].pObject;
  59
  60   pdo->cob_id = UNS16_LE(*(UNS32*)TPDO_com->pSubindex[1].pObject & 0x7FF);
  61   pdo->rtr = NOT_A_REQUEST;
  62
  63   MSG_WAR (0x3009, "  PDO CobId is : ",
  64            *(UNS32 *) TPDO_com->pSubindex[1].pObject);
  65   MSG_WAR (0x300D, "  Number of objects mapped : ", *pMappingCount);
  66
  67   do
  68     {
  69       UNS8 dataType;            /* Unused */
  70       UNS8 tmp[] = { 0, 0, 0, 0, 0, 0, 0, 0 };  /* temporary space to hold bits */
  71
  72       /* pointer fo the var which holds the mapping parameter of an mapping entry  */
  73       UNS32 *pMappingParameter =
  74         (UNS32 *) TPDO_map->pSubindex[prp_j + 1].pObject;
  75       UNS16 index = (UNS16) ((*pMappingParameter) >> 16);
  76       UNS8 Size = (UNS8) (*pMappingParameter & (UNS32) 0x000000FF);     /* Size in bits */
  77
  78       /* get variable only if Size != 0 and Size is lower than remaining bits in the PDO */
  79       if (Size && ((offset + Size) <= 64))
  80         {
  81           UNS8 ByteSize = 1 + ((Size - 1) >> 3);        /*1->8 => 1 ; 9->16 => 2, ... */
  82           UNS8 subIndex =
  83             (UNS8) (((*pMappingParameter) >> (UNS8) 8) & (UNS32) 0x000000FF);
  84
  85           MSG_WAR (0x300F, "  got mapping parameter : ", *pMappingParameter);
  86           MSG_WAR (0x3050, "    at index : ", TPDO_map->index);
  87           MSG_WAR (0x3051, "    sub-index : ", prp_j + 1);
  88
  89           if (getODentry (d, index, subIndex, tmp, &ByteSize, &dataType, 0) !=
  90               OD_SUCCESSFUL)
  91             {
  92               MSG_ERR (0x1013,
  93                        " Couldn't find mapped variable at index-subindex-size : ",
  94                        (UNS16) (*pMappingParameter));
  95               return 0xFF;
  96             }
  97           /* copy bit per bit in little endian */
  98           CopyBits (Size, ((UNS8 *) tmp), 0, 0,
  99                     (UNS8 *) & pdo->data[offset >> 3], offset % 8, 0);
 100
 101           offset += Size;
 102         }
 103       prp_j++;
 104     }
 105   while (prp_j < *pMappingCount);
 106
 107   pdo->len = 1 + ((offset - 1) >> 3);
 108
 109   MSG_WAR (0x3015, "  End scan mapped variable", 0);
 110
 111   return 0;
 112 }
 113
 114 /*!
 115 **
 116 **
 117 ** @param d
 118 ** @param cobId
 119 **
 120 ** @return
 121 **/
 122 UNS8
 123 sendPDOrequest (CO_Data * d, UNS16 RPDOIndex)
 124 {
 125   UNS16 *pwCobId;
 126   UNS16 offset = d->firstIndex->PDO_RCV;
 127   UNS16 lastIndex = d->lastIndex->PDO_RCV;
 128
 129   /* Sending the request only if the cobid have been found on the PDO
 130      receive */
 131   /* part dictionary */
 132
 133   MSG_WAR (0x3930, "sendPDOrequest RPDO Index : ", RPDOIndex);
 134
 135   if (offset && RPDOIndex >= 0x1400)
 136     {
 137       offset += RPDOIndex - 0x1400;
 138       if (offset <= lastIndex)
 139         {
 140           /* get the CobId */
 141           pwCobId = (UNS16 *) d->objdict[offset].pSubindex[1].pObject;
 142
 143           MSG_WAR (0x3930, "sendPDOrequest cobId is : ", *pwCobId);
 144           {
 145             Message pdo;
 146             pdo.cob_id = *pwCobId;
 147             pdo.rtr = REQUEST;
 148             pdo.len = 0;
 149             return canSend (d->canHandle, &pdo);
 150           }
 151         }
 152     }
 153   MSG_ERR (0x1931, "sendPDOrequest : RPDO Index not found : ", RPDOIndex);
 154   return 0xFF;
 155 }
 156
 157
 158 /*!
 159 **
 160 **
 161 ** @param d
 162 ** @param m
 163 **
 164 ** @return
 165 **/
 166 UNS8
 167 proceedPDO (CO_Data * d, Message * m)
 168 {
 169   UNS8 numPdo;
 170   UNS8 numMap;                  /* Number of the mapped varable */
 171   UNS8 *pMappingCount = NULL;   /* count of mapped objects... */
 172   /* pointer to the var which is mapped to a pdo... */
 173   /*  void *     pMappedAppObject = NULL;   */
 174   /* pointer fo the var which holds the mapping parameter of an
 175      mapping entry */
 176   UNS32 *pMappingParameter = NULL;
 177   UNS8 *pTransmissionType = NULL;       /* pointer to the transmission
 178                                            type */
 179   UNS16 *pwCobId = NULL;
 180   UNS8 Size;
 181   UNS8 offset;
 182   UNS8 status;
 183   UNS32 objDict;
 184   UNS16 offsetObjdict;
 185   UNS16 lastIndex;
 186
 187   status = state2;
 188
 189   MSG_WAR (0x3935, "proceedPDO, cobID : ", ((*m).cob_id & UNS16_LE (0x7ff)));
 190   offset = 0x00;
 191   numPdo = 0;
 192   numMap = 0;
 193   if ((*m).rtr == NOT_A_REQUEST)
 194     {                           /* The PDO received is not a
 195                                    request. */
 196
 197       offsetObjdict = d->firstIndex->PDO_RCV;
 198       lastIndex = d->lastIndex->PDO_RCV;
 199
 200       /* study of all the PDO stored in the dictionary */
 201       if (offsetObjdict)
 202         while (offsetObjdict <= lastIndex)
 203           {
 204
 205             switch (status)
 206               {
 207
 208               case state2:
 209                 /* get CobId of the dictionary correspondant to the received
 210                    PDO */
 211                 pwCobId =
 212                   (UNS16 *) d->objdict[offsetObjdict].pSubindex[1].pObject;
 213                 /* check the CobId coherance */
 214                 /*pwCobId is the cobId read in the dictionary at the state 3
 215                  */
 216                 if (*pwCobId == (*m).cob_id)
 217                   {
 218                     /* The cobId is recognized */
 219                     status = state4;
 220                     MSG_WAR (0x3936, "cobId found at index ",
 221                              0x1400 + numPdo);
 222                     break;
 223                   }
 224                 else
 225                   {
 226                     /* cobId received does not match with those write in the
 227                        dictionnary */
 228                     numPdo++;
 229                     offsetObjdict++;
 230                     status = state2;
 231                     break;
 232                   }
 233
 234               case state4:     /* Get Mapped Objects Number */
 235                 /* The cobId of the message received has been found in the
 236                    dictionnary. */
 237                 offsetObjdict = d->firstIndex->PDO_RCV_MAP;
 238                 lastIndex = d->lastIndex->PDO_RCV_MAP;
 239                 pMappingCount =
 240                   (UNS8 *) (d->objdict + offsetObjdict +
 241                             numPdo)->pSubindex[0].pObject;
 242                 numMap = 0;
 243                 while (numMap < *pMappingCount)
 244                   {
 245                     UNS8 tmp[] = { 0, 0, 0, 0, 0, 0, 0, 0 };
 246                     UNS8 ByteSize;
 247                     pMappingParameter =
 248                       (UNS32 *) (d->objdict + offsetObjdict +
 249                                  numPdo)->pSubindex[numMap + 1].pObject;
 250                     if (pMappingParameter == NULL)
 251                       {
 252                         MSG_ERR (0x1937, "Couldn't get mapping parameter : ",
 253                                  numMap + 1);
 254                         return 0xFF;
 255                       }
 256                     /* Get the addresse of the mapped variable. */
 257                     /* detail of *pMappingParameter : */
 258                     /* The 16 hight bits contains the index, the medium 8 bits
 259                        contains the subindex, */
 260                     /* and the lower 8 bits contains the size of the mapped
 261                        variable. */
 262
 263                     Size = (UNS8) (*pMappingParameter & (UNS32) 0x000000FF);
 264
 265                     /* set variable only if Size != 0 and Size is lower than remaining bits in the PDO */
 266                     if (Size && ((offset + Size) <= (m->len << 3)))
 267                       {
 268                         /* copy bit per bit in little endian */
 269                         CopyBits (Size, (UNS8 *) & m->data[offset >> 3],
 270                                   offset % 8, 0, ((UNS8 *) tmp), 0, 0);
 271
 272                         ByteSize = 1 + ((Size - 1) >> 3);       /*1->8 => 1 ; 9->16 =>
 273                                                                    2, ... */
 274
 275                         objDict =
 276                           setODentry (d, (UNS16) ((*pMappingParameter) >> 16),
 277                                       (UNS8) (((*pMappingParameter) >> 8) &
 278                                               0xFF), tmp, &ByteSize, 0);
 279
 280                         if (objDict != OD_SUCCESSFUL)
 281                           {
 282                             MSG_ERR (0x1938,
 283                                      "error accessing to the mapped var : ",
 284                                      numMap + 1);
 285                             MSG_WAR (0x2939, "         Mapped at index : ",
 286                                      (*pMappingParameter) >> 16);
 287                             MSG_WAR (0x2940, "                subindex : ",
 288                                      ((*pMappingParameter) >> 8) & 0xFF);
 289                             return 0xFF;
 290                           }
 291
 292                         MSG_WAR (0x3942,
 293                                  "Variable updated with value received by PDO cobid : ",
 294                                  m->cob_id);
 295                         MSG_WAR (0x3943, "         Mapped at index : ",
 296                                  (*pMappingParameter) >> 16);
 297                         MSG_WAR (0x3944, "                subindex : ",
 298                                  ((*pMappingParameter) >> 8) & 0xFF);
 299                         /* MSG_WAR(0x3945, "                data : ",*((UNS32*)pMappedAppObject)); */
 300                         offset += Size;
 301                       }
 302                     numMap++;
 303                   }             /* end loop while on mapped variables */
 304
 305                 offset = 0x00;
 306                 numMap = 0;
 307                 return 0;
 308
 309               }                 /* end switch status */
 310           }                     /* end while */
 311     }                           /* end if Donnees */
 312   else if ((*m).rtr == REQUEST)
 313     {
 314       MSG_WAR (0x3946, "Receive a PDO request cobId : ", m->cob_id);
 315       status = state1;
 316       offsetObjdict = d->firstIndex->PDO_TRS;
 317       lastIndex = d->lastIndex->PDO_TRS;
 318       if (offsetObjdict)
 319         while (offsetObjdict <= lastIndex)
 320           {
 321             /* study of all PDO stored in the objects dictionary */
 322
 323             switch (status)
 324               {
 325
 326               case state1:     /* check the CobId */
 327                 /* get CobId of the dictionary which match to the received PDO
 328                  */
 329                 pwCobId =
 330                   (UNS16 *) (d->objdict +
 331                              offsetObjdict)->pSubindex[1].pObject;
 332                 if (*pwCobId == (*m).cob_id)
 333                   {
 334                     status = state4;
 335                     break;
 336                   }
 337                 else
 338                   {
 339                     numPdo++;
 340                     offsetObjdict++;
 341                   }
 342                 status = state1;
 343                 break;
 344
 345
 346               case state4:     /* check transmission type */
 347                 pTransmissionType =
 348                   (UNS8 *) d->objdict[offsetObjdict].pSubindex[2].pObject;
 349                 /* If PDO is to be sampled and send on RTR, do it */
 350                 if ((*pTransmissionType == TRANS_RTR))
 351                   {
 352                     status = state5;
 353                     break;
 354                     /* RTR_SYNC mean data is prepared at SYNC, and transmitted on RTR */
 355                   }
 356                 else if ((*pTransmissionType == TRANS_RTR_SYNC))
 357                   {
 358                     if (d->PDO_status[numPdo].
 359                         transmit_type_parameter & PDO_RTR_SYNC_READY)
 360                       {
 361                         /*Data ready, just send */
 362                         canSend (d->canHandle,
 363                                  &d->PDO_status[numPdo].last_message);
 364                         return 0;
 365                       }
 366                     else
 367                       {
 368                         /* if SYNC did never occur, force emission with current data */
 369                         /* DS301 do not tell what to do in such a case... */
 370                         MSG_ERR (0x1947,
 371                                  "Not ready RTR_SYNC TPDO send current data : ",
 372                                  m->cob_id);
 373                         status = state5;
 374                       }
 375                     break;
 376                   }
 377                 else if ((*pTransmissionType == TRANS_EVENT_PROFILE) ||
 378                          (*pTransmissionType == TRANS_EVENT_SPECIFIC))
 379                   {
 380                     /* Zap all timers and inhibit flag */
 381                     d->PDO_status[numPdo].event_timer =
 382                       DelAlarm (d->PDO_status[numPdo].event_timer);
 383                     d->PDO_status[numPdo].inhibit_timer =
 384                       DelAlarm (d->PDO_status[numPdo].inhibit_timer);
 385                     d->PDO_status[numPdo].transmit_type_parameter &=
 386                       ~PDO_INHIBITED;
 387                     /* Call  PDOEventTimerAlarm for this TPDO, this will trigger emission et reset timers */
 388                     PDOEventTimerAlarm (d, numPdo);
 389                     return 0;
 390                   }
 391                 else
 392                   {
 393                     /* The requested PDO is not to send on request. So, does
 394                        nothing. */
 395                     MSG_WAR (0x2947, "PDO is not to send on request : ",
 396                              m->cob_id);
 397                     return 0xFF;
 398                   }
 399
 400               case state5:     /* build and send requested PDO */
 401                 {
 402                   Message pdo;
 403                   if (buildPDO (d, numPdo, &pdo))
 404                     {
 405                       MSG_ERR (0x1948, " Couldn't build TPDO n°", numPdo);
 406                       return 0xFF;
 407                     }
 408                   canSend (d->canHandle, &pdo);
 409                   return 0;
 410                 }
 411               }                 /* end switch status */
 412           }                     /* end while */
 413     }                           /* end if Requete */
 414
 415   return 0;
 416 }
 417
 418 /*!
 419 **
 420 **
 421 ** @param NbBits
 422 ** @param SrcByteIndex
 423 ** @param SrcBitIndex
 424 ** @param SrcBigEndian
 425 ** @param DestByteIndex
 426 ** @param DestBitIndex
 427 ** @param DestBigEndian
 428 **/
 429 void
 430 CopyBits (UNS8 NbBits, UNS8 * SrcByteIndex, UNS8 SrcBitIndex,
 431           UNS8 SrcBigEndian, UNS8 * DestByteIndex, UNS8 DestBitIndex,
 432           UNS8 DestBigEndian)
 433 {
 434   /* This loop copy as many bits that it can each time, crossing */
 435   /* successively bytes */
 436   // boundaries from LSB to MSB.
 437   while (NbBits > 0)
 438     {
 439       /* Bit missalignement between src and dest */
 440       INTEGER8 Vect = DestBitIndex - SrcBitIndex;
 441
 442       /* We can now get src and align it to dest */
 443       UNS8 Aligned =
 444         Vect > 0 ? *SrcByteIndex << Vect : *SrcByteIndex >> -Vect;
 445
 446       /* Compute the nb of bit we will be able to copy */
 447       UNS8 BoudaryLimit = (Vect > 0 ? 8 - DestBitIndex : 8 - SrcBitIndex);
 448       UNS8 BitsToCopy = BoudaryLimit > NbBits ? NbBits : BoudaryLimit;
 449
 450       /* Create a mask that will serve in: */
 451       UNS8 Mask =
 452         ((0xff << (DestBitIndex + BitsToCopy)) |
 453          (0xff >> (8 - DestBitIndex)));
 454
 455       /* - Filtering src */
 456       UNS8 Filtered = Aligned & ~Mask;
 457
 458       /* - and erase bits where we write, preserve where we don't */
 459       *DestByteIndex &= Mask;
 460
 461       /* Then write. */
 462       *DestByteIndex |= Filtered;
 463
 464       /*Compute next time cursors for src */
 465       if ((SrcBitIndex += BitsToCopy) > 7)      /* cross boundary ? */
 466         {
 467           SrcBitIndex = 0;      /* First bit */
 468           SrcByteIndex += (SrcBigEndian ? -1 : 1);      /* Next byte */
 469         }
 470
 471
 472       /*Compute next time cursors for dest */
 473       if ((DestBitIndex += BitsToCopy) > 7)
 474         {
 475           DestBitIndex = 0;     /* First bit */
 476           DestByteIndex += (DestBigEndian ? -1 : 1);    /* Next byte */
 477         }
 478
 479       /*And decrement counter. */
 480       NbBits -= BitsToCopy;
 481     }
 482
 483 }
 484
 485 /*!
 486 **
 487 **
 488 ** @param d
 489 **
 490 ** @return
 491 **/
 492
 493 UNS8
 494 sendPDOevent (CO_Data * d)
 495 {
 496   /* Calls _sendPDOevent specifying it is not a sync event */
 497   return _sendPDOevent (d, 0);
 498 }
 499
 500
 501 void
 502 PDOEventTimerAlarm (CO_Data * d, UNS32 pdoNum)
 503 {
 504   /* This is needed to avoid deletion of re-attribuated timer */
 505   d->PDO_status[pdoNum].event_timer = TIMER_NONE;
 506   /* force emission of PDO by artificially changing last emitted */
 507   d->PDO_status[pdoNum].last_message.cob_id = 0;
 508   _sendPDOevent (d, 0);         /* not a Sync Event */
 509 }
 510
 511 void
 512 PDOInhibitTimerAlarm (CO_Data * d, UNS32 pdoNum)
 513 {
 514   /* This is needed to avoid deletion of re-attribuated timer */
 515   d->PDO_status[pdoNum].inhibit_timer = TIMER_NONE;
 516   /* Remove inhibit flag */
 517   d->PDO_status[pdoNum].transmit_type_parameter &= ~PDO_INHIBITED;
 518   _sendPDOevent (d, 0);         /* not a Sync Event */
 519 }
 520
 521 /*!
 522 **
 523 **
 524 ** @param d
 525 ** @param isSyncEvent
 526 **
 527 ** @return
 528 **/
 529
 530 UNS8
 531 _sendPDOevent (CO_Data * d, UNS8 isSyncEvent)
 532 {
 533   UNS8 pdoNum = 0x00;           /* number of the actual processed pdo-nr. */
 534   UNS8 *pTransmissionType = NULL;
 535   UNS8 status = state3;
 536   UNS16 offsetObjdict = d->firstIndex->PDO_TRS;
 537   UNS16 offsetObjdictMap = d->firstIndex->PDO_TRS_MAP;
 538   UNS16 lastIndex = d->lastIndex->PDO_TRS;
 539
 540   /* study all PDO stored in the objects dictionary */
 541   if (offsetObjdict)
 542     {
 543       Message pdo = Message_Initializer;
 544       while (offsetObjdict <= lastIndex)
 545         {
 546           switch (status)
 547             {
 548             case state3:
 549               if (              /*d->objdict[offsetObjdict].bSubCount < 5 || not necessary with objdictedit (always 5) */
 550                    /* check if TPDO is not valid */
 551                    *(UNS32 *) d->objdict[offsetObjdict].pSubindex[1].
 552                    pObject & 0x80000000)
 553                 {
 554                   MSG_WAR (0x3960, "Not a valid PDO ", 0x1800 + pdoNum);
 555                   /*Go next TPDO */
 556                   status = state11;
 557                   break;
 558                 }
 559               /* get the PDO transmission type */
 560               pTransmissionType =
 561                 (UNS8 *) d->objdict[offsetObjdict].pSubindex[2].pObject;
 562               MSG_WAR (0x3962, "Reading PDO at index : ", 0x1800 + pdoNum);
 563
 564               /* check if transmission type is SYNCRONOUS */
 565               /* The message is transmited every n SYNC with n=TransmissionType */
 566               if (isSyncEvent &&
 567                   (*pTransmissionType >= TRANS_SYNC_MIN) &&
 568                   (*pTransmissionType <= TRANS_SYNC_MAX) &&
 569                   (++d->PDO_status[pdoNum].transmit_type_parameter ==
 570                    *pTransmissionType))
 571                 {
 572                   /*Reset count of SYNC */
 573                   d->PDO_status[pdoNum].transmit_type_parameter = 0;
 574                   MSG_WAR (0x3964, "  PDO is on SYNCHRO. Trans type : ",
 575                            *pTransmissionType);
 576                   {
 577                     Message msg_init = Message_Initializer;
 578                     pdo = msg_init;
 579                   }
 580                   if (buildPDO (d, pdoNum, &pdo))
 581                     {
 582                       MSG_ERR (0x1906, " Couldn't build TPDO number : ",
 583                                pdoNum);
 584                       status = state11;
 585                       break;
 586                     }
 587                   status = state5;
 588                   /* If transmission RTR, with data sampled on SYNC */
 589                 }
 590               else if (isSyncEvent && (*pTransmissionType == TRANS_RTR_SYNC))
 591                 {
 592                   if (buildPDO
 593                       (d, pdoNum, &d->PDO_status[pdoNum].last_message))
 594                     {
 595                       MSG_ERR (0x1966, " Couldn't build TPDO number : ",
 596                                pdoNum);
 597                       d->PDO_status[pdoNum].transmit_type_parameter &=
 598                         ~PDO_RTR_SYNC_READY;
 599                     }
 600                   else
 601                     {
 602                       d->PDO_status[pdoNum].transmit_type_parameter |=
 603                         PDO_RTR_SYNC_READY;
 604                     }
 605                   status = state11;
 606                   break;
 607                   /* If transmission on Event and not inhibited, check for changes */
 608                 }
 609               else
 610                 if ((isSyncEvent
 611                      && (*pTransmissionType == TRANS_SYNC_ACYCLIC))
 612                     ||
 613                     ((*pTransmissionType == TRANS_EVENT_PROFILE
 614                       || *pTransmissionType == TRANS_EVENT_SPECIFIC)
 615                      && !(d->PDO_status[pdoNum].
 616                           transmit_type_parameter & PDO_INHIBITED)))
 617                 {
 618                   MSG_WAR (0x3968, "  PDO is on EVENT. Trans type : ",
 619                            *pTransmissionType);
 620                   {
 621                     Message msg_init = Message_Initializer;
 622                     pdo = msg_init;
 623                   }
 624                   if (buildPDO (d, pdoNum, &pdo))
 625                     {
 626                       MSG_ERR (0x3907, " Couldn't build TPDO number : ",
 627                                pdoNum);
 628                       status = state11;
 629                       break;
 630                     }
 631
 632                   /*Compare new and old PDO */
 633                   if (d->PDO_status[pdoNum].last_message.cob_id == pdo.cob_id
 634                       && d->PDO_status[pdoNum].last_message.len == pdo.len &&
 635 #ifdef UNS64
 636                       *(UNS64 *) (&d->PDO_status[pdoNum].last_message.
 637                                   data[0]) == *(UNS64 *) (&pdo.data[0])
 638 #else /* don't ALLOW_64BIT_OPS */
 639                       *(UNS32 *) (&d->PDO_status[pdoNum].last_message.
 640                                   data[0]) == *(UNS32 *) (&pdo.data[0])
 641                       && *(UNS32 *) (&d->PDO_status[pdoNum].last_message.
 642                                      data[4]) == *(UNS32 *) (&pdo.data[4])
 643 #endif
 644                     )
 645                     {
 646                       /* No changes -> go to next pdo */
 647                       status = state11;
 648                     }
 649                   else
 650                     {
 651
 652                       UNS16 EventTimerDuration;
 653                       UNS16 InhibitTimerDuration;
 654
 655                       MSG_WAR (0x306A, "Changes TPDO number : ", pdoNum);
 656                       /* Changes detected -> transmit message */
 657                       EventTimerDuration =
 658                         *(UNS16 *) d->objdict[offsetObjdict].pSubindex[5].
 659                         pObject;
 660                       InhibitTimerDuration =
 661                         *(UNS16 *) d->objdict[offsetObjdict].pSubindex[3].
 662                         pObject;
 663
 664                       status = state5;
 665
 666                       /* Start both event_timer and inhibit_timer */
 667                       if (EventTimerDuration)
 668                         {
 669                           DelAlarm (d->PDO_status[pdoNum].event_timer);
 670                           d->PDO_status[pdoNum].event_timer =
 671                             SetAlarm (d, pdoNum, &PDOEventTimerAlarm,
 672                                       MS_TO_TIMEVAL (EventTimerDuration), 0);
 673                         }
 674
 675                       if (InhibitTimerDuration)
 676                         {
 677                           DelAlarm (d->PDO_status[pdoNum].inhibit_timer);
 678                           d->PDO_status[pdoNum].inhibit_timer =
 679                             SetAlarm (d, pdoNum, &PDOInhibitTimerAlarm,
 680                                       US_TO_TIMEVAL (InhibitTimerDuration *
 681                                                      100), 0);
 682                           /* and inhibit TPDO */
 683                           d->PDO_status[pdoNum].transmit_type_parameter |=
 684                             PDO_INHIBITED;
 685                         }
 686
 687                     }
 688                 }
 689               else
 690                 {
 691                   MSG_WAR (0x306C,
 692                            "  PDO is not on EVENT or synchro or not at this SYNC. Trans type : ",
 693                            *pTransmissionType);
 694                   status = state11;
 695                 }
 696               break;
 697             case state5:       /*Send the pdo */
 698               /*store_as_last_message */
 699               d->PDO_status[pdoNum].last_message = pdo;
 700               MSG_WAR (0x396D, "sendPDO cobId :", pdo.cob_id);
 701               MSG_WAR (0x396E, "     Nb octets  : ", pdo.len);
 702
 703               canSend (d->canHandle, &pdo);
 704               status = state11;
 705               break;
 706             case state11:      /*Go to next TPDO */
 707               pdoNum++;
 708               offsetObjdict++;
 709               offsetObjdictMap++;
 710               MSG_WAR (0x3970, "next pdo index : ", pdoNum);
 711               status = state3;
 712               break;
 713
 714             default:
 715               MSG_ERR (0x1972, "Unknown state has been reached : %d", status);
 716               return 0xFF;
 717             }                   /* end switch case */
 718
 719         }                       /* end while */
 720     }
 721   return 0;
 722 }
 723
 724 /*!
 725 **
 726 **
 727 ** @param d
 728 ** @param OD_entry
 729 ** @param bSubindex
 730 ** @return always 0
 731 **/
 732
 733 UNS32
 734 TPDO_Communication_Parameter_Callback (CO_Data * d,
 735                                        const indextable * OD_entry,
 736                                        UNS8 bSubindex)
 737 {
 738   /* If PDO are actives */
 739   if (d->CurrentCommunicationState.csPDO)
 740     switch (bSubindex)
 741       {
 742       case 2:                  /* Changed transmition type */
 743       case 3:                  /* Changed inhibit time */
 744       case 5:                  /* Changed event time */
 745         {
 746           const indextable *TPDO_com = d->objdict + d->firstIndex->PDO_TRS;
 747           UNS8 numPdo = OD_entry - TPDO_com;    /* number of the actual processed pdo-nr. */
 748
 749           /* Zap all timers and inhibit flag */
 750           d->PDO_status[numPdo].event_timer =
 751             DelAlarm (d->PDO_status[numPdo].event_timer);
 752           d->PDO_status[numPdo].inhibit_timer =
 753             DelAlarm (d->PDO_status[numPdo].inhibit_timer);
 754           d->PDO_status[numPdo].transmit_type_parameter = 0;
 755           /* Call  PDOEventTimerAlarm for this TPDO, this will trigger emission et reset timers */
 756           PDOEventTimerAlarm (d, numPdo);
 757           return 0;
 758         }
 759
 760       default:                 /* other subindex are ignored */
 761         break;
 762       }
 763   return 0;
 764 }
 765
 766 void
 767 PDOInit (CO_Data * d)
 768 {
 769   /* For each TPDO mapping parameters */
 770   UNS16 pdoIndex = 0x1800;      /* OD index of TDPO */
 771
 772   UNS16 offsetObjdict = d->firstIndex->PDO_TRS;
 773   UNS16 lastIndex = d->lastIndex->PDO_TRS;
 774   if (offsetObjdict)
 775     while (offsetObjdict <= lastIndex)
 776       {
 777         /* Assign callbacks to sensible TPDO mapping subindexes */
 778         UNS32 errorCode;
 779         ODCallback_t *CallbackList;
 780         /* Find callback list */
 781         scanIndexOD (d, pdoIndex, &errorCode, &CallbackList);
 782         if (errorCode == OD_SUCCESSFUL && CallbackList)
 783           {
 784             /*Assign callbacks to corresponding subindex */
 785             /* Transmission type */
 786             CallbackList[2] = &TPDO_Communication_Parameter_Callback;
 787             /* Inhibit time */
 788             CallbackList[3] = &TPDO_Communication_Parameter_Callback;
 789             /* Event timer */
 790             CallbackList[5] = &TPDO_Communication_Parameter_Callback;
 791           }
 792         pdoIndex++;
 793         offsetObjdict++;
 794       }
 795
 796   /* Trigger a non-sync event */
 797   _sendPDOevent (d, 0);
 798 }
 799
 800 void
 801 PDOStop (CO_Data * d)
 802 {
 803   /* For each TPDO mapping parameters */
 804   UNS8 pdoNum = 0x00;           /* number of the actual processed pdo-nr. */
 805   UNS16 offsetObjdict = d->firstIndex->PDO_TRS;
 806   UNS16 lastIndex = d->lastIndex->PDO_TRS;
 807   if (offsetObjdict)
 808     while (offsetObjdict <= lastIndex)
 809       {
 810         /* Delete TPDO timers */
 811         d->PDO_status[pdoNum].event_timer =
 812           DelAlarm (d->PDO_status[pdoNum].event_timer);
 813         d->PDO_status[pdoNum].inhibit_timer =
 814           DelAlarm (d->PDO_status[pdoNum].inhibit_timer);
 815         /* Reset transmit type parameter */
 816         d->PDO_status[pdoNum].transmit_type_parameter = 0;
 817         d->PDO_status[pdoNum].last_message.cob_id = 0;
 818         pdoNum++;
 819         offsetObjdict++;
 820       }
 821 }