549 } |
549 } |
550 /* read the client->server cobid */ |
550 /* read the client->server cobid */ |
551 pwCobId = (UNS32*) d->objdict[offset].pSubindex[1].pObject; |
551 pwCobId = (UNS32*) d->objdict[offset].pSubindex[1].pObject; |
552 } |
552 } |
553 /* message copy for sending */ |
553 /* message copy for sending */ |
554 m.cob_id = UNS16_LE(*pwCobId); |
554 m.cob_id = (UNS8)UNS16_LE(*pwCobId); |
555 m.rtr = NOT_A_REQUEST; |
555 m.rtr = NOT_A_REQUEST; |
556 /* the length of SDO must be 8 */ |
556 /* the length of SDO must be 8 */ |
557 m.len = 8; |
557 m.len = 8; |
558 for (i = 0 ; i < 8 ; i++) { |
558 for (i = 0 ; i < 8 ; i++) { |
559 m.data[i] = sdo.body.data[i]; |
559 m.data[i] = sdo.body.data[i]; |
574 **/ |
574 **/ |
575 UNS8 sendSDOabort (CO_Data* d, UNS8 whoami, UNS8 nodeID, UNS16 index, UNS8 subIndex, UNS32 abortCode) |
575 UNS8 sendSDOabort (CO_Data* d, UNS8 whoami, UNS8 nodeID, UNS16 index, UNS8 subIndex, UNS32 abortCode) |
576 { |
576 { |
577 s_SDO sdo; |
577 s_SDO sdo; |
578 UNS8 ret; |
578 UNS8 ret; |
579 UNS8* pNodeIdServer; |
579 |
580 UNS8 nodeIdServer; |
|
581 UNS16 offset; |
|
582 |
|
583 MSG_WAR(0x2A50,"Sending SDO abort ", abortCode); |
580 MSG_WAR(0x2A50,"Sending SDO abort ", abortCode); |
584 if(whoami == SDO_SERVER) |
581 if(whoami == SDO_SERVER) |
585 { |
582 { |
586 sdo.nodeId = *d->bDeviceNodeId; |
583 sdo.nodeId = *d->bDeviceNodeId; |
587 } |
584 } |
624 UNS32 errorCode; /* while reading or writing in the local object dictionary.*/ |
621 UNS32 errorCode; /* while reading or writing in the local object dictionary.*/ |
625 s_SDO sdo; /* SDO to transmit */ |
622 s_SDO sdo; /* SDO to transmit */ |
626 UNS16 index; |
623 UNS16 index; |
627 UNS8 subIndex; |
624 UNS8 subIndex; |
628 UNS32 abortCode; |
625 UNS32 abortCode; |
629 UNS8 i,j; |
626 UNS32 i; |
|
627 UNS8 j; |
630 UNS32 *pCobId = NULL; |
628 UNS32 *pCobId = NULL; |
631 UNS16 offset; |
629 UNS16 offset; |
632 UNS16 lastIndex; |
630 UNS16 lastIndex; |
633 |
631 |
634 MSG_WAR(0x3A60, "proceedSDO ", 0); |
632 MSG_WAR(0x3A60, "proceedSDO ", 0); |
866 /* Release of the line. */ |
864 /* Release of the line. */ |
867 resetSDOline(d, line); |
865 resetSDOline(d, line); |
868 } |
866 } |
869 else {/* So, if it is not an expedited transfert */ |
867 else {/* So, if it is not an expedited transfert */ |
870 if (getSDOs(m->data[0])) { |
868 if (getSDOs(m->data[0])) { |
871 nbBytes = m->data[4] + m->data[5]<<8 + m->data[6]<<16 + m->data[7]<<24; |
869 nbBytes = (m->data[4]) + (m->data[5]<<8) + (m->data[6]<<16) + (m->data[7]<<24); |
872 err = setSDOlineRestBytes(d, nodeId, nbBytes); |
870 err = setSDOlineRestBytes(d, nodeId, nbBytes); |
873 if (err) { |
871 if (err) { |
874 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
872 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
875 return 0xFF; |
873 return 0xFF; |
876 } |
874 } |
933 else { |
931 else { |
934 /* Last segment. */ |
932 /* Last segment. */ |
935 /* code to send the last segment. (cs = 0; c = 1)*/ |
933 /* code to send the last segment. (cs = 0; c = 1)*/ |
936 d->transfers[line].toggle = ! d->transfers[line].toggle & 1; |
934 d->transfers[line].toggle = ! d->transfers[line].toggle & 1; |
937 sdo.nodeId = nodeId; /* The server node Id; */ |
935 sdo.nodeId = nodeId; /* The server node Id; */ |
938 sdo.body.data[0] = (d->transfers[line].toggle << 4) | ((7 - nbBytes) << 1) | 1; |
936 sdo.body.data[0] = (UNS8)((d->transfers[line].toggle << 4) | ((7 - nbBytes) << 1) | 1); |
939 err = lineToSDO(d, line, nbBytes, sdo.body.data + 1); |
937 err = lineToSDO(d, line, nbBytes, sdo.body.data + 1); |
940 if (err) { |
938 if (err) { |
941 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
939 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
942 return 0xFF; |
940 return 0xFF; |
943 } |
941 } |
993 /* code to send the initiate upload response. (cs = 2) */ |
991 /* code to send the initiate upload response. (cs = 2) */ |
994 sdo.body.data[0] = (2 << 5) | 1; |
992 sdo.body.data[0] = (2 << 5) | 1; |
995 sdo.body.data[1] = index & 0xFF; /* LSB */ |
993 sdo.body.data[1] = index & 0xFF; /* LSB */ |
996 sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */ |
994 sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */ |
997 sdo.body.data[3] = subIndex; |
995 sdo.body.data[3] = subIndex; |
998 sdo.body.data[4] = nbBytes; /* Limitation of canfestival2 : Max tranfert is 256 bytes.*/ |
996 sdo.body.data[4] = (UNS8)nbBytes; /* Limitation of canfestival2 : Max tranfert is 256 bytes.*/ |
999 /* It takes too much memory to upgrate to 2^32 because the size of data is also coded */ |
997 /* It takes too much memory to upgrate to 2^32 because the size of data is also coded */ |
1000 /* in the object dictionary, at every index and subindex. */ |
998 /* in the object dictionary, at every index and subindex. */ |
1001 for (i = 5 ; i < 8 ; i++) |
999 for (i = 5 ; i < 8 ; i++) |
1002 sdo.body.data[i] = 0; |
1000 sdo.body.data[i] = 0; |
1003 MSG_WAR(0x3A95, "SDO. Sending normal upload initiate response defined at index 0x1200 + ", nodeId); |
1001 MSG_WAR(0x3A95, "SDO. Sending normal upload initiate response defined at index 0x1200 + ", nodeId); |
1004 sendSDO(d, whoami, sdo); |
1002 sendSDO(d, whoami, sdo); |
1005 } |
1003 } |
1006 else { |
1004 else { |
1007 /* Expedited upload. (cs = 2 ; e = 1) */ |
1005 /* Expedited upload. (cs = 2 ; e = 1) */ |
1008 sdo.body.data[0] = (2 << 5) | ((4 - nbBytes) << 2) | 3; |
1006 sdo.body.data[0] = (UNS8)((2 << 5) | ((4 - nbBytes) << 2) | 3); |
1009 sdo.body.data[1] = index & 0xFF; /* LSB */ |
1007 sdo.body.data[1] = index & 0xFF; /* LSB */ |
1010 sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */ |
1008 sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */ |
1011 sdo.body.data[3] = subIndex; |
1009 sdo.body.data[3] = subIndex; |
1012 err = lineToSDO(d, line, nbBytes, sdo.body.data + 4); |
1010 err = lineToSDO(d, line, nbBytes, sdo.body.data + 4); |
1013 if (err) { |
1011 if (err) { |
1058 return 0; |
1056 return 0; |
1059 } |
1057 } |
1060 else { /* So, if it is not an expedited transfert */ |
1058 else { /* So, if it is not an expedited transfert */ |
1061 /* Storing the nb of data to receive. */ |
1059 /* Storing the nb of data to receive. */ |
1062 if (getSDOs(m->data[0])) { |
1060 if (getSDOs(m->data[0])) { |
1063 nbBytes = m->data[4] + m->data[5]<<8 + m->data[6]<<16 + m->data[7]<<24; |
1061 nbBytes = m->data[4] + (m->data[5]<<8) + (m->data[6]<<16) + (m->data[7]<<24); |
1064 err = setSDOlineRestBytes(d, line, nbBytes); |
1062 err = setSDOlineRestBytes(d, line, nbBytes); |
1065 if (err) { |
1063 if (err) { |
1066 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
1064 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
1067 return 0xFF; |
1065 return 0xFF; |
1068 } |
1066 } |
1122 } |
1120 } |
1123 else { |
1121 else { |
1124 /* Last segment. */ |
1122 /* Last segment. */ |
1125 /* code to send the last segment. (cs = 0; c = 1) */ |
1123 /* code to send the last segment. (cs = 0; c = 1) */ |
1126 sdo.nodeId = nodeId; /** The server node Id; */ |
1124 sdo.nodeId = nodeId; /** The server node Id; */ |
1127 sdo.body.data[0] = (d->transfers[line].toggle << 4) | ((7 - nbBytes) << 1) | 1; |
1125 sdo.body.data[0] = (UNS8)((d->transfers[line].toggle << 4) | ((7 - nbBytes) << 1) | 1); |
1128 err = lineToSDO(d, line, nbBytes, sdo.body.data + 1); |
1126 err = lineToSDO(d, line, nbBytes, sdo.body.data + 1); |
1129 if (err) { |
1127 if (err) { |
1130 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
1128 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
1131 return 0xFF; |
1129 return 0xFF; |
1132 } |
1130 } |
1176 } |
1174 } |
1177 else { |
1175 else { |
1178 /* Last segment.*/ |
1176 /* Last segment.*/ |
1179 /* code to send the last segment. (cs = 0; c = 1) */ |
1177 /* code to send the last segment. (cs = 0; c = 1) */ |
1180 sdo.nodeId = nodeId; /* The server node Id; */ |
1178 sdo.nodeId = nodeId; /* The server node Id; */ |
1181 sdo.body.data[0] = (d->transfers[line].toggle << 4) | ((7 - nbBytes) << 1) | 1; |
1179 sdo.body.data[0] = (UNS8)((d->transfers[line].toggle << 4) | ((7 - nbBytes) << 1) | 1); |
1182 err = lineToSDO(d, line, nbBytes, sdo.body.data + 1); |
1180 err = lineToSDO(d, line, nbBytes, sdo.body.data + 1); |
1183 if (err) { |
1181 if (err) { |
1184 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
1182 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); |
1185 return 0xFF; |
1183 return 0xFF; |
1186 } |
1184 } |
1328 # endif |
1326 # endif |
1329 } |
1327 } |
1330 /* Send the SDO to the server. Initiate download, cs=1. */ |
1328 /* Send the SDO to the server. Initiate download, cs=1. */ |
1331 sdo.nodeId = nodeId; |
1329 sdo.nodeId = nodeId; |
1332 if (count <= 4) { /* Expedited transfert */ |
1330 if (count <= 4) { /* Expedited transfert */ |
1333 sdo.body.data[0] = (1 << 5) | ((4 - count) << 2) | 3; |
1331 sdo.body.data[0] = (UNS8)((1 << 5) | ((4 - count) << 2) | 3); |
1334 for (i = 4 ; i < 8 ; i++) |
1332 for (i = 4 ; i < 8 ; i++) |
1335 sdo.body.data[i] = d->transfers[line].data[i - 4]; |
1333 sdo.body.data[i] = d->transfers[line].data[i - 4]; |
1336 d->transfers[line].offset = count; |
1334 d->transfers[line].offset = count; |
1337 } |
1335 } |
1338 else { /** Normal transfert */ |
1336 else { /** Normal transfert */ |
1339 sdo.body.data[0] = (1 << 5) | 1; |
1337 sdo.body.data[0] = (1 << 5) | 1; |
1340 for (i = 0 ; i < 4 ; i++) |
1338 for (i = 0 ; i < 4 ; i++) |
1341 sdo.body.data[i+4] = count << (i<<3); /* i*8 */ |
1339 sdo.body.data[i+4] = (UNS8)((count << (i<<3))); /* i*8 */ |
1342 } |
1340 } |
1343 sdo.body.data[1] = index & 0xFF; /* LSB */ |
1341 sdo.body.data[1] = index & 0xFF; /* LSB */ |
1344 sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */ |
1342 sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */ |
1345 sdo.body.data[3] = subIndex; |
1343 sdo.body.data[3] = subIndex; |
1346 |
1344 |