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