src/sdo.c
changeset 71 95cd3376cc9f
parent 53 73bc47a0db55
child 82 81505425b1e5
equal deleted inserted replaced
70:f36f09f08b62 71:95cd3376cc9f
    18 You should have received a copy of the GNU Lesser General Public
    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
    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
    20 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
    21 */
    21 */
    22 
    22 
    23 //#define DEBUG_WAR_CONSOLE_ON
    23 /* #define DEBUG_WAR_CONSOLE_ON */
    24 //#define DEBUG_ERR_CONSOLE_ON
    24 /* #define DEBUG_ERR_CONSOLE_ON */
    25 
    25 
    26 #include "objacces.h"
    26 #include "objacces.h"
    27 #include "sdo.h"
    27 #include "sdo.h"
    28 
    28 
       
    29 /* Uncomment if your compiler does not support inline functions */
       
    30  #define NO_INLINE 
       
    31 
       
    32 #ifdef NO_INLINE
       
    33   #define INLINE 
       
    34 #else
       
    35 	#define INLINE inline
       
    36 #endif
       
    37 
    29 /*Internals prototypes*/
    38 /*Internals prototypes*/
    30 
    39 
    31 /** Called by writeNetworkDict */
    40 /** Called by writeNetworkDict */
    32 inline UNS8 _writeNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, 
    41 INLINE UNS8 _writeNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, 
    33 		       UNS8 subIndex, UNS8 count, UNS8 dataType, void *data, SDOCallback_t Callback);
    42 		       UNS8 subIndex, UNS8 count, UNS8 dataType, void *data, SDOCallback_t Callback);
    34 
    43 
    35 /** Called by readNetworkDict */
    44 /** Called by readNetworkDict */
    36 inline UNS8 _readNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, 
    45 INLINE UNS8 _readNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, 
    37 	UNS8 dataType, SDOCallback_t Callback);
    46 	UNS8 dataType, SDOCallback_t Callback);
    38 	
    47 	
    39 
    48 
    40 /***************************************************************************/
    49 /***************************************************************************/
    41 // SDO (un)packing macros
    50 /* SDO (un)packing macros */
    42 
    51 
    43 /** Returns the command specifier (cs, ccs, scs) from the first byte of the SDO   
    52 /** Returns the command specifier (cs, ccs, scs) from the first byte of the SDO   
    44  */
    53  */
    45 #define getSDOcs(byte) (byte >> 5)
    54 #define getSDOcs(byte) (byte >> 5)
    46 
    55 
    72  */
    81  */
    73 #define getSDOindex(byte1, byte2) ((byte2 << 8) | (byte1))
    82 #define getSDOindex(byte1, byte2) ((byte2 << 8) | (byte1))
    74 
    83 
    75 /** Returns the subIndex from the byte 3 of the SDO   
    84 /** Returns the subIndex from the byte 3 of the SDO   
    76  */
    85  */
       
    86 
    77 #define getSDOsubIndex(byte3) (byte3)
    87 #define getSDOsubIndex(byte3) (byte3)
    78 
    88 
    79 /***************************************************************************
    89 /***************************************************************************
    80 **
    90 **
    81 */
    91 */
    83 {
    93 {
    84     MSG_ERR(0x1A01, "SDO timeout. SDO response not received.", 0);
    94     MSG_ERR(0x1A01, "SDO timeout. SDO response not received.", 0);
    85     MSG_WAR(0x2A02, "server node : ", d->transfers[id].nodeId);
    95     MSG_WAR(0x2A02, "server node : ", d->transfers[id].nodeId);
    86     MSG_WAR(0x2A02, "      index : ", d->transfers[id].index);
    96     MSG_WAR(0x2A02, "      index : ", d->transfers[id].index);
    87     MSG_WAR(0x2A02, "   subIndex : ", d->transfers[id].subIndex); 
    97     MSG_WAR(0x2A02, "   subIndex : ", d->transfers[id].subIndex); 
    88     // Reset timer handler
    98     /* Reset timer handler */
    89     d->transfers[id].timer = TIMER_NONE;
    99     d->transfers[id].timer = TIMER_NONE;
    90     // Call the user function to inform of the problem.
   100     /* Call the user function to inform of the problem.*/
    91     (*d->SDOtimeoutError)(id);
   101     (*d->SDOtimeoutError)(id);
    92     // Sending a SDO abort
   102     /* Sending a SDO abort */
    93     sendSDOabort(d, d->transfers[id].whoami, 
   103     sendSDOabort(d, d->transfers[id].whoami, 
    94 		 d->transfers[id].index, d->transfers[id].subIndex, SDOABT_TIMED_OUT);
   104 		 d->transfers[id].index, d->transfers[id].subIndex, SDOABT_TIMED_OUT);
    95     // Reset the line
   105     /* Reset the line*/
    96     resetSDOline(d, id);
   106     resetSDOline(d, id);
    97 }
   107 }
    98 
   108 
    99 #define StopSDO_TIMER(id) \
   109 #define StopSDO_TIMER(id) \
   100 MSG_WAR(0x3A05, "StopSDO_TIMER for line : ", line);\
   110 MSG_WAR(0x3A05, "StopSDO_TIMER for line : ", line);\
   155     return errorCode;
   165     return errorCode;
   156 
   166 
   157   d->transfers[line].count = size;
   167   d->transfers[line].count = size;
   158   d->transfers[line].offset = 0;
   168   d->transfers[line].offset = 0;
   159 #if 0
   169 #if 0
   160   // Me laisser ça, please ! (FD)
   170    /*Me laisser ça, please ! (FD)*/
   161   {
   171   {
   162     UNS8 i;
   172     UNS8 i;
   163     for (i = 0 ; i < 10 ; i++) {
   173     for (i = 0 ; i < 10 ; i++) {
   164       MSG_WAR(i, "data= ", d->transfers[line].data[i]);
   174       MSG_WAR(i, "data= ", d->transfers[line].data[i]);
   165     }     
   175     }     
   211 		UNS8 subIndex, UNS32 abortCode)
   221 		UNS8 subIndex, UNS32 abortCode)
   212 {
   222 {
   213   UNS8 err;
   223   UNS8 err;
   214   UNS8 line;
   224   UNS8 line;
   215   err = getSDOlineOnUse( d, nodeId, whoami, &line );
   225   err = getSDOlineOnUse( d, nodeId, whoami, &line );
   216   if (!err) // If a line on use have been found.
   226   if (!err) /* If a line on use have been found.*/
   217     MSG_WAR(0x3A20, "FailedSDO : line found : ", line);
   227     MSG_WAR(0x3A20, "FailedSDO : line found : ", line);
   218   if ((! err) && (whoami == SDO_SERVER)) {
   228   if ((! err) && (whoami == SDO_SERVER)) {
   219     resetSDOline( d, line );
   229     resetSDOline( d, line );
   220     MSG_WAR(0x3A21, "FailedSDO : line released : ", line);
   230     MSG_WAR(0x3A21, "FailedSDO : line released : ", line);
   221   }
   231   }
   272   for (i = 0 ; i < SDO_MAX_SIMULTANEOUS_TRANSFERTS ; i++){
   282   for (i = 0 ; i < SDO_MAX_SIMULTANEOUS_TRANSFERTS ; i++){
   273     if ( d->transfers[i].state == SDO_RESET ) {
   283     if ( d->transfers[i].state == SDO_RESET ) {
   274       *line = i;
   284       *line = i;
   275       d->transfers[i].whoami = whoami;
   285       d->transfers[i].whoami = whoami;
   276       return 0;
   286       return 0;
   277     } // end if
   287     } /* end if */
   278   } // end for
   288   } /* end for */
   279   MSG_ERR(0x1A25, "Too many SDO in progress. Aborted.", i);
   289   MSG_ERR(0x1A25, "Too many SDO in progress. Aborted.", i);
   280   return 0xFF;
   290   return 0xFF;
   281 }
   291 }
   282 
   292 
   283 /***************************************************************************/
   293 /***************************************************************************/
   312 }
   322 }
   313 
   323 
   314 /***************************************************************************/
   324 /***************************************************************************/
   315 UNS8 getSDOlineRestBytes (CO_Data* d, UNS8 line, UNS8 * nbBytes)
   325 UNS8 getSDOlineRestBytes (CO_Data* d, UNS8 line, UNS8 * nbBytes)
   316 {
   326 {
   317   if (d->transfers[line].count == 0) // if received initiate SDO protocol with e=0 and s=0
   327   if (d->transfers[line].count == 0) /* if received initiate SDO protocol with e=0 and s=0 */
   318     * nbBytes = 0;
   328     * nbBytes = 0;
   319   else
   329   else
   320     * nbBytes = d->transfers[line].count - d->transfers[line].offset;
   330     * nbBytes = d->transfers[line].count - d->transfers[line].offset;
   321   return 0;
   331   return 0;
   322 }
   332 }
   396     pwCobId = d->objdict[offset].pSubindex[1].pObject;
   406     pwCobId = d->objdict[offset].pSubindex[1].pObject;
   397   }
   407   }
   398   /* message copy for sending */
   408   /* message copy for sending */
   399   m.cob_id.w = *pwCobId;
   409   m.cob_id.w = *pwCobId;
   400   m.rtr = NOT_A_REQUEST; 
   410   m.rtr = NOT_A_REQUEST; 
   401   //the length of SDO must be 8
   411   /* the length of SDO must be 8 */
   402   m.len = 8;
   412   m.len = 8;
   403   for (i = 0 ; i < 8 ; i++) {
   413   for (i = 0 ; i < 8 ; i++) {
   404     m.data[i] =  sdo.body.data[i];
   414     m.data[i] =  sdo.body.data[i];
   405   }
   415   }
   406   return (*d->canSend)(&m);
   416   return (*d->canSend)(&m);
   412   s_SDO sdo;
   422   s_SDO sdo;
   413   UNS8 ret;
   423   UNS8 ret;
   414   MSG_WAR(0x2A50,"Sending SDO abort ", abortCode);
   424   MSG_WAR(0x2A50,"Sending SDO abort ", abortCode);
   415   sdo.nodeId = *d->bDeviceNodeId;
   425   sdo.nodeId = *d->bDeviceNodeId;
   416   sdo.body.data[0] = 0x80;
   426   sdo.body.data[0] = 0x80;
   417   // Index
   427   /* Index */
   418   sdo.body.data[1] = index & 0xFF; // LSB
   428   sdo.body.data[1] = index & 0xFF; /* LSB */
   419   sdo.body.data[2] = (index >> 8) & 0xFF; // MSB
   429   sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */
   420   // Subindex
   430   /* Subindex */
   421   sdo.body.data[3] = subIndex;
   431   sdo.body.data[3] = subIndex;
   422   // Data
   432   /* Data */
   423   sdo.body.data[4] = (UNS8)(abortCode & 0xFF);
   433   sdo.body.data[4] = (UNS8)(abortCode & 0xFF);
   424   sdo.body.data[5] = (UNS8)((abortCode >> 8) & 0xFF);
   434   sdo.body.data[5] = (UNS8)((abortCode >> 8) & 0xFF);
   425   sdo.body.data[6] = (UNS8)((abortCode >> 16) & 0xFF);
   435   sdo.body.data[6] = (UNS8)((abortCode >> 16) & 0xFF);
   426   sdo.body.data[7] = (UNS8)((abortCode >> 24) & 0xFF);
   436   sdo.body.data[7] = (UNS8)((abortCode >> 24) & 0xFF);
   427   ret = sendSDO(d, whoami, sdo);
   437   ret = sendSDO(d, whoami, sdo);
   432 /***************************************************************************/
   442 /***************************************************************************/
   433 UNS8 proceedSDO (CO_Data* d, Message *m)
   443 UNS8 proceedSDO (CO_Data* d, Message *m)
   434 {
   444 {
   435   UNS8 err;
   445   UNS8 err;
   436   UNS8 line;
   446   UNS8 line;
   437   UNS8 nbBytes; // received or to be transmited.
   447   UNS8 nbBytes; /* received or to be transmited. */
   438   UNS8 nodeId = 0;  // The node from which the SDO is received
   448   UNS8 nodeId = 0;  /* The node from which the SDO is received */
   439   UNS8 *pNodeId = NULL;
   449   UNS8 *pNodeId = NULL;
   440   UNS8 whoami = SDO_UNKNOWN;  // SDO_SERVER or SDO_CLIENT.
   450   UNS8 whoami = SDO_UNKNOWN;  /* SDO_SERVER or SDO_CLIENT.*/
   441   UNS32 errorCode; // while reading or writing in the local object dictionary.
   451   UNS32 errorCode; /* while reading or writing in the local object dictionary.*/
   442   s_SDO sdo;    // SDO to transmit
   452   s_SDO sdo;    /* SDO to transmit */
   443   UNS16 index;
   453   UNS16 index;
   444   UNS8 subIndex;
   454   UNS8 subIndex;
   445   UNS32 abortCode;
   455   UNS32 abortCode;
   446   UNS8 i,j;
   456   UNS8 i,j;
   447   UNS32 *     pCobId = NULL;
   457   UNS32 *     pCobId = NULL;
   452   
   462   
   453   pSize = &size; 
   463   pSize = &size; 
   454 
   464 
   455   MSG_WAR(0x3A60, "proceedSDO ", 0);
   465   MSG_WAR(0x3A60, "proceedSDO ", 0);
   456   whoami = SDO_UNKNOWN;
   466   whoami = SDO_UNKNOWN;
   457   // Looking for the cobId in the object dictionary.
   467   /* Looking for the cobId in the object dictionary. */
   458   // Am-I a server ?
   468   /* Am-I a server ? */
   459   offset = d->firstIndex->SDO_SVR;
   469   offset = d->firstIndex->SDO_SVR;
   460   lastIndex = d->lastIndex->SDO_SVR;
   470   lastIndex = d->lastIndex->SDO_SVR;
   461   j = 0;
   471   j = 0;
   462   if(offset) while (offset <= lastIndex) {
   472   if(offset) while (offset <= lastIndex) {
   463      if (d->objdict[offset].bSubCount <= 1) {
   473      if (d->objdict[offset].bSubCount <= 1) {
   466 	}
   476 	}
   467       pCobId = d->objdict[offset].pSubindex[1].pObject;
   477       pCobId = d->objdict[offset].pSubindex[1].pObject;
   468       if ( *pCobId == (*m).cob_id.w ) {
   478       if ( *pCobId == (*m).cob_id.w ) {
   469 	whoami = SDO_SERVER;
   479 	whoami = SDO_SERVER;
   470 	MSG_WAR(0x3A62, "proceedSDO. I am server. index : ", 0x1200 + j);
   480 	MSG_WAR(0x3A62, "proceedSDO. I am server. index : ", 0x1200 + j);
   471 	// In case of server, the node id of the client may be unknown. So we put the index minus offset
   481 	/* In case of server, the node id of the client may be unknown. So we put the index minus offset */
   472 	// 0x1200 where the cobid received is defined.
   482 	/* 0x1200 where the cobid received is defined. */
   473 	nodeId = j;
   483 	nodeId = j;
   474 	break;
   484 	break;
   475       }
   485       }
   476       j++;
   486       j++;
   477       offset++;
   487       offset++;
   478   } // end while
   488   } /* end while */
   479   if (whoami == SDO_UNKNOWN) {
   489   if (whoami == SDO_UNKNOWN) {
   480     // Am-I client ?
   490     /* Am-I client ? */
   481     offset = d->firstIndex->SDO_CLT;
   491     offset = d->firstIndex->SDO_CLT;
   482     lastIndex = d->lastIndex->SDO_CLT;
   492     lastIndex = d->lastIndex->SDO_CLT;
   483     j = 0;
   493     j = 0;
   484     if(offset) while (offset <= lastIndex) {
   494     if(offset) while (offset <= lastIndex) {
   485        if (d->objdict[offset].bSubCount <= 3) {
   495        if (d->objdict[offset].bSubCount <= 3) {
   486 	 MSG_ERR(0x1A63, "Subindex 3  not found at index ", 0x1280 + j);
   496 	 MSG_ERR(0x1A63, "Subindex 3  not found at index ", 0x1280 + j);
   487 	 return 0xFF;
   497 	 return 0xFF;
   488        }
   498        }
   489        // a) Looking for the cobid received.
   499        /* a) Looking for the cobid received. */
   490        pCobId = d->objdict[offset].pSubindex[2].pObject;
   500        pCobId = d->objdict[offset].pSubindex[2].pObject;
   491        if (*pCobId == (*m).cob_id.w ) {
   501        if (*pCobId == (*m).cob_id.w ) {
   492 	 // b) cobid found, so reading the node id of the server.
   502 	 /* b) cobid found, so reading the node id of the server. */
   493 	 pNodeId = d->objdict[offset].pSubindex[3].pObject;
   503 	 pNodeId = d->objdict[offset].pSubindex[3].pObject;
   494 	 whoami = SDO_CLIENT;
   504 	 whoami = SDO_CLIENT;
   495 	 nodeId = *pNodeId;
   505 	 nodeId = *pNodeId;
   496 	 MSG_WAR(0x3A64, "proceedSDO. I am server. index : ", 0x1280 + j);
   506 	 MSG_WAR(0x3A64, "proceedSDO. I am server. index : ", 0x1280 + j);
   497 	 MSG_WAR(0x3A65, "                 Server nodeId : ", nodeId);
   507 	 MSG_WAR(0x3A65, "                 Server nodeId : ", nodeId);
   498 	 break;
   508 	 break;
   499 	}
   509 	}
   500        j++;
   510        j++;
   501        offset++;
   511        offset++;
   502     } // end while
   512     } /* end while */
   503   }
   513   }
   504   if (whoami == SDO_UNKNOWN) {
   514   if (whoami == SDO_UNKNOWN) {
   505     return 0xFF;// This SDO was not for us !
   515     return 0xFF;/* This SDO was not for us ! */
   506   }
   516   }
   507 
   517 
   508   // Test if the size of the SDO is ok
   518   /* Test if the size of the SDO is ok */
   509   if ( (*m).len != 8) {
   519   if ( (*m).len != 8) {
   510     MSG_ERR(0x1A67, "Error size SDO. CobId  : ", (*m).cob_id.w);
   520     MSG_ERR(0x1A67, "Error size SDO. CobId  : ", (*m).cob_id.w);
   511     failedSDO(d, nodeId, whoami, 0, 0, SDOABT_GENERAL_ERROR);
   521     failedSDO(d, nodeId, whoami, 0, 0, SDOABT_GENERAL_ERROR);
   512     return 0xFF;
   522     return 0xFF;
   513   }
   523   }
   517   }
   527   }
   518   else {
   528   else {
   519     MSG_WAR(0x3A69, "I am SERVER. Received SDO cobId : ", (*m).cob_id.w);
   529     MSG_WAR(0x3A69, "I am SERVER. Received SDO cobId : ", (*m).cob_id.w);
   520   }
   530   }
   521     
   531     
   522   // Testing the command specifier
   532   /* Testing the command specifier */
   523   // Allowed : cs = 0, 1, 2, 3, 4. (=  all except those for block tranfert).
   533   /* Allowed : cs = 0, 1, 2, 3, 4. (=  all except those for block tranfert). */
   524   // cs = other : Not allowed -> abort.
   534   /* cs = other : Not allowed -> abort. */
   525   switch (getSDOcs(m->data[0])) {
   535   switch (getSDOcs(m->data[0])) {
   526 
   536 
   527   case 0:
   537   case 0:
   528     // I am SERVER
   538     /* I am SERVER */
   529     if (whoami == SDO_SERVER) {
   539     if (whoami == SDO_SERVER) {
   530       // Receiving a download segment data.
   540       /* Receiving a download segment data. */
   531       // A SDO transfert should have been yet initiated.
   541       /* A SDO transfert should have been yet initiated. */
   532       err = getSDOlineOnUse( d, nodeId, whoami, &line ); 
   542       err = getSDOlineOnUse( d, nodeId, whoami, &line ); 
   533       if (!err)
   543       if (!err)
   534 	err = d->transfers[line].state != SDO_DOWNLOAD_IN_PROGRESS;
   544 	err = d->transfers[line].state != SDO_DOWNLOAD_IN_PROGRESS;
   535       if (err) {
   545       if (err) {
   536 	MSG_ERR(0x1A70, "SDO error : Received download segment for unstarted trans. index 0x1200 + ", 
   546 	MSG_ERR(0x1A70, "SDO error : Received download segment for unstarted trans. index 0x1200 + ", 
   537 		nodeId); 
   547 		nodeId); 
   538 	failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
   548 	failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
   539 	return 0xFF;
   549 	return 0xFF;
   540       }
   550       }
   541       // Reset the wathdog
   551       /* Reset the wathdog */
   542       RestartSDO_TIMER(line)
   552       RestartSDO_TIMER(line)
   543       MSG_WAR(0x3A71, "Received SDO download segment defined at index 0x1200 + ", nodeId); 
   553       MSG_WAR(0x3A71, "Received SDO download segment defined at index 0x1200 + ", nodeId); 
   544       index = d->transfers[line].index;
   554       index = d->transfers[line].index;
   545       subIndex = d->transfers[line].subIndex;
   555       subIndex = d->transfers[line].subIndex;
   546       // Toggle test.
   556       /* Toggle test. */
   547       if (d->transfers[line].toggle != getSDOt(m->data[0])) {
   557       if (d->transfers[line].toggle != getSDOt(m->data[0])) {
   548 	MSG_ERR(0x1A72, "SDO error : Toggle error : ", getSDOt(m->data[0])); 
   558 	MSG_ERR(0x1A72, "SDO error : Toggle error : ", getSDOt(m->data[0])); 
   549 	failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED);
   559 	failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED);
   550 	return 0xFF;
   560 	return 0xFF;
   551       }
   561       }
   552       // Nb of data to be downloaded
   562       /* Nb of data to be downloaded */
   553       nbBytes = 7 - getSDOn3(m->data[0]);
   563       nbBytes = 7 - getSDOn3(m->data[0]);
   554       // Store the data in the transfert structure.
   564       /* Store the data in the transfert structure. */
   555       err = SDOtoLine(d, line, nbBytes, (*m).data + 1);
   565       err = SDOtoLine(d, line, nbBytes, (*m).data + 1);
   556       if (err) {
   566       if (err) {
   557 	failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
   567 	failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
   558 	return 0xFF;
   568 	return 0xFF;
   559       }
   569       }
   560       // Sending the SDO response, CS = 1
   570       /* Sending the SDO response, CS = 1 */
   561       sdo.nodeId = *d->bDeviceNodeId; // The node id of the server, (here it is the sender).
   571       sdo.nodeId = *d->bDeviceNodeId; /* The node id of the server, (here it is the sender). */
   562       sdo.body.data[0] = (1 << 5) | (d->transfers[line].toggle << 4);
   572       sdo.body.data[0] = (1 << 5) | (d->transfers[line].toggle << 4);
   563       for (i = 1 ; i < 8 ; i++)
   573       for (i = 1 ; i < 8 ; i++)
   564 	sdo.body.data[i] = 0;
   574 	sdo.body.data[i] = 0;
   565       MSG_WAR(0x3A73, "SDO. Send response to download request defined at index 0x1200 + ", nodeId); 
   575       MSG_WAR(0x3A73, "SDO. Send response to download request defined at index 0x1200 + ", nodeId); 
   566       sendSDO(d, whoami, sdo);
   576       sendSDO(d, whoami, sdo);
   567       // Inverting the toggle for the next segment.
   577       /* Inverting the toggle for the next segment. */
   568       d->transfers[line].toggle = ! d->transfers[line].toggle & 1;
   578       d->transfers[line].toggle = ! d->transfers[line].toggle & 1;
   569       // If it was the last segment,
   579       /* If it was the last segment, */
   570       if (getSDOc(m->data[0])) {
   580       if (getSDOc(m->data[0])) {
   571 	// Transfering line data to object dictionary.
   581 	/* Transfering line data to object dictionary. */
   572 	// The code does not use the "d" of initiate frame. So it is safe if e=s=0
   582 	/* The code does not use the "d" of initiate frame. So it is safe if e=s=0 */
   573 	errorCode = SDOlineToObjdict(d, line);
   583 	errorCode = SDOlineToObjdict(d, line);
   574 	if (errorCode) {
   584 	if (errorCode) {
   575 	  MSG_ERR(0x1A54, "SDO error : Unable to copy the data in the object dictionary", 0); 
   585 	  MSG_ERR(0x1A54, "SDO error : Unable to copy the data in the object dictionary", 0); 
   576 	  failedSDO(d, nodeId, whoami, index, subIndex, errorCode);
   586 	  failedSDO(d, nodeId, whoami, index, subIndex, errorCode);
   577 	  return 0xFF;	  
   587 	  return 0xFF;	  
   578 	}
   588 	}
   579 	// Release of the line
   589 	/* Release of the line */
   580 	resetSDOline(d, line);
   590 	resetSDOline(d, line);
   581 	MSG_WAR(0x3A74, "SDO. End of download defined at index 0x1200 + ", nodeId); 
   591 	MSG_WAR(0x3A74, "SDO. End of download defined at index 0x1200 + ", nodeId); 
   582       }
   592       }
   583     } // end if SERVER
   593     } /* end if SERVER */
   584     else { // if CLIENT
   594     else { /* if CLIENT */
   585       // I am CLIENT
   595       /* I am CLIENT */
   586       // It is a request for a previous upload segment. We should find a line opened for this.
   596       /* It is a request for a previous upload segment. We should find a line opened for this.*/
   587       err = getSDOlineOnUse( d, nodeId, whoami, &line);
   597       err = getSDOlineOnUse( d, nodeId, whoami, &line);
   588       if (!err)
   598       if (!err)
   589 	err = d->transfers[line].state != SDO_UPLOAD_IN_PROGRESS;
   599 	err = d->transfers[line].state != SDO_UPLOAD_IN_PROGRESS;
   590       if (err) {
   600       if (err) {
   591 	MSG_ERR(0x1A75, "SDO error : Received segment response for unknown trans. from nodeId", nodeId); 
   601 	MSG_ERR(0x1A75, "SDO error : Received segment response for unknown trans. from nodeId", nodeId); 
   592 	failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
   602 	failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
   593 	return 0xFF;
   603 	return 0xFF;
   594       }
   604       }
   595       // Reset the wathdog
   605       /* Reset the wathdog */
   596       RestartSDO_TIMER(line)
   606       RestartSDO_TIMER(line)
   597       index = d->transfers[line].index;
   607       index = d->transfers[line].index;
   598       subIndex = d->transfers[line].subIndex;
   608       subIndex = d->transfers[line].subIndex;
   599       // test of the toggle;
   609       /* test of the toggle; */
   600       if (d->transfers[line].toggle != getSDOt(m->data[0])) {
   610       if (d->transfers[line].toggle != getSDOt(m->data[0])) {
   601 	MSG_ERR(0x1A76, "SDO error : Received segment response Toggle error. from nodeId", nodeId); 
   611 	MSG_ERR(0x1A76, "SDO error : Received segment response Toggle error. from nodeId", nodeId); 
   602 	failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED);
   612 	failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED);
   603 	return 0xFF;
   613 	return 0xFF;
   604       }
   614       }
   605       // nb of data to be uploaded
   615       /* nb of data to be uploaded */
   606       nbBytes = 7 - getSDOn3(m->data[0]);
   616       nbBytes = 7 - getSDOn3(m->data[0]);
   607       // Storing the data in the line structure.
   617       /* Storing the data in the line structure. */
   608       err = SDOtoLine(d, line, nbBytes, (*m).data + 1);
   618       err = SDOtoLine(d, line, nbBytes, (*m).data + 1);
   609       if (err) {
   619       if (err) {
   610 	failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
   620 	failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
   611 	return 0xFF;
   621 	return 0xFF;
   612       }
   622       }
   613       // Inverting the toggle for the next segment.
   623       /* Inverting the toggle for the next segment. */
   614       d->transfers[line].toggle = ! d->transfers[line].toggle & 1;
   624       d->transfers[line].toggle = ! d->transfers[line].toggle & 1;
   615       // If it was the last segment,
   625       /* If it was the last segment,*/
   616       if ( getSDOc(m->data[0])) {
   626       if ( getSDOc(m->data[0])) {
   617 	// Put in state finished
   627 	/* Put in state finished */
   618 	// The code is safe for the case e=s=0 in initiate frame.
   628 	/* The code is safe for the case e=s=0 in initiate frame. */
   619 	StopSDO_TIMER(line)
   629 	StopSDO_TIMER(line)
   620 	d->transfers[line].state = SDO_FINISHED;
   630 	d->transfers[line].state = SDO_FINISHED;
   621 	if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId);
   631 	if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId);
   622 	
   632 	
   623 	MSG_WAR(0x3A77, "SDO. End of upload from node : ", nodeId);
   633 	MSG_WAR(0x3A77, "SDO. End of upload from node : ", nodeId);
   624       }
   634       }
   625       else { // more segments to receive
   635       else { /* more segments to receive */
   626 	     // Sending the request for the next segment.
   636 	     /* Sending the request for the next segment. */
   627 	sdo.nodeId = nodeId;
   637 	sdo.nodeId = nodeId;
   628 	sdo.body.data[0] = (3 << 5) | (d->transfers[line].toggle << 4);
   638 	sdo.body.data[0] = (3 << 5) | (d->transfers[line].toggle << 4);
   629 	for (i = 1 ; i < 8 ; i++)
   639 	for (i = 1 ; i < 8 ; i++)
   630 	  sdo.body.data[i] = 0;
   640 	  sdo.body.data[i] = 0;
   631 	sendSDO(d, whoami, sdo);
   641 	sendSDO(d, whoami, sdo);
   632 	MSG_WAR(0x3A78, "SDO send upload segment request to nodeId", nodeId);
   642 	MSG_WAR(0x3A78, "SDO send upload segment request to nodeId", nodeId);
   633       }            
   643       }            
   634     } // End if CLIENT
   644     } /* End if CLIENT */
   635     break;
   645     break;
   636 
   646 
   637   case 1:
   647   case 1:
   638     // I am SERVER
   648     /* I am SERVER */
   639     // Receive of an initiate download
   649     /* Receive of an initiate download */
   640     if (whoami == SDO_SERVER) {
   650     if (whoami == SDO_SERVER) {
   641       index = getSDOindex(m->data[1],m->data[2]);
   651       index = getSDOindex(m->data[1],m->data[2]);
   642       subIndex = getSDOsubIndex(m->data[3]);
   652       subIndex = getSDOsubIndex(m->data[3]);
   643       MSG_WAR(0x3A79, "Received SDO Initiate Download (to store data) defined at index 0x1200 + ", 
   653       MSG_WAR(0x3A79, "Received SDO Initiate Download (to store data) defined at index 0x1200 + ", 
   644 	      nodeId); 
   654 	      nodeId); 
   645       MSG_WAR(0x3A80, "Writing at index : ", index);
   655       MSG_WAR(0x3A80, "Writing at index : ", index);
   646       MSG_WAR(0x3A80, "Writing at subIndex : ", subIndex);
   656       MSG_WAR(0x3A80, "Writing at subIndex : ", subIndex);
   647       
   657       
   648       // Search if a SDO transfert have been yet initiated
   658       /* Search if a SDO transfert have been yet initiated */
   649       err = getSDOlineOnUse( d, nodeId, whoami, &line );
   659       err = getSDOlineOnUse( d, nodeId, whoami, &line );
   650       if (! err) {
   660       if (! err) {
   651 	MSG_ERR(0x1A81, "SDO error : Transmission yet started.", 0); 
   661 	MSG_ERR(0x1A81, "SDO error : Transmission yet started.", 0); 
   652 	failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR);
   662 	failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR);
   653 	return 0xFF;
   663 	return 0xFF;
   654       }
   664       }
   655       // No line on use. Great !
   665       /* No line on use. Great ! */
   656       // Try to open a new line.
   666       /* Try to open a new line. */
   657       err = getSDOfreeLine( d, whoami, &line );
   667       err = getSDOfreeLine( d, whoami, &line );
   658       if (err) {
   668       if (err) {
   659 	MSG_ERR(0x1A82, "SDO error : No line free, too many SDO in progress. Aborted.", 0);
   669 	MSG_ERR(0x1A82, "SDO error : No line free, too many SDO in progress. Aborted.", 0);
   660 	failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR);
   670 	failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR);
   661 	return 0xFF;
   671 	return 0xFF;
   662       }
   672       }
   663       initSDOline(d, line, nodeId, index, subIndex, SDO_DOWNLOAD_IN_PROGRESS);      
   673       initSDOline(d, line, nodeId, index, subIndex, SDO_DOWNLOAD_IN_PROGRESS);      
   664 
   674 
   665       if (getSDOe(m->data[0])) { // If SDO expedited
   675       if (getSDOe(m->data[0])) { /* If SDO expedited */
   666 	// nb of data to be downloaded
   676 	/* nb of data to be downloaded */
   667 	nbBytes = 4 - getSDOn2(m->data[0]);
   677 	nbBytes = 4 - getSDOn2(m->data[0]);
   668 	// Storing the data in the line structure.
   678 	/* Storing the data in the line structure. */
   669 	d->transfers[line].count = nbBytes;
   679 	d->transfers[line].count = nbBytes;
   670 	err = SDOtoLine(d, line, nbBytes, (*m).data + 4);
   680 	err = SDOtoLine(d, line, nbBytes, (*m).data + 4);
   671 	
   681 	
   672 	if (err) {
   682 	if (err) {
   673 	  failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
   683 	  failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
   674 	  return 0xFF;
   684 	  return 0xFF;
   675 	}	  
   685 	}	  
   676 
   686 
   677 	// SDO expedited -> transfert finished. Data can be stored in the dictionary.
   687 	/* SDO expedited -> transfert finished. Data can be stored in the dictionary. */
   678 	// The line will be reseted when it is downloading in the dictionary.
   688 	/* The line will be reseted when it is downloading in the dictionary. */
   679 	MSG_WAR(0x3A83, "SDO Initiate Download is an expedited transfert. Finished.: ", nodeId);
   689 	MSG_WAR(0x3A83, "SDO Initiate Download is an expedited transfert. Finished.: ", nodeId);
   680 	// Transfering line data to object dictionary.
   690 	/* Transfering line data to object dictionary. */
   681 	errorCode = SDOlineToObjdict(d, line);
   691 	errorCode = SDOlineToObjdict(d, line);
   682 	if (errorCode) {
   692 	if (errorCode) {
   683 	  MSG_ERR(0x1A84, "SDO error : Unable to copy the data in the object dictionary", 0); 
   693 	  MSG_ERR(0x1A84, "SDO error : Unable to copy the data in the object dictionary", 0); 
   684 	  failedSDO(d, nodeId, whoami, index, subIndex, errorCode);
   694 	  failedSDO(d, nodeId, whoami, index, subIndex, errorCode);
   685 	  return 0xFF;
   695 	  return 0xFF;
   686 	}
   696 	}
   687 	// Release of the line.
   697 	/* Release of the line. */
   688 	resetSDOline(d, line);
   698 	resetSDOline(d, line);
   689       }
   699       }
   690       else {// So, if it is not an expedited transfert
   700       else {/* So, if it is not an expedited transfert */
   691 	if (getSDOs(m->data[0])) {
   701 	if (getSDOs(m->data[0])) {
   692 	  // TODO : if e and s = 0, not reading m->data[4] but put nbBytes = 0
   702 	  /* TODO : if e and s = 0, not reading m->data[4] but put nbBytes = 0 */
   693 	  nbBytes = m->data[4]; // Transfert limited to 255 bytes.
   703 	  nbBytes = m->data[4]; /* Transfert limited to 255 bytes. */
   694 	  err = setSDOlineRestBytes(d, nodeId, nbBytes);
   704 	  err = setSDOlineRestBytes(d, nodeId, nbBytes);
   695 	  if (err) {
   705 	  if (err) {
   696 	    failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
   706 	    failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
   697 	    return 0xFF;
   707 	    return 0xFF;
   698 	  }	
   708 	  }	
   699 	}
   709 	}
   700       }
   710       }
   701       //Sending a SDO, cs=3
   711       /*Sending a SDO, cs=3*/
   702       sdo.nodeId = *d->bDeviceNodeId; // The node id of the server, (here it is the sender).
   712       sdo.nodeId = *d->bDeviceNodeId; /* The node id of the server, (here it is the sender).*/
   703       sdo.body.data[0] = 3 << 5;
   713       sdo.body.data[0] = 3 << 5;
   704       sdo.body.data[1] = index & 0xFF;        // LSB
   714       sdo.body.data[1] = index & 0xFF;        /* LSB */
   705       sdo.body.data[2] = (index >> 8) & 0xFF; // MSB 
   715       sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */
   706       sdo.body.data[3] = subIndex;
   716       sdo.body.data[3] = subIndex;
   707       for (i = 4 ; i < 8 ; i++)
   717       for (i = 4 ; i < 8 ; i++)
   708 		sdo.body.data[i] = 0;
   718 		sdo.body.data[i] = 0;
   709       sendSDO(d, whoami, sdo);
   719       sendSDO(d, whoami, sdo);
   710     } // end if I am SERVER
   720     } /* end if I am SERVER */
   711     else {
   721     else {
   712       // I am CLIENT
   722       /* I am CLIENT */
   713       // It is a response for a previous download segment. We should find a line opened for this.
   723       /* It is a response for a previous download segment. We should find a line opened for this. */
   714       err = getSDOlineOnUse( d, nodeId, whoami, &line);
   724       err = getSDOlineOnUse( d, nodeId, whoami, &line);
   715       if (!err)
   725       if (!err)
   716 	err = d->transfers[line].state != SDO_DOWNLOAD_IN_PROGRESS;
   726 	err = d->transfers[line].state != SDO_DOWNLOAD_IN_PROGRESS;
   717       if (err) {
   727       if (err) {
   718 	MSG_ERR(0x1A85, "SDO error : Received segment response for unknown trans. from nodeId", nodeId); 
   728 	MSG_ERR(0x1A85, "SDO error : Received segment response for unknown trans. from nodeId", nodeId); 
   719 	failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
   729 	failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
   720 	return 0xFF;
   730 	return 0xFF;
   721       }
   731       }
   722       // Reset the wathdog
   732       /* Reset the wathdog */
   723       RestartSDO_TIMER(line)
   733       RestartSDO_TIMER(line)
   724       index = d->transfers[line].index;
   734       index = d->transfers[line].index;
   725       subIndex = d->transfers[line].subIndex;
   735       subIndex = d->transfers[line].subIndex;
   726       // test of the toggle;
   736       /* test of the toggle; */
   727       if (d->transfers[line].toggle != getSDOt(m->data[0])) {
   737       if (d->transfers[line].toggle != getSDOt(m->data[0])) {
   728 	MSG_ERR(0x1A86, "SDO error : Received segment response Toggle error. from nodeId", nodeId); 
   738 	MSG_ERR(0x1A86, "SDO error : Received segment response Toggle error. from nodeId", nodeId); 
   729 	failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED);
   739 	failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED);
   730 	return 0xFF;
   740 	return 0xFF;
   731       }
   741       }
   732 
   742 
   733       // End transmission or downloading next segment. We need to know if it will be the last one.
   743       /* End transmission or downloading next segment. We need to know if it will be the last one. */
   734       getSDOlineRestBytes(d, line, &nbBytes);
   744       getSDOlineRestBytes(d, line, &nbBytes);
   735       if (nbBytes == 0) {
   745       if (nbBytes == 0) {
   736 	MSG_WAR(0x3A87, "SDO End download. segment response received. OK. from nodeId", nodeId); 
   746 	MSG_WAR(0x3A87, "SDO End download. segment response received. OK. from nodeId", nodeId); 
   737 	StopSDO_TIMER(line)
   747 	StopSDO_TIMER(line)
   738 	d->transfers[line].state = SDO_FINISHED;
   748 	d->transfers[line].state = SDO_FINISHED;
   739 	if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId);
   749 	if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId);
   740 	return 0x00;
   750 	return 0x00;
   741       }
   751       }
   742       // At least one transfer to send.	  
   752       /* At least one transfer to send.	*/
   743       if (nbBytes > 7) {
   753       if (nbBytes > 7) {
   744 	// several segments to download.
   754 	/* several segments to download.*/
   745 	// code to send the next segment. (cs = 0; c = 0)
   755 	/* code to send the next segment. (cs = 0; c = 0) */
   746 	d->transfers[line].toggle = ! d->transfers[line].toggle & 1;
   756 	d->transfers[line].toggle = ! d->transfers[line].toggle & 1;
   747 	sdo.nodeId = nodeId; // The server node Id;
   757 	sdo.nodeId = nodeId; /* The server node Id; */
   748 	sdo.body.data[0] = (d->transfers[line].toggle << 4);
   758 	sdo.body.data[0] = (d->transfers[line].toggle << 4);
   749 	err = lineToSDO(d, line, 7, sdo.body.data + 1);	 
   759 	err = lineToSDO(d, line, 7, sdo.body.data + 1);	 
   750 	if (err) {
   760 	if (err) {
   751 	  failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
   761 	  failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
   752 	  return 0xFF;
   762 	  return 0xFF;
   753 	}
   763 	}
   754       } 
   764       } 
   755       else {
   765       else {
   756 	// Last segment.
   766 	/* Last segment. */
   757 	// code to send the last segment. (cs = 0; c = 1)
   767 	/* code to send the last segment. (cs = 0; c = 1)*/
   758 	d->transfers[line].toggle = ! d->transfers[line].toggle & 1;
   768 	d->transfers[line].toggle = ! d->transfers[line].toggle & 1;
   759 	sdo.nodeId = nodeId; // The server node Id;
   769 	sdo.nodeId = nodeId; /* The server node Id; */
   760 	sdo.body.data[0] = (d->transfers[line].toggle << 4) | ((7 - nbBytes) << 1) | 1;
   770 	sdo.body.data[0] = (d->transfers[line].toggle << 4) | ((7 - nbBytes) << 1) | 1;
   761 	err = lineToSDO(d, line, nbBytes, sdo.body.data + 1);	 
   771 	err = lineToSDO(d, line, nbBytes, sdo.body.data + 1);	 
   762 	if (err) {
   772 	if (err) {
   763 	  failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
   773 	  failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
   764 	  return 0xFF;
   774 	  return 0xFF;
   766 	for (i = nbBytes + 1 ; i < 8 ; i++)
   776 	for (i = nbBytes + 1 ; i < 8 ; i++)
   767 	  sdo.body.data[i] = 0;
   777 	  sdo.body.data[i] = 0;
   768       }
   778       }
   769       MSG_WAR(0x3A88, "SDO sending download segment to nodeId", nodeId); 
   779       MSG_WAR(0x3A88, "SDO sending download segment to nodeId", nodeId); 
   770       sendSDO(d, whoami, sdo); 
   780       sendSDO(d, whoami, sdo); 
   771     } // end if I am a CLIENT			  
   781     } /* end if I am a CLIENT */			  
   772     break;
   782     break;
   773 
   783 
   774   case 2:
   784   case 2:
   775     // I am SERVER
   785     /* I am SERVER */
   776     // Receive of an initiate upload.
   786     /* Receive of an initiate upload.*/
   777     if (whoami == SDO_SERVER) {
   787     if (whoami == SDO_SERVER) {
   778       index = getSDOindex(m->data[1],m->data[2]);
   788       index = getSDOindex(m->data[1],m->data[2]);
   779       subIndex = getSDOsubIndex(m->data[3]);
   789       subIndex = getSDOsubIndex(m->data[3]);
   780       MSG_WAR(0x3A89, "Received SDO Initiate upload (to send data) defined at index 0x1200 + ", 
   790       MSG_WAR(0x3A89, "Received SDO Initiate upload (to send data) defined at index 0x1200 + ", 
   781 	      nodeId); 
   791 	      nodeId); 
   782       MSG_WAR(0x3A90, "Reading at index : ", index);
   792       MSG_WAR(0x3A90, "Reading at index : ", index);
   783       MSG_WAR(0x3A91, "Reading at subIndex : ", subIndex);
   793       MSG_WAR(0x3A91, "Reading at subIndex : ", subIndex);
   784       // Search if a SDO transfert have been yet initiated
   794       /* Search if a SDO transfert have been yet initiated*/
   785       err = getSDOlineOnUse( d, nodeId, whoami, &line );
   795       err = getSDOlineOnUse( d, nodeId, whoami, &line );
   786       if (! err) {
   796       if (! err) {
   787 	    MSG_ERR(0x1A92, "SDO error : Transmission yet started at line : ", line); 
   797 	    MSG_ERR(0x1A92, "SDO error : Transmission yet started at line : ", line); 
   788         MSG_WAR(0x3A93, "nodeId = ", nodeId); 
   798         MSG_WAR(0x3A93, "nodeId = ", nodeId); 
   789 	    failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR);
   799 	    failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR);
   790 	    return 0xFF;
   800 	    return 0xFF;
   791       }
   801       }
   792       // No line on use. Great !
   802       /* No line on use. Great !*/
   793       // Try to open a new line.
   803       /* Try to open a new line.*/
   794       err = getSDOfreeLine( d, whoami, &line );
   804       err = getSDOfreeLine( d, whoami, &line );
   795       if (err) {
   805       if (err) {
   796 	MSG_ERR(0x1A71, "SDO error : No line free, too many SDO in progress. Aborted.", 0);
   806 	MSG_ERR(0x1A71, "SDO error : No line free, too many SDO in progress. Aborted.", 0);
   797 	failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR);
   807 	failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR);
   798 	return 0xFF;
   808 	return 0xFF;
   799       }
   809       }
   800       initSDOline(d, line, nodeId, index, subIndex, SDO_UPLOAD_IN_PROGRESS);
   810       initSDOline(d, line, nodeId, index, subIndex, SDO_UPLOAD_IN_PROGRESS);
   801       // Transfer data from dictionary to the line structure.
   811       /* Transfer data from dictionary to the line structure. */
   802       errorCode = objdictToSDOline(d, line);
   812       errorCode = objdictToSDOline(d, line);
   803      
   813      
   804       if (errorCode) {
   814       if (errorCode) {
   805 	MSG_ERR(0x1A94, "SDO error : Unable to copy the data from object dictionary. Err code : ", 
   815 	MSG_ERR(0x1A94, "SDO error : Unable to copy the data from object dictionary. Err code : ", 
   806 		errorCode); 
   816 		errorCode); 
   807 	failedSDO(d, nodeId, whoami, index, subIndex, errorCode);
   817 	failedSDO(d, nodeId, whoami, index, subIndex, errorCode);
   808 	return 0xFF;
   818 	return 0xFF;
   809 	}
   819 	}
   810       // Preparing the response.
   820       /* Preparing the response.*/
   811       getSDOlineRestBytes(d, line, &nbBytes);	// Nb bytes to transfer ?
   821       getSDOlineRestBytes(d, line, &nbBytes);	/* Nb bytes to transfer ? */
   812       sdo.nodeId = nodeId; // The server node Id;
   822       sdo.nodeId = nodeId; /* The server node Id; */
   813       if (nbBytes > 4) {
   823       if (nbBytes > 4) {
   814 	// normal transfert. (segmented).
   824 	/* normal transfert. (segmented). */
   815 	// code to send the initiate upload response. (cs = 2)
   825 	/* code to send the initiate upload response. (cs = 2) */
   816 	sdo.body.data[0] = (2 << 5) | 1;
   826 	sdo.body.data[0] = (2 << 5) | 1;
   817 	sdo.body.data[1] = index & 0xFF;        // LSB
   827 	sdo.body.data[1] = index & 0xFF;        /* LSB */
   818 	sdo.body.data[2] = (index >> 8) & 0xFF; // MSB 
   828 	sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */
   819 	sdo.body.data[3] = subIndex;
   829 	sdo.body.data[3] = subIndex;
   820         sdo.body.data[4] = nbBytes; // Limitation of canfestival2 : Max tranfert is 256 bytes.
   830         sdo.body.data[4] = nbBytes; /* Limitation of canfestival2 : Max tranfert is 256 bytes.*/
   821 	// It takes too much memory to upgrate to 2^32 because the size of data is also coded
   831 	/* It takes too much memory to upgrate to 2^32 because the size of data is also coded */
   822 	// in the object dictionary, at every index and subindex.
   832 	/* in the object dictionary, at every index and subindex. */
   823 	for (i = 5 ; i < 8 ; i++)
   833 	for (i = 5 ; i < 8 ; i++)
   824 	  sdo.body.data[i] = 0;
   834 	  sdo.body.data[i] = 0;
   825 	MSG_WAR(0x3A95, "SDO. Sending normal upload initiate response defined at index 0x1200 + ", nodeId); 
   835 	MSG_WAR(0x3A95, "SDO. Sending normal upload initiate response defined at index 0x1200 + ", nodeId); 
   826 	sendSDO(d, whoami, sdo); 
   836 	sendSDO(d, whoami, sdo); 
   827       }
   837       }
   828       else {
   838       else {
   829 	// Expedited upload. (cs = 2 ; e = 1) 
   839 	/* Expedited upload. (cs = 2 ; e = 1) */
   830 	sdo.body.data[0] = (2 << 5) | ((4 - nbBytes) << 2) | 3;  
   840 	sdo.body.data[0] = (2 << 5) | ((4 - nbBytes) << 2) | 3;  
   831 	sdo.body.data[1] = index & 0xFF;        // LSB
   841 	sdo.body.data[1] = index & 0xFF;        /* LSB */
   832 	sdo.body.data[2] = (index >> 8) & 0xFF; // MSB 
   842 	sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */
   833 	sdo.body.data[3] = subIndex;
   843 	sdo.body.data[3] = subIndex;
   834 	err = lineToSDO(d, line, nbBytes, sdo.body.data + 4);	 
   844 	err = lineToSDO(d, line, nbBytes, sdo.body.data + 4);	 
   835 	if (err) {
   845 	if (err) {
   836 	  failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
   846 	  failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
   837 	  return 0xFF;
   847 	  return 0xFF;
   839 	for (i = 4 + nbBytes ; i < 8 ; i++)
   849 	for (i = 4 + nbBytes ; i < 8 ; i++)
   840 	  sdo.body.data[i] = 0;
   850 	  sdo.body.data[i] = 0;
   841 	MSG_WAR(0x3A96, "SDO. Sending expedited upload initiate response defined at index 0x1200 + ", 
   851 	MSG_WAR(0x3A96, "SDO. Sending expedited upload initiate response defined at index 0x1200 + ", 
   842 		nodeId); 
   852 		nodeId); 
   843 	sendSDO(d, whoami, sdo); 
   853 	sendSDO(d, whoami, sdo); 
   844 	// Release the line.
   854 	/* Release the line.*/
   845 	resetSDOline(d, line);
   855 	resetSDOline(d, line);
   846       }
   856       }
   847     } // end if I am SERVER
   857     } /* end if I am SERVER*/
   848     else {
   858     else {
   849       // I am CLIENT
   859       /* I am CLIENT */
   850       // It is the response for the previous initiate upload request. 
   860       /* It is the response for the previous initiate upload request.*/
   851       // We should find a line opened for this.
   861       /* We should find a line opened for this. */
   852       err = getSDOlineOnUse( d, nodeId, whoami, &line);
   862       err = getSDOlineOnUse( d, nodeId, whoami, &line);
   853       if (!err)
   863       if (!err)
   854 	err = d->transfers[line].state != SDO_UPLOAD_IN_PROGRESS;
   864 	err = d->transfers[line].state != SDO_UPLOAD_IN_PROGRESS;
   855       if (err) {
   865       if (err) {
   856 	MSG_ERR(0x1A97, "SDO error : Received response for unknown upload request from nodeId", nodeId); 
   866 	MSG_ERR(0x1A97, "SDO error : Received response for unknown upload request from nodeId", nodeId); 
   857 	failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
   867 	failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
   858 	return 0xFF;
   868 	return 0xFF;
   859       }
   869       }
   860       // Reset the wathdog
   870       /* Reset the wathdog */
   861       RestartSDO_TIMER(line)
   871       RestartSDO_TIMER(line)
   862       index = d->transfers[line].index;
   872       index = d->transfers[line].index;
   863       subIndex = d->transfers[line].subIndex;
   873       subIndex = d->transfers[line].subIndex;
   864       
   874       
   865       if (getSDOe(m->data[0])) { // If SDO expedited
   875       if (getSDOe(m->data[0])) { /* If SDO expedited */
   866 	// nb of data to be uploaded
   876 	/* nb of data to be uploaded */
   867 	  nbBytes = 4 - getSDOn2(m->data[0]);
   877 	  nbBytes = 4 - getSDOn2(m->data[0]);
   868 	// Storing the data in the line structure.
   878 	/* Storing the data in the line structure. */
   869 	err = SDOtoLine(d, line, nbBytes, (*m).data + 4);
   879 	err = SDOtoLine(d, line, nbBytes, (*m).data + 4);
   870 	if (err) {
   880 	if (err) {
   871 	  failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
   881 	  failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
   872 	  return 0xFF;
   882 	  return 0xFF;
   873 	}
   883 	}
   874 	// SDO expedited -> transfert finished. data are available via  getReadResultNetworkDict().
   884 	/* SDO expedited -> transfert finished. data are available via  getReadResultNetworkDict(). */
   875 	MSG_WAR(0x3A98, "SDO expedited upload finished. Response received from node : ", nodeId);
   885 	MSG_WAR(0x3A98, "SDO expedited upload finished. Response received from node : ", nodeId);
   876 	StopSDO_TIMER(line)
   886 	StopSDO_TIMER(line)
   877 	d->transfers[line].count = nbBytes;
   887 	d->transfers[line].count = nbBytes;
   878 	d->transfers[line].state = SDO_FINISHED;
   888 	d->transfers[line].state = SDO_FINISHED;
   879 	if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId);
   889 	if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId);
   880 	return 0;
   890 	return 0;
   881       }
   891       }
   882       else { // So, if it is not an expedited transfert
   892       else { /* So, if it is not an expedited transfert */
   883 	// Storing the nb of data to receive.
   893 	/* Storing the nb of data to receive. */
   884 	if (getSDOs(m->data[0])) {
   894 	if (getSDOs(m->data[0])) {
   885 	  nbBytes = m->data[4]; // Remember the limitation to 255 bytes to transfert
   895 	  nbBytes = m->data[4]; /* Remember the limitation to 255 bytes to transfert */
   886 	  err = setSDOlineRestBytes(d, line, nbBytes);
   896 	  err = setSDOlineRestBytes(d, line, nbBytes);
   887 	  if (err) {
   897 	  if (err) {
   888 	    failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
   898 	    failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
   889 	    return 0xFF;
   899 	    return 0xFF;
   890 	  }	
   900 	  }	
   891 	}
   901 	}
   892 	// Requesting next segment. (cs = 3)
   902 	/* Requesting next segment. (cs = 3) */
   893 	sdo.nodeId = nodeId;
   903 	sdo.nodeId = nodeId;
   894 	sdo.body.data[0] = 3 << 5;
   904 	sdo.body.data[0] = 3 << 5;
   895 	for (i = 1 ; i < 8 ; i++)
   905 	for (i = 1 ; i < 8 ; i++)
   896 	  sdo.body.data[i] = 0;
   906 	  sdo.body.data[i] = 0;
   897 	MSG_WAR(0x3A99, "SDO. Sending upload segment request to node : ", nodeId); 
   907 	MSG_WAR(0x3A99, "SDO. Sending upload segment request to node : ", nodeId); 
   898 	sendSDO(d, whoami, sdo);  
   908 	sendSDO(d, whoami, sdo);  
   899       }
   909       }
   900     } // End if CLIENT
   910     } /* End if CLIENT */
   901     break;
   911     break;
   902 
   912 
   903   case 3:
   913   case 3:
   904     // I am SERVER
   914     /* I am SERVER */
   905     if (whoami == SDO_SERVER) {
   915     if (whoami == SDO_SERVER) {
   906       // Receiving a upload segment.
   916       /* Receiving a upload segment. */
   907       // A SDO transfert should have been yet initiated.
   917       /* A SDO transfert should have been yet initiated. */
   908       err = getSDOlineOnUse( d, nodeId, whoami, &line ); 
   918       err = getSDOlineOnUse( d, nodeId, whoami, &line ); 
   909       if (!err)
   919       if (!err)
   910 	err = d->transfers[line].state != SDO_UPLOAD_IN_PROGRESS;
   920 	err = d->transfers[line].state != SDO_UPLOAD_IN_PROGRESS;
   911       if (err) {
   921       if (err) {
   912 	MSG_ERR(0x1AA0, "SDO error : Received upload segment for unstarted trans. index 0x1200 + ", 
   922 	MSG_ERR(0x1AA0, "SDO error : Received upload segment for unstarted trans. index 0x1200 + ", 
   913 		nodeId); 
   923 		nodeId); 
   914 	failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
   924 	failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
   915 	return 0xFF;
   925 	return 0xFF;
   916       }
   926       }
   917       // Reset the wathdog
   927       /* Reset the wathdog */
   918       RestartSDO_TIMER(line)
   928       RestartSDO_TIMER(line)
   919       MSG_WAR(0x3AA1, "Received SDO upload segment defined at index 0x1200 + ", nodeId); 
   929       MSG_WAR(0x3AA1, "Received SDO upload segment defined at index 0x1200 + ", nodeId); 
   920       index = d->transfers[line].index;
   930       index = d->transfers[line].index;
   921       subIndex = d->transfers[line].subIndex;
   931       subIndex = d->transfers[line].subIndex;
   922       // Toggle test.
   932       /* Toggle test.*/
   923       if (d->transfers[line].toggle != getSDOt(m->data[0])) {
   933       if (d->transfers[line].toggle != getSDOt(m->data[0])) {
   924 	MSG_ERR(0x1AA2, "SDO error : Toggle error : ", getSDOt(m->data[0])); 
   934 	MSG_ERR(0x1AA2, "SDO error : Toggle error : ", getSDOt(m->data[0])); 
   925 	failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED);
   935 	failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED);
   926 	return 0xFF;
   936 	return 0xFF;
   927       }
   937       }
   928       // Uploading next segment. We need to know if it will be the last one.
   938       /* Uploading next segment. We need to know if it will be the last one. */
   929       getSDOlineRestBytes(d, line, &nbBytes);	  	  
   939       getSDOlineRestBytes(d, line, &nbBytes);	  	  
   930       if (nbBytes > 7) {
   940       if (nbBytes > 7) {
   931 	// The segment to transfer is not the last one.
   941 	/* The segment to transfer is not the last one.*/
   932 	// code to send the next segment. (cs = 0; c = 0)
   942 	/* code to send the next segment. (cs = 0; c = 0) */
   933 	sdo.nodeId = nodeId; // The server node Id;
   943 	sdo.nodeId = nodeId; /* The server node Id; */
   934 	sdo.body.data[0] = (d->transfers[line].toggle << 4);
   944 	sdo.body.data[0] = (d->transfers[line].toggle << 4);
   935 	err = lineToSDO(d, line, 7, sdo.body.data + 1);	 
   945 	err = lineToSDO(d, line, 7, sdo.body.data + 1);	 
   936 	if (err) {
   946 	if (err) {
   937 	  failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
   947 	  failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
   938 	  return 0xFF;
   948 	  return 0xFF;
   939 	}
   949 	}
   940 	// Inverting the toggle for the next tranfert.
   950 	/* Inverting the toggle for the next tranfert. */
   941 	d->transfers[line].toggle = ! d->transfers[line].toggle & 1;
   951 	d->transfers[line].toggle = ! d->transfers[line].toggle & 1;
   942 	MSG_WAR(0x3AA3, "SDO. Sending upload segment defined at index 0x1200 + ", nodeId); 
   952 	MSG_WAR(0x3AA3, "SDO. Sending upload segment defined at index 0x1200 + ", nodeId); 
   943 	sendSDO(d, whoami, sdo); 
   953 	sendSDO(d, whoami, sdo); 
   944       } 
   954       } 
   945       else {
   955       else {
   946 	// Last segment.
   956 	/* Last segment. */
   947 	// code to send the last segment. (cs = 0; c = 1)	    
   957 	/* code to send the last segment. (cs = 0; c = 1) */	    
   948 	sdo.nodeId = nodeId; // The server node Id;
   958 	sdo.nodeId = nodeId; /* The server node Id; */
   949 	sdo.body.data[0] = (d->transfers[line].toggle << 4) | ((7 - nbBytes) << 1) | 1;
   959 	sdo.body.data[0] = (d->transfers[line].toggle << 4) | ((7 - nbBytes) << 1) | 1;
   950 	err = lineToSDO(d, line, nbBytes, sdo.body.data + 1);	 
   960 	err = lineToSDO(d, line, nbBytes, sdo.body.data + 1);	 
   951 	if (err) {
   961 	if (err) {
   952 	  failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
   962 	  failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
   953 	  return 0xFF;
   963 	  return 0xFF;
   954 	}
   964 	}
   955 	for (i = nbBytes + 1 ; i < 8 ; i++)
   965 	for (i = nbBytes + 1 ; i < 8 ; i++)
   956 	  sdo.body.data[i] = 0;
   966 	  sdo.body.data[i] = 0;
   957 	MSG_WAR(0x3AA4, "SDO. Sending last upload segment defined at index 0x1200 + ", nodeId);      
   967 	MSG_WAR(0x3AA4, "SDO. Sending last upload segment defined at index 0x1200 + ", nodeId);      
   958 	sendSDO(d, whoami, sdo);
   968 	sendSDO(d, whoami, sdo);
   959 	// Release the line
   969 	/* Release the line */
   960 	resetSDOline(d, line);
   970 	resetSDOline(d, line);
   961       }
   971       }
   962     } // end if SERVER	
   972     } /* end if SERVER*/
   963     else {
   973     else {
   964       // I am CLIENT
   974       /* I am CLIENT */
   965       // It is the response for the previous initiate download request. 
   975       /* It is the response for the previous initiate download request. */
   966       // We should find a line opened for this.
   976       /* We should find a line opened for this. */
   967       err = getSDOlineOnUse( d, nodeId, whoami, &line);
   977       err = getSDOlineOnUse( d, nodeId, whoami, &line);
   968       if (!err)
   978       if (!err)
   969 	err = d->transfers[line].state != SDO_DOWNLOAD_IN_PROGRESS;
   979 	err = d->transfers[line].state != SDO_DOWNLOAD_IN_PROGRESS;
   970       if (err) {
   980       if (err) {
   971 	MSG_ERR(0x1AA5, "SDO error : Received response for unknown download request from nodeId", nodeId); 
   981 	MSG_ERR(0x1AA5, "SDO error : Received response for unknown download request from nodeId", nodeId); 
   972 	failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
   982 	failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
   973 	return 0xFF;
   983 	return 0xFF;
   974       }
   984       }
   975       // Reset the wathdog
   985       /* Reset the wathdog */
   976       RestartSDO_TIMER(line)
   986       RestartSDO_TIMER(line)
   977       index = d->transfers[line].index;
   987       index = d->transfers[line].index;
   978       subIndex = d->transfers[line].subIndex;
   988       subIndex = d->transfers[line].subIndex;
   979       // End transmission or requesting  next segment. 
   989       /* End transmission or requesting  next segment. */
   980       getSDOlineRestBytes(d, line, &nbBytes);
   990       getSDOlineRestBytes(d, line, &nbBytes);
   981       if (nbBytes == 0) {
   991       if (nbBytes == 0) {
   982 	MSG_WAR(0x3AA6, "SDO End download expedited. Response received. from nodeId", nodeId); 
   992 	MSG_WAR(0x3AA6, "SDO End download expedited. Response received. from nodeId", nodeId); 
   983 	StopSDO_TIMER(line)
   993 	StopSDO_TIMER(line)
   984 	d->transfers[line].state = SDO_FINISHED;
   994 	d->transfers[line].state = SDO_FINISHED;
   985 	if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId);
   995 	if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId);
   986 	return 0x00;
   996 	return 0x00;
   987       }	  
   997       }	  
   988       if (nbBytes > 7) {
   998       if (nbBytes > 7) {
   989 	// more than one request to send
   999 	/* more than one request to send */
   990 	// code to send the next segment. (cs = 0; c = 0)	    
  1000 	/* code to send the next segment. (cs = 0; c = 0)	*/    
   991 	sdo.nodeId = nodeId; // The server node Id;
  1001 	sdo.nodeId = nodeId; /* The server node Id; */
   992 	sdo.body.data[0] = (d->transfers[line].toggle << 4);
  1002 	sdo.body.data[0] = (d->transfers[line].toggle << 4);
   993 	err = lineToSDO(d, line, 7, sdo.body.data + 1);	 
  1003 	err = lineToSDO(d, line, 7, sdo.body.data + 1);	 
   994 	if (err) {
  1004 	if (err) {
   995 	  failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
  1005 	  failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
   996 	  return 0xFF;
  1006 	  return 0xFF;
   997 	}
  1007 	}
   998       } 
  1008       } 
   999       else {
  1009       else {
  1000 	// Last segment.
  1010 	/* Last segment.*/
  1001 	// code to send the last segment. (cs = 0; c = 1)	   
  1011 	/* code to send the last segment. (cs = 0; c = 1)	*/   
  1002 	sdo.nodeId = nodeId; // The server node Id;
  1012 	sdo.nodeId = nodeId; /* The server node Id; */
  1003 	sdo.body.data[0] = (d->transfers[line].toggle << 4) | ((7 - nbBytes) << 1) | 1;
  1013 	sdo.body.data[0] = (d->transfers[line].toggle << 4) | ((7 - nbBytes) << 1) | 1;
  1004 	err = lineToSDO(d, line, nbBytes, sdo.body.data + 1);	 
  1014 	err = lineToSDO(d, line, nbBytes, sdo.body.data + 1);	 
  1005 	if (err) {
  1015 	if (err) {
  1006 	  failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
  1016 	  failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
  1007 	  return 0xFF;
  1017 	  return 0xFF;
  1010 	  sdo.body.data[i] = 0;
  1020 	  sdo.body.data[i] = 0;
  1011       }
  1021       }
  1012       MSG_WAR(0x3AA7, "SDO sending download segment to nodeId", nodeId); 
  1022       MSG_WAR(0x3AA7, "SDO sending download segment to nodeId", nodeId); 
  1013       sendSDO(d, whoami, sdo); 
  1023       sendSDO(d, whoami, sdo); 
  1014 
  1024 
  1015     } // end if I am a CLIENT			  
  1025     } /* end if I am a CLIENT		*/	  
  1016     break;  
  1026     break;  
  1017 
  1027 
  1018   case 4:
  1028   case 4:
  1019     abortCode = (*m).data[3] | 
  1029     abortCode = (*m).data[3] | 
  1020       (m->data[5] << 8) |
  1030       (m->data[5] << 8) |
  1021       (m->data[6] << 16) |
  1031       (m->data[6] << 16) |
  1022       (m->data[7] << 24);
  1032       (m->data[7] << 24);
  1023     // Received SDO abort.
  1033     /* Received SDO abort. */
  1024     // Looking for the line concerned.
  1034     /* Looking for the line concerned. */
  1025     if (whoami == SDO_SERVER) {
  1035     if (whoami == SDO_SERVER) {
  1026       err = getSDOlineOnUse( d, nodeId, whoami, &line );
  1036       err = getSDOlineOnUse( d, nodeId, whoami, &line );
  1027       if (!err) {
  1037       if (!err) {
  1028 	resetSDOline( d, line );
  1038 	resetSDOline( d, line );
  1029 	MSG_WAR(0x3AA8, "SD0. Received SDO abort. Line released. Code : ", abortCode);
  1039 	MSG_WAR(0x3AA8, "SD0. Received SDO abort. Line released. Code : ", abortCode);
  1030       }
  1040       }
  1031       else
  1041       else
  1032 	MSG_WAR(0x3AA9, "SD0. Received SDO abort. No line found. Code : ", abortCode);
  1042 	MSG_WAR(0x3AA9, "SD0. Received SDO abort. No line found. Code : ", abortCode);
  1033       // Tips : The end user has no way to know that the server node has received an abort SDO. 
  1043       /* Tips : The end user has no way to know that the server node has received an abort SDO. */
  1034       // Its is ok, I think.
  1044       /* Its is ok, I think.*/
  1035     }
  1045     }
  1036     else { // If I am CLIENT
  1046     else { /* If I am CLIENT */
  1037       err = getSDOlineOnUse( d, nodeId, whoami, &line );
  1047       err = getSDOlineOnUse( d, nodeId, whoami, &line );
  1038       if (!err) {
  1048       if (!err) {
  1039 	// The line *must* be released by the core program.
  1049 	/* The line *must* be released by the core program. */
  1040 	StopSDO_TIMER(line)
  1050 	StopSDO_TIMER(line)
  1041 	d->transfers[line].state = SDO_ABORTED_RCV;
  1051 	d->transfers[line].state = SDO_ABORTED_RCV;
  1042 	MSG_WAR(0x3AB0, "SD0. Received SDO abort. Line state ABORTED. Code : ", abortCode);
  1052 	MSG_WAR(0x3AB0, "SD0. Received SDO abort. Line state ABORTED. Code : ", abortCode);
  1043       }
  1053       }
  1044       else
  1054       else
  1045 	MSG_WAR(0x3AB1, "SD0. Received SDO abort. No line found. Code : ", abortCode);
  1055 	MSG_WAR(0x3AB1, "SD0. Received SDO abort. No line found. Code : ", abortCode);
  1046     } 
  1056     } 
  1047     break;
  1057     break;
  1048   default:
  1058   default:
  1049     // Error : Unknown cs
  1059     /* Error : Unknown cs */
  1050     MSG_ERR(0x1AB2, "SDO. Received unknown command specifier : ", getSDOcs(m->data[0]));
  1060     MSG_ERR(0x1AB2, "SDO. Received unknown command specifier : ", getSDOcs(m->data[0]));
  1051     return 0xFF;
  1061     return 0xFF;
  1052 
  1062 
  1053   } // End switch
  1063   } /* End switch */
  1054   return 0;     
  1064   return 0;     
  1055 }
  1065 }
  1056 
  1066 
  1057 /*******************************************************************)******/
  1067 /*******************************************************************)******/
  1058 inline UNS8 _writeNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, 
  1068 INLINE UNS8 _writeNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, 
  1059 		       UNS8 subIndex, UNS8 count, UNS8 dataType, void *data, SDOCallback_t Callback)
  1069 		       UNS8 subIndex, UNS8 count, UNS8 dataType, void *data, SDOCallback_t Callback)
  1060 {
  1070 {
  1061   UNS8 err;
  1071   UNS8 err;
  1062   UNS8 SDOfound = 0;
  1072   UNS8 SDOfound = 0;
  1063   UNS8 line;
  1073   UNS8 line;
  1064   s_SDO sdo;    // SDO to transmit
  1074   s_SDO sdo;    /* SDO to transmit */
  1065   UNS8 i, j;
  1075   UNS8 i, j;
  1066   UNS8 *    pSize;
  1076   UNS8 *    pSize;
  1067   UNS8      size;
  1077   UNS8      size;
  1068   UNS16     lastIndex;
  1078   UNS16     lastIndex;
  1069   UNS16     offset;
  1079   UNS16     offset;
  1074   MSG_WAR(0x3AC0, "Send SDO to write in the dictionary of node : ", nodeId);
  1084   MSG_WAR(0x3AC0, "Send SDO to write in the dictionary of node : ", nodeId);
  1075   MSG_WAR(0x3AC1, "                                   At index : ", index);
  1085   MSG_WAR(0x3AC1, "                                   At index : ", index);
  1076   MSG_WAR(0x3AC2, "                                   subIndex : ", subIndex);
  1086   MSG_WAR(0x3AC2, "                                   subIndex : ", subIndex);
  1077   MSG_WAR(0x3AC3, "                                   nb bytes : ", count);
  1087   MSG_WAR(0x3AC3, "                                   nb bytes : ", count);
  1078 
  1088 
  1079   // Verify that there is no SDO communication yet.
  1089   /* Verify that there is no SDO communication yet. */
  1080   err = getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line);
  1090   err = getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line);
  1081   if (!err) {
  1091   if (!err) {
  1082     MSG_ERR(0x1AC4, "SDO error : Communication yet established. with node : ", nodeId); 
  1092     MSG_ERR(0x1AC4, "SDO error : Communication yet established. with node : ", nodeId); 
  1083     return 0xFF;
  1093     return 0xFF;
  1084   }
  1094   }
  1085   // Taking the line ...
  1095   /* Taking the line ... */
  1086   err = getSDOfreeLine( d, SDO_CLIENT, &line );
  1096   err = getSDOfreeLine( d, SDO_CLIENT, &line );
  1087   if (err) {
  1097   if (err) {
  1088     MSG_ERR(0x1AC5, "SDO error : No line free, too many SDO in progress. Aborted for node : ", nodeId); 
  1098     MSG_ERR(0x1AC5, "SDO error : No line free, too many SDO in progress. Aborted for node : ", nodeId); 
  1089     return (0xFF);
  1099     return (0xFF);
  1090   }
  1100   }
  1091   // Check which SDO to use to communicate with the node
  1101   /* Check which SDO to use to communicate with the node */
  1092   offset = d->firstIndex->SDO_CLT;
  1102   offset = d->firstIndex->SDO_CLT;
  1093   lastIndex = d->lastIndex->SDO_CLT;
  1103   lastIndex = d->lastIndex->SDO_CLT;
  1094   if (offset == 0) {
  1104   if (offset == 0) {
  1095     MSG_ERR(0x1AC6, "writeNetworkDict : No SDO client index found", 0); 
  1105     MSG_ERR(0x1AC6, "writeNetworkDict : No SDO client index found", 0); 
  1096     return 0xFF;
  1106     return 0xFF;
  1099    while (offset <= lastIndex) {
  1109    while (offset <= lastIndex) {
  1100      if (d->objdict[offset].bSubCount <= 3) {
  1110      if (d->objdict[offset].bSubCount <= 3) {
  1101 	 MSG_ERR(0x1AC8, "Subindex 3  not found at index ", 0x1280 + i);
  1111 	 MSG_ERR(0x1AC8, "Subindex 3  not found at index ", 0x1280 + i);
  1102 	 return 0xFF;
  1112 	 return 0xFF;
  1103      }
  1113      }
  1104      // looking for the nodeId server
  1114      /* looking for the nodeId server */
  1105      pNodeIdServer = d->objdict[offset].pSubindex[3].pObject;
  1115      pNodeIdServer = d->objdict[offset].pSubindex[3].pObject;
  1106      nodeIdServer = *pNodeIdServer;
  1116      nodeIdServer = *pNodeIdServer;
  1107      MSG_WAR(0x1AD2, "index : ", 0x1280 + i);
  1117      MSG_WAR(0x1AD2, "index : ", 0x1280 + i);
  1108      MSG_WAR(0x1AD3, "nodeIdServer : ", nodeIdServer);
  1118      MSG_WAR(0x1AD3, "nodeIdServer : ", nodeIdServer);
  1109    
  1119    
  1111       SDOfound = 1;
  1121       SDOfound = 1;
  1112       break;
  1122       break;
  1113     }
  1123     }
  1114     offset++;
  1124     offset++;
  1115     i++;
  1125     i++;
  1116   } // end while
  1126   } /* end while */
  1117   if (!SDOfound) {
  1127   if (!SDOfound) {
  1118     MSG_ERR(0x1AC9, "SDO. Error. No client found to communicate with node : ", nodeId);
  1128     MSG_ERR(0x1AC9, "SDO. Error. No client found to communicate with node : ", nodeId);
  1119     return 0xFF;
  1129     return 0xFF;
  1120   }
  1130   }
  1121   MSG_WAR(0x3AD0,"        SDO client defined at index  : ", 0x1280 + i);
  1131   MSG_WAR(0x3AD0,"        SDO client defined at index  : ", 0x1280 + i);
  1122   initSDOline(d, line, nodeId, index, subIndex, SDO_DOWNLOAD_IN_PROGRESS);
  1132   initSDOline(d, line, nodeId, index, subIndex, SDO_DOWNLOAD_IN_PROGRESS);
  1123   d->transfers[line].count = count;
  1133   d->transfers[line].count = count;
  1124   d->transfers[line].dataType = dataType;
  1134   d->transfers[line].dataType = dataType;
  1125   
  1135   
  1126   // Copy data to transfers structure.
  1136   /* Copy data to transfers structure. */
  1127   for (j = 0 ; j < count ; j++) {
  1137   for (j = 0 ; j < count ; j++) {
  1128 # ifdef CANOPEN_BIG_ENDIAN
  1138 # ifdef CANOPEN_BIG_ENDIAN
  1129     if (dataType == 0)
  1139     if (dataType == 0)
  1130       d->transfers[line].data[count - 1 - j] = ((char *)data)[j];
  1140       d->transfers[line].data[count - 1 - j] = ((char *)data)[j];
  1131     else // String of bytes.
  1141     else /* String of bytes. */
  1132       d->transfers[line].data[j] = ((char *)data)[j];
  1142       d->transfers[line].data[j] = ((char *)data)[j];
  1133 #  else 
  1143 #  else 
  1134     d->transfers[line].data[j] = ((char *)data)[j];
  1144     d->transfers[line].data[j] = ((char *)data)[j];
  1135 #  endif
  1145 #  endif
  1136   }
  1146   }
  1137   // Send the SDO to the server. Initiate download, cs=1.
  1147   /* Send the SDO to the server. Initiate download, cs=1. */
  1138   sdo.nodeId = nodeId;
  1148   sdo.nodeId = nodeId;
  1139   if (count <= 4) { // Expedited transfert
  1149   if (count <= 4) { /* Expedited transfert */
  1140     sdo.body.data[0] = (1 << 5) | ((4 - count) << 2) | 3;
  1150     sdo.body.data[0] = (1 << 5) | ((4 - count) << 2) | 3;
  1141     for (i = 4 ; i < 8 ; i++)
  1151     for (i = 4 ; i < 8 ; i++)
  1142       sdo.body.data[i] = d->transfers[line].data[i - 4];
  1152       sdo.body.data[i] = d->transfers[line].data[i - 4];
  1143     d->transfers[line].offset = count;
  1153     d->transfers[line].offset = count;
  1144   }	
  1154   }	
  1145   else { // Normal transfert
  1155   else { /* Normal transfert */
  1146     sdo.body.data[0] = (1 << 5) | 1;
  1156     sdo.body.data[0] = (1 << 5) | 1;
  1147     sdo.body.data[4] = count; // nb of byte to transmit. Max = 255. (canfestival2 limitation).
  1157     sdo.body.data[4] = count; /* nb of byte to transmit. Max = 255. (canfestival2 limitation). */
  1148     for (i = 5 ; i < 8 ; i++)
  1158     for (i = 5 ; i < 8 ; i++)
  1149       sdo.body.data[i] = 0;
  1159       sdo.body.data[i] = 0;
  1150   }
  1160   }
  1151   sdo.body.data[1] = index & 0xFF;        // LSB
  1161   sdo.body.data[1] = index & 0xFF;        /* LSB */
  1152   sdo.body.data[2] = (index >> 8) & 0xFF; // MSB 
  1162   sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */
  1153   sdo.body.data[3] = subIndex;
  1163   sdo.body.data[3] = subIndex;
  1154   
  1164   
  1155   err = sendSDO(d, SDO_CLIENT, sdo);
  1165   err = sendSDO(d, SDO_CLIENT, sdo);
  1156   if (err) {
  1166   if (err) {
  1157     MSG_ERR(0x1AD1, "SDO. Error while sending SDO to node : ", nodeId);
  1167     MSG_ERR(0x1AD1, "SDO. Error while sending SDO to node : ", nodeId);
  1158     // release the line
  1168     /* release the line */
  1159     resetSDOline(d, line);
  1169     resetSDOline(d, line);
  1160     return 0xFF;
  1170     return 0xFF;
  1161   }
  1171   }
  1162   d->transfers[line].Callback = Callback;
  1172   d->transfers[line].Callback = Callback;
  1163   return 0;
  1173   return 0;
  1179 	return _writeNetworkDict (d, nodeId, index, subIndex, count, dataType, data, Callback);	
  1189 	return _writeNetworkDict (d, nodeId, index, subIndex, count, dataType, data, Callback);	
  1180 }
  1190 }
  1181 
  1191 
  1182 
  1192 
  1183 /***************************************************************************/
  1193 /***************************************************************************/
  1184 inline UNS8 _readNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType, SDOCallback_t Callback)
  1194 INLINE UNS8 _readNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType, SDOCallback_t Callback)
  1185 {
  1195 {
  1186   UNS8 err;
  1196   UNS8 err;
  1187   UNS8 SDOfound = 0;
  1197   UNS8 SDOfound = 0;
  1188   UNS8 i;
  1198   UNS8 i;
  1189   UNS8 line;
  1199   UNS8 line;
  1190   UNS8 *    pSize;
  1200   UNS8 *    pSize;
  1191   UNS8      size;
  1201   UNS8      size;
  1192   s_SDO sdo;    // SDO to transmit
  1202   s_SDO sdo;    /* SDO to transmit */
  1193   UNS8      *pNodeIdServer;
  1203   UNS8      *pNodeIdServer;
  1194   UNS8      nodeIdServer;
  1204   UNS8      nodeIdServer;
  1195   UNS16     offset;
  1205   UNS16     offset;
  1196   UNS16     lastIndex;
  1206   UNS16     lastIndex;
  1197   pSize = &size;
  1207   pSize = &size;
  1198   MSG_WAR(0x3AD5, "Send SDO to read in the dictionary of node : ", nodeId);
  1208   MSG_WAR(0x3AD5, "Send SDO to read in the dictionary of node : ", nodeId);
  1199   MSG_WAR(0x3AD6, "                                  At index : ", index);
  1209   MSG_WAR(0x3AD6, "                                  At index : ", index);
  1200   MSG_WAR(0x3AD7, "                                  subIndex : ", subIndex);
  1210   MSG_WAR(0x3AD7, "                                  subIndex : ", subIndex);
  1201 
  1211 
  1202 
  1212 
  1203   // Verify that there is no SDO communication yet.
  1213   /* Verify that there is no SDO communication yet. */
  1204   err = getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line);
  1214   err = getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line);
  1205   if (!err) {
  1215   if (!err) {
  1206     MSG_ERR(0x1AD8, "SDO error : Communication yet established. with node : ", nodeId); 
  1216     MSG_ERR(0x1AD8, "SDO error : Communication yet established. with node : ", nodeId); 
  1207     return 0xFF;
  1217     return 0xFF;
  1208   }
  1218   }
  1209   // Taking the line ...
  1219   /* Taking the line ... */
  1210   err = getSDOfreeLine( d, SDO_CLIENT, &line );
  1220   err = getSDOfreeLine( d, SDO_CLIENT, &line );
  1211   if (err) {
  1221   if (err) {
  1212     MSG_ERR(0x1AD9, "SDO error : No line free, too many SDO in progress. Aborted for node : ", nodeId); 
  1222     MSG_ERR(0x1AD9, "SDO error : No line free, too many SDO in progress. Aborted for node : ", nodeId); 
  1213     return (0xFF);
  1223     return (0xFF);
  1214   }
  1224   }
  1215   else
  1225   else
  1216     MSG_WAR(0x3AE0, "Transmission on line : ", line);
  1226     MSG_WAR(0x3AE0, "Transmission on line : ", line);
  1217 
  1227 
  1218   // Check which SDO to use to communicate with the node
  1228   /* Check which SDO to use to communicate with the node */
  1219   offset = d->firstIndex->SDO_CLT;
  1229   offset = d->firstIndex->SDO_CLT;
  1220   lastIndex = d->lastIndex->SDO_CLT;
  1230   lastIndex = d->lastIndex->SDO_CLT;
  1221   if (offset == 0) {
  1231   if (offset == 0) {
  1222     MSG_ERR(0x1AE1, "writeNetworkDict : No SDO client index found", 0); 
  1232     MSG_ERR(0x1AE1, "writeNetworkDict : No SDO client index found", 0); 
  1223     return 0xFF;
  1233     return 0xFF;
  1226   while (offset <= lastIndex) {
  1236   while (offset <= lastIndex) {
  1227      if (d->objdict[offset].bSubCount <= 3) {
  1237      if (d->objdict[offset].bSubCount <= 3) {
  1228 	 MSG_ERR(0x1AE2, "Subindex 3  not found at index ", 0x1280 + i);
  1238 	 MSG_ERR(0x1AE2, "Subindex 3  not found at index ", 0x1280 + i);
  1229 	 return 0xFF;
  1239 	 return 0xFF;
  1230      }
  1240      }
  1231      // looking for the nodeId server
  1241      /* looking for the nodeId server */
  1232      pNodeIdServer = d->objdict[offset].pSubindex[3].pObject;
  1242      pNodeIdServer = d->objdict[offset].pSubindex[3].pObject;
  1233      nodeIdServer = *pNodeIdServer;
  1243      nodeIdServer = *pNodeIdServer;
  1234    
  1244    
  1235     if(nodeIdServer == nodeId) {
  1245     if(nodeIdServer == nodeId) {
  1236       SDOfound = 1;
  1246       SDOfound = 1;
  1237       break;
  1247       break;
  1238     }
  1248     }
  1239     offset++;
  1249     offset++;
  1240     i++;
  1250     i++;
  1241   } // end while
  1251   } /* end while */
  1242   if (!SDOfound) {
  1252   if (!SDOfound) {
  1243     MSG_ERR(0x1AE3, "SDO. Error. No client found to communicate with node : ", nodeId);
  1253     MSG_ERR(0x1AE3, "SDO. Error. No client found to communicate with node : ", nodeId);
  1244     return 0xFF;
  1254     return 0xFF;
  1245   }
  1255   }
  1246   MSG_WAR(0x3AE4,"        SDO client defined at index  : ", 0x1280 + i);
  1256   MSG_WAR(0x3AE4,"        SDO client defined at index  : ", 0x1280 + i);
  1247   initSDOline(d, line, nodeId, index, subIndex, SDO_UPLOAD_IN_PROGRESS);
  1257   initSDOline(d, line, nodeId, index, subIndex, SDO_UPLOAD_IN_PROGRESS);
  1248   getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line);
  1258   getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line);
  1249   sdo.nodeId = nodeId;
  1259   sdo.nodeId = nodeId;
  1250   // Send the SDO to the server. Initiate upload, cs=2.
  1260   /* Send the SDO to the server. Initiate upload, cs=2. */
  1251   d->transfers[line].dataType = dataType;				
  1261   d->transfers[line].dataType = dataType;				
  1252   sdo.body.data[0] = (2 << 5);	
  1262   sdo.body.data[0] = (2 << 5);	
  1253   sdo.body.data[1] = index & 0xFF;        // LSB
  1263   sdo.body.data[1] = index & 0xFF;        /* LSB */
  1254   sdo.body.data[2] = (index >> 8) & 0xFF; // MSB 
  1264   sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */
  1255   sdo.body.data[3] = subIndex;
  1265   sdo.body.data[3] = subIndex;
  1256   for (i = 4 ; i < 8 ; i++)
  1266   for (i = 4 ; i < 8 ; i++)
  1257     sdo.body.data[i] = 0;
  1267     sdo.body.data[i] = 0;
  1258   err = sendSDO(d, SDO_CLIENT, sdo);
  1268   err = sendSDO(d, SDO_CLIENT, sdo);
  1259   if (err) {
  1269   if (err) {
  1260     MSG_ERR(0x1AE5, "SDO. Error while sending SDO to node : ", nodeId);
  1270     MSG_ERR(0x1AE5, "SDO. Error while sending SDO to node : ", nodeId);
  1261     // release the line
  1271     /* release the line */
  1262     resetSDOline(d, line);
  1272     resetSDOline(d, line);
  1263     return 0xFF;
  1273     return 0xFF;
  1264   }		
  1274   }		
  1265   d->transfers[line].Callback = Callback;
  1275   d->transfers[line].Callback = Callback;
  1266   return 0;
  1276   return 0;
  1287   UNS8 i;
  1297   UNS8 i;
  1288   UNS8 err;
  1298   UNS8 err;
  1289   UNS8 line;
  1299   UNS8 line;
  1290   * size = 0;
  1300   * size = 0;
  1291 
  1301 
  1292   // Looking for the line tranfert.
  1302   /* Looking for the line tranfert. */
  1293   err = getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line);
  1303   err = getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line);
  1294   if (err) {
  1304   if (err) {
  1295     MSG_ERR(0x1AF0, "SDO error : No line found for communication with node : ", nodeId); 
  1305     MSG_ERR(0x1AF0, "SDO error : No line found for communication with node : ", nodeId); 
  1296     return SDO_ABORTED_INTERNAL;
  1306     return SDO_ABORTED_INTERNAL;
  1297   }
  1307   }
  1298   if (d->transfers[line].state != SDO_FINISHED)
  1308   if (d->transfers[line].state != SDO_FINISHED)
  1299     return d->transfers[line].state;
  1309     return d->transfers[line].state;
  1300 
  1310 
  1301   // Transfert is finished. Put the value in the data.
  1311   /* Transfert is finished. Put the value in the data. */
  1302   * size = d->transfers[line].count;
  1312   * size = d->transfers[line].count;
  1303   for  ( i = 0 ; i < *size ; i++) {
  1313   for  ( i = 0 ; i < *size ; i++) {
  1304 # ifdef CANOPEN_BIG_ENDIAN
  1314 # ifdef CANOPEN_BIG_ENDIAN
  1305     if (d->transfers[line].dataType != visible_string)
  1315     if (d->transfers[line].dataType != visible_string)
  1306       ( (char *) data)[*size - 1 - i] = d->transfers[line].data[i];
  1316       ( (char *) data)[*size - 1 - i] = d->transfers[line].data[i];
  1307     else // String of bytes.
  1317     else /* String of bytes. */
  1308       ( (char *) data)[i] = d->transfers[line].data[i];
  1318       ( (char *) data)[i] = d->transfers[line].data[i];
  1309 # else 
  1319 # else 
  1310     ( (char *) data)[i] = d->transfers[line].data[i];
  1320     ( (char *) data)[i] = d->transfers[line].data[i];
  1311 # endif
  1321 # endif
  1312   } 
  1322   } 
  1319 {
  1329 {
  1320   UNS8 line = 0;
  1330   UNS8 line = 0;
  1321   UNS8 err;
  1331   UNS8 err;
  1322 
  1332 
  1323   * abortCode = 0;
  1333   * abortCode = 0;
  1324   // Looking for the line tranfert.
  1334   /* Looking for the line tranfert. */
  1325   err = getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line);
  1335   err = getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line);
  1326   if (err) {
  1336   if (err) {
  1327     MSG_ERR(0x1AF1, "SDO error : No line found for communication with node : ", nodeId); 
  1337     MSG_ERR(0x1AF1, "SDO error : No line found for communication with node : ", nodeId); 
  1328     return SDO_ABORTED_INTERNAL;
  1338     return SDO_ABORTED_INTERNAL;
  1329   }
  1339   }