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 /**************************************************************************/
  84 UNS8 buildPDO(CO_Data* d, UNS16 index)
  85 { /* DO NOT USE MSG_ERR because the macro may send a PDO -> infinite loop if it fails. */
  86   UNS16 ind;
  87   UNS8      subInd;
  88
  89   UNS8 *     pMappingCount = NULL;      /* count of mapped objects... */
  90   /* pointer to the var which is mapped to a pdo */
  91 /*  void *     pMappedAppObject = NULL;  */
  92   /* pointer fo the var which holds the mapping parameter of an mapping entry  */
  93   UNS32 *    pMappingParameter = NULL;
  94
  95   UNS8      Size;
  96   UNS8      dataType;
  97   UNS8      offset;
  98   UNS16     offsetObjdict;
  99   UNS16     offsetObjdictPrm;
 100   UNS32     objDict;
 101
 102   subInd=(UNS8)0x00;
 103   offset = 0x00;
 104   ind = index - 0x1800;
 105
 106   MSG_WAR(0x3910,"Prepare PDO to send index :", index);
 107
 108   /* only operational state allows PDO transmission */
 109   if( d->nodeState != Operational ) {
 110     MSG_WAR(0x2911, "Unable to send the PDO (node not in OPERATIONAL mode). Node : ", index);
 111     return 0xFF;
 112   }
 113   offsetObjdictPrm = d->firstIndex->PDO_TRS;
 114   offsetObjdict = d->firstIndex->PDO_TRS_MAP;
 115
 116   if (offsetObjdictPrm && offsetObjdict)
 117   {
 118           /* get mapped objects number to transmit with this PDO */
 119           pMappingCount = (d->objdict + offsetObjdict + ind)->pSubindex[0].pObject;
 120           MSG_WAR(0x3912, "Nb maped objects : ",* pMappingCount);
 121           MSG_WAR(0x3913, "        at index : ", 0x1A00 + ind);
 122           while (subInd < *pMappingCount) { /* Loop on mapped variables */
 123             /* get mapping parameters */
 124             pMappingParameter = (d->objdict + offsetObjdict + ind)->pSubindex[subInd + 1].pObject;
 125             MSG_WAR(0x3914, "Get the mapping      at index : ", (UNS16)0x1A00 + ind);
 126             MSG_WAR(0x3915, "                     subIndex : ", subInd + 1);
 127             MSG_WAR(0x3916, "                     value    : ", *(UNS32 *)pMappingParameter);
 128             /* Get the mapped variable */
 129              Size = ((UNS8)(((*pMappingParameter) & 0xFF) >> 3));
 130              objDict = getODentry(d, (UNS16)((*pMappingParameter) >> 16),
 131                                     (UNS8)(((*pMappingParameter) >> 8 ) & 0x000000FF),
 132                                     (void *)&d->process_var.data[offset], &Size, &dataType, 0 );
 133
 134              if (objDict != OD_SUCCESSFUL) {
 135                 MSG_WAR(0x2919, "error accessing to the mapped var : ", subInd + 1);
 136                 MSG_WAR(0x2920, "         Mapped at index : ", (*pMappingParameter) >> 16);
 137                 MSG_WAR(0x2921, "                subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF);
 138                 return 0xFF;
 139              }
 140
 141               offset += Size;
 142               d->process_var.count = offset;
 143               subInd++;
 144         }/* end Loop on mapped variables  */
 145   }
 146   return 0;
 147 }
 148
 149 /**************************************************************************/
 150 UNS8 sendPDOrequest( CO_Data* d, UNS32 cobId )
 151 {
 152   UNS32 *        pwCobId;
 153   UNS16          offset;
 154   UNS16          lastIndex;
 155   UNS8           err;
 156
 157   MSG_WAR(0x3930, "sendPDOrequest ",0);
 158   /* Sending the request only if the cobid have been found on the PDO receive */
 159   /* part dictionary */
 160   offset = d->firstIndex->PDO_RCV;
 161   lastIndex = d->lastIndex->PDO_RCV;
 162   if (offset)
 163           while (offset <= lastIndex) {
 164             /*get the CobId*/
 165             pwCobId = d->objdict[offset].pSubindex[1].pObject;
 166
 167             if ( *pwCobId  == cobId ) {
 168               s_PDO pdo;
 169               pdo.cobId = *pwCobId;
 170               pdo.len = 0;
 171               err  = sendPDO(d, pdo, REQUEST);
 172               return err;
 173             }
 174             offset++;
 175           }
 176   MSG_WAR(0x1931, "sendPDOrequest : COBID not found : ", cobId);
 177   return 0xFF;
 178 }
 179
 180
 181
 182 /***********************************************************************/
 183 UNS8 proceedPDO(CO_Data* d, Message *m)
 184 {
 185   UNS8   numPdo;
 186   UNS8   numMap;  /* Number of the mapped varable */
 187   UNS8 i;
 188   UNS8 *     pMappingCount = NULL;    /* count of mapped objects... */
 189   /* pointer to the var which is mapped to a pdo... */
 190 /*  void *     pMappedAppObject = NULL;   */
 191   /* pointer fo the var which holds the mapping parameter of an mapping entry */
 192   UNS32 *    pMappingParameter = NULL;
 193   UNS8  *    pTransmissionType = NULL; /* pointer to the transmission type */
 194   UNS32 *    pwCobId = NULL;
 195   UNS8       Size;
 196   UNS8       dataType;
 197   UNS8       offset;
 198   UNS8       status;
 199   UNS32      objDict;
 200   UNS16      offsetObjdict;
 201   UNS16      lastIndex;
 202   status = state1;
 203
 204   MSG_WAR(0x3935, "proceedPDO, cobID : ", ((*m).cob_id.w & 0x7ff));
 205   offset = 0x00;
 206   numPdo = 0;
 207   numMap = 0;
 208   if((*m).rtr == NOT_A_REQUEST ) { /* The PDO received is not a request. */
 209     offsetObjdict = d->firstIndex->PDO_RCV;
 210     lastIndex = d->lastIndex->PDO_RCV;
 211
 212     /* study of all the PDO stored in the dictionary */
 213     if(offsetObjdict)
 214             while (offsetObjdict <= lastIndex) {
 215
 216               switch( status ) {
 217
 218                 case state1:    /* data are stored in process_var array */
 219                   /* memcpy(&(process_var.data), &m->data, (*m).len); */
 220                   /* Ce memcpy devrait être portable. */
 221                   for ( i = 0 ; i < m->len ; i++)
 222                     d->process_var.data[i] = m->data[i];
 223                   d->process_var.count = (*m).len;
 224
 225                   status = state2;
 226                   break;
 227
 228                 case state2:
 229                   /* get CobId of the dictionary correspondant to the received PDO */
 230                   pwCobId = d->objdict[offsetObjdict].pSubindex[1].pObject;
 231                   /* check the CobId coherance */
 232                   /*pwCobId is the cobId read in the dictionary at the state 3 */
 233                   if ( *pwCobId == (*m).cob_id.w ){
 234                     /* The cobId is recognized */
 235                     status = state4;
 236                     MSG_WAR(0x3936, "cobId found at index ", 0x1400 + numPdo);
 237                     break;
 238                   }
 239                   else {
 240                     /* cobId received does not match with those write in the dictionnary */
 241                     numPdo++;
 242                     offsetObjdict++;
 243                     status = state2;
 244                     break;
 245                   }
 246
 247                 case state4:    /* get mapped objects number */
 248                   /* The cobId of the message received has been found in the dictionnary. */
 249                   offsetObjdict = d->firstIndex->PDO_RCV_MAP;
 250                   lastIndex = d->lastIndex->PDO_RCV_MAP;
 251                   pMappingCount = (d->objdict + offsetObjdict + numPdo)->pSubindex[0].pObject;
 252                   numMap = 0;
 253                   while (numMap < *pMappingCount) {
 254                     pMappingParameter = (d->objdict + offsetObjdict + numPdo)->pSubindex[numMap + 1].pObject;
 255                     if (pMappingParameter == NULL) {
 256                       MSG_ERR(0x1937, "Couldn't get mapping parameter : ", numMap + 1);
 257                       return 0xFF;
 258                     }
 259                     /* Get the addresse of the mapped variable. */
 260                     /* detail of *pMappingParameter : */
 261                     /* The 16 hight bits contains the index, the medium 8 bits contains the subindex, */
 262                     /* and the lower 8 bits contains the size of the mapped variable. */
 263
 264                     Size = ((UNS8)(((*pMappingParameter) & 0xFF) >> 3));
 265
 266                     objDict = setODentry(d, (UNS16)((*pMappingParameter) >> 16),
 267                                             (UNS8)(((*pMappingParameter) >> 8 ) & 0xFF),
 268                                             (void *)&d->process_var.data[offset], &Size, 0 );
 269
 270                     if(objDict != OD_SUCCESSFUL) {
 271                       MSG_ERR(0x1938, "error accessing to the mapped var : ", numMap + 1);
 272                       MSG_WAR(0x2939, "         Mapped at index : ", (*pMappingParameter) >> 16);
 273                       MSG_WAR(0x2940, "                subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF);
 274                       return 0xFF;
 275                     }
 276
 277                     MSG_WAR(0x3942, "Variable updated with value received by PDO cobid : ", m->cob_id.w);
 278                     MSG_WAR(0x3943, "         Mapped at index : ", (*pMappingParameter) >> 16);
 279                     MSG_WAR(0x3944, "                subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF);
 280                     /* MSG_WAR(0x3945, "                data : ",*((UNS32 *)pMappedAppObject)); */
 281                     offset += Size;
 282                     numMap++;
 283                   } /* end loop while on mapped variables */
 284
 285                   offset=0x00;
 286                   numMap = 0;
 287                   return 0;
 288
 289               }/* end switch status     */
 290             }/* end while       */
 291   }/* end if Donnees */
 292
 293
 294   else if ((*m).rtr == REQUEST ){
 295       MSG_WAR(0x3946, "Receive a PDO request cobId : ", m->cob_id.w);
 296       status = state1;
 297       offsetObjdict = d->firstIndex->PDO_TRS;
 298       lastIndex = d->lastIndex->PDO_TRS;
 299       if(offsetObjdict) while( offsetObjdict  <= lastIndex ){
 300         /* study of all PDO stored in the objects dictionary */
 301
 302         switch( status ){
 303
 304         case state1:    /* check the CobId */
 305                         /* get CobId of the dictionary which match to the received PDO */
 306           pwCobId = (d->objdict + offsetObjdict)->pSubindex[1].pObject;
 307           if ( *pwCobId == (*m).cob_id.w ) {
 308             status = state4;
 309             break;
 310           }
 311           else {
 312             numPdo++;
 313             offsetObjdict++;
 314           }
 315           status = state1;
 316           break;
 317
 318
 319         case state4:    /* check transmission type (after request?) */
 320           pTransmissionType = d->objdict[offsetObjdict].pSubindex[2].pObject;
 321           if ( (*pTransmissionType == TRANS_RTR) || (*pTransmissionType == TRANS_RTR_SYNC ) || (*pTransmissionType == TRANS_EVENT) ) {
 322             status = state5;
 323             break;
 324           }
 325           else {
 326             /* The requested PDO is not to send on request. So, does nothing. */
 327             MSG_WAR(0x2947, "PDO is not to send on request : ", m->cob_id.w);
 328             return 0xFF;
 329           }
 330
 331         case state5:    /* get mapped objects number */
 332           offsetObjdict = d->firstIndex->PDO_TRS_MAP;
 333           lastIndex = d->lastIndex->PDO_TRS_MAP;
 334           pMappingCount = (d->objdict + offsetObjdict + numPdo)->pSubindex[0].pObject;
 335           numMap = 0;
 336           while (numMap < *pMappingCount) {
 337             pMappingParameter = (d->objdict + offsetObjdict + numPdo)->pSubindex[numMap + 1].pObject;
 338             /* Get the mapped variable */
 339             Size = ((UNS8)(((*pMappingParameter) & 0xFF) >> 3));
 340             objDict = getODentry( d, (UNS16)((*pMappingParameter) >> (UNS8)16),
 341                                      (UNS8)(( (*pMappingParameter) >> (UNS8)8 ) & 0xFF),
 342                                      (void *)&d->process_var.data[offset], &Size, &dataType, 0 );
 343             if (objDict != OD_SUCCESSFUL) {
 344               MSG_ERR(0x1948, "error accessing to the mapped var : ", numMap + 1);
 345               MSG_WAR(0x2949, "         Mapped at index : ", (*pMappingParameter) >> 16);
 346               MSG_WAR(0x2950, "                subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF);
 347               return 0xFF;
 348             }
 349             offset += (UNS8) (((*pMappingParameter) & 0xFF) >> 3);
 350             d->process_var.count = offset;
 351             numMap++;
 352
 353           } /* end while */
 354           PDOmGR( d, *pwCobId ); /* Transmit the PDO */
 355           return 0;
 356
 357         }/* end switch status */
 358       }/* end while      */
 359     }/* end if Requete */
 360
 361   return 0;
 362 }
 363
 364
 365
 366
 367 #if 0
 368
 369 /*********************************************************************/
 370 /* TODO : reimplement this using CallBacks                           */
 371 /*********************************************************************/
 372
 373 UNS8 sendPDOevent( CO_Data* d, void * variable )
 374 { /* DO NOT USE MSG_ERR because the macro may send a PDO -> infinite loop if it fails.  */
 375   UNS32           objDict = 0;
 376   UNS8            ind, sub_ind;
 377   UNS8            status;
 378   UNS8            offset;
 379   UNS8 *     pMappingCount = NULL;
 380   UNS32 *    pMappingParameter = NULL;
 381   void *     pMappedAppObject = NULL;
 382   UNS8 *     pTransmissionType = NULL; /* pointer to the transmission type */
 383   UNS32 *    pwCobId = NULL;
 384   UNS8 *     pSize;
 385   UNS8       size;
 386   UNS8       dataType;
 387   UNS16      offsetObjdict;
 388   UNS16      offsetObjdictPrm;
 389   UNS16      lastIndex;
 390   UNS8       numMap;
 391   ind     = 0x00;
 392   sub_ind = 1;
 393   offset  = 0x00;
 394   pSize   = &size;
 395   status  = state1;
 396
 397   /* look for the index and subindex where the variable is mapped */
 398   /* Then, send the pdo which contains the variable. */
 399
 400   MSG_WAR (0x3960, "sendPDOevent", 0);
 401   offsetObjdictPrm = d->firstIndex->PDO_TRS;
 402
 403   offsetObjdict = d->firstIndex->PDO_TRS_MAP;
 404   lastIndex = d->lastIndex->PDO_TRS_MAP;
 405
 406   if (offsetObjdictPrm && offsetObjdict)
 407           /* Loop on PDO Transmit */
 408           while(offsetObjdict <= lastIndex){
 409             /* Check the transmission mode */
 410             pTransmissionType = d->objdict[offsetObjdictPrm].pSubindex[2].pObject;
 411             if (*pTransmissionType != TRANS_EVENT) {
 412               ind++;
 413               offsetObjdict++;
 414               offsetObjdictPrm++;
 415               continue;
 416             }
 417             pMappingCount = d->objdict[offsetObjdict].pSubindex[0].pObject;
 418             numMap = 1; /* mapped variable */
 419             while (numMap <= *pMappingCount) {
 420               pMappingParameter = d->objdict[offsetObjdict].pSubindex[numMap].pObject;
 421               /* Get the variable */
 422               objDict = getODentry( d,
 423                                     (UNS16)((*pMappingParameter) >> 16),
 424                                     (UNS8)(( (*pMappingParameter) >> (UNS8)8 ) & (UNS32)0x000000FF),
 425                                     (void * *)&pMappedAppObject, pSize, &dataType, 0 );
 426               if( objDict != OD_SUCCESSFUL ) {
 427                 MSG_WAR(0x2961, "Error in dict. at index : ",
 428                         (*pMappingParameter) >> (UNS8)16);
 429
 430                 MSG_WAR(0x2962, "               subindex : ",
 431                         ((*pMappingParameter) >> (UNS8)8 ) & (UNS32)0x000000FF);
 432                 return 0xFF;
 433               }
 434               if (pMappedAppObject == variable) { // Variable found !
 435                 MSG_WAR(0x3963, "Variable to send found at index : ",
 436                         (*pMappingParameter) >> 16);
 437                 MSG_WAR(0x3964, "                       subIndex : ",
 438                         ((*pMappingParameter) >> 8 ) & 0x000000FF);
 439                 buildPDO(d, 0x1800 + ind);
 440                 /* Get the cobId */
 441                 pwCobId = d->objdict[offsetObjdictPrm].pSubindex[1].pObject;
 442                 PDOmGR( d, *pwCobId ); /* Send the PDO */
 443                 return 0;
 444               }
 445               numMap++;
 446             } /* End loop on mapped variable */
 447             ind++;
 448             offsetObjdict++;
 449             offsetObjdictPrm++;
 450           } /* End loop while on PDO */
 451
 452   MSG_WAR(0x2965, "Variable not found in a PDO to send on event", 0);
 453   return 0xFF;
 454
 455 }
 456 #endif