491 UNS16 offset; |
491 UNS16 offset; |
492 UNS16 lastIndex; |
492 UNS16 lastIndex; |
493 UNS8 found = 0; |
493 UNS8 found = 0; |
494 Message m; |
494 Message m; |
495 UNS8 i; |
495 UNS8 i; |
496 UNS16 * pwCobId = NULL; |
496 UNS32 * pwCobId = NULL; |
497 UNS8 * pwNodeId = NULL; |
497 UNS8 * pwNodeId = NULL; |
498 |
498 |
499 MSG_WAR(0x3A38, "sendSDO",0); |
499 MSG_WAR(0x3A38, "sendSDO",0); |
500 if( !((d->nodeState == Operational) || (d->nodeState == Pre_operational ))) { |
500 if( !((d->nodeState == Operational) || (d->nodeState == Pre_operational ))) { |
501 MSG_WAR(0x2A39, "unable to send the SDO (not in op or pre-op mode", d->nodeState); |
501 MSG_WAR(0x2A39, "unable to send the SDO (not in op or pre-op mode", d->nodeState); |
507 offset = d->firstIndex->SDO_SVR; |
507 offset = d->firstIndex->SDO_SVR; |
508 if (offset == 0) { |
508 if (offset == 0) { |
509 MSG_ERR(0x1A42, "SendSDO : No SDO server found", 0); |
509 MSG_ERR(0x1A42, "SendSDO : No SDO server found", 0); |
510 return 0xFF; |
510 return 0xFF; |
511 } |
511 } |
512 pwCobId = (UNS16*) d->objdict[offset].pSubindex[2].pObject; |
512 pwCobId = (UNS32*) d->objdict[offset].pSubindex[2].pObject; |
513 MSG_WAR(0x3A41, "I am server. cobId : ", *pwCobId); |
513 MSG_WAR(0x3A41, "I am server. cobId : ", *pwCobId); |
514 } |
514 } |
515 else { /*case client*/ |
515 else { /*case client*/ |
516 /* Get the client->server cobid.*/ |
516 /* Get the client->server cobid.*/ |
517 UNS16 sdoNum = 0; |
517 UNS16 sdoNum = 0; |
540 if (! found){ |
540 if (! found){ |
541 MSG_WAR (0x2A45, "No SDO client corresponds to the mesage to send to node ", sdo.nodeId); |
541 MSG_WAR (0x2A45, "No SDO client corresponds to the mesage to send to node ", sdo.nodeId); |
542 return 0xFF; |
542 return 0xFF; |
543 } |
543 } |
544 /* Second, read the cobid client->server */ |
544 /* Second, read the cobid client->server */ |
545 pwCobId = (UNS16*) d->objdict[offset].pSubindex[1].pObject; |
545 pwCobId = (UNS32*) d->objdict[offset].pSubindex[1].pObject; |
546 } |
546 } |
547 /* message copy for sending */ |
547 /* message copy for sending */ |
548 m.cob_id = *pwCobId; |
548 m.cob_id = UNS16_LE(*pwCobId); |
549 m.rtr = NOT_A_REQUEST; |
549 m.rtr = NOT_A_REQUEST; |
550 /* the length of SDO must be 8 */ |
550 /* the length of SDO must be 8 */ |
551 m.len = 8; |
551 m.len = 8; |
552 for (i = 0 ; i < 8 ; i++) { |
552 for (i = 0 ; i < 8 ; i++) { |
553 m.data[i] = sdo.body.data[i]; |
553 m.data[i] = sdo.body.data[i]; |
624 if(offset) while (offset <= lastIndex) { |
624 if(offset) while (offset <= lastIndex) { |
625 if (d->objdict[offset].bSubCount <= 1) { |
625 if (d->objdict[offset].bSubCount <= 1) { |
626 MSG_ERR(0x1A61, "Subindex 1 not found at index ", 0x1200 + j); |
626 MSG_ERR(0x1A61, "Subindex 1 not found at index ", 0x1200 + j); |
627 return 0xFF; |
627 return 0xFF; |
628 } |
628 } |
629 pCobId = (UNS16*) d->objdict[offset].pSubindex[1].pObject; |
629 pCobId = (UNS32*) d->objdict[offset].pSubindex[1].pObject; |
630 if ( *pCobId == (*m).cob_id ) { |
630 if ( *pCobId == UNS16_LE(m->cob_id) ) { |
631 whoami = SDO_SERVER; |
631 whoami = SDO_SERVER; |
632 MSG_WAR(0x3A62, "proceedSDO. I am server. index : ", 0x1200 + j); |
632 MSG_WAR(0x3A62, "proceedSDO. I am server. index : ", 0x1200 + j); |
633 /* In case of server, the node id of the client may be unknown. So we put the index minus offset */ |
633 /* In case of server, the node id of the client may be unknown. So we put the index minus offset */ |
634 /* 0x1200 where the cobid received is defined. */ |
634 /* 0x1200 where the cobid received is defined. */ |
635 nodeId = j; |
635 nodeId = j; |
647 if (d->objdict[offset].bSubCount <= 3) { |
647 if (d->objdict[offset].bSubCount <= 3) { |
648 MSG_ERR(0x1A63, "Subindex 3 not found at index ", 0x1280 + j); |
648 MSG_ERR(0x1A63, "Subindex 3 not found at index ", 0x1280 + j); |
649 return 0xFF; |
649 return 0xFF; |
650 } |
650 } |
651 /* a) Looking for the cobid received. */ |
651 /* a) Looking for the cobid received. */ |
652 pCobId = (UNS16*) d->objdict[offset].pSubindex[2].pObject; |
652 pCobId = (UNS32*) d->objdict[offset].pSubindex[2].pObject; |
653 if (*pCobId == (*m).cob_id ) { |
653 if (*pCobId == UNS16_LE(m->cob_id) ) { |
654 /* b) cobid found, so reading the node id of the server. */ |
654 /* b) cobid found, so reading the node id of the server. */ |
655 pNodeId = (UNS8*) d->objdict[offset].pSubindex[3].pObject; |
655 pNodeId = (UNS8*) d->objdict[offset].pSubindex[3].pObject; |
656 whoami = SDO_CLIENT; |
656 whoami = SDO_CLIENT; |
657 nodeId = *pNodeId; |
657 nodeId = *pNodeId; |
658 MSG_WAR(0x3A64, "proceedSDO. I am server. index : ", 0x1280 + j); |
658 MSG_WAR(0x3A64, "proceedSDO. I am server. index : ", 0x1280 + j); |
667 return 0xFF;/* This SDO was not for us ! */ |
667 return 0xFF;/* This SDO was not for us ! */ |
668 } |
668 } |
669 |
669 |
670 /* Test if the size of the SDO is ok */ |
670 /* Test if the size of the SDO is ok */ |
671 if ( (*m).len != 8) { |
671 if ( (*m).len != 8) { |
672 MSG_ERR(0x1A67, "Error size SDO. CobId : ", (*m).cob_id); |
672 MSG_ERR(0x1A67, "Error size SDO. CobId : ", UNS16_LE(m->cob_id)); |
673 failedSDO(d, nodeId, whoami, 0, 0, SDOABT_GENERAL_ERROR); |
673 failedSDO(d, nodeId, whoami, 0, 0, SDOABT_GENERAL_ERROR); |
674 return 0xFF; |
674 return 0xFF; |
675 } |
675 } |
676 |
676 |
677 if (whoami == SDO_CLIENT) { |
677 if (whoami == SDO_CLIENT) { |
678 MSG_WAR(0x3A68, "I am CLIENT. Received SDO from nodeId : ", nodeId); |
678 MSG_WAR(0x3A68, "I am CLIENT. Received SDO from nodeId : ", nodeId); |
679 } |
679 } |
680 else { |
680 else { |
681 MSG_WAR(0x3A69, "I am SERVER. Received SDO cobId : ", (*m).cob_id); |
681 MSG_WAR(0x3A69, "I am SERVER. Received SDO cobId : ", UNS16_LE(m->cob_id)); |
682 } |
682 } |
683 |
683 |
684 /* Testing the command specifier */ |
684 /* Testing the command specifier */ |
685 /* Allowed : cs = 0, 1, 2, 3, 4. (= all except those for block tranfert). */ |
685 /* Allowed : cs = 0, 1, 2, 3, 4. (= all except those for block tranfert). */ |
686 /* cs = other : Not allowed -> abort. */ |
686 /* cs = other : Not allowed -> abort. */ |