/** Prepare a read request (RRQ) with filename
*
* \return Zero on success, otherwise a negative error code.
*/
int ec_foe_prepare_rrq_send(
ec_fsm_foe_t *fsm, /**< Finite state machine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
size_t current_size;
uint8_t *data;
current_size = fsm->rx_filename_len;
data = ec_slave_mbox_prepare_send(fsm->slave, datagram,
EC_MBOX_TYPE_FILEACCESS, current_size + EC_FOE_HEADER_SIZE);
if (IS_ERR(data)) {
return -1;
}
EC_WRITE_U16(data, EC_FOE_OPCODE_RRQ); // fsm read request
EC_WRITE_U32(data + 2, 0x00000000); // no passwd
memcpy(data + EC_FOE_HEADER_SIZE, fsm->rx_filename, current_size);
if (fsm->slave->master->debug_level) {
EC_SLAVE_DBG(fsm->slave, 1, "FoE Read Request:\n");
ec_print_data(data, current_size + EC_FOE_HEADER_SIZE);
}
return 0;
}
/** Sends a file or the next fragment.
*
* \return Zero on success, otherwise a negative error code.
*/
int ec_foe_prepare_data_send(
ec_fsm_foe_t *fsm, /**< Finite state machine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
size_t remaining_size, current_size;
uint8_t *data;
remaining_size = fsm->tx_buffer_size - fsm->tx_buffer_offset;
if (remaining_size < fsm->slave->configured_tx_mailbox_size
- EC_MBOX_HEADER_SIZE - EC_FOE_HEADER_SIZE) {
current_size = remaining_size;
fsm->tx_last_packet = 1;
} else {
current_size = fsm->slave->configured_tx_mailbox_size
- EC_MBOX_HEADER_SIZE - EC_FOE_HEADER_SIZE;
}
data = ec_slave_mbox_prepare_send(fsm->slave,
datagram, EC_MBOX_TYPE_FILEACCESS,
current_size + EC_FOE_HEADER_SIZE);
if (IS_ERR(data)) {
return -1;
}
EC_WRITE_U16(data, EC_FOE_OPCODE_DATA); // OpCode = DataBlock req.
EC_WRITE_U32(data + 2, fsm->tx_packet_no); // PacketNo, Password
memcpy(data + EC_FOE_HEADER_SIZE,
fsm->tx_buffer + fsm->tx_buffer_offset, current_size);
fsm->tx_current_size = current_size;
return 0;
}
/** Prepare a write request (WRQ) with filename
*
* \return Zero on success, otherwise a negative error code.
*/
int ec_foe_prepare_wrq_send(
ec_fsm_foe_t *fsm, /**< Finite state machine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
size_t current_size;
uint8_t *data;
fsm->tx_buffer_offset = 0;
fsm->tx_current_size = 0;
fsm->tx_packet_no = 0;
fsm->tx_last_packet = 0;
current_size = fsm->tx_filename_len;
data = ec_slave_mbox_prepare_send(fsm->slave, datagram,
EC_MBOX_TYPE_FILEACCESS, current_size + EC_FOE_HEADER_SIZE);
if (IS_ERR(data)) {
return -1;
}
EC_WRITE_U16( data, EC_FOE_OPCODE_WRQ); // fsm write request
EC_WRITE_U32( data + 2, fsm->tx_packet_no );
memcpy(data + EC_FOE_HEADER_SIZE, fsm->tx_filename, current_size);
return 0;
}
/** Prepare to send an acknowledge.
*
* \return Zero on success, otherwise a negative error code.
*/
int ec_foe_prepare_send_ack(
ec_fsm_foe_t *fsm, /**< FoE statemachine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
uint8_t *data;
data = ec_slave_mbox_prepare_send(fsm->slave, datagram,
EC_MBOX_TYPE_FILEACCESS, EC_FOE_HEADER_SIZE);
if (IS_ERR(data)) {
return -1;
}
EC_WRITE_U16(data, EC_FOE_OPCODE_ACK);
EC_WRITE_U32(data + 2, fsm->rx_expected_packet_no);
return 0;
}
void write_sdo(ec_sdo_request_t *req, unsigned data)
{
EC_WRITE_U32(ecrt_sdo_request_data(req), data&0xffffffff);
switch (ecrt_sdo_request_state(req)) {
case EC_REQUEST_UNUSED: // request was not used yet
ecrt_sdo_request_write(req); // trigger first read
break;
case EC_REQUEST_BUSY:
fprintf(stderr, "SDO write still busy...\n");
break;
case EC_REQUEST_SUCCESS:
logmsg(1, "SDO value written: 0x%X\n", data);
ecrt_sdo_request_write(req); // trigger next read ???
break;
case EC_REQUEST_ERROR:
fprintf(stderr, "Failed to write SDO!\n");
ecrt_sdo_request_write(req); // retry writing
break;
}
}
/** Initializes an FMMU configuration page.
*
* The referenced memory (\a data) must be at least EC_FMMU_PAGE_SIZE bytes.
*/
void ec_fmmu_config_page(
const ec_fmmu_config_t *fmmu, /**< EtherCAT FMMU configuration. */
const ec_sync_t *sync, /**< Sync manager. */
uint8_t *data /**> Configuration page memory. */
)
{
EC_CONFIG_DBG(fmmu->sc, 1, "FMMU: LogAddr 0x%08X, Size %3u,"
" PhysAddr 0x%04X, SM%u, Dir %s\n",
fmmu->logical_start_address, fmmu->data_size,
sync->physical_start_address, fmmu->sync_index,
fmmu->dir == EC_DIR_INPUT ? "in" : "out");
EC_WRITE_U32(data, fmmu->logical_start_address);
EC_WRITE_U16(data + 4, fmmu->data_size); // size of fmmu
EC_WRITE_U8 (data + 6, 0x00); // logical start bit
EC_WRITE_U8 (data + 7, 0x07); // logical end bit
EC_WRITE_U16(data + 8, sync->physical_start_address);
EC_WRITE_U8 (data + 10, 0x00); // physical start bit
EC_WRITE_U8 (data + 11, fmmu->dir == EC_DIR_INPUT ? 0x01 : 0x02);
EC_WRITE_U16(data + 12, 0x0001); // enable
EC_WRITE_U16(data + 14, 0x0000); // reserved
}
/** Starts to add a PDO entry.
*/
void ec_fsm_pdo_entry_conf_action_map(
ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
uint32_t value;
EC_SLAVE_DBG(fsm->slave, 1, "Mapping PDO entry 0x%04X:%02X (%u bit)"
" at position %u.\n",
fsm->entry->index, fsm->entry->subindex,
fsm->entry->bit_length, fsm->entry_pos);
value = fsm->entry->index << 16
| fsm->entry->subindex << 8 | fsm->entry->bit_length;
EC_WRITE_U32(fsm->request.data, value);
fsm->request.data_size = 4;
ecrt_sdo_request_index(&fsm->request, fsm->source_pdo->index,
fsm->entry_pos);
ecrt_sdo_request_write(&fsm->request);
fsm->state = ec_fsm_pdo_entry_conf_state_map_entry;
ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
ec_fsm_coe_exec(fsm->fsm_coe, datagram); // execute immediately
}
/** Prepare a set IP parameters operation.
*
* \return 0 on success, otherwise a negative error code.
*/
int ec_fsm_eoe_prepare_set(
ec_fsm_eoe_t *fsm, /**< finite state machine */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
uint8_t *data, *cur;
ec_slave_t *slave = fsm->slave;
ec_master_t *master = slave->master;
ec_eoe_request_t *req = fsm->request;
size_t size = 8;
if (req->mac_address_included) {
size += ETH_ALEN;
}
if (req->ip_address_included) {
size += 4;
}
if (req->subnet_mask_included) {
size += 4;
}
if (req->gateway_included) {
size += 4;
}
if (req->dns_included) {
size += 4;
}
if (req->name_included) {
size += EC_MAX_HOSTNAME_SIZE;
}
data = ec_slave_mbox_prepare_send(slave, datagram, EC_MBOX_TYPE_EOE,
size);
if (IS_ERR(data)) {
return PTR_ERR(data);
}
EC_WRITE_U8(data, EC_EOE_FRAMETYPE_SET_IP_REQ); // Set IP parameter req.
EC_WRITE_U8(data + 1, 0x01); // last fragment, no timestamps
EC_WRITE_U16(data + 2, 0x0000); // fragment no., offset, frame no.
EC_WRITE_U32(data + 4,
((req->mac_address_included != 0) << 0) |
((req->ip_address_included != 0) << 1) |
((req->subnet_mask_included != 0) << 2) |
((req->gateway_included != 0) << 3) |
((req->dns_included != 0) << 4) |
((req->name_included != 0) << 5)
);
cur = data + 8;
if (req->mac_address_included) {
memcpy(cur, req->mac_address, ETH_ALEN);
cur += ETH_ALEN;
}
if (req->ip_address_included) {
memcpy(cur, &req->ip_address, 4);
cur += 4;
}
if (req->subnet_mask_included) {
memcpy(cur, &req->subnet_mask, 4);
cur += 4;
}
if (req->gateway_included) {
memcpy(cur, &req->gateway, 4);
cur += 4;
}
if (req->dns_included) {
memcpy(cur, &req->dns, 4);
cur += 4;
}
if (req->name_included) {
memcpy(cur, req->name, EC_MAX_HOSTNAME_SIZE);
cur += EC_MAX_HOSTNAME_SIZE;
}
if (master->debug_level) {
EC_SLAVE_DBG(slave, 0, "Set IP parameter request:\n");
ec_print_data(data, cur - data);
}
fsm->request->jiffies_sent = jiffies;
return 0;
}
void pdo_handle_ecat(master_setup_variables_t *master_setup,
ctrlproto_slv_handle *slv_handles,
unsigned int total_no_of_slaves)
{
int slv;
if(sig_alarms == user_alarms) pause();
while (sig_alarms != user_alarms)
{
/* sync the dc clock of the slaves */
// ecrt_master_sync_slave_clocks(master);
// receive process data
ecrt_master_receive(master_setup->master);
ecrt_domain_process(master_setup->domain);
// check process data state (optional)
//check_domain1_state(master_setup);
// check for master state (optional)
//check_master_state(master_setup);
// check for islave configuration state(s) (optional)
// check_slave_config_states();
for(slv=0;slv<total_no_of_slaves;++slv)
{
/* Read process data */
slv_handles[slv].motorctrl_status_in = EC_READ_U16(master_setup->domain_pd + slv_handles[slv].__ecat_slave_in[0]);
slv_handles[slv].operation_mode_disp = EC_READ_U8(master_setup->domain_pd + slv_handles[slv].__ecat_slave_in[1]);
slv_handles[slv].position_in = EC_READ_U32(master_setup->domain_pd + slv_handles[slv].__ecat_slave_in[2]);
slv_handles[slv].speed_in = EC_READ_U32(master_setup->domain_pd + slv_handles[slv].__ecat_slave_in[3]);
slv_handles[slv].torque_in = EC_READ_U16(master_setup->domain_pd + slv_handles[slv].__ecat_slave_in[4]);
}
/* printf("\n%x", slv_handles[slv].motorctrl_status_in);
printf("\n%x", slv_handles[slv].operation_mode_disp);
printf("\n%x", slv_handles[slv].position_in);
printf("\n%x", slv_handles[slv].speed_in);
printf("\n%x", slv_handles[slv].torque_in);
*/
for(slv=0;slv<total_no_of_slaves;++slv)
{
EC_WRITE_U16(master_setup->domain_pd + slv_handles[slv].__ecat_slave_out[0], (slv_handles[slv].motorctrl_out)&0xffff);
EC_WRITE_U8(master_setup->domain_pd + slv_handles[slv].__ecat_slave_out[1], (slv_handles[slv].operation_mode)&0xff);
EC_WRITE_U16(master_setup->domain_pd + slv_handles[slv].__ecat_slave_out[2], (slv_handles[slv].torque_setpoint)&0xffff);
EC_WRITE_U32(master_setup->domain_pd + slv_handles[slv].__ecat_slave_out[3], slv_handles[slv].position_setpoint);
EC_WRITE_U32(master_setup->domain_pd + slv_handles[slv].__ecat_slave_out[4], slv_handles[slv].speed_setpoint);
}
// send process data
ecrt_domain_queue(master_setup->domain);
ecrt_master_send(master_setup->master);
//Check for master und domain state
ecrt_master_state(master_setup->master, &master_setup->master_state);
ecrt_domain_state(master_setup->domain, &master_setup->domain_state);
if (master_setup->domain_state.wc_state == EC_WC_COMPLETE && !master_setup->op_flag)
{
//printf("System up!\n");
master_setup->op_flag = 1;
}
else
{
if(master_setup->domain_state.wc_state != EC_WC_COMPLETE && master_setup->op_flag)
{
//printf("System down!\n");
master_setup->op_flag = 0;
}
}
user_alarms++;
}
}
void cyclic_task()
{
/* sync the dc clock of the slaves */
ecrt_master_sync_slave_clocks(master);
// receive process data
ecrt_master_receive(master);
ecrt_domain_process(domain1);
// check process data state (optional)
check_domain1_state();
if (counter) {
counter--;
} else { // do this at 1 Hz
counter = FREQUENCY;
// calculate new process data
blink = !blink;
// check for master state (optional)
check_master_state();
// check for islave configuration state(s) (optional)
check_slave_config_states();
#if SDO_ACCESS
// read process data SDO
read_sdo(sdo);
read_sdo(request[0]);
read_sdo(request[1]);
read_sdo(request[2]);
write_sdo(sdo_download_requests[0], sdoexample); /* SDO download value to the node */
#endif
}
/* Read process data */
unsigned int sn_status = EC_READ_U16(domain1_pd + off_pdo1_in);
unsigned int sn_modes = EC_READ_U8(domain1_pd + off_pdo2_in);
unsigned int sn_position = EC_READ_U32(domain1_pd + off_pdo3_in);
unsigned int sn_velocity = EC_READ_U32(domain1_pd + off_pdo4_in);
unsigned int sn_torque = EC_READ_U16(domain1_pd + off_pdo5_in);
logmsg(2, "[REC] 0x%4x 0x%4x 0x%8x 0x%8x 0x%4x\n",
sn_status, sn_modes,
sn_position, sn_velocity, sn_torque);
#if 0
// read process data
printf("AnaIn: state %u value %u\n",
EC_READ_U8(domain1_pd + off_ana_in_status),
EC_READ_U16(domain1_pd + off_ana_in_value));
#endif
// write process data
//EC_WRITE_U8(domain1_pd + off_dig_out, blink ? 0x06 : 0x09);
#ifdef CIA402
#define STATUSW1 0x88AA
#define STATUSW2 0xAA88
#define OPMODES1 0xf1
#define OPMODES2 0x1f
#define TORVAL1 0xabab
#define TORVAL2 0xbaba
#define VELVAL1 0x2d2d4d4d
#define VELVAL2 0xd4d4d2d2
#define POSVAL1 0xe4e4e2e2
#define POSVAL2 0x2e2e4e4e
EC_WRITE_U16(domain1_pd + off_pdo1_out, (blink ? STATUSW1 : STATUSW2)&0xffff);
EC_WRITE_U8(domain1_pd + off_pdo2_out, (blink ? OPMODES1 : OPMODES2)&0xff);
EC_WRITE_U16(domain1_pd + off_pdo3_out, (blink ? TORVAL1 : TORVAL2)&0xffff);
EC_WRITE_U32(domain1_pd + off_pdo4_out, blink ? POSVAL1 : POSVAL2);
EC_WRITE_U32(domain1_pd + off_pdo5_out, blink ? VELVAL1 : VELVAL2);
#else
#define TESTWORD1 0xdead
#define TESTWORD2 0xbeef
#define TESTWORD3 0xfefe
#define TESTWORD4 0xa5a5
EC_WRITE_U16(domain1_pd + off_pdo1_out, blink ? TESTWORD1 : TESTWORD2);
EC_WRITE_U16(domain1_pd + off_pdo2_out, blink ? TESTWORD3 : TESTWORD4);
#endif
// send process data
ecrt_domain_queue(domain1);
ecrt_master_send(master);
//printf("Wrote %x to slave\n", blink ? TESTWORD1 : TESTWORD2);
}
