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
  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 UNS8 sendPDO(CO_Data* d, s_PDO pdo, UNS8 req)
  38 {
  39   UNS8 i;
  40   if( d->nodeState == Operational ) {
  41     Message m;
  42
  43     /*! Message copy for sending */
  44     m.cob_id.w = pdo.cobId & 0x7FF; /*! Because the cobId is 11 bytes
  45                                       length */
  46     if ( req == NOT_A_REQUEST ) {
  47       UNS8 i;
  48       m.rtr = NOT_A_REQUEST;
  49       m.len = pdo.len;
  50       /*! memcpy(&m.data, &pdo.data, m.len); */
  51       /*! This Memcpy depends on packing structure. Avoid */
  52       for (i = 0 ; i < pdo.len ; i++)
  53         m.data[i] = pdo.data[i];
  54     }
  55     else {
  56       m.rtr = REQUEST;
  57       m.len = 0;
  58     }
  59
  60     MSG_WAR(0x3901, "sendPDO cobId :", m.cob_id.w);
  61     MSG_WAR(0x3902,  "     Nb octets  : ",  m.len);
  62     for (i = 0 ; i < m.len ; i++) {
  63       MSG_WAR(0x3903,"           data : ", m.data[i]);
  64     }
  65
  66     return canSend(d->canHandle,&m);
  67   } /*! end if */
  68   return 0xFF;
  69 }
  70
  71 /*!
  72 ** PDO Manager
  73 **
  74 ** @param d
  75 ** @param cobId
  76 **
  77 ** @return
  78 **/
  79 UNS8 PDOmGR(CO_Data* d, UNS32 cobId)
  80 {
  81   UNS8 res;
  82   UNS8 i;
  83   s_PDO pdo;
  84
  85   MSG_WAR(0x3905, "PDOmGR",0);
  86
  87   /*! if PDO is waiting for transmission,
  88     preparation of the message to send */
  89   pdo.cobId = cobId;
  90   pdo.len =  d->process_var.count;
  91   /*! memcpy(&(pdo.data), &(process_var.data), pdo.len); */
  92      /* Ce memcpy devrait tre portable */
  93     for ( i = 0 ; i < pdo.len ; i++)
  94       pdo.data[i] = d->process_var.data[i];
  95
  96     res = sendPDO(d, pdo, NOT_A_REQUEST);
  97
  98     return res;
  99 }
 100
 101 #if 0
 102 /*********************************************************************/
 103 /* TODO : implement bit mapping                                                  */
 104 /*********************************************************************/
 105
 106 UNS8 buildPDO(CO_Data* d, UNS16 index)
 107 { /* DO NOT USE MSG_ERR because the macro may send a PDO -> infinite loop if it fails. */
 108   UNS16 ind;
 109   UNS8      subInd;
 110
 111   UNS8 *     pMappingCount = NULL;      /* count of mapped objects... */
 112   /* pointer to the var which is mapped to a pdo */
 113 /*  void *     pMappedAppObject = NULL;  */
 114   /* pointer fo the var which holds the mapping parameter of an mapping entry  */
 115
 116   UNS32 *    pMappingParameter = NULL;
 117
 118   UNS8      Size;
 119   UNS8      dataType;
 120   UNS8      offset;
 121   UNS16     offsetObjdict;
 122   UNS16     offsetObjdictPrm;
 123   UNS32     objDict;
 124
 125   subInd=(UNS8)0x00;
 126   offset = 0x00;
 127   ind = index - 0x1800;
 128
 129   MSG_WAR(0x3910,"Prepare PDO to send index :", index);
 130
 131   /*! only operational state allows PDO transmission */
 132   if( d->nodeState != Operational ) {
 133     MSG_WAR(0x2911, "Unable to send the PDO (node not in OPERATIONAL mode). Node : ", index);
 134     return 0xFF;
 135   }
 136   offsetObjdictPrm = d->firstIndex->PDO_TRS;
 137   offsetObjdict = d->firstIndex->PDO_TRS_MAP;
 138
 139   if (offsetObjdictPrm && offsetObjdict)
 140     {
 141       /*! get mapped objects number to transmit with this PDO */
 142       pMappingCount = (d->objdict + offsetObjdict + ind)->pSubindex[0].pObject;
 143       MSG_WAR(0x3912, "Nb maped objects : ",* pMappingCount);
 144       MSG_WAR(0x3913, "        at index : ", 0x1A00 + ind);
 145       while (subInd < *pMappingCount) { /*! Loop on mapped variables */
 146         /*! get mapping parameters */
 147         pMappingParameter = (d->objdict + offsetObjdict + ind)->pSubindex[subInd + 1].pObject;
 148         MSG_WAR(0x3914, "Get the mapping      at index : ", (UNS16)0x1A00 + ind);
 149         MSG_WAR(0x3915, "                     subIndex : ", subInd + 1);
 150         MSG_WAR(0x3916, "                     value    : ", *(UNS32 *)pMappingParameter);
 151         /*! Get the mapped variable */
 152         Size = ((UNS8)(((*pMappingParameter) & 0xFF) >> 3));
 153         objDict = getODentry(d, (UNS16)((*pMappingParameter) >> 16),
 154                              (UNS8)(((*pMappingParameter) >> 8 ) & 0x000000FF),
 155                              (void *)&d->process_var.data[offset], &Size, &dataType, 0 );
 156
 157         if (objDict != OD_SUCCESSFUL) {
 158           MSG_WAR(0x2919, "error accessing to the mapped var : ", subInd + 1);
 159           MSG_WAR(0x2920, "         Mapped at index : ", (*pMappingParameter) >> 16);
 160           MSG_WAR(0x2921, "                subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF);
 161           return 0xFF;
 162         }
 163
 164         offset += Size;
 165         d->process_var.count = offset;
 166         subInd++;
 167       }/*! end Loop on mapped variables  */
 168     }
 169   return 0;
 170 }
 171 #endif
 172
 173 /*!
 174 **
 175 **
 176 ** @param d
 177 ** @param cobId
 178 **
 179 ** @return
 180 **/
 181 UNS8 sendPDOrequest( CO_Data* d, UNS32 cobId )
 182 {
 183   UNS32 * pwCobId;
 184   UNS16          offset;
 185   UNS16          lastIndex;
 186   UNS8           err;
 187
 188   MSG_WAR(0x3930, "sendPDOrequest ",0);
 189   /*! Sending the request only if the cobid have been found on the PDO
 190      receive */
 191   /*! part dictionary */
 192   offset = d->firstIndex->PDO_RCV;
 193   lastIndex = d->lastIndex->PDO_RCV;
 194   if (offset)
 195     while (offset <= lastIndex) {
 196       /*! get the CobId*/
 197       pwCobId = d->objdict[offset].pSubindex[1].pObject;
 198
 199       if ( *pwCobId  == cobId ) {
 200         s_PDO pdo;
 201         pdo.cobId = *pwCobId;
 202         pdo.len = 0;
 203         err  = sendPDO(d, pdo, REQUEST);
 204         return err;
 205       }
 206       offset++;
 207     }
 208   MSG_WAR(0x1931, "sendPDOrequest : COBID not found : ", cobId);
 209   return 0xFF;
 210 }
 211
 212
 213 /*!
 214 **
 215 **
 216 ** @param d
 217 ** @param m
 218 **
 219 ** @return
 220 **/
 221 UNS8 proceedPDO(CO_Data* d, Message *m)
 222 {
 223   UNS8   numPdo;
 224   UNS8   numMap;  /*! Number of the mapped varable */
 225   UNS8 i;
 226   UNS8 *     pMappingCount = NULL;    /*! count of mapped objects... */
 227   /*! pointer to the var which is mapped to a pdo... */
 228   /*!  void *     pMappedAppObject = NULL;   */
 229   /*! pointer fo the var which holds the mapping parameter of an
 230      mapping entry */
 231   UNS32 *    pMappingParameter = NULL;
 232   UNS8  *    pTransmissionType = NULL; /*! pointer to the transmission
 233                                          type */
 234   UNS32 *    pwCobId = NULL;
 235   UNS8       Size;
 236   UNS8       dataType;
 237   UNS8       offset;
 238   UNS8       status;
 239   UNS32      objDict;
 240   UNS16      offsetObjdict;
 241   UNS16      lastIndex;
 242   status = state1;
 243
 244   MSG_WAR(0x3935, "proceedPDO, cobID : ", ((*m).cob_id.w & 0x7ff));
 245   offset = 0x00;
 246   numPdo = 0;
 247   numMap = 0;
 248   if((*m).rtr == NOT_A_REQUEST ) { /*! The PDO received is not a
 249                                      request. */
 250     offsetObjdict = d->firstIndex->PDO_RCV;
 251     lastIndex = d->lastIndex->PDO_RCV;
 252
 253     /*! study of all the PDO stored in the dictionary */
 254     if(offsetObjdict)
 255       while (offsetObjdict <= lastIndex) {
 256
 257         switch( status ) {
 258
 259         case state1:/*! data are stored in process_var array */
 260           /*! memcpy(&(process_var.data), &m->data, (*m).len); */
 261           /*! Ce memcpy devrait etre portable */
 262           for ( i = 0 ; i < m->len ; i++)
 263             d->process_var.data[i] = m->data[i];
 264           d->process_var.count = (*m).len;
 265
 266           status = state2;
 267           break;
 268
 269         case state2:
 270           /*! get CobId of the dictionary correspondant to the received
 271              PDO */
 272           pwCobId = d->objdict[offsetObjdict].pSubindex[1].pObject;
 273           /*! check the CobId coherance */
 274           /*!pwCobId is the cobId read in the dictionary at the state 3
 275             */
 276           if ( *pwCobId == (*m).cob_id.w ){
 277             /*! The cobId is recognized */
 278             status = state4;
 279             MSG_WAR(0x3936, "cobId found at index ", 0x1400 + numPdo);
 280             break;
 281           }
 282           else {
 283             /*! cobId received does not match with those write in the
 284               dictionnary */
 285             numPdo++;
 286             offsetObjdict++;
 287             status = state2;
 288             break;
 289           }
 290
 291             case state4:/*! Get Mapped Objects Number */
 292                /*! The cobId of the message received has been found in the
 293                  dictionnary. */
 294                offsetObjdict = d->firstIndex->PDO_RCV_MAP;
 295              lastIndex = d->lastIndex->PDO_RCV_MAP;
 296              pMappingCount = (d->objdict + offsetObjdict + numPdo)->pSubindex[0].pObject;
 297              numMap = 0;
 298              while (numMap < *pMappingCount) {
 299                UNS8 tmp[]= {0,0,0,0,0,0,0,0};
 300                UNS8 ByteSize;
 301                pMappingParameter = (d->objdict + offsetObjdict + numPdo)->pSubindex[numMap + 1].pObject;
 302                if (pMappingParameter == NULL) {
 303                  MSG_ERR(0x1937, "Couldn't get mapping parameter : ", numMap + 1);
 304                  return 0xFF;
 305                }
 306                /*! Get the addresse of the mapped variable. */
 307                /*! detail of *pMappingParameter : */
 308                /*! The 16 hight bits contains the index, the medium 8 bits
 309                  contains the subindex, */
 310                /*! and the lower 8 bits contains the size of the mapped
 311                  variable. */
 312
 313                Size = (UNS8)(*pMappingParameter);
 314
 315                /*! copy bit per bit in little endian */
 316                CopyBits(Size, (UNS8*)&d->process_var.data[offset>>3], offset%8, 0, ((UNS8*)tmp), 0, 0);
 317
 318                ByteSize = 1 + ((Size - 1) >> 3); /*1->8 => 1 ; 9->16 =>
 319                                                    2, ... */
 320
 321                objDict = setODentry(d, (UNS16)((*pMappingParameter) >> 16),
 322                                     (UNS8)(((*pMappingParameter) >> 8 ) & 0xFF),
 323                                  tmp, &ByteSize, 0 );
 324
 325                if(objDict != OD_SUCCESSFUL) {
 326                  MSG_ERR(0x1938, "error accessing to the mapped var : ", numMap + 1);
 327                  MSG_WAR(0x2939, "         Mapped at index : ", (*pMappingParameter) >> 16);
 328                  MSG_WAR(0x2940, "                subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF);
 329                  return 0xFF;
 330                }
 331
 332                MSG_WAR(0x3942, "Variable updated with value received by PDO cobid : ", m->cob_id.w);
 333                MSG_WAR(0x3943, "         Mapped at index : ", (*pMappingParameter) >> 16);
 334                MSG_WAR(0x3944, "                subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF);
 335                /*! MSG_WAR(0x3945, "                data : ",*((UNS32*)pMappedAppObject)); */
 336                offset += Size;
 337                numMap++;
 338              } /*! end loop while on mapped variables */
 339
 340              offset=0x00;
 341              numMap = 0;
 342              return 0;
 343
 344         }/*! end switch status*/
 345       }/*! end while*/
 346   }/*! end if Donnees */
 347   else if ((*m).rtr == REQUEST ){
 348     MSG_WAR(0x3946, "Receive a PDO request cobId : ", m->cob_id.w);
 349     status = state1;
 350     offsetObjdict = d->firstIndex->PDO_TRS;
 351     lastIndex = d->lastIndex->PDO_TRS;
 352     if(offsetObjdict) while( offsetObjdict  <= lastIndex ){
 353       /*! study of all PDO stored in the objects dictionary */
 354
 355       switch( status ){
 356
 357       case state1:/*! check the CobId */
 358         /*! get CobId of the dictionary which match to the received PDO
 359          */
 360         pwCobId = (d->objdict + offsetObjdict)->pSubindex[1].pObject;
 361         if ( *pwCobId == (*m).cob_id.w ) {
 362           status = state4;
 363           break;
 364         }
 365         else {
 366           numPdo++;
 367           offsetObjdict++;
 368         }
 369         status = state1;
 370         break;
 371
 372
 373       case state4:/*! check transmission type (after request?) */
 374         pTransmissionType = d->objdict[offsetObjdict].pSubindex[2].pObject;
 375         if ( (*pTransmissionType == TRANS_RTR) || (*pTransmissionType == TRANS_RTR_SYNC ) || (*pTransmissionType == TRANS_EVENT) ) {
 376           status = state5;
 377           break;
 378         }
 379         else {
 380           /*! The requested PDO is not to send on request. So, does
 381             nothing. */
 382           MSG_WAR(0x2947, "PDO is not to send on request : ", m->cob_id.w);
 383           return 0xFF;
 384         }
 385
 386       case state5:/*! get mapped objects number */
 387         offsetObjdict = d->firstIndex->PDO_TRS_MAP;
 388         lastIndex = d->lastIndex->PDO_TRS_MAP;
 389         pMappingCount = (d->objdict + offsetObjdict + numPdo)->pSubindex[0].pObject;
 390         numMap = 0;
 391         while (numMap < *pMappingCount) {
 392           pMappingParameter = (d->objdict + offsetObjdict + numPdo)->pSubindex[numMap + 1].pObject;
 393           /*! Get the mapped variable */
 394           Size = ((UNS8)(((*pMappingParameter) & 0xFF) >> 3));
 395           objDict = getODentry( d, (UNS16)((*pMappingParameter) >> (UNS8)16),
 396                                 (UNS8)(( (*pMappingParameter) >> (UNS8)8 ) & 0xFF),
 397                                 (void *)&d->process_var.data[offset], &Size, &dataType, 0 );
 398           if (objDict != OD_SUCCESSFUL) {
 399             MSG_ERR(0x1948, "error accessing to the mapped var : ", numMap + 1);
 400             MSG_WAR(0x2949, "         Mapped at index : ", (*pMappingParameter) >> 16);
 401             MSG_WAR(0x2950, "                subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF);
 402             return 0xFF;
 403           }
 404           offset += (UNS8) (((*pMappingParameter) & 0xFF) >> 3);
 405           d->process_var.count = offset;
 406           numMap++;
 407
 408         } /*! end while */
 409         PDOmGR( d, *pwCobId ); /*! Transmit the PDO */
 410         return 0;
 411
 412       }/*! end switch status */
 413     }/*! end while */
 414   }/*! end if Requete */
 415
 416   return 0;
 417 }
 418
 419 /*!
 420 **
 421 **
 422 ** @param NbBits
 423 ** @param SrcByteIndex
 424 ** @param SrcBitIndex
 425 ** @param SrcBigEndian
 426 ** @param DestByteIndex
 427 ** @param DestBitIndex
 428 ** @param DestBigEndian
 429 **/
 430 void CopyBits(UNS8 NbBits, UNS8* SrcByteIndex, UNS8 SrcBitIndex, UNS8 SrcBigEndian, UNS8* DestByteIndex, UNS8 DestBitIndex, 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 = Vect>0 ? *SrcByteIndex << Vect : *SrcByteIndex >> -Vect;
 442
 443       /*! Compute the nb of bit we will be able to copy*/
 444       UNS8 BoudaryLimit = (Vect>0 ? 8 - DestBitIndex :  8 - SrcBitIndex );
 445       UNS8 BitsToCopy = BoudaryLimit > NbBits ? NbBits : BoudaryLimit;
 446
 447       /*! Create a mask that will serve in:*/
 448       UNS8 Mask = ((0xff << (DestBitIndex + BitsToCopy)) | (0xff >> (8 - DestBitIndex)));
 449
 450       /*! - Filtering src*/
 451       UNS8 Filtered = Aligned & ~Mask;
 452
 453       /*! - and erase bits where we write, preserve where we don't*/
 454       *DestByteIndex &= Mask;
 455
 456       /*! Then write.*/
 457       *DestByteIndex |= Filtered ;
 458
 459       /*!Compute next time cursors for src*/
 460       if((SrcBitIndex += BitsToCopy)>7)/*! cross boundary ?*/
 461         {
 462           SrcBitIndex = 0;/*! First bit*/
 463           SrcByteIndex += (SrcBigEndian ? -1 : 1);/*! Next byte*/
 464         }
 465
 466
 467       /*!Compute next time cursors for dest*/
 468       if((DestBitIndex += BitsToCopy)>7)
 469         {
 470           DestBitIndex = 0;/*! First bit*/
 471           DestByteIndex += (DestBigEndian ? -1 : 1);/*! Next byte*/
 472         }
 473
 474       /*!And decrement counter.*/
 475       NbBits -= BitsToCopy;
 476     }
 477
 478 }
 479
 480 #if 0
 481
 482 /*********************************************************************/
 483 /* TODO : reimplement this using CallBacks
 484  */
 485 /*********************************************************************/
 486
 487 /*!
 488 **
 489 **
 490 ** @param d
 491 ** @param variable
 492 **
 493 ** @return
 494 **/
 495 UNS8 sendPDOevent( CO_Data* d, void * variable )
 496 { /*! DO NOT USE MSG_ERR because the macro may send a PDO -> infinite
 497     loop if it fails.*/
 498   UNS32           objDict = 0;
 499   UNS8            ind, sub_ind;
 500   UNS8            status;
 501   UNS8            offset;
 502   UNS8 *     pMappingCount = NULL;
 503   UNS32 *    pMappingParameter = NULL;
 504   void *     pMappedAppObject = NULL;
 505   UNS8 *     pTransmissionType = NULL; /*! pointer to the transmission
 506                                          type */
 507   UNS32 *    pwCobId = NULL;
 508   UNS8 *     pSize;
 509   UNS8       size;
 510   UNS8       dataType;
 511   UNS16      offsetObjdict;
 512   UNS16      offsetObjdictPrm;
 513   UNS16      lastIndex;
 514   UNS8       numMap;
 515   ind     = 0x00;
 516   sub_ind = 1;
 517   offset  = 0x00;
 518   pSize   = &size;
 519   status  = state1;
 520
 521
 522   /*! look for the index and subindex where the variable is mapped */
 523   /*! Then, send the pdo which contains the variable. */
 524
 525   MSG_WAR (0x3960, "sendPDOevent", 0);
 526   offsetObjdictPrm = d->firstIndex->PDO_TRS;
 527
 528   offsetObjdict = d->firstIndex->PDO_TRS_MAP;
 529   lastIndex = d->lastIndex->PDO_TRS_MAP;
 530
 531   if (offsetObjdictPrm && offsetObjdict)
 532     /*! Loop on PDO Transmit */
 533     while(offsetObjdict <= lastIndex){
 534       /*! Check the transmission mode */
 535       pTransmissionType = d->objdict[offsetObjdictPrm].pSubindex[2].pObject;
 536       if (*pTransmissionType != TRANS_EVENT) {
 537         ind++;
 538         offsetObjdict++;
 539         offsetObjdictPrm++;
 540         continue;
 541       }
 542       pMappingCount = d->objdict[offsetObjdict].pSubindex[0].pObject;
 543       numMap = 1; /*! mapped variable */
 544       while (numMap <= *pMappingCount) {
 545         pMappingParameter = d->objdict[offsetObjdict].pSubindex[numMap].pObject;
 546         /*! Get the variable */
 547         objDict = getODentry( d,
 548                               (UNS16)((*pMappingParameter) >> 16),
 549                               (UNS8)(( (*pMappingParameter) >> (UNS8)8 ) & (UNS32)0x000000FF),
 550                               (void * *)&pMappedAppObject, pSize, &dataType, 0 );
 551         if( objDict != OD_SUCCESSFUL ) {
 552           MSG_WAR(0x2961, "Error in dict. at index : ",
 553                   (*pMappingParameter) >> (UNS8)16);
 554
 555           MSG_WAR(0x2962, "               subindex : ",
 556                   ((*pMappingParameter) >> (UNS8)8 ) & (UNS32)0x000000FF);
 557           return 0xFF;
 558         }
 559         if (pMappedAppObject == variable) { // Variable found !
 560           MSG_WAR(0x3963, "Variable to send found at index : ",
 561                   (*pMappingParameter) >> 16);
 562           MSG_WAR(0x3964, "                       subIndex : ",
 563                   ((*pMappingParameter) >> 8 ) & 0x000000FF);
 564           buildPDO(d, 0x1800 + ind);
 565           /*! Get the cobId */
 566           pwCobId = d->objdict[offsetObjdictPrm].pSubindex[1].pObject;
 567           PDOmGR( d, *pwCobId ); /*! Send the PDO */
 568           return 0;
 569         }
 570         numMap++;
 571       } /*! End loop on mapped variable */
 572       ind++;
 573       offsetObjdict++;
 574       offsetObjdictPrm++;
 575     } /*! End loop while on PDO */
 576
 577   MSG_WAR(0x2965, "Variable not found in a PDO to send on event", 0);
 578   return 0xFF;
 579
 580 }
 581 #endif
 582
 583
 584