src/pdo.c

   1 /*
   2 This file is part of CanFestival, a library implementing CanOpen Stack.
   3
   4 Copyright (C): Edouard TISSERANT and Francis DUPIN
   5
   6 See COPYING file for copyrights details.
   7
   8 This library is free software; you can redistribute it and/or
   9 modify it under the terms of the GNU Lesser General Public
  10 License as published by the Free Software Foundation; either
  11 version 2.1 of the License, or (at your option) any later version.
  12
  13 This library is distributed in the hope that it will be useful,
  14 but WITHOUT ANY WARRANTY; without even the implied warranty of
  15 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  16 Lesser General Public License for more details.
  17
  18 You should have received a copy of the GNU Lesser General Public
  19 License along with this library; if not, write to the Free Software
  20 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  21 */
  22 #include "pdo.h"
  23 #include "objacces.h"
  24 #include "canfestival.h"
  25
  26 /****************************************************************************/
  27 UNS8 sendPDO(CO_Data* d, s_PDO pdo, UNS8 req)
  28 {
  29   UNS8 i;
  30   if( d->nodeState == Operational ) {
  31     Message m;
  32
  33     /* Message copy for sending */
  34     m.cob_id.w = pdo.cobId & 0x7FF; /* Because the cobId is 11 bytes length */
  35     if ( req == NOT_A_REQUEST ) {
  36       UNS8 i;
  37       m.rtr = NOT_A_REQUEST;
  38       m.len = pdo.len;
  39       /* memcpy(&m.data, &pdo.data, m.len); */
  40       /* This Memcpy depends on packing structure. Avoid */
  41       for (i = 0 ; i < pdo.len ; i++)
  42         m.data[i] = pdo.data[i];
  43     }
  44     else {
  45       m.rtr = REQUEST;
  46       m.len = 0;
  47     }
  48
  49     MSG_WAR(0x3901, "sendPDO cobId :", m.cob_id.w);
  50     MSG_WAR(0x3902,  "     Nb octets  : ",  m.len);
  51     for (i = 0 ; i < m.len ; i++) {
  52       MSG_WAR(0x3903,"           data : ", m.data[i]);
  53     }
  54
  55     return canSend(d->canHandle,&m);
  56   } /* end if */
  57   return 0xFF;
  58 }
  59
  60 /***************************************************************************/
  61 UNS8 PDOmGR(CO_Data* d, UNS32 cobId) /* PDO Manager */
  62 {
  63   UNS8 res;
  64   UNS8 i;
  65   s_PDO pdo;
  66
  67   MSG_WAR(0x3905, "PDOmGR",0);
  68
  69   /* if PDO is waiting for transmission,
  70      preparation of the message to send */
  71     pdo.cobId = cobId;
  72     pdo.len =  d->process_var.count;
  73     /* memcpy(&(pdo.data), &(process_var.data), pdo.len); */
  74     /* Ce memcpy devrait être portable */
  75     for ( i = 0 ; i < pdo.len ; i++)
  76       pdo.data[i] = d->process_var.data[i];
  77
  78     res = sendPDO(d, pdo, NOT_A_REQUEST);
  79
  80     return res;
  81 }
  82
  83 #if 0
  84 /*********************************************************************/
  85 /* TODO : implement bit mapping                                                  */
  86 /*********************************************************************/
  87
  88 UNS8 buildPDO(CO_Data* d, UNS16 index)
  89 { /* DO NOT USE MSG_ERR because the macro may send a PDO -> infinite loop if it fails. */
  90   UNS16 ind;
  91   UNS8      subInd;
  92
  93   UNS8 *     pMappingCount = NULL;      /* count of mapped objects... */
  94   /* pointer to the var which is mapped to a pdo */
  95 /*  void *     pMappedAppObject = NULL;  */
  96   /* pointer fo the var which holds the mapping parameter of an mapping entry  */
  97   UNS32 *    pMappingParameter = NULL;
  98
  99   UNS8      Size;
 100   UNS8      dataType;
 101   UNS8      offset;
 102   UNS16     offsetObjdict;
 103   UNS16     offsetObjdictPrm;
 104   UNS32     objDict;
 105
 106   subInd=(UNS8)0x00;
 107   offset = 0x00;
 108   ind = index - 0x1800;
 109
 110   MSG_WAR(0x3910,"Prepare PDO to send index :", index);
 111
 112   /* only operational state allows PDO transmission */
 113   if( d->nodeState != Operational ) {
 114     MSG_WAR(0x2911, "Unable to send the PDO (node not in OPERATIONAL mode). Node : ", index);
 115     return 0xFF;
 116   }
 117   offsetObjdictPrm = d->firstIndex->PDO_TRS;
 118   offsetObjdict = d->firstIndex->PDO_TRS_MAP;
 119
 120   if (offsetObjdictPrm && offsetObjdict)
 121   {
 122           /* get mapped objects number to transmit with this PDO */
 123           pMappingCount = (d->objdict + offsetObjdict + ind)->pSubindex[0].pObject;
 124           MSG_WAR(0x3912, "Nb maped objects : ",* pMappingCount);
 125           MSG_WAR(0x3913, "        at index : ", 0x1A00 + ind);
 126           while (subInd < *pMappingCount) { /* Loop on mapped variables */
 127             /* get mapping parameters */
 128             pMappingParameter = (d->objdict + offsetObjdict + ind)->pSubindex[subInd + 1].pObject;
 129             MSG_WAR(0x3914, "Get the mapping      at index : ", (UNS16)0x1A00 + ind);
 130             MSG_WAR(0x3915, "                     subIndex : ", subInd + 1);
 131             MSG_WAR(0x3916, "                     value    : ", *(UNS32 *)pMappingParameter);
 132             /* Get the mapped variable */
 133             Size = ((UNS8)(((*pMappingParameter) & 0xFF) >> 3));
 134                 objDict = getODentry(d, (UNS16)((*pMappingParameter) >> 16),
 135                                     (UNS8)(((*pMappingParameter) >> 8 ) & 0x000000FF),
 136                                     (void *)&d->process_var.data[offset], &Size, &dataType, 0 );
 137
 138              if (objDict != OD_SUCCESSFUL) {
 139                 MSG_WAR(0x2919, "error accessing to the mapped var : ", subInd + 1);
 140                 MSG_WAR(0x2920, "         Mapped at index : ", (*pMappingParameter) >> 16);
 141                 MSG_WAR(0x2921, "                subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF);
 142                 return 0xFF;
 143              }
 144
 145               offset += Size;
 146               d->process_var.count = offset;
 147               subInd++;
 148         }/* end Loop on mapped variables  */
 149   }
 150   return 0;
 151 }
 152 #endif
 153 /**************************************************************************/
 154 UNS8 sendPDOrequest( CO_Data* d, UNS32 cobId )
 155 {
 156   UNS32 *        pwCobId;
 157   UNS16          offset;
 158   UNS16          lastIndex;
 159   UNS8           err;
 160
 161   MSG_WAR(0x3930, "sendPDOrequest ",0);
 162   /* Sending the request only if the cobid have been found on the PDO receive */
 163   /* part dictionary */
 164   offset = d->firstIndex->PDO_RCV;
 165   lastIndex = d->lastIndex->PDO_RCV;
 166   if (offset)
 167           while (offset <= lastIndex) {
 168             /*get the CobId*/
 169             pwCobId = d->objdict[offset].pSubindex[1].pObject;
 170
 171             if ( *pwCobId  == cobId ) {
 172               s_PDO pdo;
 173               pdo.cobId = *pwCobId;
 174               pdo.len = 0;
 175               err  = sendPDO(d, pdo, REQUEST);
 176               return err;
 177             }
 178             offset++;
 179           }
 180   MSG_WAR(0x1931, "sendPDOrequest : COBID not found : ", cobId);
 181   return 0xFF;
 182 }
 183
 184
 185
 186 /***********************************************************************/
 187 UNS8 proceedPDO(CO_Data* d, Message *m)
 188 {
 189   UNS8   numPdo;
 190   UNS8   numMap;  /* Number of the mapped varable */
 191   UNS8 i;
 192   UNS8 *     pMappingCount = NULL;    /* count of mapped objects... */
 193   /* pointer to the var which is mapped to a pdo... */
 194 /*  void *     pMappedAppObject = NULL;   */
 195   /* pointer fo the var which holds the mapping parameter of an mapping entry */
 196   UNS32 *    pMappingParameter = NULL;
 197   UNS8  *    pTransmissionType = NULL; /* pointer to the transmission type */
 198   UNS32 *    pwCobId = NULL;
 199   UNS8       Size;
 200   UNS8       dataType;
 201   UNS8       offset;
 202   UNS8       status;
 203   UNS32      objDict;
 204   UNS16      offsetObjdict;
 205   UNS16      lastIndex;
 206   UNS16      Index;
 207   UNS8       Sindex;
 208   status = state1;
 209
 210   MSG_WAR(0x3935, "proceedPDO, cobID : ", ((*m).cob_id.w & 0x7ff));
 211   offset = 0x00;
 212   numPdo = 0;
 213   numMap = 0;
 214   if((*m).rtr == NOT_A_REQUEST ) { /* The PDO received is not a request. */
 215     offsetObjdict = d->firstIndex->PDO_RCV;
 216     lastIndex = d->lastIndex->PDO_RCV;
 217
 218     /* study of all the PDO stored in the dictionary */
 219     if(offsetObjdict)
 220             while (offsetObjdict <= lastIndex) {
 221
 222               switch( status ) {
 223
 224                 case state1:    /* data are stored in process_var array */
 225                   /* memcpy(&(process_var.data), &m->data, (*m).len); */
 226                   /* Ce memcpy devrait être portable. */
 227                   for ( i = 0 ; i < m->len ; i++)
 228                     d->process_var.data[i] = m->data[i];
 229                   d->process_var.count = (*m).len;
 230
 231                   status = state2;
 232                   break;
 233
 234                 case state2:
 235                   /* get CobId of the dictionary correspondant to the received PDO */
 236                   pwCobId = d->objdict[offsetObjdict].pSubindex[1].pObject;
 237                   /* check the CobId coherance */
 238                   /*pwCobId is the cobId read in the dictionary at the state 3 */
 239                   if ( *pwCobId == (*m).cob_id.w ){
 240                     /* The cobId is recognized */
 241                     status = state4;
 242                     MSG_WAR(0x3936, "cobId found at index ", 0x1400 + numPdo);
 243                     break;
 244                   }
 245                   else {
 246                     /* cobId received does not match with those write in the dictionnary */
 247                     numPdo++;
 248                     offsetObjdict++;
 249                     status = state2;
 250                     break;
 251                   }
 252
 253                 case state4:    /* get mapped objects number */
 254                   /* The cobId of the message received has been found in the dictionnary. */
 255                   offsetObjdict = d->firstIndex->PDO_RCV_MAP;
 256                   lastIndex = d->lastIndex->PDO_RCV_MAP;
 257                   pMappingCount = (d->objdict + offsetObjdict + numPdo)->pSubindex[0].pObject;
 258                   numMap = 0;
 259                   while (numMap < *pMappingCount) {
 260                         UNS8 tmp[]= {0,0,0,0,0,0,0,0};
 261                         UNS8 ByteSize;
 262                     pMappingParameter = (d->objdict + offsetObjdict + numPdo)->pSubindex[numMap + 1].pObject;
 263                     if (pMappingParameter == NULL) {
 264                       MSG_ERR(0x1937, "Couldn't get mapping parameter : ", numMap + 1);
 265                       return 0xFF;
 266                     }
 267                         /* Get the addresse of the mapped variable. */
 268                     /* detail of *pMappingParameter : */
 269                     /* The 16 hight bits contains the index, the medium 8 bits contains the subindex, */
 270                     /* and the lower 8 bits contains the size of the mapped variable. */
 271
 272                     Size = (UNS8)(*pMappingParameter);
 273
 274                         /* copy bit per bit in little endian */
 275                         CopyBits(Size, (UNS8*)&d->process_var.data[offset>>3], offset%8, 0, ((UNS8*)tmp), 0, 0);
 276
 277                         ByteSize = 1 + ((Size - 1) >> 3); /*1->8 => 1 ; 9->16 => 2, ... */
 278
 279                     objDict = setODentry(d, (UNS16)((*pMappingParameter) >> 16),
 280                                             (UNS8)(((*pMappingParameter) >> 8 ) & 0xFF),
 281                                                 tmp, &ByteSize, 0 );
 282
 283                     if(objDict != OD_SUCCESSFUL) {
 284                       MSG_ERR(0x1938, "error accessing to the mapped var : ", numMap + 1);
 285                       MSG_WAR(0x2939, "         Mapped at index : ", (*pMappingParameter) >> 16);
 286                       MSG_WAR(0x2940, "                subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF);
 287                       return 0xFF;
 288                     }
 289
 290                     MSG_WAR(0x3942, "Variable updated with value received by PDO cobid : ", m->cob_id.w);
 291                     MSG_WAR(0x3943, "         Mapped at index : ", (*pMappingParameter) >> 16);
 292                     MSG_WAR(0x3944, "                subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF);
 293                     /* MSG_WAR(0x3945, "                data : ",*((UNS32 *)pMappedAppObject)); */
 294                     offset += Size;
 295                     numMap++;
 296                   } /* end loop while on mapped variables */
 297
 298                   offset=0x00;
 299                   numMap = 0;
 300                   return 0;
 301
 302               }/* end switch status     */
 303             }/* end while       */
 304   }/* end if Donnees */
 305
 306
 307   else if ((*m).rtr == REQUEST ){
 308       MSG_WAR(0x3946, "Receive a PDO request cobId : ", m->cob_id.w);
 309       status = state1;
 310       offsetObjdict = d->firstIndex->PDO_TRS;
 311       lastIndex = d->lastIndex->PDO_TRS;
 312       if(offsetObjdict) while( offsetObjdict  <= lastIndex ){
 313         /* study of all PDO stored in the objects dictionary */
 314
 315         switch( status ){
 316
 317         case state1:    /* check the CobId */
 318                         /* get CobId of the dictionary which match to the received PDO */
 319           pwCobId = (d->objdict + offsetObjdict)->pSubindex[1].pObject;
 320           if ( *pwCobId == (*m).cob_id.w ) {
 321             status = state4;
 322             break;
 323           }
 324           else {
 325             numPdo++;
 326             offsetObjdict++;
 327           }
 328           status = state1;
 329           break;
 330
 331
 332         case state4:    /* check transmission type (after request?) */
 333           pTransmissionType = d->objdict[offsetObjdict].pSubindex[2].pObject;
 334           if ( (*pTransmissionType == TRANS_RTR) || (*pTransmissionType == TRANS_RTR_SYNC ) || (*pTransmissionType == TRANS_EVENT) ) {
 335             status = state5;
 336             break;
 337           }
 338           else {
 339             /* The requested PDO is not to send on request. So, does nothing. */
 340             MSG_WAR(0x2947, "PDO is not to send on request : ", m->cob_id.w);
 341             return 0xFF;
 342           }
 343
 344         case state5:    /* get mapped objects number */
 345           offsetObjdict = d->firstIndex->PDO_TRS_MAP;
 346           lastIndex = d->lastIndex->PDO_TRS_MAP;
 347           pMappingCount = (d->objdict + offsetObjdict + numPdo)->pSubindex[0].pObject;
 348           numMap = 0;
 349           while (numMap < *pMappingCount) {
 350             pMappingParameter = (d->objdict + offsetObjdict + numPdo)->pSubindex[numMap + 1].pObject;
 351             /* Get the mapped variable */
 352             Size = ((UNS8)(((*pMappingParameter) & 0xFF) >> 3));
 353             objDict = getODentry( d, (UNS16)((*pMappingParameter) >> (UNS8)16),
 354                                      (UNS8)(( (*pMappingParameter) >> (UNS8)8 ) & 0xFF),
 355                                      (void *)&d->process_var.data[offset], &Size, &dataType, 0 );
 356             if (objDict != OD_SUCCESSFUL) {
 357               MSG_ERR(0x1948, "error accessing to the mapped var : ", numMap + 1);
 358               MSG_WAR(0x2949, "         Mapped at index : ", (*pMappingParameter) >> 16);
 359               MSG_WAR(0x2950, "                subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF);
 360               return 0xFF;
 361             }
 362             offset += (UNS8) (((*pMappingParameter) & 0xFF) >> 3);
 363             d->process_var.count = offset;
 364             numMap++;
 365
 366           } /* end while */
 367           PDOmGR( d, *pwCobId ); /* Transmit the PDO */
 368           return 0;
 369
 370         }/* end switch status */
 371       }/* end while      */
 372     }/* end if Requete */
 373
 374   return 0;
 375 }
 376
 377
 378 void CopyBits(UNS8 NbBits, UNS8* SrcByteIndex, UNS8 SrcBitIndex, UNS8 SrcBigEndian, UNS8* DestByteIndex, UNS8 DestBitIndex, UNS8 DestBigEndian)
 379 {
 380         //This loop copy as many bits that it can each time, crossing successively bytes
 381         // boundaries from LSB to MSB.
 382         while(NbBits > 0)
 383         {
 384                 // Bit missalignement between src and dest
 385                 INTEGER8 Vect = DestBitIndex - SrcBitIndex;
 386
 387                 // We can now get src and align it to dest
 388                 UNS8 Aligned = Vect>0 ? *SrcByteIndex << Vect : *SrcByteIndex >> -Vect;
 389
 390                 // Compute the nb of bit we will be able to copy
 391                 UNS8 BoudaryLimit = (Vect>0 ? 8 - DestBitIndex :  8 - SrcBitIndex );
 392                 UNS8 BitsToCopy = BoudaryLimit > NbBits ? NbBits : BoudaryLimit;
 393
 394                 // Create a mask that will serve in:
 395                 UNS8 Mask = ((0xff << (DestBitIndex + BitsToCopy)) | (0xff >> (8 - DestBitIndex)));
 396
 397                 // - Filtering src
 398                 UNS8 Filtered = Aligned & ~Mask;
 399
 400                 // - and erase bits where we write, preserve where we don't
 401                 *DestByteIndex &= Mask;
 402
 403                 // Then write.
 404                 *DestByteIndex |= Filtered ;
 405
 406                 //Compute next time cursors for src
 407                 if((SrcBitIndex += BitsToCopy)>7)       // cross boundary ?
 408                 {
 409                         SrcBitIndex = 0;                                                        // First bit
 410                         SrcByteIndex += (SrcBigEndian ? -1 : 1);        // Next byte
 411                 }
 412
 413                 //Compute next time cursors for dest
 414                 if((DestBitIndex += BitsToCopy)>7)
 415                 {
 416                         DestBitIndex = 0;                                                       // First bit
 417                         DestByteIndex += (DestBigEndian ? -1 : 1);// Next byte
 418                 }
 419
 420                 //And decrement counter.
 421                 NbBits -= BitsToCopy;
 422         }
 423
 424 }
 425
 426 #if 0
 427
 428 /*********************************************************************/
 429 /* TODO : reimplement this using CallBacks                           */
 430 /*********************************************************************/
 431
 432 UNS8 sendPDOevent( CO_Data* d, void * variable )
 433 { /* DO NOT USE MSG_ERR because the macro may send a PDO -> infinite loop if it fails.  */
 434   UNS32           objDict = 0;
 435   UNS8            ind, sub_ind;
 436   UNS8            status;
 437   UNS8            offset;
 438   UNS8 *     pMappingCount = NULL;
 439   UNS32 *    pMappingParameter = NULL;
 440   void *     pMappedAppObject = NULL;
 441   UNS8 *     pTransmissionType = NULL; /* pointer to the transmission type */
 442   UNS32 *    pwCobId = NULL;
 443   UNS8 *     pSize;
 444   UNS8       size;
 445   UNS8       dataType;
 446   UNS16      offsetObjdict;
 447   UNS16      offsetObjdictPrm;
 448   UNS16      lastIndex;
 449   UNS8       numMap;
 450   ind     = 0x00;
 451   sub_ind = 1;
 452   offset  = 0x00;
 453   pSize   = &size;
 454   status  = state1;
 455
 456   /* look for the index and subindex where the variable is mapped */
 457   /* Then, send the pdo which contains the variable. */
 458
 459   MSG_WAR (0x3960, "sendPDOevent", 0);
 460   offsetObjdictPrm = d->firstIndex->PDO_TRS;
 461
 462   offsetObjdict = d->firstIndex->PDO_TRS_MAP;
 463   lastIndex = d->lastIndex->PDO_TRS_MAP;
 464
 465   if (offsetObjdictPrm && offsetObjdict)
 466           /* Loop on PDO Transmit */
 467           while(offsetObjdict <= lastIndex){
 468             /* Check the transmission mode */
 469             pTransmissionType = d->objdict[offsetObjdictPrm].pSubindex[2].pObject;
 470             if (*pTransmissionType != TRANS_EVENT) {
 471               ind++;
 472               offsetObjdict++;
 473               offsetObjdictPrm++;
 474               continue;
 475             }
 476             pMappingCount = d->objdict[offsetObjdict].pSubindex[0].pObject;
 477             numMap = 1; /* mapped variable */
 478             while (numMap <= *pMappingCount) {
 479               pMappingParameter = d->objdict[offsetObjdict].pSubindex[numMap].pObject;
 480               /* Get the variable */
 481               objDict = getODentry( d,
 482                                     (UNS16)((*pMappingParameter) >> 16),
 483                                     (UNS8)(( (*pMappingParameter) >> (UNS8)8 ) & (UNS32)0x000000FF),
 484                                     (void * *)&pMappedAppObject, pSize, &dataType, 0 );
 485               if( objDict != OD_SUCCESSFUL ) {
 486                 MSG_WAR(0x2961, "Error in dict. at index : ",
 487                         (*pMappingParameter) >> (UNS8)16);
 488
 489                 MSG_WAR(0x2962, "               subindex : ",
 490                         ((*pMappingParameter) >> (UNS8)8 ) & (UNS32)0x000000FF);
 491                 return 0xFF;
 492               }
 493               if (pMappedAppObject == variable) { // Variable found !
 494                 MSG_WAR(0x3963, "Variable to send found at index : ",
 495                         (*pMappingParameter) >> 16);
 496                 MSG_WAR(0x3964, "                       subIndex : ",
 497                         ((*pMappingParameter) >> 8 ) & 0x000000FF);
 498                 buildPDO(d, 0x1800 + ind);
 499                 /* Get the cobId */
 500                 pwCobId = d->objdict[offsetObjdictPrm].pSubindex[1].pObject;
 501                 PDOmGR( d, *pwCobId ); /* Send the PDO */
 502                 return 0;
 503               }
 504               numMap++;
 505             } /* End loop on mapped variable */
 506             ind++;
 507             offsetObjdict++;
 508             offsetObjdictPrm++;
 509           } /* End loop while on PDO */
 510
 511   MSG_WAR(0x2965, "Variable not found in a PDO to send on event", 0);
 512   return 0xFF;
 513
 514 }
 515 #endif