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   UNS32 *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 = d->objdict[offset].pSubindex[1].pObject;
 142
 143           MSG_WAR (0x3930, "sendPDOrequest cobId is : ", *pwCobId);
 144           {
 145             Message pdo;
 146             pdo.cob_id = UNS16_LE(*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   UNS32 *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 : ", (UNS16_LE(m->cob_id) & 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                   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 == UNS16_LE(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                                  UNS16_LE(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                 return 0;
 306
 307               }                 /* end switch status */
 308           }                     /* end while */
 309     }                           /* end if Donnees */
 310   else if ((*m).rtr == REQUEST)
 311     {
 312       MSG_WAR (0x3946, "Receive a PDO request cobId : ", UNS16_LE(m->cob_id));
 313       status = state1;
 314       offsetObjdict = d->firstIndex->PDO_TRS;
 315       lastIndex = d->lastIndex->PDO_TRS;
 316       if (offsetObjdict)
 317         while (offsetObjdict <= lastIndex)
 318           {
 319             /* study of all PDO stored in the objects dictionary */
 320
 321             switch (status)
 322               {
 323
 324               case state1:     /* check the CobId */
 325                 /* get CobId of the dictionary which match to the received PDO
 326                  */
 327                 pwCobId =
 328                    (d->objdict +
 329                              offsetObjdict)->pSubindex[1].pObject;
 330                 if (*pwCobId == UNS16_LE(m->cob_id))
 331                   {
 332                     status = state4;
 333                     break;
 334                   }
 335                 else
 336                   {
 337                     numPdo++;
 338                     offsetObjdict++;
 339                   }
 340                 status = state1;
 341                 break;
 342
 343
 344               case state4:     /* check transmission type */
 345                 pTransmissionType =
 346                   (UNS8 *) d->objdict[offsetObjdict].pSubindex[2].pObject;
 347                 /* If PDO is to be sampled and send on RTR, do it */
 348                 if ((*pTransmissionType == TRANS_RTR))
 349                   {
 350                     status = state5;
 351                     break;
 352                     /* RTR_SYNC mean data is prepared at SYNC, and transmitted on RTR */
 353                   }
 354                 else if ((*pTransmissionType == TRANS_RTR_SYNC))
 355                   {
 356                     if (d->PDO_status[numPdo].
 357                         transmit_type_parameter & PDO_RTR_SYNC_READY)
 358                       {
 359                         /*Data ready, just send */
 360                         canSend (d->canHandle,
 361                                  &d->PDO_status[numPdo].last_message);
 362                         return 0;
 363                       }
 364                     else
 365                       {
 366                         /* if SYNC did never occur, force emission with current data */
 367                         /* DS301 do not tell what to do in such a case... */
 368                         MSG_ERR (0x1947,
 369                                  "Not ready RTR_SYNC TPDO send current data : ",
 370                                  UNS16_LE(m->cob_id));
 371                         status = state5;
 372                       }
 373                     break;
 374                   }
 375                 else if ((*pTransmissionType == TRANS_EVENT_PROFILE) ||
 376                          (*pTransmissionType == TRANS_EVENT_SPECIFIC))
 377                   {
 378                     /* Zap all timers and inhibit flag */
 379                     d->PDO_status[numPdo].event_timer =
 380                       DelAlarm (d->PDO_status[numPdo].event_timer);
 381                     d->PDO_status[numPdo].inhibit_timer =
 382                       DelAlarm (d->PDO_status[numPdo].inhibit_timer);
 383                     d->PDO_status[numPdo].transmit_type_parameter &=
 384                       ~PDO_INHIBITED;
 385                     /* Call  PDOEventTimerAlarm for this TPDO, this will trigger emission et reset timers */
 386                     PDOEventTimerAlarm (d, numPdo);
 387                     return 0;
 388                   }
 389                 else
 390                   {
 391                     /* The requested PDO is not to send on request. So, does
 392                        nothing. */
 393                     MSG_WAR (0x2947, "PDO is not to send on request : ",
 394                              UNS16_LE(m->cob_id));
 395                     return 0xFF;
 396                   }
 397
 398               case state5:     /* build and send requested PDO */
 399                 {
 400                   Message pdo;
 401                   if (buildPDO (d, numPdo, &pdo))
 402                     {
 403                       MSG_ERR (0x1948, " Couldn't build TPDO n�", numPdo);
 404                       return 0xFF;
 405                     }
 406                   canSend (d->canHandle, &pdo);
 407                   return 0;
 408                 }
 409               }                 /* end switch status */
 410           }                     /* end while */
 411     }                           /* end if Requete */
 412
 413   return 0;
 414 }
 415
 416 /*!
 417 **
 418 **
 419 ** @param NbBits
 420 ** @param SrcByteIndex
 421 ** @param SrcBitIndex
 422 ** @param SrcBigEndian
 423 ** @param DestByteIndex
 424 ** @param DestBitIndex
 425 ** @param DestBigEndian
 426 **/
 427 void
 428 CopyBits (UNS8 NbBits, UNS8 * SrcByteIndex, UNS8 SrcBitIndex,
 429           UNS8 SrcBigEndian, UNS8 * DestByteIndex, UNS8 DestBitIndex,
 430           UNS8 DestBigEndian)
 431 {
 432   /* This loop copy as many bits that it can each time, crossing */
 433   /* successively bytes */
 434   // boundaries from LSB to MSB.
 435   while (NbBits > 0)
 436     {
 437       /* Bit missalignement between src and dest */
 438       INTEGER8 Vect = DestBitIndex - SrcBitIndex;
 439
 440       /* We can now get src and align it to dest */
 441       UNS8 Aligned =
 442         Vect > 0 ? *SrcByteIndex << Vect : *SrcByteIndex >> -Vect;
 443
 444       /* Compute the nb of bit we will be able to copy */
 445       UNS8 BoudaryLimit = (Vect > 0 ? 8 - DestBitIndex : 8 - SrcBitIndex);
 446       UNS8 BitsToCopy = BoudaryLimit > NbBits ? NbBits : BoudaryLimit;
 447
 448       /* Create a mask that will serve in: */
 449       UNS8 Mask =
 450         ((0xff << (DestBitIndex + BitsToCopy)) |
 451          (0xff >> (8 - DestBitIndex)));
 452
 453       /* - Filtering src */
 454       UNS8 Filtered = Aligned & ~Mask;
 455
 456       /* - and erase bits where we write, preserve where we don't */
 457       *DestByteIndex &= Mask;
 458
 459       /* Then write. */
 460       *DestByteIndex |= Filtered;
 461
 462       /*Compute next time cursors for src */
 463       if ((SrcBitIndex += BitsToCopy) > 7)      /* cross boundary ? */
 464         {
 465           SrcBitIndex = 0;      /* First bit */
 466           SrcByteIndex += (SrcBigEndian ? -1 : 1);      /* Next byte */
 467         }
 468
 469
 470       /*Compute next time cursors for dest */
 471       if ((DestBitIndex += BitsToCopy) > 7)
 472         {
 473           DestBitIndex = 0;     /* First bit */
 474           DestByteIndex += (DestBigEndian ? -1 : 1);    /* Next byte */
 475         }
 476
 477       /*And decrement counter. */
 478       NbBits -= BitsToCopy;
 479     }
 480
 481 }
 482
 483 /*!
 484 **
 485 **
 486 ** @param d
 487 **
 488 ** @return
 489 **/
 490
 491 UNS8
 492 sendPDOevent (CO_Data * d)
 493 {
 494   /* Calls _sendPDOevent specifying it is not a sync event */
 495   return _sendPDOevent (d, 0);
 496 }
 497
 498
 499 void
 500 PDOEventTimerAlarm (CO_Data * d, UNS32 pdoNum)
 501 {
 502   /* This is needed to avoid deletion of re-attribuated timer */
 503   d->PDO_status[pdoNum].event_timer = TIMER_NONE;
 504   /* force emission of PDO by artificially changing last emitted */
 505   d->PDO_status[pdoNum].last_message.cob_id = 0;
 506   _sendPDOevent (d, 0);         /* not a Sync Event */
 507 }
 508
 509 void
 510 PDOInhibitTimerAlarm (CO_Data * d, UNS32 pdoNum)
 511 {
 512   /* This is needed to avoid deletion of re-attribuated timer */
 513   d->PDO_status[pdoNum].inhibit_timer = TIMER_NONE;
 514   /* Remove inhibit flag */
 515   d->PDO_status[pdoNum].transmit_type_parameter &= ~PDO_INHIBITED;
 516   _sendPDOevent (d, 0);         /* not a Sync Event */
 517 }
 518
 519 /*!
 520 **
 521 **
 522 ** @param d
 523 ** @param isSyncEvent
 524 **
 525 ** @return
 526 **/
 527
 528 UNS8
 529 _sendPDOevent (CO_Data * d, UNS8 isSyncEvent)
 530 {
 531   UNS8 pdoNum = 0x00;           /* number of the actual processed pdo-nr. */
 532   UNS8 *pTransmissionType = NULL;
 533   UNS8 status = state3;
 534   UNS16 offsetObjdict = d->firstIndex->PDO_TRS;
 535   UNS16 offsetObjdictMap = d->firstIndex->PDO_TRS_MAP;
 536   UNS16 lastIndex = d->lastIndex->PDO_TRS;
 537
 538   /* study all PDO stored in the objects dictionary */
 539   if (offsetObjdict)
 540     {
 541       Message pdo = Message_Initializer;
 542       while (offsetObjdict <= lastIndex)
 543         {
 544           switch (status)
 545             {
 546             case state3:
 547               if (              /*d->objdict[offsetObjdict].bSubCount < 5 || not necessary with objdictedit (always 5) */
 548                    /* check if TPDO is not valid */
 549                    *(UNS32 *) d->objdict[offsetObjdict].pSubindex[1].
 550                    pObject & 0x80000000)
 551                 {
 552                   MSG_WAR (0x3960, "Not a valid PDO ", 0x1800 + pdoNum);
 553                   /*Go next TPDO */
 554                   status = state11;
 555                   break;
 556                 }
 557               /* get the PDO transmission type */
 558               pTransmissionType =
 559                 (UNS8 *) d->objdict[offsetObjdict].pSubindex[2].pObject;
 560               MSG_WAR (0x3962, "Reading PDO at index : ", 0x1800 + pdoNum);
 561
 562               /* check if transmission type is SYNCRONOUS */
 563               /* The message is transmited every n SYNC with n=TransmissionType */
 564               if (isSyncEvent &&
 565                   (*pTransmissionType >= TRANS_SYNC_MIN) &&
 566                   (*pTransmissionType <= TRANS_SYNC_MAX) &&
 567                   (++d->PDO_status[pdoNum].transmit_type_parameter ==
 568                    *pTransmissionType))
 569                 {
 570                   /*Reset count of SYNC */
 571                   d->PDO_status[pdoNum].transmit_type_parameter = 0;
 572                   MSG_WAR (0x3964, "  PDO is on SYNCHRO. Trans type : ",
 573                            *pTransmissionType);
 574                   {
 575                     Message msg_init = Message_Initializer;
 576                     pdo = msg_init;
 577                   }
 578                   if (buildPDO (d, pdoNum, &pdo))
 579                     {
 580                       MSG_ERR (0x1906, " Couldn't build TPDO number : ",
 581                                pdoNum);
 582                       status = state11;
 583                       break;
 584                     }
 585                   status = state5;
 586                   /* If transmission RTR, with data sampled on SYNC */
 587                 }
 588               else if (isSyncEvent && (*pTransmissionType == TRANS_RTR_SYNC))
 589                 {
 590                   if (buildPDO
 591                       (d, pdoNum, &d->PDO_status[pdoNum].last_message))
 592                     {
 593                       MSG_ERR (0x1966, " Couldn't build TPDO number : ",
 594                                pdoNum);
 595                       d->PDO_status[pdoNum].transmit_type_parameter &=
 596                         ~PDO_RTR_SYNC_READY;
 597                     }
 598                   else
 599                     {
 600                       d->PDO_status[pdoNum].transmit_type_parameter |=
 601                         PDO_RTR_SYNC_READY;
 602                     }
 603                   status = state11;
 604                   break;
 605                   /* If transmission on Event and not inhibited, check for changes */
 606                 }
 607               else
 608                 if ((isSyncEvent
 609                      && (*pTransmissionType == TRANS_SYNC_ACYCLIC))
 610                     ||
 611                     ((*pTransmissionType == TRANS_EVENT_PROFILE
 612                       || *pTransmissionType == TRANS_EVENT_SPECIFIC)
 613                      && !(d->PDO_status[pdoNum].
 614                           transmit_type_parameter & PDO_INHIBITED)))
 615                 {
 616                   MSG_WAR (0x3968, "  PDO is on EVENT. Trans type : ",
 617                            *pTransmissionType);
 618                   {
 619                     Message msg_init = Message_Initializer;
 620                     pdo = msg_init;
 621                   }
 622                   if (buildPDO (d, pdoNum, &pdo))
 623                     {
 624                       MSG_ERR (0x3907, " Couldn't build TPDO number : ",
 625                                pdoNum);
 626                       status = state11;
 627                       break;
 628                     }
 629
 630                   /*Compare new and old PDO */
 631                   if (d->PDO_status[pdoNum].last_message.cob_id == pdo.cob_id
 632                       && d->PDO_status[pdoNum].last_message.len == pdo.len &&
 633                       memcmp(d->PDO_status[pdoNum].last_message.data,
 634                                                         pdo.data, 8) == 0
 635                     )
 636                     {
 637                       /* No changes -> go to next pdo */
 638                       status = state11;
 639                     }
 640                   else
 641                     {
 642
 643                       UNS16 EventTimerDuration;
 644                       UNS16 InhibitTimerDuration;
 645
 646                       MSG_WAR (0x306A, "Changes TPDO number : ", pdoNum);
 647                       /* Changes detected -> transmit message */
 648                       EventTimerDuration =
 649                         *(UNS16 *) d->objdict[offsetObjdict].pSubindex[5].
 650                         pObject;
 651                       InhibitTimerDuration =
 652                         *(UNS16 *) d->objdict[offsetObjdict].pSubindex[3].
 653                         pObject;
 654
 655                       status = state5;
 656
 657                       /* Start both event_timer and inhibit_timer */
 658                       if (EventTimerDuration)
 659                         {
 660                           DelAlarm (d->PDO_status[pdoNum].event_timer);
 661                           d->PDO_status[pdoNum].event_timer =
 662                             SetAlarm (d, pdoNum, &PDOEventTimerAlarm,
 663                                       MS_TO_TIMEVAL (EventTimerDuration), 0);
 664                         }
 665
 666                       if (InhibitTimerDuration)
 667                         {
 668                           DelAlarm (d->PDO_status[pdoNum].inhibit_timer);
 669                           d->PDO_status[pdoNum].inhibit_timer =
 670                             SetAlarm (d, pdoNum, &PDOInhibitTimerAlarm,
 671                                       US_TO_TIMEVAL (InhibitTimerDuration *
 672                                                      100), 0);
 673                           /* and inhibit TPDO */
 674                           d->PDO_status[pdoNum].transmit_type_parameter |=
 675                             PDO_INHIBITED;
 676                         }
 677
 678                     }
 679                 }
 680               else
 681                 {
 682                   MSG_WAR (0x306C,
 683                            "  PDO is not on EVENT or synchro or not at this SYNC. Trans type : ",
 684                            *pTransmissionType);
 685                   status = state11;
 686                 }
 687               break;
 688             case state5:       /*Send the pdo */
 689               /*store_as_last_message */
 690               d->PDO_status[pdoNum].last_message = pdo;
 691               MSG_WAR (0x396D, "sendPDO cobId :", UNS16_LE(pdo.cob_id));
 692               MSG_WAR (0x396E, "     Nb octets  : ", pdo.len);
 693
 694               canSend (d->canHandle, &pdo);
 695               status = state11;
 696               break;
 697             case state11:      /*Go to next TPDO */
 698               pdoNum++;
 699               offsetObjdict++;
 700               offsetObjdictMap++;
 701               MSG_WAR (0x3970, "next pdo index : ", pdoNum);
 702               status = state3;
 703               break;
 704
 705             default:
 706               MSG_ERR (0x1972, "Unknown state has been reached : %d", status);
 707               return 0xFF;
 708             }                   /* end switch case */
 709
 710         }                       /* end while */
 711     }
 712   return 0;
 713 }
 714
 715 /*!
 716 **
 717 **
 718 ** @param d
 719 ** @param OD_entry
 720 ** @param bSubindex
 721 ** @return always 0
 722 **/
 723
 724 UNS32
 725 TPDO_Communication_Parameter_Callback (CO_Data * d,
 726                                        const indextable * OD_entry,
 727                                        UNS8 bSubindex)
 728 {
 729   /* If PDO are actives */
 730   if (d->CurrentCommunicationState.csPDO)
 731     switch (bSubindex)
 732       {
 733       case 2:                  /* Changed transmition type */
 734       case 3:                  /* Changed inhibit time */
 735       case 5:                  /* Changed event time */
 736         {
 737           const indextable *TPDO_com = d->objdict + d->firstIndex->PDO_TRS;
 738           UNS8 numPdo = OD_entry - TPDO_com;    /* number of the actual processed pdo-nr. */
 739
 740           /* Zap all timers and inhibit flag */
 741           d->PDO_status[numPdo].event_timer =
 742             DelAlarm (d->PDO_status[numPdo].event_timer);
 743           d->PDO_status[numPdo].inhibit_timer =
 744             DelAlarm (d->PDO_status[numPdo].inhibit_timer);
 745           d->PDO_status[numPdo].transmit_type_parameter = 0;
 746           /* Call  PDOEventTimerAlarm for this TPDO, this will trigger emission et reset timers */
 747           PDOEventTimerAlarm (d, numPdo);
 748           return 0;
 749         }
 750
 751       default:                 /* other subindex are ignored */
 752         break;
 753       }
 754   return 0;
 755 }
 756
 757 void
 758 PDOInit (CO_Data * d)
 759 {
 760   /* For each TPDO mapping parameters */
 761   UNS16 pdoIndex = 0x1800;      /* OD index of TDPO */
 762
 763   UNS16 offsetObjdict = d->firstIndex->PDO_TRS;
 764   UNS16 lastIndex = d->lastIndex->PDO_TRS;
 765   if (offsetObjdict)
 766     while (offsetObjdict <= lastIndex)
 767       {
 768         /* Assign callbacks to sensible TPDO mapping subindexes */
 769         UNS32 errorCode;
 770         ODCallback_t *CallbackList;
 771         /* Find callback list */
 772         scanIndexOD (d, pdoIndex, &errorCode, &CallbackList);
 773         if (errorCode == OD_SUCCESSFUL && CallbackList)
 774           {
 775             /*Assign callbacks to corresponding subindex */
 776             /* Transmission type */
 777             CallbackList[2] = &TPDO_Communication_Parameter_Callback;
 778             /* Inhibit time */
 779             CallbackList[3] = &TPDO_Communication_Parameter_Callback;
 780             /* Event timer */
 781             CallbackList[5] = &TPDO_Communication_Parameter_Callback;
 782           }
 783         pdoIndex++;
 784         offsetObjdict++;
 785       }
 786
 787   /* Trigger a non-sync event */
 788   _sendPDOevent (d, 0);
 789 }
 790
 791 void
 792 PDOStop (CO_Data * d)
 793 {
 794   /* For each TPDO mapping parameters */
 795   UNS8 pdoNum = 0x00;           /* number of the actual processed pdo-nr. */
 796   UNS16 offsetObjdict = d->firstIndex->PDO_TRS;
 797   UNS16 lastIndex = d->lastIndex->PDO_TRS;
 798   if (offsetObjdict)
 799     while (offsetObjdict <= lastIndex)
 800       {
 801         /* Delete TPDO timers */
 802         d->PDO_status[pdoNum].event_timer =
 803           DelAlarm (d->PDO_status[pdoNum].event_timer);
 804         d->PDO_status[pdoNum].inhibit_timer =
 805           DelAlarm (d->PDO_status[pdoNum].inhibit_timer);
 806         /* Reset transmit type parameter */
 807         d->PDO_status[pdoNum].transmit_type_parameter = 0;
 808         d->PDO_status[pdoNum].last_message.cob_id = 0;
 809         pdoNum++;
 810         offsetObjdict++;
 811       }
 812 }