src/sync.c

   1 /*\r
   2 This file is part of CanFestival, a library implementing CanOpen Stack. \r
   3 \r
   4 Copyright (C): Edouard TISSERANT and Francis DUPIN\r
   5 \r
   6 See COPYING file for copyrights details.\r
   7 \r
   8 This library is free software; you can redistribute it and/or\r
   9 modify it under the terms of the GNU Lesser General Public\r
  10 License as published by the Free Software Foundation; either\r
  11 version 2.1 of the License, or (at your option) any later version.\r
  12 \r
  13 This library is distributed in the hope that it will be useful,\r
  14 but WITHOUT ANY WARRANTY; without even the implied warranty of\r
  15 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU\r
  16 Lesser General Public License for more details.\r
  17 \r
  18 You should have received a copy of the GNU Lesser General Public\r
  19 License along with this library; if not, write to the Free Software\r
  20 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA\r
  21 */\r
  22 \r
  23 /*!\r
  24 ** @file   sync.c\r
  25 ** @author Edouard TISSERANT and Francis DUPIN\r
  26 ** @date   Tue Jun  5 09:32:32 2007\r
  27 **\r
  28 ** @brief\r
  29 **\r
  30 **\r
  31 */\r
  32 \r
  33 #include "data.h"\r
  34 #include "sync.h"\r
  35 #include "canfestival.h"\r
  36 \r
  37 /* Prototypes for internals functions */\r
  38 \r
  39 /*!                                                                                                \r
  40 **                                                                                                 \r
  41 **                                                                                                 \r
  42 ** @param d                                                                                        \r
  43 ** @param id                                                                                       \r
  44 **/  \r
  45 void SyncAlarm(CO_Data* d, UNS32 id);\r
  46 UNS32 OnCOB_ID_SyncUpdate(CO_Data* d, const indextable * unsused_indextable, \r
  47         UNS8 unsused_bSubindex);\r
  48 \r
  49 /*!                                                                                                \r
  50 **                                                                                                 \r
  51 **                                                                                                 \r
  52 ** @param d                                                                                        \r
  53 ** @param id                                                                                       \r
  54 **/   \r
  55 void SyncAlarm(CO_Data* d, UNS32 id)\r
  56 {\r
  57         sendSYNC(d, *d->COB_ID_Sync & 0x1FFFFFFF) ;\r
  58 }\r
  59 \r
  60 /*!                                                                                                \r
  61 ** This is called when Index 0x1005 is updated.                                                                                                \r
  62 **                                                                                                 \r
  63 ** @param d                                                                                        \r
  64 ** @param unsused_indextable                                                                       \r
  65 ** @param unsused_bSubindex                                                                        \r
  66 **                                                                                                 \r
  67 ** @return                                                                                         \r
  68 **/  \r
  69 UNS32 OnCOB_ID_SyncUpdate(CO_Data* d, const indextable * unsused_indextable, UNS8 unsused_bSubindex)\r
  70 {\r
  71         startSYNC(d);\r
  72         return 0;\r
  73 }\r
  74 \r
  75 /*!                                                                                                \r
  76 **                                                                                                 \r
  77 **                                                                                                 \r
  78 ** @param d                                                                                        \r
  79 **/ \r
  80 void startSYNC(CO_Data* d)\r
  81 {\r
  82         RegisterSetODentryCallBack(d, 0x1005, 0, &OnCOB_ID_SyncUpdate);\r
  83         RegisterSetODentryCallBack(d, 0x1006, 0, &OnCOB_ID_SyncUpdate);\r
  84 \r
  85         if(d->syncTimer != TIMER_NONE){\r
  86                 stopSYNC(d);\r
  87         }\r
  88         \r
  89         if(*d->COB_ID_Sync & 0x40000000 && *d->Sync_Cycle_Period)\r
  90         {\r
  91                 d->syncTimer = SetAlarm(\r
  92                                 d,\r
  93                                 0 /*No id needed*/,\r
  94                                 &SyncAlarm,\r
  95                                 US_TO_TIMEVAL(*d->Sync_Cycle_Period), \r
  96                                 US_TO_TIMEVAL(*d->Sync_Cycle_Period));\r
  97         }\r
  98 }\r
  99 \r
 100 /*!                                                                                                \r
 101 **                                                                                                 \r
 102 **                                                                                                 \r
 103 ** @param d                                                                                        \r
 104 **/   \r
 105 void stopSYNC(CO_Data* d)\r
 106 {\r
 107         d->syncTimer = DelAlarm(d->syncTimer);\r
 108 }\r
 109 \r
 110 /*!                                                                                                \r
 111 **                                                                                                 \r
 112 **                                                                                                 \r
 113 ** @param d                                                                                        \r
 114 ** @param cob_id                                                                                   \r
 115 **                                                                                                 \r
 116 ** @return                                                                                         \r
 117 **/  \r
 118 UNS8 sendSYNC(CO_Data* d, UNS32 cob_id)\r
 119 {\r
 120   Message m;\r
 121   UNS8 resultat ;\r
 122   \r
 123   MSG_WAR(0x3001, "sendSYNC ", 0);\r
 124   \r
 125   m.cob_id.w = cob_id ;\r
 126   m.rtr = NOT_A_REQUEST;\r
 127   m.len = 0;\r
 128   resultat = canSend(d->canHandle,&m) ;\r
 129   proceedSYNC(d, &m) ; \r
 130   return resultat ;\r
 131 }\r
 132 \r
 133 /*!                                                                                                \r
 134 **                                                                                                 \r
 135 **                                                                                                 \r
 136 ** @param d                                                                                        \r
 137 ** @param m                                                                                        \r
 138 **                                                                                                 \r
 139 ** @return                                                                                         \r
 140 **/ \r
 141 UNS8 proceedSYNC(CO_Data* d, Message *m)\r
 142 {\r
 143 \r
 144   UNS8  pdoNum,       /* number of the actual processed pdo-nr. */\r
 145         prp_j;\r
 146 \r
 147   const UNS8 *     pMappingCount = NULL;      /* count of mapped objects...*/\r
 148   /** pointer to the var which is mapped to a pdo */\r
 149   /* void *     pMappedAppObject = NULL; */\r
 150   /** pointer fo the var which holds the mapping parameter of an mapping entry  */\r
 151   UNS32 *    pMappingParameter = NULL;  \r
 152   /** pointer to the transmissiontype...*/\r
 153   UNS8 *     pTransmissionType = NULL;  \r
 154   UNS32 *    pwCobId = NULL;    \r
 155 \r
 156   UNS8 dataType;\r
 157   UNS16 index;\r
 158   UNS8 subIndex;\r
 159   UNS8 offset;\r
 160   UNS8 status;\r
 161   UNS8 Size;\r
 162   UNS32 objDict;        \r
 163   UNS16 offsetObjdict;\r
 164   UNS16 offsetObjdictMap;\r
 165   UNS16 lastIndex;\r
 166   \r
 167   status = state3;\r
 168   pdoNum = 0x00;\r
 169   prp_j = 0x00;\r
 170   offset = 0x00;\r
 171   \r
 172   MSG_WAR(0x3002, "SYNC received. Proceed. ", 0);\r
 173   \r
 174   (*d->post_sync)();\r
 175 \r
 176   /** only operational state allows PDO transmission */\r
 177   if( d->nodeState != Operational ) \r
 178     return 0;\r
 179   \r
 180   /** So, the node is in operational state */\r
 181   /** study all PDO stored in the objects dictionary */ \r
 182  \r
 183   offsetObjdict = d->firstIndex->PDO_TRS;\r
 184   lastIndex = d->lastIndex->PDO_TRS;\r
 185   offsetObjdictMap = d->firstIndex->PDO_TRS_MAP;\r
 186   \r
 187   if(offsetObjdict) while( offsetObjdict <= lastIndex) {  \r
 188     switch( status ) {\r
 189                     \r
 190     case state3:    /** get the PDO transmission type */\r
 191       if (d->objdict[offsetObjdict].bSubCount <= 2) {\r
 192           MSG_ERR(0x1004, "Subindex 2  not found at index ", 0x1800 + pdoNum);\r
 193           return 0xFF;\r
 194         }\r
 195       pTransmissionType = d->objdict[offsetObjdict].pSubindex[2].pObject;    \r
 196       MSG_WAR(0x3005, "Reading PDO at index : ", 0x1800 + pdoNum);\r
 197       status = state4; \r
 198       break;     \r
 199     case state4:        /** check if transmission type is after (this) SYNC */\r
 200                         /** The message may not be transmited every SYNC but every n SYNC */      \r
 201       if( (*pTransmissionType >= TRANS_SYNC_MIN) && (*pTransmissionType <= TRANS_SYNC_MAX) &&\r
 202           (++d->count_sync[pdoNum] == *pTransmissionType) ) {   \r
 203         d->count_sync[pdoNum] = 0;\r
 204         MSG_WAR(0x3007, "  PDO is on SYNCHRO. Trans type : ", *pTransmissionType);\r
 205         status = state5;\r
 206         break;\r
 207       }\r
 208       else {\r
 209         MSG_WAR(0x3008, "  Not on synchro or not at this SYNC. Trans type : ", \r
 210                 *pTransmissionType);\r
 211         pdoNum++;\r
 212         offsetObjdict++;\r
 213         offsetObjdictMap++;\r
 214         status = state11;\r
 215         break;\r
 216       }      \r
 217     case state5:        /** get PDO CobId */\r
 218         pwCobId = d->objdict[offsetObjdict].pSubindex[1].pObject;     \r
 219         MSG_WAR(0x3009, "  PDO CobId is : ", *pwCobId);\r
 220         status = state7;\r
 221         break;     \r
 222     case state7:  /** get mapped objects number to transmit with this PDO */\r
 223       pMappingCount = d->objdict[offsetObjdictMap].pSubindex[0].pObject;\r
 224         MSG_WAR(0x300D, "  Number of objects mapped : ",*pMappingCount );\r
 225         status = state8;\r
 226     case state8:        /** get mapping parameters */\r
 227       pMappingParameter = d->objdict[offsetObjdictMap].pSubindex[prp_j + 1].pObject;\r
 228         MSG_WAR(0x300F, "  got mapping parameter : ", *pMappingParameter);\r
 229         MSG_WAR(0x3050, "    at index : ", 0x1A00 + pdoNum);\r
 230         MSG_WAR(0x3051, "    sub-index : ", prp_j + 1);\r
 231         status = state9;\r
 232     \r
 233     case state9:        /** get data to transmit */ \r
 234         {\r
 235           UNS8 ByteSize;\r
 236           UNS8 tmp[]= {0,0,0,0,0,0,0,0};\r
 237           index = (UNS16)((*pMappingParameter) >> 16);\r
 238           subIndex = (UNS8)(( (*pMappingParameter) >> (UNS8)8 ) & (UNS32)0x000000FF);\r
 239           Size = (UNS8)(*pMappingParameter); /* Size in bits */\r
 240           ByteSize = 1 + ((Size - 1) >> 3); /*1->8 => 1 ; 9->16 => 2, ... */\r
 241           objDict = getODentry(d, index, subIndex, tmp, &ByteSize, &dataType, 0 );\r
 242           /** copy bit per bit in little endian*/\r
 243           CopyBits(Size, ((UNS8*)tmp), 0 , 0, (UNS8*)&d->process_var.data[offset>>3], offset%8, 0);\r
 244         }   \r
 245         if( objDict != OD_SUCCESSFUL ){\r
 246           MSG_ERR(0x1013, " Couldn't find mapped variable at index-subindex-size : ", (UNS16)(*pMappingParameter));\r
 247           return 0xFF;\r
 248         }\r
 249         \r
 250         offset += Size ;\r
 251         d->process_var.count = 1 + ((offset - 1) >> 3);\r
 252         prp_j++;\r
 253         status = state10;        \r
 254         break;                                  \r
 255       \r
 256     case state10:       /** loop to get all the data to transmit */\r
 257       if( prp_j < *pMappingCount ){\r
 258         MSG_WAR(0x3014, "  next variable mapped : ", prp_j);\r
 259         status = state8;\r
 260         break;\r
 261       }\r
 262       else {\r
 263         MSG_WAR(0x3015, "  End scan mapped variable", 0);\r
 264         PDOmGR( d, *pwCobId );  \r
 265         MSG_WAR(0x3016, "  End of this pdo. Should have been sent", 0);\r
 266         pdoNum++;\r
 267         offsetObjdict++;\r
 268         offsetObjdictMap++;\r
 269         offset = 0x00;\r
 270         prp_j = 0x00;\r
 271         status = state11;\r
 272         break;\r
 273       }\r
 274       \r
 275     case state11:     \r
 276       MSG_WAR(0x3017, "next pdo index : ", pdoNum);\r
 277       status = state3;\r
 278       break;\r
 279       \r
 280     default:\r
 281       MSG_ERR(0x1019,"Unknown state has been reached : %d",status);\r
 282       return 0xFF;\r
 283     }/* end switch case */\r
 284     \r
 285   }/* end while( prp_i<dict_cstes.max_count_of_PDO_transmit ) */\r
 286    \r
 287   (*d->post_TPDO)();\r
 288 \r
 289   return 0;\r
 290 }\r
 291 \r
 292 \r
 293 void _post_sync(){}\r
 294 void _post_TPDO(){}\r