src/sdo.c
author etisserant
Thu, 10 Aug 2006 17:38:53 +0200
changeset 45 45885c4e3e56
parent 32 8afa33692372
child 53 73bc47a0db55
permissions -rw-r--r--
Updated doc
0
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
     1
/*
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
     2
This file is part of CanFestival, a library implementing CanOpen Stack. 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
     3
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
     4
Copyright (C): Edouard TISSERANT and Francis DUPIN
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
     5
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
     6
See COPYING file for copyrights details.
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
     7
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
     8
This library is free software; you can redistribute it and/or
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
     9
modify it under the terms of the GNU Lesser General Public
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    10
License as published by the Free Software Foundation; either
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    11
version 2.1 of the License, or (at your option) any later version.
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    12
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    13
This library is distributed in the hope that it will be useful,
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    14
but WITHOUT ANY WARRANTY; without even the implied warranty of
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    15
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    16
Lesser General Public License for more details.
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    17
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    18
You should have received a copy of the GNU Lesser General Public
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    19
License along with this library; if not, write to the Free Software
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    20
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    21
*/
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    22
6
8038ced64796 pas de debogue
dejoigny
parents: 1
diff changeset
    23
//#define DEBUG_WAR_CONSOLE_ON
8038ced64796 pas de debogue
dejoigny
parents: 1
diff changeset
    24
//#define DEBUG_ERR_CONSOLE_ON
0
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    25
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    26
#include "objacces.h"
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    27
#include "sdo.h"
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    28
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    29
/***************************************************************************/
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    30
// SDO (un)packing macros
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    31
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    32
/** Returns the command specifier (cs, ccs, scs) from the first byte of the SDO   
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    33
 */
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    34
#define getSDOcs(byte) (byte >> 5)
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    35
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    36
/** Returns the number of bytes without data from the first byte of the SDO. Coded in 2 bits   
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    37
 */
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    38
#define getSDOn2(byte) ((byte >> 2) & 3)
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    39
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    40
/** Returns the number of bytes without data from the first byte of the SDO. Coded in 3 bits   
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    41
 */
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    42
#define getSDOn3(byte) ((byte >> 1) & 7)
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    43
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    44
/** Returns the transfer type from the first byte of the SDO   
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    45
 */
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    46
#define getSDOe(byte) ((byte >> 1) & 1)
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    47
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    48
/** Returns the size indicator from the first byte of the SDO   
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    49
 */
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    50
#define getSDOs(byte) (byte & 1)
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    51
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    52
/** Returns the indicator of end transmission from the first byte of the SDO   
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    53
 */
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    54
#define getSDOc(byte) (byte & 1)
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    55
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    56
/** Returns the toggle from the first byte of the SDO   
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    57
 */
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    58
#define getSDOt(byte) ((byte >> 4) & 1)
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    59
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    60
/** Returns the index from the bytes 1 and 2 of the SDO   
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    61
 */
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    62
#define getSDOindex(byte1, byte2) ((byte2 << 8) | (byte1))
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    63
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    64
/** Returns the subIndex from the byte 3 of the SDO   
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    65
 */
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    66
#define getSDOsubIndex(byte3) (byte3)
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    67
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    68
/***************************************************************************
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    69
**
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    70
*/
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    71
void SDOTimeoutAlarm(CO_Data* d, UNS32 id)
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    72
{
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    73
    MSG_ERR(0x1A01, "SDO timeout. SDO response not received.", 0);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    74
    MSG_WAR(0x2A02, "server node : ", d->transfers[id].nodeId);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    75
    MSG_WAR(0x2A02, "      index : ", d->transfers[id].index);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    76
    MSG_WAR(0x2A02, "   subIndex : ", d->transfers[id].subIndex); 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    77
    // Reset timer handler
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    78
    d->transfers[id].timer = TIMER_NONE;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    79
    // Call the user function to inform of the problem.
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    80
    (*d->SDOtimeoutError)(id);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    81
    // Sending a SDO abort
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    82
    sendSDOabort(d, d->transfers[id].whoami, 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    83
		 d->transfers[id].index, d->transfers[id].subIndex, SDOABT_TIMED_OUT);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    84
    // Reset the line
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    85
    resetSDOline(d, id);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    86
}
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    87
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    88
#define StopSDO_TIMER(id) \
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    89
MSG_WAR(0x3A05, "StopSDO_TIMER for line : ", line);\
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    90
d->transfers[id].timer = DelAlarm(d->transfers[id].timer);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    91
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    92
#define StartSDO_TIMER(id) \
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    93
MSG_WAR(0x3A06, "StartSDO_TIMER for line : ", line);\
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    94
d->transfers[id].timer = SetAlarm(d,id,&SDOTimeoutAlarm,MS_TO_TIMEVAL(SDO_TIMEOUT_MS),0);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    95
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    96
#define RestartSDO_TIMER(id) \
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    97
MSG_WAR(0x3A07, "restartSDO_TIMER for line : ", line);\
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    98
if(d->transfers[id].timer != TIMER_NONE) { StopSDO_TIMER(id) StartSDO_TIMER(id) }
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
    99
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   100
/***************************************************************************/
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   101
/** Reset all sdo buffers
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   102
 */
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   103
void resetSDO (CO_Data* d)
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   104
{
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   105
  UNS8 j;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   106
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   107
  /* transfer structure initialization */
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   108
    for (j = 0 ; j < SDO_MAX_SIMULTANEOUS_TRANSFERTS ; j++) 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   109
      resetSDOline(d, j);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   110
}
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   111
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   112
/***************************************************************************/
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   113
UNS32 SDOlineToObjdict (CO_Data* d, UNS8 line)
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   114
{
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   115
  UNS8      size;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   116
  UNS32 errorCode;
1
b3dc740b4b04 Commit some Fix from Francis on sdo.c and abjacces.c. Fix endianisation problems while accessing OD. Also fix SDO abord handling bug.
etisserant
parents: 0
diff changeset
   117
  MSG_WAR(0x3A08, "Enter in SDOlineToObjdict ", line);
0
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   118
  size = d->transfers[line].count;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   119
  errorCode = setODentry(d, d->transfers[line].index, d->transfers[line].subIndex, 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   120
			 (void *) d->transfers[line].data, &size, 1);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   121
  if (errorCode != OD_SUCCESSFUL)
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   122
    return errorCode;
1
b3dc740b4b04 Commit some Fix from Francis on sdo.c and abjacces.c. Fix endianisation problems while accessing OD. Also fix SDO abord handling bug.
etisserant
parents: 0
diff changeset
   123
  MSG_WAR(0x3A08, "exit of SDOlineToObjdict ", line);
0
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   124
  return 0;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   125
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   126
}
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   127
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   128
/***************************************************************************/
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   129
UNS32 objdictToSDOline (CO_Data* d, UNS8 line)
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   130
{
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   131
  UNS8  size = 0;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   132
  UNS8  dataType;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   133
  UNS32 errorCode;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   134
12
1d3b9ad366c8 CANOPEN_BIG_ENDIAN :
dejoigny
parents: 6
diff changeset
   135
  MSG_WAR(0x3A05, "objdict->line index : ", d->transfers[line].index);
1d3b9ad366c8 CANOPEN_BIG_ENDIAN :
dejoigny
parents: 6
diff changeset
   136
  MSG_WAR(0x3A06, "  subIndex : ", d->transfers[line].subIndex);
0
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   137
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   138
  errorCode = getODentry(d, 	d->transfers[line].index,
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   139
  				d->transfers[line].subIndex,
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   140
  				(void *)d->transfers[line].data,
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   141
  				&size, &dataType, 0);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   142
  
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   143
  if (errorCode != OD_SUCCESSFUL)
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   144
    return errorCode;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   145
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   146
  d->transfers[line].count = size;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   147
  d->transfers[line].offset = 0;
12
1d3b9ad366c8 CANOPEN_BIG_ENDIAN :
dejoigny
parents: 6
diff changeset
   148
#if 0
1d3b9ad366c8 CANOPEN_BIG_ENDIAN :
dejoigny
parents: 6
diff changeset
   149
  // Me laisser ça, please ! (FD)
1d3b9ad366c8 CANOPEN_BIG_ENDIAN :
dejoigny
parents: 6
diff changeset
   150
  {
1d3b9ad366c8 CANOPEN_BIG_ENDIAN :
dejoigny
parents: 6
diff changeset
   151
    UNS8 i;
1d3b9ad366c8 CANOPEN_BIG_ENDIAN :
dejoigny
parents: 6
diff changeset
   152
    for (i = 0 ; i < 10 ; i++) {
1d3b9ad366c8 CANOPEN_BIG_ENDIAN :
dejoigny
parents: 6
diff changeset
   153
      MSG_WAR(i, "data= ", d->transfers[line].data[i]);
1d3b9ad366c8 CANOPEN_BIG_ENDIAN :
dejoigny
parents: 6
diff changeset
   154
    }     
1d3b9ad366c8 CANOPEN_BIG_ENDIAN :
dejoigny
parents: 6
diff changeset
   155
  }
1d3b9ad366c8 CANOPEN_BIG_ENDIAN :
dejoigny
parents: 6
diff changeset
   156
#endif
0
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   157
  return 0;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   158
}
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   159
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   160
/***************************************************************************/
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   161
UNS8 lineToSDO (CO_Data* d, UNS8 line, UNS8 nbBytes, UNS8* data) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   162
  UNS8 i;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   163
  UNS8 offset;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   164
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   165
  if ((d->transfers[line].offset + nbBytes) > SDO_MAX_LENGTH_TRANSFERT) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   166
    MSG_ERR(0x1A10,"SDO Size of data too large. Exceed SDO_MAX_LENGTH_TRANSFERT", nbBytes);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   167
    return 0xFF;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   168
  }
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   169
    if ((d->transfers[line].offset + nbBytes) > d->transfers[line].count) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   170
    MSG_ERR(0x1A11,"SDO Size of data too large. Exceed count", nbBytes);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   171
    return 0xFF;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   172
  }
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   173
  offset = d->transfers[line].offset;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   174
  for (i = 0 ; i < nbBytes ; i++) 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   175
    * (data + i) = d->transfers[line].data[offset + i];
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   176
  d->transfers[line].offset = d->transfers[line].offset + nbBytes;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   177
  return 0;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   178
}
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   179
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   180
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   181
/***************************************************************************/
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   182
UNS8 SDOtoLine (CO_Data* d, UNS8 line, UNS8 nbBytes, UNS8* data)
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   183
{
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   184
  UNS8 i;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   185
  UNS8 offset;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   186
  
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   187
  if ((d->transfers[line].offset + nbBytes) > SDO_MAX_LENGTH_TRANSFERT) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   188
    MSG_ERR(0x1A15,"SDO Size of data too large. Exceed SDO_MAX_LENGTH_TRANSFERT", nbBytes);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   189
    return 0xFF;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   190
  }
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   191
  offset = d->transfers[line].offset;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   192
  for (i = 0 ; i < nbBytes ; i++) 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   193
    d->transfers[line].data[offset + i] = * (data + i);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   194
  d->transfers[line].offset = d->transfers[line].offset + nbBytes;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   195
  return 0;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   196
}
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   197
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   198
/***************************************************************************/
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   199
UNS8 failedSDO (CO_Data* d, UNS8 nodeId, UNS8 whoami, UNS16 index, 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   200
		UNS8 subIndex, UNS32 abortCode)
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   201
{
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   202
  UNS8 err;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   203
  UNS8 line;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   204
  err = getSDOlineOnUse( d, nodeId, whoami, &line );
1
b3dc740b4b04 Commit some Fix from Francis on sdo.c and abjacces.c. Fix endianisation problems while accessing OD. Also fix SDO abord handling bug.
etisserant
parents: 0
diff changeset
   205
  if (!err) // If a line on use have been found.
0
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   206
    MSG_WAR(0x3A20, "FailedSDO : line found : ", line);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   207
  if ((! err) && (whoami == SDO_SERVER)) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   208
    resetSDOline( d, line );
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   209
    MSG_WAR(0x3A21, "FailedSDO : line released : ", line);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   210
  }
1
b3dc740b4b04 Commit some Fix from Francis on sdo.c and abjacces.c. Fix endianisation problems while accessing OD. Also fix SDO abord handling bug.
etisserant
parents: 0
diff changeset
   211
  if ((! err) && (whoami == SDO_CLIENT)) {
b3dc740b4b04 Commit some Fix from Francis on sdo.c and abjacces.c. Fix endianisation problems while accessing OD. Also fix SDO abord handling bug.
etisserant
parents: 0
diff changeset
   212
    StopSDO_TIMER(line);
0
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   213
    d->transfers[line].state = SDO_ABORTED_INTERNAL;
1
b3dc740b4b04 Commit some Fix from Francis on sdo.c and abjacces.c. Fix endianisation problems while accessing OD. Also fix SDO abord handling bug.
etisserant
parents: 0
diff changeset
   214
  }
0
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   215
  MSG_WAR(0x3A22, "Sending SDO abort ", 0);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   216
  err = sendSDOabort(d, whoami, index, subIndex, abortCode);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   217
  if (err) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   218
    MSG_WAR(0x3A23, "Unable to send the SDO abort", 0);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   219
    return 0xFF;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   220
  }
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   221
  return 0;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   222
}
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   223
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   224
/***************************************************************************/
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   225
void resetSDOline ( CO_Data* d, UNS8 line )
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   226
{
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   227
  UNS8 i; 
1
b3dc740b4b04 Commit some Fix from Francis on sdo.c and abjacces.c. Fix endianisation problems while accessing OD. Also fix SDO abord handling bug.
etisserant
parents: 0
diff changeset
   228
  MSG_WAR(0x3A25, "reset SDO line nb : ", line); 
0
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   229
  initSDOline(d, line, 0, 0, 0, SDO_RESET);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   230
  for (i = 0 ; i < SDO_MAX_LENGTH_TRANSFERT ; i++)
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   231
    d->transfers[line].data[i] = 0;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   232
}
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   233
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   234
/***************************************************************************/
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   235
UNS8 initSDOline (CO_Data* d, UNS8 line, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 state)
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   236
{
1
b3dc740b4b04 Commit some Fix from Francis on sdo.c and abjacces.c. Fix endianisation problems while accessing OD. Also fix SDO abord handling bug.
etisserant
parents: 0
diff changeset
   237
  MSG_WAR(0x3A25, "init SDO line nb : ", line); 
0
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   238
  if (state == SDO_DOWNLOAD_IN_PROGRESS || state == SDO_UPLOAD_IN_PROGRESS){
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   239
  	StartSDO_TIMER(line)
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   240
  }else{
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   241
  	StopSDO_TIMER(line)
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   242
  }
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   243
  d->transfers[line].nodeId = nodeId; 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   244
  d->transfers[line].index = index;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   245
  d->transfers[line].subIndex = subIndex;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   246
  d->transfers[line].state = state;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   247
  d->transfers[line].toggle = 0;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   248
  d->transfers[line].count = 0;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   249
  d->transfers[line].offset = 0;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   250
  d->transfers[line].dataType = 0;
32
8afa33692372 SDO callbacks.
etisserant
parents: 31
diff changeset
   251
  d->transfers[line].Callback = NULL;  
0
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   252
  return 0;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   253
}
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   254
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   255
/***************************************************************************/
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   256
UNS8 getSDOfreeLine ( CO_Data* d, UNS8 whoami, UNS8 *line )
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   257
{
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   258
	
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   259
  UNS8 i;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   260
    
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   261
  for (i = 0 ; i < SDO_MAX_SIMULTANEOUS_TRANSFERTS ; i++){
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   262
    if ( d->transfers[i].state == SDO_RESET ) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   263
      *line = i;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   264
      d->transfers[i].whoami = whoami;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   265
      return 0;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   266
    } // end if
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   267
  } // end for
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   268
  MSG_ERR(0x1A25, "Too many SDO in progress. Aborted.", i);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   269
  return 0xFF;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   270
}
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   271
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   272
/***************************************************************************/
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   273
UNS8 getSDOlineOnUse (CO_Data* d, UNS8 nodeId, UNS8 whoami, UNS8 *line)
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   274
{
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   275
	
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   276
  UNS8 i;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   277
    
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   278
  for (i = 0 ; i < SDO_MAX_SIMULTANEOUS_TRANSFERTS ; i++){
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   279
    if ( (d->transfers[i].state != SDO_RESET) &&
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   280
	 (d->transfers[i].nodeId == nodeId) && 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   281
	 (d->transfers[i].whoami == whoami) ) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   282
      *line = i;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   283
      return 0;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   284
    }
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   285
  } 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   286
  return 0xFF;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   287
}
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   288
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   289
/***************************************************************************/
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   290
UNS8 closeSDOtransfer (CO_Data* d, UNS8 nodeId, UNS8 whoami)
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   291
{
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   292
  UNS8 err;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   293
  UNS8 line;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   294
  err = getSDOlineOnUse(d, nodeId, whoami, &line);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   295
  if (err) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   296
    MSG_WAR(0x2A30, "No SDO communication to close for node : ", nodeId); 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   297
    return 0xFF;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   298
  }
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   299
  resetSDOline(d, line);  
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   300
  return 0;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   301
}
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   302
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   303
/***************************************************************************/
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   304
UNS8 getSDOlineRestBytes (CO_Data* d, UNS8 line, UNS8 * nbBytes)
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   305
{
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   306
  if (d->transfers[line].count == 0) // if received initiate SDO protocol with e=0 and s=0
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   307
    * nbBytes = 0;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   308
  else
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   309
    * nbBytes = d->transfers[line].count - d->transfers[line].offset;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   310
  return 0;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   311
}
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   312
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   313
/***************************************************************************/
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   314
UNS8 setSDOlineRestBytes (CO_Data* d, UNS8 line, UNS8 nbBytes)
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   315
{
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   316
  if (nbBytes > SDO_MAX_LENGTH_TRANSFERT) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   317
    MSG_ERR(0x1A35,"SDO Size of data too large. Exceed SDO_MAX_LENGTH_TRANSFERT", nbBytes);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   318
    return 0xFF;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   319
  }
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   320
  d->transfers[line].count = nbBytes;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   321
  return 0;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   322
}
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   323
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   324
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   325
/***************************************************************************/
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   326
UNS8 sendSDO (CO_Data* d, UNS8 whoami, s_SDO sdo)
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   327
{	
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   328
  UNS16 offset;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   329
  UNS16 lastIndex;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   330
  UNS8 found = 0;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   331
  Message m;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   332
  UNS8 i;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   333
  UNS32 * pwCobId = NULL;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   334
  UNS8 * pwNodeId = NULL;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   335
  UNS8 *    pSize;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   336
  UNS8      size;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   337
  pSize = &size;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   338
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   339
  MSG_WAR(0x3A38, "sendSDO",0);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   340
  if( !((d->nodeState == Operational) ||  (d->nodeState == Pre_operational ))) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   341
    MSG_WAR(0x2A39, "unable to send the SDO (not in op or pre-op mode", d->nodeState);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   342
    return 0xFF;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   343
  }  				
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   344
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   345
  /*get the server->client cobid*/
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   346
  if ( whoami == SDO_SERVER )	{/*case server. Easy because today only one server SDO is authorized in CanFestival*/
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   347
    offset = d->firstIndex->SDO_SVR;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   348
    if (offset == 0) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   349
      MSG_ERR(0x1A42, "SendSDO : No SDO server found", 0); 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   350
      return 0xFF;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   351
    }
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   352
    pwCobId = d->objdict[offset].pSubindex[2].pObject;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   353
    MSG_WAR(0x3A41, "I am server. cobId : ", *pwCobId); 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   354
  }
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   355
  else {			/*case client*/
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   356
    /* Get the client->server cobid.*/
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   357
    UNS16 sdoNum = 0;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   358
    offset = d->firstIndex->SDO_CLT;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   359
    lastIndex = d->lastIndex->SDO_CLT;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   360
    if (offset == 0) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   361
      MSG_ERR(0x1A42, "SendSDO : No SDO client index found", 0); 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   362
      return 0xFF;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   363
    }
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   364
    /* First, have to find at the index where is defined the communication with the server node */
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   365
    while (offset <= lastIndex){
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   366
      MSG_WAR(0x3A43,"Reading index : ", 0x1280 + sdoNum);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   367
      if (d->objdict[offset].bSubCount <= 3) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   368
	MSG_ERR(0x1A28, "Subindex 3  not found at index ", 0x1280 + sdoNum);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   369
	return 0xFF;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   370
      }
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   371
      pwNodeId = d->objdict[offset].pSubindex[3].pObject;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   372
      MSG_WAR(0x3A44, "Found nodeId server = ", *pwNodeId);	
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   373
      if(*pwNodeId == sdo.nodeId) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   374
	found = 1;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   375
	break;          
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   376
      }      
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   377
      offset ++;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   378
      sdoNum ++;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   379
    }
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   380
    if (! found){
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   381
      MSG_WAR (0x2A45, "No SDO client corresponds to the mesage to send to node ", sdo.nodeId);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   382
      return 0xFF;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   383
    }
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   384
    /* Second, read the cobid client->server */
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   385
    pwCobId = d->objdict[offset].pSubindex[1].pObject;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   386
  }
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   387
  /* message copy for sending */
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   388
  m.cob_id.w = *pwCobId;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   389
  m.rtr = NOT_A_REQUEST; 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   390
  //the length of SDO must be 8
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   391
  m.len = 8;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   392
  for (i = 0 ; i < 8 ; i++) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   393
    m.data[i] =  sdo.body.data[i];
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   394
  }
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   395
  return (*d->canSend)(&m);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   396
}
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   397
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   398
/***************************************************************************/
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   399
UNS8 sendSDOabort (CO_Data* d, UNS8 whoami, UNS16 index, UNS8 subIndex, UNS32 abortCode)
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   400
{
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   401
  s_SDO sdo;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   402
  UNS8 ret;
1
b3dc740b4b04 Commit some Fix from Francis on sdo.c and abjacces.c. Fix endianisation problems while accessing OD. Also fix SDO abord handling bug.
etisserant
parents: 0
diff changeset
   403
  MSG_WAR(0x2A50,"Sending SDO abort ", abortCode);
0
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   404
  sdo.nodeId = *d->bDeviceNodeId;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   405
  sdo.body.data[0] = 0x80;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   406
  // Index
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   407
  sdo.body.data[1] = index & 0xFF; // LSB
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   408
  sdo.body.data[2] = (index >> 8) & 0xFF; // MSB
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   409
  // Subindex
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   410
  sdo.body.data[3] = subIndex;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   411
  // Data
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   412
  sdo.body.data[4] = (UNS8)(abortCode & 0xFF);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   413
  sdo.body.data[5] = (UNS8)((abortCode >> 8) & 0xFF);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   414
  sdo.body.data[6] = (UNS8)((abortCode >> 16) & 0xFF);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   415
  sdo.body.data[7] = (UNS8)((abortCode >> 24) & 0xFF);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   416
  ret = sendSDO(d, whoami, sdo);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   417
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   418
  return ret;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   419
}
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   420
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   421
/***************************************************************************/
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   422
UNS8 proceedSDO (CO_Data* d, Message *m)
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   423
{
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   424
  UNS8 err;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   425
  UNS8 line;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   426
  UNS8 nbBytes; // received or to be transmited.
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   427
  UNS8 nodeId = 0;  // The node from which the SDO is received
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   428
  UNS8 *pNodeId = NULL;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   429
  UNS8 whoami = SDO_UNKNOWN;  // SDO_SERVER or SDO_CLIENT.
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   430
  UNS32 errorCode; // while reading or writing in the local object dictionary.
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   431
  s_SDO sdo;    // SDO to transmit
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   432
  UNS16 index;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   433
  UNS8 subIndex;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   434
  UNS32 abortCode;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   435
  UNS8 i,j;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   436
  UNS32 *     pCobId = NULL;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   437
  UNS8 *      pSize;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   438
  UNS8        size;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   439
  UNS16 offset;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   440
  UNS16 lastIndex;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   441
  
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   442
  pSize = &size; 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   443
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   444
  MSG_WAR(0x3A60, "proceedSDO ", 0);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   445
  whoami = SDO_UNKNOWN;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   446
  // Looking for the cobId in the object dictionary.
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   447
  // Am-I a server ?
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   448
  offset = d->firstIndex->SDO_SVR;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   449
  lastIndex = d->lastIndex->SDO_SVR;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   450
  j = 0;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   451
  if(offset) while (offset <= lastIndex) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   452
     if (d->objdict[offset].bSubCount <= 1) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   453
	  MSG_ERR(0x1A61, "Subindex 1  not found at index ", 0x1200 + j);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   454
	  return 0xFF;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   455
	}
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   456
      pCobId = d->objdict[offset].pSubindex[1].pObject;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   457
      if ( *pCobId == (*m).cob_id.w ) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   458
	whoami = SDO_SERVER;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   459
	MSG_WAR(0x3A62, "proceedSDO. I am server. index : ", 0x1200 + j);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   460
	// In case of server, the node id of the client may be unknown. So we put the index minus offset
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   461
	// 0x1200 where the cobid received is defined.
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   462
	nodeId = j;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   463
	break;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   464
      }
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   465
      j++;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   466
      offset++;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   467
  } // end while
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   468
  if (whoami == SDO_UNKNOWN) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   469
    // Am-I client ?
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   470
    offset = d->firstIndex->SDO_CLT;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   471
    lastIndex = d->lastIndex->SDO_CLT;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   472
    j = 0;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   473
    if(offset) while (offset <= lastIndex) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   474
       if (d->objdict[offset].bSubCount <= 3) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   475
	 MSG_ERR(0x1A63, "Subindex 3  not found at index ", 0x1280 + j);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   476
	 return 0xFF;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   477
       }
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   478
       // a) Looking for the cobid received.
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   479
       pCobId = d->objdict[offset].pSubindex[2].pObject;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   480
       if (*pCobId == (*m).cob_id.w ) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   481
	 // b) cobid found, so reading the node id of the server.
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   482
	 pNodeId = d->objdict[offset].pSubindex[3].pObject;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   483
	 whoami = SDO_CLIENT;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   484
	 nodeId = *pNodeId;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   485
	 MSG_WAR(0x3A64, "proceedSDO. I am server. index : ", 0x1280 + j);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   486
	 MSG_WAR(0x3A65, "                 Server nodeId : ", nodeId);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   487
	 break;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   488
	}
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   489
       j++;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   490
       offset++;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   491
    } // end while
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   492
  }
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   493
  if (whoami == SDO_UNKNOWN) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   494
    return 0xFF;// This SDO was not for us !
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   495
  }
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   496
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   497
  // Test if the size of the SDO is ok
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   498
  if ( (*m).len != 8) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   499
    MSG_ERR(0x1A67, "Error size SDO. CobId  : ", (*m).cob_id.w);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   500
    failedSDO(d, nodeId, whoami, 0, 0, SDOABT_GENERAL_ERROR);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   501
    return 0xFF;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   502
  }
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   503
  
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   504
  if (whoami == SDO_CLIENT) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   505
    MSG_WAR(0x3A68, "I am CLIENT. Received SDO from nodeId : ", nodeId);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   506
  }
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   507
  else {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   508
    MSG_WAR(0x3A69, "I am SERVER. Received SDO cobId : ", (*m).cob_id.w);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   509
  }
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   510
    
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   511
  // Testing the command specifier
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   512
  // Allowed : cs = 0, 1, 2, 3, 4. (=  all except those for block tranfert).
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   513
  // cs = other : Not allowed -> abort.
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   514
  switch (getSDOcs(m->data[0])) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   515
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   516
  case 0:
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   517
    // I am SERVER
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   518
    if (whoami == SDO_SERVER) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   519
      // Receiving a download segment data.
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   520
      // A SDO transfert should have been yet initiated.
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   521
      err = getSDOlineOnUse( d, nodeId, whoami, &line ); 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   522
      if (!err)
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   523
	err = d->transfers[line].state != SDO_DOWNLOAD_IN_PROGRESS;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   524
      if (err) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   525
	MSG_ERR(0x1A70, "SDO error : Received download segment for unstarted trans. index 0x1200 + ", 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   526
		nodeId); 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   527
	failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   528
	return 0xFF;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   529
      }
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   530
      // Reset the wathdog
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   531
      RestartSDO_TIMER(line)
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   532
      MSG_WAR(0x3A71, "Received SDO download segment defined at index 0x1200 + ", nodeId); 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   533
      index = d->transfers[line].index;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   534
      subIndex = d->transfers[line].subIndex;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   535
      // Toggle test.
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   536
      if (d->transfers[line].toggle != getSDOt(m->data[0])) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   537
	MSG_ERR(0x1A72, "SDO error : Toggle error : ", getSDOt(m->data[0])); 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   538
	failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   539
	return 0xFF;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   540
      }
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   541
      // Nb of data to be downloaded
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   542
      nbBytes = 7 - getSDOn3(m->data[0]);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   543
      // Store the data in the transfert structure.
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   544
      err = SDOtoLine(d, line, nbBytes, (*m).data + 1);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   545
      if (err) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   546
	failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   547
	return 0xFF;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   548
      }
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   549
      // Sending the SDO response, CS = 1
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   550
      sdo.nodeId = *d->bDeviceNodeId; // The node id of the server, (here it is the sender).
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   551
      sdo.body.data[0] = (1 << 5) | (d->transfers[line].toggle << 4);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   552
      for (i = 1 ; i < 8 ; i++)
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   553
	sdo.body.data[i] = 0;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   554
      MSG_WAR(0x3A73, "SDO. Send response to download request defined at index 0x1200 + ", nodeId); 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   555
      sendSDO(d, whoami, sdo);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   556
      // Inverting the toggle for the next segment.
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   557
      d->transfers[line].toggle = ! d->transfers[line].toggle & 1;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   558
      // If it was the last segment,
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   559
      if (getSDOc(m->data[0])) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   560
	// Transfering line data to object dictionary.
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   561
	// The code does not use the "d" of initiate frame. So it is safe if e=s=0
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   562
	errorCode = SDOlineToObjdict(d, line);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   563
	if (errorCode) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   564
	  MSG_ERR(0x1A54, "SDO error : Unable to copy the data in the object dictionary", 0); 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   565
	  failedSDO(d, nodeId, whoami, index, subIndex, errorCode);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   566
	  return 0xFF;	  
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   567
	}
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   568
	// Release of the line
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   569
	resetSDOline(d, line);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   570
	MSG_WAR(0x3A74, "SDO. End of download defined at index 0x1200 + ", nodeId); 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   571
      }
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   572
    } // end if SERVER
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   573
    else { // if CLIENT
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   574
      // I am CLIENT
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   575
      // It is a request for a previous upload segment. We should find a line opened for this.
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   576
      err = getSDOlineOnUse( d, nodeId, whoami, &line);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   577
      if (!err)
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   578
	err = d->transfers[line].state != SDO_UPLOAD_IN_PROGRESS;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   579
      if (err) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   580
	MSG_ERR(0x1A75, "SDO error : Received segment response for unknown trans. from nodeId", nodeId); 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   581
	failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   582
	return 0xFF;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   583
      }
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   584
      // Reset the wathdog
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   585
      RestartSDO_TIMER(line)
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   586
      index = d->transfers[line].index;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   587
      subIndex = d->transfers[line].subIndex;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   588
      // test of the toggle;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   589
      if (d->transfers[line].toggle != getSDOt(m->data[0])) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   590
	MSG_ERR(0x1A76, "SDO error : Received segment response Toggle error. from nodeId", nodeId); 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   591
	failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   592
	return 0xFF;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   593
      }
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   594
      // nb of data to be uploaded
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   595
      nbBytes = 7 - getSDOn3(m->data[0]);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   596
      // Storing the data in the line structure.
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   597
      err = SDOtoLine(d, line, nbBytes, (*m).data + 1);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   598
      if (err) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   599
	failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   600
	return 0xFF;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   601
      }
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   602
      // Inverting the toggle for the next segment.
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   603
      d->transfers[line].toggle = ! d->transfers[line].toggle & 1;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   604
      // If it was the last segment,
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   605
      if ( getSDOc(m->data[0])) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   606
	// Put in state finished
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   607
	// The code is safe for the case e=s=0 in initiate frame.
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   608
	StopSDO_TIMER(line)
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   609
	d->transfers[line].state = SDO_FINISHED;
32
8afa33692372 SDO callbacks.
etisserant
parents: 31
diff changeset
   610
	if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId);
8afa33692372 SDO callbacks.
etisserant
parents: 31
diff changeset
   611
	
0
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   612
	MSG_WAR(0x3A77, "SDO. End of upload from node : ", nodeId);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   613
      }
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   614
      else { // more segments to receive
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   615
	     // Sending the request for the next segment.
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   616
	sdo.nodeId = nodeId;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   617
	sdo.body.data[0] = (3 << 5) | (d->transfers[line].toggle << 4);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   618
	for (i = 1 ; i < 8 ; i++)
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   619
	  sdo.body.data[i] = 0;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   620
	sendSDO(d, whoami, sdo);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   621
	MSG_WAR(0x3A78, "SDO send upload segment request to nodeId", nodeId);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   622
      }            
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   623
    } // End if CLIENT
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   624
    break;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   625
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   626
  case 1:
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   627
    // I am SERVER
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   628
    // Receive of an initiate download
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   629
    if (whoami == SDO_SERVER) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   630
      index = getSDOindex(m->data[1],m->data[2]);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   631
      subIndex = getSDOsubIndex(m->data[3]);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   632
      MSG_WAR(0x3A79, "Received SDO Initiate Download (to store data) defined at index 0x1200 + ", 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   633
	      nodeId); 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   634
      MSG_WAR(0x3A80, "Writing at index : ", index);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   635
      MSG_WAR(0x3A80, "Writing at subIndex : ", subIndex);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   636
      
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   637
      // Search if a SDO transfert have been yet initiated
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   638
      err = getSDOlineOnUse( d, nodeId, whoami, &line );
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   639
      if (! err) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   640
	MSG_ERR(0x1A81, "SDO error : Transmission yet started.", 0); 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   641
	failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   642
	return 0xFF;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   643
      }
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   644
      // No line on use. Great !
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   645
      // Try to open a new line.
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   646
      err = getSDOfreeLine( d, whoami, &line );
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   647
      if (err) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   648
	MSG_ERR(0x1A82, "SDO error : No line free, too many SDO in progress. Aborted.", 0);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   649
	failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   650
	return 0xFF;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   651
      }
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   652
      initSDOline(d, line, nodeId, index, subIndex, SDO_DOWNLOAD_IN_PROGRESS);      
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   653
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   654
      if (getSDOe(m->data[0])) { // If SDO expedited
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   655
	// nb of data to be downloaded
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   656
	nbBytes = 4 - getSDOn2(m->data[0]);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   657
	// Storing the data in the line structure.
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   658
	d->transfers[line].count = nbBytes;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   659
	err = SDOtoLine(d, line, nbBytes, (*m).data + 4);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   660
	
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   661
	if (err) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   662
	  failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   663
	  return 0xFF;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   664
	}	  
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   665
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   666
	// SDO expedited -> transfert finished. Data can be stored in the dictionary.
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   667
	// The line will be reseted when it is downloading in the dictionary.
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   668
	MSG_WAR(0x3A83, "SDO Initiate Download is an expedited transfert. Finished.: ", nodeId);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   669
	// Transfering line data to object dictionary.
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   670
	errorCode = SDOlineToObjdict(d, line);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   671
	if (errorCode) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   672
	  MSG_ERR(0x1A84, "SDO error : Unable to copy the data in the object dictionary", 0); 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   673
	  failedSDO(d, nodeId, whoami, index, subIndex, errorCode);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   674
	  return 0xFF;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   675
	}
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   676
	// Release of the line.
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   677
	resetSDOline(d, line);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   678
      }
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   679
      else {// So, if it is not an expedited transfert
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   680
	if (getSDOs(m->data[0])) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   681
	  // TODO : if e and s = 0, not reading m->data[4] but put nbBytes = 0
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   682
	  nbBytes = m->data[4]; // Transfert limited to 255 bytes.
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   683
	  err = setSDOlineRestBytes(d, nodeId, nbBytes);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   684
	  if (err) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   685
	    failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   686
	    return 0xFF;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   687
	  }	
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   688
	}
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   689
      }
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   690
      //Sending a SDO, cs=3
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   691
      sdo.nodeId = *d->bDeviceNodeId; // The node id of the server, (here it is the sender).
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   692
      sdo.body.data[0] = 3 << 5;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   693
      sdo.body.data[1] = index & 0xFF;        // LSB
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   694
      sdo.body.data[2] = (index >> 8) & 0xFF; // MSB 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   695
      sdo.body.data[3] = subIndex;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   696
      for (i = 4 ; i < 8 ; i++)
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   697
		sdo.body.data[i] = 0;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   698
      sendSDO(d, whoami, sdo);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   699
    } // end if I am SERVER
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   700
    else {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   701
      // I am CLIENT
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   702
      // It is a response for a previous download segment. We should find a line opened for this.
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   703
      err = getSDOlineOnUse( d, nodeId, whoami, &line);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   704
      if (!err)
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   705
	err = d->transfers[line].state != SDO_DOWNLOAD_IN_PROGRESS;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   706
      if (err) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   707
	MSG_ERR(0x1A85, "SDO error : Received segment response for unknown trans. from nodeId", nodeId); 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   708
	failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   709
	return 0xFF;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   710
      }
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   711
      // Reset the wathdog
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   712
      RestartSDO_TIMER(line)
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   713
      index = d->transfers[line].index;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   714
      subIndex = d->transfers[line].subIndex;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   715
      // test of the toggle;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   716
      if (d->transfers[line].toggle != getSDOt(m->data[0])) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   717
	MSG_ERR(0x1A86, "SDO error : Received segment response Toggle error. from nodeId", nodeId); 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   718
	failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   719
	return 0xFF;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   720
      }
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   721
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   722
      // End transmission or downloading next segment. We need to know if it will be the last one.
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   723
      getSDOlineRestBytes(d, line, &nbBytes);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   724
      if (nbBytes == 0) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   725
	MSG_WAR(0x3A87, "SDO End download. segment response received. OK. from nodeId", nodeId); 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   726
	StopSDO_TIMER(line)
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   727
	d->transfers[line].state = SDO_FINISHED;
32
8afa33692372 SDO callbacks.
etisserant
parents: 31
diff changeset
   728
	if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId);
0
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   729
	return 0x00;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   730
      }
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   731
      // At least one transfer to send.	  
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   732
      if (nbBytes > 7) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   733
	// several segments to download.
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   734
	// code to send the next segment. (cs = 0; c = 0)
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   735
	d->transfers[line].toggle = ! d->transfers[line].toggle & 1;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   736
	sdo.nodeId = nodeId; // The server node Id;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   737
	sdo.body.data[0] = (d->transfers[line].toggle << 4);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   738
	err = lineToSDO(d, line, 7, sdo.body.data + 1);	 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   739
	if (err) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   740
	  failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   741
	  return 0xFF;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   742
	}
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   743
      } 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   744
      else {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   745
	// Last segment.
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   746
	// code to send the last segment. (cs = 0; c = 1)
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   747
	d->transfers[line].toggle = ! d->transfers[line].toggle & 1;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   748
	sdo.nodeId = nodeId; // The server node Id;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   749
	sdo.body.data[0] = (d->transfers[line].toggle << 4) | ((7 - nbBytes) << 1) | 1;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   750
	err = lineToSDO(d, line, nbBytes, sdo.body.data + 1);	 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   751
	if (err) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   752
	  failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   753
	  return 0xFF;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   754
	}
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   755
	for (i = nbBytes + 1 ; i < 8 ; i++)
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   756
	  sdo.body.data[i] = 0;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   757
      }
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   758
      MSG_WAR(0x3A88, "SDO sending download segment to nodeId", nodeId); 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   759
      sendSDO(d, whoami, sdo); 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   760
    } // end if I am a CLIENT			  
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   761
    break;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   762
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   763
  case 2:
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   764
    // I am SERVER
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   765
    // Receive of an initiate upload.
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   766
    if (whoami == SDO_SERVER) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   767
      index = getSDOindex(m->data[1],m->data[2]);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   768
      subIndex = getSDOsubIndex(m->data[3]);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   769
      MSG_WAR(0x3A89, "Received SDO Initiate upload (to send data) defined at index 0x1200 + ", 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   770
	      nodeId); 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   771
      MSG_WAR(0x3A90, "Reading at index : ", index);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   772
      MSG_WAR(0x3A91, "Reading at subIndex : ", subIndex);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   773
      // Search if a SDO transfert have been yet initiated
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   774
      err = getSDOlineOnUse( d, nodeId, whoami, &line );
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   775
      if (! err) {
12
1d3b9ad366c8 CANOPEN_BIG_ENDIAN :
dejoigny
parents: 6
diff changeset
   776
	    MSG_ERR(0x1A92, "SDO error : Transmission yet started at line : ", line); 
0
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   777
        MSG_WAR(0x3A93, "nodeId = ", nodeId); 
12
1d3b9ad366c8 CANOPEN_BIG_ENDIAN :
dejoigny
parents: 6
diff changeset
   778
	    failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR);
1d3b9ad366c8 CANOPEN_BIG_ENDIAN :
dejoigny
parents: 6
diff changeset
   779
	    return 0xFF;
0
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   780
      }
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   781
      // No line on use. Great !
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   782
      // Try to open a new line.
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   783
      err = getSDOfreeLine( d, whoami, &line );
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   784
      if (err) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   785
	MSG_ERR(0x1A71, "SDO error : No line free, too many SDO in progress. Aborted.", 0);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   786
	failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   787
	return 0xFF;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   788
      }
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   789
      initSDOline(d, line, nodeId, index, subIndex, SDO_UPLOAD_IN_PROGRESS);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   790
      // Transfer data from dictionary to the line structure.
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   791
      errorCode = objdictToSDOline(d, line);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   792
     
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   793
      if (errorCode) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   794
	MSG_ERR(0x1A94, "SDO error : Unable to copy the data from object dictionary. Err code : ", 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   795
		errorCode); 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   796
	failedSDO(d, nodeId, whoami, index, subIndex, errorCode);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   797
	return 0xFF;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   798
	}
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   799
      // Preparing the response.
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   800
      getSDOlineRestBytes(d, line, &nbBytes);	// Nb bytes to transfer ?
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   801
      sdo.nodeId = nodeId; // The server node Id;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   802
      if (nbBytes > 4) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   803
	// normal transfert. (segmented).
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   804
	// code to send the initiate upload response. (cs = 2)
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   805
	sdo.body.data[0] = (2 << 5) | 1;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   806
	sdo.body.data[1] = index & 0xFF;        // LSB
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   807
	sdo.body.data[2] = (index >> 8) & 0xFF; // MSB 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   808
	sdo.body.data[3] = subIndex;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   809
        sdo.body.data[4] = nbBytes; // Limitation of canfestival2 : Max tranfert is 256 bytes.
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   810
	// It takes too much memory to upgrate to 2^32 because the size of data is also coded
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   811
	// in the object dictionary, at every index and subindex.
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   812
	for (i = 5 ; i < 8 ; i++)
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   813
	  sdo.body.data[i] = 0;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   814
	MSG_WAR(0x3A95, "SDO. Sending normal upload initiate response defined at index 0x1200 + ", nodeId); 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   815
	sendSDO(d, whoami, sdo); 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   816
      }
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   817
      else {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   818
	// Expedited upload. (cs = 2 ; e = 1) 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   819
	sdo.body.data[0] = (2 << 5) | ((4 - nbBytes) << 2) | 3;  
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   820
	sdo.body.data[1] = index & 0xFF;        // LSB
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   821
	sdo.body.data[2] = (index >> 8) & 0xFF; // MSB 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   822
	sdo.body.data[3] = subIndex;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   823
	err = lineToSDO(d, line, nbBytes, sdo.body.data + 4);	 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   824
	if (err) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   825
	  failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   826
	  return 0xFF;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   827
	}
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   828
	for (i = 4 + nbBytes ; i < 8 ; i++)
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   829
	  sdo.body.data[i] = 0;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   830
	MSG_WAR(0x3A96, "SDO. Sending expedited upload initiate response defined at index 0x1200 + ", 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   831
		nodeId); 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   832
	sendSDO(d, whoami, sdo); 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   833
	// Release the line.
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   834
	resetSDOline(d, line);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   835
      }
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   836
    } // end if I am SERVER
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   837
    else {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   838
      // I am CLIENT
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   839
      // It is the response for the previous initiate upload request. 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   840
      // We should find a line opened for this.
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   841
      err = getSDOlineOnUse( d, nodeId, whoami, &line);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   842
      if (!err)
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   843
	err = d->transfers[line].state != SDO_UPLOAD_IN_PROGRESS;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   844
      if (err) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   845
	MSG_ERR(0x1A97, "SDO error : Received response for unknown upload request from nodeId", nodeId); 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   846
	failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   847
	return 0xFF;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   848
      }
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   849
      // Reset the wathdog
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   850
      RestartSDO_TIMER(line)
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   851
      index = d->transfers[line].index;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   852
      subIndex = d->transfers[line].subIndex;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   853
      
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   854
      if (getSDOe(m->data[0])) { // If SDO expedited
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   855
	// nb of data to be uploaded
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   856
	  nbBytes = 4 - getSDOn2(m->data[0]);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   857
	// Storing the data in the line structure.
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   858
	err = SDOtoLine(d, line, nbBytes, (*m).data + 4);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   859
	if (err) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   860
	  failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   861
	  return 0xFF;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   862
	}
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   863
	// SDO expedited -> transfert finished. data are available via  getReadResultNetworkDict().
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   864
	MSG_WAR(0x3A98, "SDO expedited upload finished. Response received from node : ", nodeId);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   865
	StopSDO_TIMER(line)
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   866
	d->transfers[line].count = nbBytes;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   867
	d->transfers[line].state = SDO_FINISHED;
32
8afa33692372 SDO callbacks.
etisserant
parents: 31
diff changeset
   868
	if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId);
0
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   869
	return 0;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   870
      }
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   871
      else { // So, if it is not an expedited transfert
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   872
	// Storing the nb of data to receive.
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   873
	if (getSDOs(m->data[0])) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   874
	  nbBytes = m->data[4]; // Remember the limitation to 255 bytes to transfert
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   875
	  err = setSDOlineRestBytes(d, line, nbBytes);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   876
	  if (err) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   877
	    failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   878
	    return 0xFF;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   879
	  }	
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   880
	}
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   881
	// Requesting next segment. (cs = 3)
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   882
	sdo.nodeId = nodeId;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   883
	sdo.body.data[0] = 3 << 5;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   884
	for (i = 1 ; i < 8 ; i++)
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   885
	  sdo.body.data[i] = 0;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   886
	MSG_WAR(0x3A99, "SDO. Sending upload segment request to node : ", nodeId); 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   887
	sendSDO(d, whoami, sdo);  
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   888
      }
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   889
    } // End if CLIENT
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   890
    break;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   891
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   892
  case 3:
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   893
    // I am SERVER
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   894
    if (whoami == SDO_SERVER) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   895
      // Receiving a upload segment.
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   896
      // A SDO transfert should have been yet initiated.
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   897
      err = getSDOlineOnUse( d, nodeId, whoami, &line ); 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   898
      if (!err)
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   899
	err = d->transfers[line].state != SDO_UPLOAD_IN_PROGRESS;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   900
      if (err) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   901
	MSG_ERR(0x1AA0, "SDO error : Received upload segment for unstarted trans. index 0x1200 + ", 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   902
		nodeId); 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   903
	failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   904
	return 0xFF;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   905
      }
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   906
      // Reset the wathdog
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   907
      RestartSDO_TIMER(line)
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   908
      MSG_WAR(0x3AA1, "Received SDO upload segment defined at index 0x1200 + ", nodeId); 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   909
      index = d->transfers[line].index;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   910
      subIndex = d->transfers[line].subIndex;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   911
      // Toggle test.
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   912
      if (d->transfers[line].toggle != getSDOt(m->data[0])) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   913
	MSG_ERR(0x1AA2, "SDO error : Toggle error : ", getSDOt(m->data[0])); 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   914
	failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   915
	return 0xFF;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   916
      }
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   917
      // Uploading next segment. We need to know if it will be the last one.
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   918
      getSDOlineRestBytes(d, line, &nbBytes);	  	  
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   919
      if (nbBytes > 7) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   920
	// The segment to transfer is not the last one.
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   921
	// code to send the next segment. (cs = 0; c = 0)
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   922
	sdo.nodeId = nodeId; // The server node Id;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   923
	sdo.body.data[0] = (d->transfers[line].toggle << 4);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   924
	err = lineToSDO(d, line, 7, sdo.body.data + 1);	 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   925
	if (err) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   926
	  failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   927
	  return 0xFF;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   928
	}
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   929
	// Inverting the toggle for the next tranfert.
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   930
	d->transfers[line].toggle = ! d->transfers[line].toggle & 1;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   931
	MSG_WAR(0x3AA3, "SDO. Sending upload segment defined at index 0x1200 + ", nodeId); 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   932
	sendSDO(d, whoami, sdo); 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   933
      } 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   934
      else {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   935
	// Last segment.
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   936
	// code to send the last segment. (cs = 0; c = 1)	    
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   937
	sdo.nodeId = nodeId; // The server node Id;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   938
	sdo.body.data[0] = (d->transfers[line].toggle << 4) | ((7 - nbBytes) << 1) | 1;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   939
	err = lineToSDO(d, line, nbBytes, sdo.body.data + 1);	 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   940
	if (err) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   941
	  failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   942
	  return 0xFF;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   943
	}
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   944
	for (i = nbBytes + 1 ; i < 8 ; i++)
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   945
	  sdo.body.data[i] = 0;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   946
	MSG_WAR(0x3AA4, "SDO. Sending last upload segment defined at index 0x1200 + ", nodeId);      
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   947
	sendSDO(d, whoami, sdo);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   948
	// Release the line
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   949
	resetSDOline(d, line);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   950
      }
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   951
    } // end if SERVER	
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   952
    else {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   953
      // I am CLIENT
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   954
      // It is the response for the previous initiate download request. 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   955
      // We should find a line opened for this.
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   956
      err = getSDOlineOnUse( d, nodeId, whoami, &line);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   957
      if (!err)
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   958
	err = d->transfers[line].state != SDO_DOWNLOAD_IN_PROGRESS;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   959
      if (err) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   960
	MSG_ERR(0x1AA5, "SDO error : Received response for unknown download request from nodeId", nodeId); 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   961
	failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   962
	return 0xFF;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   963
      }
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   964
      // Reset the wathdog
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   965
      RestartSDO_TIMER(line)
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   966
      index = d->transfers[line].index;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   967
      subIndex = d->transfers[line].subIndex;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   968
      // End transmission or requesting  next segment. 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   969
      getSDOlineRestBytes(d, line, &nbBytes);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   970
      if (nbBytes == 0) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   971
	MSG_WAR(0x3AA6, "SDO End download expedited. Response received. from nodeId", nodeId); 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   972
	StopSDO_TIMER(line)
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   973
	d->transfers[line].state = SDO_FINISHED;
32
8afa33692372 SDO callbacks.
etisserant
parents: 31
diff changeset
   974
	if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId);
0
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   975
	return 0x00;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   976
      }	  
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   977
      if (nbBytes > 7) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   978
	// more than one request to send
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   979
	// code to send the next segment. (cs = 0; c = 0)	    
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   980
	sdo.nodeId = nodeId; // The server node Id;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   981
	sdo.body.data[0] = (d->transfers[line].toggle << 4);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   982
	err = lineToSDO(d, line, 7, sdo.body.data + 1);	 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   983
	if (err) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   984
	  failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   985
	  return 0xFF;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   986
	}
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   987
      } 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   988
      else {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   989
	// Last segment.
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   990
	// code to send the last segment. (cs = 0; c = 1)	   
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   991
	sdo.nodeId = nodeId; // The server node Id;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   992
	sdo.body.data[0] = (d->transfers[line].toggle << 4) | ((7 - nbBytes) << 1) | 1;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   993
	err = lineToSDO(d, line, nbBytes, sdo.body.data + 1);	 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   994
	if (err) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   995
	  failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   996
	  return 0xFF;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   997
	}
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   998
	for (i = nbBytes + 1 ; i < 8 ; i++)
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
   999
	  sdo.body.data[i] = 0;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1000
      }
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1001
      MSG_WAR(0x3AA7, "SDO sending download segment to nodeId", nodeId); 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1002
      sendSDO(d, whoami, sdo); 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1003
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1004
    } // end if I am a CLIENT			  
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1005
    break;  
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1006
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1007
  case 4:
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1008
    abortCode = (*m).data[3] | 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1009
      (m->data[5] << 8) |
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1010
      (m->data[6] << 16) |
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1011
      (m->data[7] << 24);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1012
    // Received SDO abort.
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1013
    // Looking for the line concerned.
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1014
    if (whoami == SDO_SERVER) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1015
      err = getSDOlineOnUse( d, nodeId, whoami, &line );
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1016
      if (!err) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1017
	resetSDOline( d, line );
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1018
	MSG_WAR(0x3AA8, "SD0. Received SDO abort. Line released. Code : ", abortCode);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1019
      }
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1020
      else
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1021
	MSG_WAR(0x3AA9, "SD0. Received SDO abort. No line found. Code : ", abortCode);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1022
      // Tips : The end user has no way to know that the server node has received an abort SDO. 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1023
      // Its is ok, I think.
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1024
    }
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1025
    else { // If I am CLIENT
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1026
      err = getSDOlineOnUse( d, nodeId, whoami, &line );
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1027
      if (!err) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1028
	// The line *must* be released by the core program.
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1029
	StopSDO_TIMER(line)
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1030
	d->transfers[line].state = SDO_ABORTED_RCV;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1031
	MSG_WAR(0x3AB0, "SD0. Received SDO abort. Line state ABORTED. Code : ", abortCode);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1032
      }
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1033
      else
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1034
	MSG_WAR(0x3AB1, "SD0. Received SDO abort. No line found. Code : ", abortCode);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1035
    } 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1036
    break;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1037
  default:
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1038
    // Error : Unknown cs
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1039
    MSG_ERR(0x1AB2, "SDO. Received unknown command specifier : ", getSDOcs(m->data[0]));
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1040
    return 0xFF;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1041
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1042
  } // End switch
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1043
  return 0;     
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1044
}
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1045
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1046
/*******************************************************************)******/
32
8afa33692372 SDO callbacks.
etisserant
parents: 31
diff changeset
  1047
inline UNS8 _writeNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, 
8afa33692372 SDO callbacks.
etisserant
parents: 31
diff changeset
  1048
		       UNS8 subIndex, UNS8 count, UNS8 dataType, void *data, SDOCallback_t Callback)
0
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1049
{
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1050
  UNS8 err;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1051
  UNS8 SDOfound = 0;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1052
  UNS8 line;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1053
  s_SDO sdo;    // SDO to transmit
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1054
  UNS8 i, j;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1055
  UNS8 *    pSize;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1056
  UNS8      size;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1057
  UNS16     lastIndex;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1058
  UNS16     offset;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1059
  UNS8      *pNodeIdServer;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1060
  UNS8      nodeIdServer;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1061
  pSize = &size;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1062
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1063
  MSG_WAR(0x3AC0, "Send SDO to write in the dictionary of node : ", nodeId);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1064
  MSG_WAR(0x3AC1, "                                   At index : ", index);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1065
  MSG_WAR(0x3AC2, "                                   subIndex : ", subIndex);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1066
  MSG_WAR(0x3AC3, "                                   nb bytes : ", count);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1067
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1068
  // Verify that there is no SDO communication yet.
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1069
  err = getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1070
  if (!err) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1071
    MSG_ERR(0x1AC4, "SDO error : Communication yet established. with node : ", nodeId); 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1072
    return 0xFF;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1073
  }
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1074
  // Taking the line ...
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1075
  err = getSDOfreeLine( d, SDO_CLIENT, &line );
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1076
  if (err) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1077
    MSG_ERR(0x1AC5, "SDO error : No line free, too many SDO in progress. Aborted for node : ", nodeId); 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1078
    return (0xFF);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1079
  }
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1080
  // Check which SDO to use to communicate with the node
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1081
  offset = d->firstIndex->SDO_CLT;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1082
  lastIndex = d->lastIndex->SDO_CLT;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1083
  if (offset == 0) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1084
    MSG_ERR(0x1AC6, "writeNetworkDict : No SDO client index found", 0); 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1085
    return 0xFF;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1086
  }
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1087
  i = 0;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1088
   while (offset <= lastIndex) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1089
     if (d->objdict[offset].bSubCount <= 3) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1090
	 MSG_ERR(0x1AC8, "Subindex 3  not found at index ", 0x1280 + i);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1091
	 return 0xFF;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1092
     }
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1093
     // looking for the nodeId server
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1094
     pNodeIdServer = d->objdict[offset].pSubindex[3].pObject;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1095
     nodeIdServer = *pNodeIdServer;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1096
     MSG_WAR(0x1AD2, "index : ", 0x1280 + i);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1097
     MSG_WAR(0x1AD3, "nodeIdServer : ", nodeIdServer);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1098
   
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1099
    if(nodeIdServer == nodeId) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1100
      SDOfound = 1;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1101
      break;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1102
    }
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1103
    offset++;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1104
    i++;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1105
  } // end while
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1106
  if (!SDOfound) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1107
    MSG_ERR(0x1AC9, "SDO. Error. No client found to communicate with node : ", nodeId);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1108
    return 0xFF;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1109
  }
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1110
  MSG_WAR(0x3AD0,"        SDO client defined at index  : ", 0x1280 + i);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1111
  initSDOline(d, line, nodeId, index, subIndex, SDO_DOWNLOAD_IN_PROGRESS);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1112
  d->transfers[line].count = count;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1113
  d->transfers[line].dataType = dataType;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1114
  
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1115
  // Copy data to transfers structure.
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1116
  for (j = 0 ; j < count ; j++) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1117
# ifdef CANOPEN_BIG_ENDIAN
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1118
    if (dataType == 0)
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1119
      d->transfers[line].data[count - 1 - j] = ((char *)data)[j];
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1120
    else // String of bytes.
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1121
      d->transfers[line].data[j] = ((char *)data)[j];
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1122
#  else 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1123
    d->transfers[line].data[j] = ((char *)data)[j];
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1124
#  endif
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1125
  }
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1126
  // Send the SDO to the server. Initiate download, cs=1.
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1127
  sdo.nodeId = nodeId;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1128
  if (count <= 4) { // Expedited transfert
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1129
    sdo.body.data[0] = (1 << 5) | ((4 - count) << 2) | 3;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1130
    for (i = 4 ; i < 8 ; i++)
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1131
      sdo.body.data[i] = d->transfers[line].data[i - 4];
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1132
    d->transfers[line].offset = count;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1133
  }	
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1134
  else { // Normal transfert
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1135
    sdo.body.data[0] = (1 << 5) | 1;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1136
    sdo.body.data[4] = count; // nb of byte to transmit. Max = 255. (canfestival2 limitation).
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1137
    for (i = 5 ; i < 8 ; i++)
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1138
      sdo.body.data[i] = 0;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1139
  }
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1140
  sdo.body.data[1] = index & 0xFF;        // LSB
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1141
  sdo.body.data[2] = (index >> 8) & 0xFF; // MSB 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1142
  sdo.body.data[3] = subIndex;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1143
  
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1144
  err = sendSDO(d, SDO_CLIENT, sdo);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1145
  if (err) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1146
    MSG_ERR(0x1AD1, "SDO. Error while sending SDO to node : ", nodeId);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1147
    // release the line
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1148
    resetSDOline(d, line);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1149
    return 0xFF;
32
8afa33692372 SDO callbacks.
etisserant
parents: 31
diff changeset
  1150
  }
8afa33692372 SDO callbacks.
etisserant
parents: 31
diff changeset
  1151
  d->transfers[line].Callback = Callback;
0
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1152
  return 0;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1153
}
32
8afa33692372 SDO callbacks.
etisserant
parents: 31
diff changeset
  1154
UNS8 writeNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, 
8afa33692372 SDO callbacks.
etisserant
parents: 31
diff changeset
  1155
		       UNS8 subIndex, UNS8 count, UNS8 dataType, void *data)
8afa33692372 SDO callbacks.
etisserant
parents: 31
diff changeset
  1156
{
8afa33692372 SDO callbacks.
etisserant
parents: 31
diff changeset
  1157
	return _writeNetworkDict (d, nodeId, index, subIndex, count, dataType, data, NULL);
8afa33692372 SDO callbacks.
etisserant
parents: 31
diff changeset
  1158
}
8afa33692372 SDO callbacks.
etisserant
parents: 31
diff changeset
  1159
8afa33692372 SDO callbacks.
etisserant
parents: 31
diff changeset
  1160
UNS8 writeNetworkDictCallBack (CO_Data* d, UNS8 nodeId, UNS16 index, 
8afa33692372 SDO callbacks.
etisserant
parents: 31
diff changeset
  1161
		       UNS8 subIndex, UNS8 count, UNS8 dataType, void *data, SDOCallback_t Callback)
8afa33692372 SDO callbacks.
etisserant
parents: 31
diff changeset
  1162
{
8afa33692372 SDO callbacks.
etisserant
parents: 31
diff changeset
  1163
	return _writeNetworkDict (d, nodeId, index, subIndex, count, dataType, data, Callback);	
8afa33692372 SDO callbacks.
etisserant
parents: 31
diff changeset
  1164
}
8afa33692372 SDO callbacks.
etisserant
parents: 31
diff changeset
  1165
8afa33692372 SDO callbacks.
etisserant
parents: 31
diff changeset
  1166
8afa33692372 SDO callbacks.
etisserant
parents: 31
diff changeset
  1167
/***************************************************************************/
8afa33692372 SDO callbacks.
etisserant
parents: 31
diff changeset
  1168
UNS8 _readNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType, SDOCallback_t Callback)
0
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1169
{
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1170
  UNS8 err;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1171
  UNS8 SDOfound = 0;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1172
  UNS8 i;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1173
  UNS8 line;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1174
  UNS8 *    pSize;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1175
  UNS8      size;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1176
  s_SDO sdo;    // SDO to transmit
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1177
  UNS8      *pNodeIdServer;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1178
  UNS8      nodeIdServer;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1179
  UNS16     offset;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1180
  UNS16     lastIndex;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1181
  pSize = &size;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1182
  MSG_WAR(0x3AD5, "Send SDO to read in the dictionary of node : ", nodeId);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1183
  MSG_WAR(0x3AD6, "                                  At index : ", index);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1184
  MSG_WAR(0x3AD7, "                                  subIndex : ", subIndex);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1185
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1186
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1187
  // Verify that there is no SDO communication yet.
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1188
  err = getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1189
  if (!err) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1190
    MSG_ERR(0x1AD8, "SDO error : Communication yet established. with node : ", nodeId); 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1191
    return 0xFF;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1192
  }
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1193
  // Taking the line ...
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1194
  err = getSDOfreeLine( d, SDO_CLIENT, &line );
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1195
  if (err) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1196
    MSG_ERR(0x1AD9, "SDO error : No line free, too many SDO in progress. Aborted for node : ", nodeId); 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1197
    return (0xFF);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1198
  }
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1199
  else
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1200
    MSG_WAR(0x3AE0, "Transmission on line : ", line);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1201
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1202
  // Check which SDO to use to communicate with the node
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1203
  offset = d->firstIndex->SDO_CLT;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1204
  lastIndex = d->lastIndex->SDO_CLT;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1205
  if (offset == 0) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1206
    MSG_ERR(0x1AE1, "writeNetworkDict : No SDO client index found", 0); 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1207
    return 0xFF;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1208
  }
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1209
  i = 0;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1210
  while (offset <= lastIndex) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1211
     if (d->objdict[offset].bSubCount <= 3) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1212
	 MSG_ERR(0x1AE2, "Subindex 3  not found at index ", 0x1280 + i);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1213
	 return 0xFF;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1214
     }
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1215
     // looking for the nodeId server
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1216
     pNodeIdServer = d->objdict[offset].pSubindex[3].pObject;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1217
     nodeIdServer = *pNodeIdServer;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1218
   
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1219
    if(nodeIdServer == nodeId) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1220
      SDOfound = 1;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1221
      break;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1222
    }
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1223
    offset++;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1224
    i++;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1225
  } // end while
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1226
  if (!SDOfound) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1227
    MSG_ERR(0x1AE3, "SDO. Error. No client found to communicate with node : ", nodeId);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1228
    return 0xFF;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1229
  }
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1230
  MSG_WAR(0x3AE4,"        SDO client defined at index  : ", 0x1280 + i);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1231
  initSDOline(d, line, nodeId, index, subIndex, SDO_UPLOAD_IN_PROGRESS);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1232
  getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1233
  sdo.nodeId = nodeId;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1234
  // Send the SDO to the server. Initiate upload, cs=2.
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1235
  d->transfers[line].dataType = dataType;				
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1236
  sdo.body.data[0] = (2 << 5);	
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1237
  sdo.body.data[1] = index & 0xFF;        // LSB
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1238
  sdo.body.data[2] = (index >> 8) & 0xFF; // MSB 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1239
  sdo.body.data[3] = subIndex;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1240
  for (i = 4 ; i < 8 ; i++)
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1241
    sdo.body.data[i] = 0;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1242
  err = sendSDO(d, SDO_CLIENT, sdo);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1243
  if (err) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1244
    MSG_ERR(0x1AE5, "SDO. Error while sending SDO to node : ", nodeId);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1245
    // release the line
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1246
    resetSDOline(d, line);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1247
    return 0xFF;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1248
  }		
32
8afa33692372 SDO callbacks.
etisserant
parents: 31
diff changeset
  1249
  d->transfers[line].Callback = Callback;
0
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1250
  return 0;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1251
}
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1252
32
8afa33692372 SDO callbacks.
etisserant
parents: 31
diff changeset
  1253
UNS8 readNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType)
8afa33692372 SDO callbacks.
etisserant
parents: 31
diff changeset
  1254
{
8afa33692372 SDO callbacks.
etisserant
parents: 31
diff changeset
  1255
	return _readNetworkDict (d, nodeId, index, subIndex, dataType, NULL);
8afa33692372 SDO callbacks.
etisserant
parents: 31
diff changeset
  1256
}
8afa33692372 SDO callbacks.
etisserant
parents: 31
diff changeset
  1257
8afa33692372 SDO callbacks.
etisserant
parents: 31
diff changeset
  1258
UNS8 readNetworkDictCallback (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType, SDOCallback_t Callback)
8afa33692372 SDO callbacks.
etisserant
parents: 31
diff changeset
  1259
{
8afa33692372 SDO callbacks.
etisserant
parents: 31
diff changeset
  1260
	return _readNetworkDict (d, nodeId, index, subIndex, dataType, Callback);
8afa33692372 SDO callbacks.
etisserant
parents: 31
diff changeset
  1261
}
0
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1262
/***************************************************************************/
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1263
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1264
UNS8 getReadResultNetworkDict (CO_Data* d, UNS8 nodeId, void* data, UNS8 *size, 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1265
			       UNS32 * abortCode)
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1266
{
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1267
  UNS8 i;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1268
  UNS8 err;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1269
  UNS8 line;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1270
  * size = 0;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1271
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1272
  // Looking for the line tranfert.
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1273
  err = getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1274
  if (err) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1275
    MSG_ERR(0x1AF0, "SDO error : No line found for communication with node : ", nodeId); 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1276
    return SDO_ABORTED_INTERNAL;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1277
  }
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1278
  if (d->transfers[line].state != SDO_FINISHED)
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1279
    return d->transfers[line].state;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1280
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1281
  // Transfert is finished. Put the value in the data.
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1282
  * size = d->transfers[line].count;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1283
  for  ( i = 0 ; i < *size ; i++) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1284
# ifdef CANOPEN_BIG_ENDIAN
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1285
    if (d->transfers[line].dataType != visible_string)
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1286
      ( (char *) data)[*size - 1 - i] = d->transfers[line].data[i];
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1287
    else // String of bytes.
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1288
      ( (char *) data)[i] = d->transfers[line].data[i];
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1289
# else 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1290
    ( (char *) data)[i] = d->transfers[line].data[i];
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1291
# endif
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1292
  } 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1293
  return SDO_FINISHED;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1294
}
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1295
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1296
/***************************************************************************/
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1297
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1298
UNS8 getWriteResultNetworkDict (CO_Data* d, UNS8 nodeId, UNS32 * abortCode)
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1299
{
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1300
  UNS8 line = 0;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1301
  UNS8 err;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1302
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1303
  * abortCode = 0;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1304
  // Looking for the line tranfert.
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1305
  err = getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line);
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1306
  if (err) {
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1307
    MSG_ERR(0x1AF1, "SDO error : No line found for communication with node : ", nodeId); 
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1308
    return SDO_ABORTED_INTERNAL;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1309
  }
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1310
  * abortCode = d->transfers[line].abortCode;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1311
  return d->transfers[line].state;
4472ee7c6c3e Commit a new cvs repo.
etisserant
parents:
diff changeset
  1312
}
1
b3dc740b4b04 Commit some Fix from Francis on sdo.c and abjacces.c. Fix endianisation problems while accessing OD. Also fix SDO abord handling bug.
etisserant
parents: 0
diff changeset
  1313