552 } |
552 } |
553 /* Second, read the cobid client->server */ |
553 /* Second, read the cobid client->server */ |
554 pwCobId = (UNS32*) d->objdict[offset].pSubindex[1].pObject; |
554 pwCobId = (UNS32*) d->objdict[offset].pSubindex[1].pObject; |
555 } |
555 } |
556 /* message copy for sending */ |
556 /* message copy for sending */ |
557 m.cob_id.w = *pwCobId; |
557 m.cob_id = *pwCobId; |
558 m.rtr = NOT_A_REQUEST; |
558 m.rtr = NOT_A_REQUEST; |
559 /* the length of SDO must be 8 */ |
559 /* the length of SDO must be 8 */ |
560 m.len = 8; |
560 m.len = 8; |
561 for (i = 0 ; i < 8 ; i++) { |
561 for (i = 0 ; i < 8 ; i++) { |
562 m.data[i] = sdo.body.data[i]; |
562 m.data[i] = sdo.body.data[i]; |
634 if (d->objdict[offset].bSubCount <= 1) { |
634 if (d->objdict[offset].bSubCount <= 1) { |
635 MSG_ERR(0x1A61, "Subindex 1 not found at index ", 0x1200 + j); |
635 MSG_ERR(0x1A61, "Subindex 1 not found at index ", 0x1200 + j); |
636 return 0xFF; |
636 return 0xFF; |
637 } |
637 } |
638 pCobId = (UNS32*) d->objdict[offset].pSubindex[1].pObject; |
638 pCobId = (UNS32*) d->objdict[offset].pSubindex[1].pObject; |
639 if ( *pCobId == (*m).cob_id.w ) { |
639 if ( *pCobId == (*m).cob_id ) { |
640 whoami = SDO_SERVER; |
640 whoami = SDO_SERVER; |
641 MSG_WAR(0x3A62, "proceedSDO. I am server. index : ", 0x1200 + j); |
641 MSG_WAR(0x3A62, "proceedSDO. I am server. index : ", 0x1200 + j); |
642 /* In case of server, the node id of the client may be unknown. So we put the index minus offset */ |
642 /* In case of server, the node id of the client may be unknown. So we put the index minus offset */ |
643 /* 0x1200 where the cobid received is defined. */ |
643 /* 0x1200 where the cobid received is defined. */ |
644 nodeId = j; |
644 nodeId = j; |
657 MSG_ERR(0x1A63, "Subindex 3 not found at index ", 0x1280 + j); |
657 MSG_ERR(0x1A63, "Subindex 3 not found at index ", 0x1280 + j); |
658 return 0xFF; |
658 return 0xFF; |
659 } |
659 } |
660 /* a) Looking for the cobid received. */ |
660 /* a) Looking for the cobid received. */ |
661 pCobId = (UNS32*) d->objdict[offset].pSubindex[2].pObject; |
661 pCobId = (UNS32*) d->objdict[offset].pSubindex[2].pObject; |
662 if (*pCobId == (*m).cob_id.w ) { |
662 if (*pCobId == (*m).cob_id ) { |
663 /* b) cobid found, so reading the node id of the server. */ |
663 /* b) cobid found, so reading the node id of the server. */ |
664 pNodeId = (UNS8*) d->objdict[offset].pSubindex[3].pObject; |
664 pNodeId = (UNS8*) d->objdict[offset].pSubindex[3].pObject; |
665 whoami = SDO_CLIENT; |
665 whoami = SDO_CLIENT; |
666 nodeId = *pNodeId; |
666 nodeId = *pNodeId; |
667 MSG_WAR(0x3A64, "proceedSDO. I am server. index : ", 0x1280 + j); |
667 MSG_WAR(0x3A64, "proceedSDO. I am server. index : ", 0x1280 + j); |
676 return 0xFF;/* This SDO was not for us ! */ |
676 return 0xFF;/* This SDO was not for us ! */ |
677 } |
677 } |
678 |
678 |
679 /* Test if the size of the SDO is ok */ |
679 /* Test if the size of the SDO is ok */ |
680 if ( (*m).len != 8) { |
680 if ( (*m).len != 8) { |
681 MSG_ERR(0x1A67, "Error size SDO. CobId : ", (*m).cob_id.w); |
681 MSG_ERR(0x1A67, "Error size SDO. CobId : ", (*m).cob_id); |
682 failedSDO(d, nodeId, whoami, 0, 0, SDOABT_GENERAL_ERROR); |
682 failedSDO(d, nodeId, whoami, 0, 0, SDOABT_GENERAL_ERROR); |
683 return 0xFF; |
683 return 0xFF; |
684 } |
684 } |
685 |
685 |
686 if (whoami == SDO_CLIENT) { |
686 if (whoami == SDO_CLIENT) { |
687 MSG_WAR(0x3A68, "I am CLIENT. Received SDO from nodeId : ", nodeId); |
687 MSG_WAR(0x3A68, "I am CLIENT. Received SDO from nodeId : ", nodeId); |
688 } |
688 } |
689 else { |
689 else { |
690 MSG_WAR(0x3A69, "I am SERVER. Received SDO cobId : ", (*m).cob_id.w); |
690 MSG_WAR(0x3A69, "I am SERVER. Received SDO cobId : ", (*m).cob_id); |
691 } |
691 } |
692 |
692 |
693 /* Testing the command specifier */ |
693 /* Testing the command specifier */ |
694 /* Allowed : cs = 0, 1, 2, 3, 4. (= all except those for block tranfert). */ |
694 /* Allowed : cs = 0, 1, 2, 3, 4. (= all except those for block tranfert). */ |
695 /* cs = other : Not allowed -> abort. */ |
695 /* cs = other : Not allowed -> abort. */ |