141 /* Get the client->server cobid.*/ |
141 /* Get the client->server cobid.*/ |
142 offset = d->firstIndex->SDO_CLT; |
142 offset = d->firstIndex->SDO_CLT; |
143 if ((offset == 0) || ((offset+d->transfers[id].CliServNbr) > d->lastIndex->SDO_CLT)) { |
143 if ((offset == 0) || ((offset+d->transfers[id].CliServNbr) > d->lastIndex->SDO_CLT)) { |
144 return ; |
144 return ; |
145 } |
145 } |
146 nodeId = *((UNS32*) d->objdict[offset+d->transfers[id].CliServNbr].pSubindex[3].pObject); |
146 nodeId = (UNS8) *((UNS32*) d->objdict[offset+d->transfers[id].CliServNbr].pSubindex[3].pObject); |
147 MSG_ERR(0x1A01, "SDO timeout. SDO response not received.", 0); |
147 MSG_ERR(0x1A01, "SDO timeout. SDO response not received.", 0); |
148 MSG_WAR(0x2A02, "server node id : ", nodeId); |
148 MSG_WAR(0x2A02, "server node id : ", nodeId); |
149 MSG_WAR(0x2A02, " index : ", d->transfers[id].index); |
149 MSG_WAR(0x2A02, " index : ", d->transfers[id].index); |
150 MSG_WAR(0x2A02, " subIndex : ", d->transfers[id].subIndex); |
150 MSG_WAR(0x2A02, " subIndex : ", d->transfers[id].subIndex); |
151 /* Reset timer handler */ |
151 /* Reset timer handler */ |
676 offset = d->firstIndex->SDO_SVR; |
676 offset = d->firstIndex->SDO_SVR; |
677 if ((offset == 0) || ((offset+CliServNbr) > d->lastIndex->SDO_SVR)) { |
677 if ((offset == 0) || ((offset+CliServNbr) > d->lastIndex->SDO_SVR)) { |
678 MSG_ERR(0x1A42, "SendSDO : SDO server not found", 0); |
678 MSG_ERR(0x1A42, "SendSDO : SDO server not found", 0); |
679 return 0xFF; |
679 return 0xFF; |
680 } |
680 } |
681 m.cob_id = *((UNS32*) d->objdict[offset+CliServNbr].pSubindex[2].pObject); |
681 m.cob_id = (UNS16) *((UNS32*) d->objdict[offset+CliServNbr].pSubindex[2].pObject); |
682 MSG_WAR(0x3A41, "I am server Tx cobId : ", m.cob_id); |
682 MSG_WAR(0x3A41, "I am server Tx cobId : ", m.cob_id); |
683 } |
683 } |
684 else { /*case client*/ |
684 else { /*case client*/ |
685 /* Get the client->server cobid.*/ |
685 /* Get the client->server cobid.*/ |
686 offset = d->firstIndex->SDO_CLT; |
686 offset = d->firstIndex->SDO_CLT; |
687 if ((offset == 0) || ((offset+CliServNbr) > d->lastIndex->SDO_CLT)) { |
687 if ((offset == 0) || ((offset+CliServNbr) > d->lastIndex->SDO_CLT)) { |
688 MSG_ERR(0x1A42, "SendSDO : SDO client not found", 0); |
688 MSG_ERR(0x1A42, "SendSDO : SDO client not found", 0); |
689 return 0xFF; |
689 return 0xFF; |
690 } |
690 } |
691 m.cob_id = *((UNS32*) d->objdict[offset+CliServNbr].pSubindex[1].pObject); |
691 m.cob_id = (UNS16) *((UNS32*) d->objdict[offset+CliServNbr].pSubindex[1].pObject); |
692 MSG_WAR(0x3A41, "I am client Tx cobId : ", m.cob_id); |
692 MSG_WAR(0x3A41, "I am client Tx cobId : ", m.cob_id); |
693 } |
693 } |
694 /* message copy for sending */ |
694 /* message copy for sending */ |
695 m.rtr = NOT_A_REQUEST; |
695 m.rtr = NOT_A_REQUEST; |
696 /* the length of SDO must be 8 */ |
696 /* the length of SDO must be 8 */ |
1135 /* code to send the initiate upload response. (cs = 2) */ |
1135 /* code to send the initiate upload response. (cs = 2) */ |
1136 data[0] = (2 << 5) | 1; |
1136 data[0] = (2 << 5) | 1; |
1137 data[1] = index & 0xFF; /* LSB */ |
1137 data[1] = index & 0xFF; /* LSB */ |
1138 data[2] = (index >> 8) & 0xFF; /* MSB */ |
1138 data[2] = (index >> 8) & 0xFF; /* MSB */ |
1139 data[3] = subIndex; |
1139 data[3] = subIndex; |
1140 data[4] = nbBytes; |
1140 data[4] = (UNS8) nbBytes; |
1141 data[5] = nbBytes >> 8; |
1141 data[5] = (UNS8) (nbBytes >> 8); |
1142 data[6] = nbBytes >> 16; |
1142 data[6] = (UNS8) (nbBytes >> 16); |
1143 data[7] = nbBytes >> 24; |
1143 data[7] = (UNS8) (nbBytes >> 24); |
1144 MSG_WAR(0x3A95, "SDO. Sending normal upload initiate response defined at index 0x1200 + ", nodeId); |
1144 MSG_WAR(0x3A95, "SDO. Sending normal upload initiate response defined at index 0x1200 + ", nodeId); |
1145 sendSDO(d, whoami, CliServNbr, data); |
1145 sendSDO(d, whoami, CliServNbr, data); |
1146 } |
1146 } |
1147 else { |
1147 else { |
1148 /* Expedited upload. (cs = 2 ; e = 1) */ |
1148 /* Expedited upload. (cs = 2 ; e = 1) */ |
1397 d->transfers[line].objsize = nbBytes; |
1397 d->transfers[line].objsize = nbBytes; |
1398 data[0] = (6 << 5) | (1 << 1) | SDO_BSS_INITIATE_UPLOAD_RESPONSE; |
1398 data[0] = (6 << 5) | (1 << 1) | SDO_BSS_INITIATE_UPLOAD_RESPONSE; |
1399 data[1] = index & 0xFF; /* LSB */ |
1399 data[1] = index & 0xFF; /* LSB */ |
1400 data[2] = (index >> 8) & 0xFF; /* MSB */ |
1400 data[2] = (index >> 8) & 0xFF; /* MSB */ |
1401 data[3] = subIndex; |
1401 data[3] = subIndex; |
1402 data[4] = nbBytes; |
1402 data[4] = (UNS8) nbBytes; |
1403 data[5] = nbBytes >> 8; |
1403 data[5] = (UNS8) (nbBytes >> 8); |
1404 data[6] = nbBytes >> 16; |
1404 data[6] = (UNS8) (nbBytes >> 16); |
1405 data[7] = nbBytes >> 24; |
1405 data[7] = (UNS8) (nbBytes >> 24); |
1406 MSG_WAR(0x3A9A, "SDO. Sending normal block upload initiate response defined at index 0x1200 + ", nodeId); |
1406 MSG_WAR(0x3A9A, "SDO. Sending normal block upload initiate response defined at index 0x1200 + ", nodeId); |
1407 sendSDO(d, whoami, CliServNbr, data); |
1407 sendSDO(d, whoami, CliServNbr, data); |
1408 } |
1408 } |
1409 else if (SubCommand == SDO_BCS_END_UPLOAD_REQUEST) { |
1409 else if (SubCommand == SDO_BCS_END_UPLOAD_REQUEST) { |
1410 MSG_WAR(0x3AA2, "Received SDO block END upload request defined at index 0x1200 + ", CliServNbr); |
1410 MSG_WAR(0x3AA2, "Received SDO block END upload request defined at index 0x1200 + ", CliServNbr); |
1457 return 0xFF; |
1457 return 0xFF; |
1458 } |
1458 } |
1459 } |
1459 } |
1460 else |
1460 else |
1461 MSG_WAR(0x3AA2, "Received SDO block START upload defined at index 0x1200 + ", CliServNbr); |
1461 MSG_WAR(0x3AA2, "Received SDO block START upload defined at index 0x1200 + ", CliServNbr); |
1462 d->transfers[line].lastblockoffset = d->transfers[line].offset; |
1462 d->transfers[line].lastblockoffset = (UNS8) d->transfers[line].offset; |
1463 for(SeqNo = 1 ; SeqNo <= d->transfers[line].blksize ; SeqNo++) { |
1463 for(SeqNo = 1 ; SeqNo <= d->transfers[line].blksize ; SeqNo++) { |
1464 d->transfers[line].seqno = SeqNo; |
1464 d->transfers[line].seqno = SeqNo; |
1465 getSDOlineRestBytes(d, line, &nbBytes); |
1465 getSDOlineRestBytes(d, line, &nbBytes); |
1466 if (nbBytes > 7) { |
1466 if (nbBytes > 7) { |
1467 /* The segment to transfer is not the last one.*/ |
1467 /* The segment to transfer is not the last one.*/ |
1484 } |
1484 } |
1485 for (i = nbBytes + 1 ; i < 8 ; i++) |
1485 for (i = nbBytes + 1 ; i < 8 ; i++) |
1486 data[i] = 0; |
1486 data[i] = 0; |
1487 MSG_WAR(0x3AA5, "SDO. Sending last upload segment defined at index 0x1200 + ", CliServNbr); |
1487 MSG_WAR(0x3AA5, "SDO. Sending last upload segment defined at index 0x1200 + ", CliServNbr); |
1488 sendSDO(d, whoami, CliServNbr, data); |
1488 sendSDO(d, whoami, CliServNbr, data); |
1489 d->transfers[line].endfield = 7 - nbBytes; |
1489 d->transfers[line].endfield = (UNS8) (7 - nbBytes); |
1490 break; |
1490 break; |
1491 } |
1491 } |
1492 } |
1492 } |
1493 } |
1493 } |
1494 } /* end if SERVER */ |
1494 } /* end if SERVER */ |
1529 CliServNbr); |
1529 CliServNbr); |
1530 failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR); |
1530 failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR); |
1531 return 0xFF; |
1531 return 0xFF; |
1532 } |
1532 } |
1533 } |
1533 } |
1534 d->transfers[line].lastblockoffset = d->transfers[line].offset; |
1534 d->transfers[line].lastblockoffset = (UNS8) d->transfers[line].offset; |
1535 for(SeqNo = 1 ; SeqNo <= d->transfers[line].blksize ; SeqNo++) { |
1535 for(SeqNo = 1 ; SeqNo <= d->transfers[line].blksize ; SeqNo++) { |
1536 d->transfers[line].seqno = SeqNo; |
1536 d->transfers[line].seqno = SeqNo; |
1537 getSDOlineRestBytes(d, line, &nbBytes); |
1537 getSDOlineRestBytes(d, line, &nbBytes); |
1538 if (nbBytes > 7) { |
1538 if (nbBytes > 7) { |
1539 /* The segment to transfer is not the last one.*/ |
1539 /* The segment to transfer is not the last one.*/ |
1556 } |
1556 } |
1557 for (i = nbBytes + 1 ; i < 8 ; i++) |
1557 for (i = nbBytes + 1 ; i < 8 ; i++) |
1558 data[i] = 0; |
1558 data[i] = 0; |
1559 MSG_WAR(0x3AAB, "SDO. Sending last download segment to node id ", nodeId); |
1559 MSG_WAR(0x3AAB, "SDO. Sending last download segment to node id ", nodeId); |
1560 sendSDO(d, whoami, CliServNbr, data); |
1560 sendSDO(d, whoami, CliServNbr, data); |
1561 d->transfers[line].endfield = 7 - nbBytes; |
1561 d->transfers[line].endfield = (UNS8) (7 - nbBytes); |
1562 break; |
1562 break; |
1563 } |
1563 } |
1564 } |
1564 } |
1565 } |
1565 } |
1566 else if (SubCommand == SDO_BSS_END_DOWNLOAD_RESPONSE) { |
1566 else if (SubCommand == SDO_BSS_END_DOWNLOAD_RESPONSE) { |
1604 d->transfers[line].rxstep = RXSTEP_STARTED; |
1604 d->transfers[line].rxstep = RXSTEP_STARTED; |
1605 d->transfers[line].peerCRCsupport = ((m->data[0])>>2) & 1; |
1605 d->transfers[line].peerCRCsupport = ((m->data[0])>>2) & 1; |
1606 if ((m->data[0]) & 2) /* if data set size is indicated */ |
1606 if ((m->data[0]) & 2) /* if data set size is indicated */ |
1607 d->transfers[line].objsize = (UNS32)m->data[4] + (UNS32)m->data[5]*256 + (UNS32)m->data[6]*256*256 + (UNS32)m->data[7]*256*256*256; |
1607 d->transfers[line].objsize = (UNS32)m->data[4] + (UNS32)m->data[5]*256 + (UNS32)m->data[6]*256*256 + (UNS32)m->data[7]*256*256*256; |
1608 data[0] = (5 << 5) | SDO_BSS_INITIATE_DOWNLOAD_RESPONSE; |
1608 data[0] = (5 << 5) | SDO_BSS_INITIATE_DOWNLOAD_RESPONSE; |
1609 data[1] = index; /* LSB */ |
1609 data[1] = (UNS8) index; /* LSB */ |
1610 data[2] = index >> 8; /* MSB */ |
1610 data[2] = (UNS8) (index >> 8); /* MSB */ |
1611 data[3] = subIndex; |
1611 data[3] = subIndex; |
1612 data[4] = SDO_BLOCK_SIZE; |
1612 data[4] = SDO_BLOCK_SIZE; |
1613 data[5] = data[6] = data[7] = 0; |
1613 data[5] = data[6] = data[7] = 0; |
1614 MSG_WAR(0x3AAD, "SDO. Sending block download initiate response - index 0x1200 + ", CliServNbr); |
1614 MSG_WAR(0x3AAD, "SDO. Sending block download initiate response - index 0x1200 + ", CliServNbr); |
1615 sendSDO(d, whoami, CliServNbr, data); |
1615 sendSDO(d, whoami, CliServNbr, data); |