diff -r d28360ee74c5 -r d9cf40facbc4 master/fsm_coe.c --- a/master/fsm_coe.c Mon May 03 10:13:25 2010 +0200 +++ b/master/fsm_coe.c Mon May 03 18:07:57 2010 +0200 @@ -144,23 +144,21 @@ /*****************************************************************************/ -/** - Outputs an SDO abort message. -*/ - -void ec_canopen_abort_msg(uint32_t abort_code) +/** Outputs an SDO abort message. + */ +void ec_canopen_abort_msg(const ec_slave_t *slave, uint32_t abort_code) { const ec_code_msg_t *abort_msg; for (abort_msg = sdo_abort_messages; abort_msg->code; abort_msg++) { if (abort_msg->code == abort_code) { - EC_ERR("SDO abort message 0x%08X: \"%s\".\n", + EC_SLAVE_ERR(slave, "SDO abort message 0x%08X: \"%s\".\n", abort_msg->code, abort_msg->message); return; } } - EC_ERR("Unknown SDO abort code 0x%08X.\n", abort_code); + EC_SLAVE_ERR(slave, "Unknown SDO abort code 0x%08X.\n", abort_code); } /*****************************************************************************/ @@ -265,15 +263,14 @@ return 0; if (size < 10) { - EC_WARN("Received incomplete CoE Emergency request from slave %u:\n", - fsm->slave->ring_position); + EC_SLAVE_WARN(fsm->slave, "Received incomplete CoE Emergency" + " request:\n"); ec_print_data(data, size); return 1; } - EC_INFO("CoE Emergency Request received from slave %u:\n", - fsm->slave->ring_position); - EC_INFO("Error code 0x%04X, Error register 0x%02X, data:\n", + EC_SLAVE_WARN(fsm->slave, "CoE Emergency Request received:\n" + "Error code 0x%04X, Error register 0x%02X, data:\n", EC_READ_U16(data + 2), EC_READ_U8(data + 4)); ec_print_data(data + 5, 5); return 1; @@ -294,14 +291,14 @@ uint8_t *data; if (!(slave->sii.mailbox_protocols & EC_MBOX_COE)) { - EC_ERR("Slave %u does not support CoE!\n", slave->ring_position); + EC_SLAVE_ERR(slave, "Slave does not support CoE!\n"); fsm->state = ec_fsm_coe_error; return; } if (slave->sii.has_general && !slave->sii.coe_details.enable_sdo_info) { - EC_ERR("Slave %u does not support SDO information service!\n", - slave->ring_position); + EC_SLAVE_ERR(slave, "Slave does not support" + " SDO information service!\n"); fsm->state = ec_fsm_coe_error; return; } @@ -339,16 +336,15 @@ if (datagram->state != EC_DATAGRAM_RECEIVED) { fsm->state = ec_fsm_coe_error; - EC_ERR("Failed to receive CoE dictionary request datagram for" - " slave %u: ", slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to receive CoE dictionary" + " request datagram: "); ec_datagram_print_state(datagram); return; } if (datagram->working_counter != 1) { fsm->state = ec_fsm_coe_error; - EC_ERR("Reception of CoE dictionary request failed on slave %u: ", - slave->ring_position); + EC_SLAVE_ERR(slave, "Reception of CoE dictionary request failed: "); ec_datagram_print_wc_error(datagram); return; } @@ -376,16 +372,15 @@ if (datagram->state != EC_DATAGRAM_RECEIVED) { fsm->state = ec_fsm_coe_error; - EC_ERR("Failed to receive CoE mailbox check datagram for slave %u: ", - slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check datagram: "); ec_datagram_print_state(datagram); return; } if (datagram->working_counter != 1) { fsm->state = ec_fsm_coe_error; - EC_ERR("Reception of CoE mailbox check datagram failed on slave %u: ", - slave->ring_position); + EC_SLAVE_ERR(slave,"Reception of CoE mailbox check" + " datagram failed: "); ec_datagram_print_wc_error(datagram); return; } @@ -395,8 +390,8 @@ (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; if (diff_ms >= EC_FSM_COE_DICT_TIMEOUT) { fsm->state = ec_fsm_coe_error; - EC_ERR("Timeout while waiting for SDO dictionary list response " - "on slave %u.\n", slave->ring_position); + EC_SLAVE_ERR(slave, "Timeout while waiting for" + " SDO dictionary list response.\n"); return; } @@ -433,16 +428,15 @@ if (datagram->state != EC_DATAGRAM_RECEIVED) { fsm->state = ec_fsm_coe_error; - EC_ERR("Failed to receive CoE dictionary response datagram for" - " slave %u: ", slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to receive CoE dictionary" + " response datagram: "); ec_datagram_print_state(datagram); return; } if (datagram->working_counter != 1) { fsm->state = ec_fsm_coe_error; - EC_ERR("Reception of CoE dictionary response failed on slave %u: ", - slave->ring_position); + EC_SLAVE_ERR(slave, "Reception of CoE dictionary response failed: "); ec_datagram_print_wc_error(datagram); return; } @@ -454,7 +448,8 @@ } if (mbox_prot != 0x03) { // CoE - EC_ERR("Received mailbox protocol 0x%02X as response.\n", mbox_prot); + EC_SLAVE_ERR(slave, "Received mailbox protocol 0x%02X as response.\n", + mbox_prot); fsm->state = ec_fsm_coe_error; return; } @@ -468,21 +463,21 @@ } if (rec_size < 3) { - EC_ERR("Received corrupted SDO dictionary response (size %zu).\n", - rec_size); + EC_SLAVE_ERR(slave, "Received corrupted SDO dictionary response" + " (size %zu).\n", rec_size); fsm->state = ec_fsm_coe_error; return; } if (EC_READ_U16(data) >> 12 == 0x8 && // SDO information (EC_READ_U8(data + 2) & 0x7F) == 0x07) { // error response - EC_ERR("SDO information error response at slave %u!\n", - slave->ring_position); + EC_SLAVE_ERR(slave, "SDO information error response!\n"); if (rec_size < 10) { - EC_ERR("Incomplete SDO information error response:\n"); + EC_SLAVE_ERR(slave, "Incomplete SDO information" + " error response:\n"); ec_print_data(data, rec_size); } else { - ec_canopen_abort_msg(EC_READ_U32(data + 6)); + ec_canopen_abort_msg(slave, EC_READ_U32(data + 6)); } fsm->state = ec_fsm_coe_error; return; @@ -491,8 +486,8 @@ if (EC_READ_U16(data) >> 12 != 0x8 || // SDO information (EC_READ_U8 (data + 2) & 0x7F) != 0x02) { // Get OD List response if (fsm->slave->master->debug_level) { - EC_DBG("Invalid SDO list response at slave %u! Retrying...\n", - slave->ring_position); + EC_SLAVE_DBG(slave, 1, "Invalid SDO list response!" + " Retrying...\n"); ec_print_data(data, rec_size); } ec_slave_mbox_prepare_check(slave, datagram); // can not fail. @@ -502,7 +497,7 @@ } if (rec_size < 8 || rec_size % 2) { - EC_ERR("Invalid data size %zu!\n", rec_size); + EC_SLAVE_ERR(slave, "Invalid data size %zu!\n", rec_size); ec_print_data(data, rec_size); fsm->state = ec_fsm_coe_error; return; @@ -513,14 +508,12 @@ for (i = 0; i < sdo_count; i++) { sdo_index = EC_READ_U16(data + 8 + i * 2); if (!sdo_index) { - if (slave->master->debug_level) - EC_WARN("SDO dictionary of slave %u contains index 0x0000.\n", - slave->ring_position); + EC_SLAVE_DBG(slave, 1, "SDO dictionary contains index 0x0000.\n"); continue; } if (!(sdo = (ec_sdo_t *) kmalloc(sizeof(ec_sdo_t), GFP_KERNEL))) { - EC_ERR("Failed to allocate memory for SDO!\n"); + EC_SLAVE_ERR(slave, "Failed to allocate memory for SDO!\n"); fsm->state = ec_fsm_coe_error; return; } @@ -531,10 +524,12 @@ fragments_left = EC_READ_U16(data + 4); if (slave->master->debug_level && fragments_left) { - EC_DBG("SDO list fragments left: %u\n", fragments_left); - } - - if (EC_READ_U8(data + 2) & 0x80 || fragments_left) { // more messages waiting. check again. + EC_SLAVE_DBG(slave, 1, "SDO list fragments left: %u\n", + fragments_left); + } + + if (EC_READ_U8(data + 2) & 0x80 || fragments_left) { + // more messages waiting. check again. fsm->jiffies_start = datagram->jiffies_sent; ec_slave_mbox_prepare_check(slave, datagram); // can not fail. fsm->retries = EC_FSM_RETRIES; @@ -584,16 +579,16 @@ if (datagram->state != EC_DATAGRAM_RECEIVED) { fsm->state = ec_fsm_coe_error; - EC_ERR("Failed to receive CoE SDO description request datagram for" - " slave %u: ", slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to receive CoE SDO" + " description request datagram: "); ec_datagram_print_state(datagram); return; } if (datagram->working_counter != 1) { fsm->state = ec_fsm_coe_error; - EC_ERR("Reception of CoE SDO description" - " request failed on slave %u: ", slave->ring_position); + EC_SLAVE_ERR(slave, "Reception of CoE SDO description" + " request failed: "); ec_datagram_print_wc_error(datagram); return; } @@ -621,16 +616,15 @@ if (datagram->state != EC_DATAGRAM_RECEIVED) { fsm->state = ec_fsm_coe_error; - EC_ERR("Failed to receive CoE mailbox check datagram from slave %u: ", - slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check datagram: "); ec_datagram_print_state(datagram); return; } if (datagram->working_counter != 1) { fsm->state = ec_fsm_coe_error; - EC_ERR("Reception of CoE mailbox check" - " datagram failed on slave %u: ", slave->ring_position); + EC_SLAVE_ERR(slave, "Reception of CoE mailbox check" + " datagram failed: "); ec_datagram_print_wc_error(datagram); return; } @@ -640,9 +634,9 @@ (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; if (diff_ms >= EC_FSM_COE_DICT_TIMEOUT) { fsm->state = ec_fsm_coe_error; - EC_ERR("Timeout while waiting for SDO 0x%04x object description " - "response on slave %u.\n", fsm->sdo->index, - slave->ring_position); + EC_SLAVE_ERR(slave, "Timeout while waiting for" + " SDO 0x%04x object description response.\n", + fsm->sdo->index); return; } @@ -678,16 +672,16 @@ if (datagram->state != EC_DATAGRAM_RECEIVED) { fsm->state = ec_fsm_coe_error; - EC_ERR("Failed to receive CoE SDO description response datagram from" - " slave %u: ", slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to receive CoE SDO description" + " response datagram: "); ec_datagram_print_state(datagram); return; } if (datagram->working_counter != 1) { fsm->state = ec_fsm_coe_error; - EC_ERR("Reception of CoE SDO description" - " response failed on slave %u: ", slave->ring_position); + EC_SLAVE_ERR(slave, "Reception of CoE SDO description" + " response failed: "); ec_datagram_print_wc_error(datagram); return; } @@ -699,7 +693,8 @@ } if (mbox_prot != 0x03) { // CoE - EC_ERR("Received mailbox protocol 0x%02X as response.\n", mbox_prot); + EC_SLAVE_ERR(slave, "Received mailbox protocol 0x%02X as response.\n", + mbox_prot); fsm->state = ec_fsm_coe_error; return; } @@ -713,25 +708,24 @@ } if (rec_size < 3) { - EC_ERR("Received corrupted SDO description response (size %zu).\n", - rec_size); + EC_SLAVE_ERR(slave, "Received corrupted SDO description response" + " (size %zu).\n", rec_size); fsm->state = ec_fsm_coe_error; return; } if (EC_READ_U16(data) >> 12 == 0x8 && // SDO information (EC_READ_U8 (data + 2) & 0x7F) == 0x07) { // error response - EC_ERR("SDO information error response at slave %u while" - " fetching SDO 0x%04X!\n", slave->ring_position, - sdo->index); - ec_canopen_abort_msg(EC_READ_U32(data + 6)); + EC_SLAVE_ERR(slave, "SDO information error response while" + " fetching SDO 0x%04X!\n", sdo->index); + ec_canopen_abort_msg(slave, EC_READ_U32(data + 6)); fsm->state = ec_fsm_coe_error; return; } if (rec_size < 8) { - EC_ERR("Received corrupted SDO description response (size %zu).\n", - rec_size); + EC_SLAVE_ERR(slave, "Received corrupted SDO" + " description response (size %zu).\n", rec_size); fsm->state = ec_fsm_coe_error; return; } @@ -740,9 +734,8 @@ (EC_READ_U8 (data + 2) & 0x7F) != 0x04 || // Object desc. response EC_READ_U16(data + 6) != sdo->index) { // SDO index if (fsm->slave->master->debug_level) { - EC_DBG("Invalid object description response at slave %u while" - " fetching SDO 0x%04X!\n", slave->ring_position, - sdo->index); + EC_SLAVE_DBG(slave, 1, "Invalid object description response while" + " fetching SDO 0x%04X!\n", sdo->index); ec_print_data(data, rec_size); } // check for CoE response again @@ -753,7 +746,7 @@ } if (rec_size < 12) { - EC_ERR("Invalid data size!\n"); + EC_SLAVE_ERR(slave, "Invalid data size!\n"); ec_print_data(data, rec_size); fsm->state = ec_fsm_coe_error; return; @@ -765,7 +758,7 @@ name_size = rec_size - 12; if (name_size) { if (!(sdo->name = kmalloc(name_size + 1, GFP_KERNEL))) { - EC_ERR("Failed to allocate SDO name!\n"); + EC_SLAVE_ERR(slave, "Failed to allocate SDO name!\n"); fsm->state = ec_fsm_coe_error; return; } @@ -775,7 +768,7 @@ } if (EC_READ_U8(data + 2) & 0x80) { - EC_ERR("Fragment follows (not implemented)!\n"); + EC_SLAVE_ERR(slave, "Fragment follows (not implemented)!\n"); fsm->state = ec_fsm_coe_error; return; } @@ -820,16 +813,15 @@ if (datagram->state != EC_DATAGRAM_RECEIVED) { fsm->state = ec_fsm_coe_error; - EC_ERR("Failed to receive CoE SDO entry request datagram for" - " slave %u: ", slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to receive CoE SDO entry" + " request datagram: "); ec_datagram_print_state(datagram); return; } if (datagram->working_counter != 1) { fsm->state = ec_fsm_coe_error; - EC_ERR("Reception of CoE SDO entry request failed on slave %u: ", - slave->ring_position); + EC_SLAVE_ERR(slave, "Reception of CoE SDO entry request failed: "); ec_datagram_print_wc_error(datagram); return; } @@ -858,16 +850,15 @@ if (datagram->state != EC_DATAGRAM_RECEIVED) { fsm->state = ec_fsm_coe_error; - EC_ERR("Failed to receive CoE mailbox check datagram from slave %u" - ": ", slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check datagram: "); ec_datagram_print_state(datagram); return; } if (datagram->working_counter != 1) { fsm->state = ec_fsm_coe_error; - EC_ERR("Reception of CoE mailbox check" - " datagram failed on slave %u: ", slave->ring_position); + EC_SLAVE_ERR(slave, "Reception of CoE mailbox check" + " datagram failed: "); ec_datagram_print_wc_error(datagram); return; } @@ -877,9 +868,9 @@ (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; if (diff_ms >= EC_FSM_COE_DICT_TIMEOUT) { fsm->state = ec_fsm_coe_error; - EC_ERR("Timeout while waiting for SDO entry 0x%04x:%x" - " description response on slave %u.\n", - fsm->sdo->index, fsm->subindex, slave->ring_position); + EC_SLAVE_ERR(slave, "Timeout while waiting for" + " SDO entry 0x%04x:%x description response.\n", + fsm->sdo->index, fsm->subindex); return; } @@ -917,16 +908,16 @@ if (datagram->state != EC_DATAGRAM_RECEIVED) { fsm->state = ec_fsm_coe_error; - EC_ERR("Failed to receive CoE SDO description response datagram from" - " slave %u: ", slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to receive CoE SDO" + " description response datagram: "); ec_datagram_print_state(datagram); return; } if (datagram->working_counter != 1) { fsm->state = ec_fsm_coe_error; - EC_ERR("Reception of CoE SDO description" - " response failed on slave %u: ", slave->ring_position); + EC_SLAVE_ERR(slave, "Reception of CoE SDO description" + " response failed: "); ec_datagram_print_wc_error(datagram); return; } @@ -938,7 +929,8 @@ } if (mbox_prot != 0x03) { // CoE - EC_ERR("Received mailbox protocol 0x%02X as response.\n", mbox_prot); + EC_SLAVE_ERR(slave, "Received mailbox protocol" + " 0x%02X as response.\n", mbox_prot); fsm->state = ec_fsm_coe_error; return; } @@ -952,25 +944,25 @@ } if (rec_size < 3) { - EC_ERR("Received corrupted SDO entry description response " - "(size %zu).\n", rec_size); + EC_SLAVE_ERR(slave, "Received corrupted SDO entry" + " description response (size %zu).\n", rec_size); fsm->state = ec_fsm_coe_error; return; } if (EC_READ_U16(data) >> 12 == 0x8 && // SDO information (EC_READ_U8 (data + 2) & 0x7F) == 0x07) { // error response - EC_ERR("SDO information error response at slave %u while" - " fetching SDO entry 0x%04X:%02X!\n", slave->ring_position, + EC_SLAVE_ERR(slave, "SDO information error response while" + " fetching SDO entry 0x%04X:%02X!\n", sdo->index, fsm->subindex); - ec_canopen_abort_msg(EC_READ_U32(data + 6)); + ec_canopen_abort_msg(slave, EC_READ_U32(data + 6)); fsm->state = ec_fsm_coe_error; return; } if (rec_size < 9) { - EC_ERR("Received corrupted SDO entry description response " - "(size %zu).\n", rec_size); + EC_SLAVE_ERR(slave, "Received corrupted SDO entry" + " description response (size %zu).\n", rec_size); fsm->state = ec_fsm_coe_error; return; } @@ -980,8 +972,8 @@ EC_READ_U16(data + 6) != sdo->index || // SDO index EC_READ_U8(data + 8) != fsm->subindex) { // SDO subindex if (fsm->slave->master->debug_level) { - EC_DBG("Invalid entry description response at slave %u while" - " fetching SDO entry 0x%04X:%02X!\n", slave->ring_position, + EC_SLAVE_DBG(slave, 1, "Invalid entry description response while" + " fetching SDO entry 0x%04X:%02X!\n", sdo->index, fsm->subindex); ec_print_data(data, rec_size); } @@ -993,7 +985,7 @@ } if (rec_size < 16) { - EC_ERR("Invalid data size %zu!\n", rec_size); + EC_SLAVE_ERR(slave, "Invalid data size %zu!\n", rec_size); ec_print_data(data, rec_size); fsm->state = ec_fsm_coe_error; return; @@ -1003,7 +995,7 @@ if (!(entry = (ec_sdo_entry_t *) kmalloc(sizeof(ec_sdo_entry_t), GFP_KERNEL))) { - EC_ERR("Failed to allocate entry!\n"); + EC_SLAVE_ERR(slave, "Failed to allocate entry!\n"); fsm->state = ec_fsm_coe_error; return; } @@ -1024,7 +1016,7 @@ if (data_size) { uint8_t *desc; if (!(desc = kmalloc(data_size + 1, GFP_KERNEL))) { - EC_ERR("Failed to allocate SDO entry name!\n"); + EC_SLAVE_ERR(slave, "Failed to allocate SDO entry name!\n"); fsm->state = ec_fsm_coe_error; return; } @@ -1104,20 +1096,20 @@ } else { sprintf(subidxstr, ":%02X", request->subindex); } - EC_DBG("Downloading SDO 0x%04X%s to slave %u.\n", - request->index, subidxstr, slave->ring_position); + EC_SLAVE_DBG(slave, 1, "Downloading SDO 0x%04X%s.\n", + request->index, subidxstr); ec_print_data(request->data, request->data_size); } if (!(slave->sii.mailbox_protocols & EC_MBOX_COE)) { - EC_ERR("Slave %u does not support CoE!\n", slave->ring_position); + EC_SLAVE_ERR(slave, "Slave does not support CoE!\n"); fsm->state = ec_fsm_coe_error; return; } if (slave->configured_rx_mailbox_size < EC_MBOX_HEADER_SIZE + EC_COE_DOWN_REQ_HEADER_SIZE) { - EC_ERR("Mailbox too small!\n"); + EC_SLAVE_ERR(slave, "Mailbox too small!\n"); fsm->state = ec_fsm_coe_error; return; } @@ -1146,7 +1138,7 @@ memset(data + 6 + request->data_size, 0x00, 4 - request->data_size); if (slave->master->debug_level) { - EC_DBG("Expedited download request:\n"); + EC_SLAVE_DBG(slave, 1, "Expedited download request:\n"); ec_print_data(data, EC_COE_DOWN_REQ_HEADER_SIZE); } } @@ -1193,7 +1185,7 @@ } if (slave->master->debug_level) { - EC_DBG("Normal download request:\n"); + EC_SLAVE_DBG(slave, 1, "Normal download request:\n"); ec_print_data(data, data_size); } } @@ -1221,8 +1213,8 @@ if (datagram->state != EC_DATAGRAM_RECEIVED) { fsm->state = ec_fsm_coe_error; - EC_ERR("Failed to receive CoE download request datagram for" - " slave %u: ", slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to receive CoE download" + " request datagram: "); ec_datagram_print_state(datagram); return; } @@ -1233,30 +1225,26 @@ if (!datagram->working_counter) { if (diff_ms < fsm->request->response_timeout) { #if DEBUG_RETRIES - if (fsm->slave->master->debug_level) { - EC_DBG("Slave %u did not respond to SDO download request. " - "Retrying after %u ms...\n", - slave->ring_position, (u32) diff_ms); - } + EC_SLAVE_DBG(slave, 1, "Slave did not respond to" + " SDO download request. Retrying after %u ms...\n", + (u32) diff_ms); #endif // no response; send request datagram again return; } } fsm->state = ec_fsm_coe_error; - EC_ERR("Reception of CoE download request for SDO 0x%04x:%x failed" - " with timeout after %u ms on slave %u: ", - fsm->request->index, fsm->request->subindex, (u32) diff_ms, - fsm->slave->ring_position); + EC_SLAVE_ERR(slave, "Reception of CoE download request" + " for SDO 0x%04x:%x failed with timeout after %u ms: ", + fsm->request->index, fsm->request->subindex, (u32) diff_ms); ec_datagram_print_wc_error(datagram); return; } #if DEBUG_LONG if (diff_ms > 200) { - EC_WARN("SDO 0x%04x:%x download took %u ms on slave %u.\n", - fsm->request->index, fsm->request->subindex, (u32) diff_ms, - fsm->slave->ring_position); + EC_SLAVE_WARN(slave, "SDO 0x%04x:%x download took %u ms.\n", + fsm->request->index, fsm->request->subindex, (u32) diff_ms); } #endif @@ -1281,16 +1269,16 @@ if (datagram->state != EC_DATAGRAM_RECEIVED) { fsm->state = ec_fsm_coe_error; - EC_ERR("Failed to receive CoE mailbox check datagram for slave %u: ", - slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check" + " datagram: "); ec_datagram_print_state(datagram); return; } if (datagram->working_counter != 1) { fsm->state = ec_fsm_coe_error; - EC_ERR("Reception of CoE mailbox check" - " datagram failed on slave %u: ", slave->ring_position); + EC_SLAVE_ERR(slave, "Reception of CoE mailbox check" + " datagram failed: "); ec_datagram_print_wc_error(datagram); return; } @@ -1300,10 +1288,9 @@ (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; if (diff_ms >= fsm->request->response_timeout) { fsm->state = ec_fsm_coe_error; - EC_ERR("Timeout after %u ms while waiting for SDO 0x%04x:%x" - " download response on slave %u.\n", (u32) diff_ms, - fsm->request->index, fsm->request->subindex, - slave->ring_position); + EC_SLAVE_ERR(slave, "Timeout after %u ms while waiting" + " for SDO 0x%04x:%x download response.\n", (u32) diff_ms, + fsm->request->index, fsm->request->subindex); return; } @@ -1376,7 +1363,7 @@ fsm->remaining -= segment_size; if (slave->master->debug_level) { - EC_DBG("Download segment request:\n"); + EC_SLAVE_DBG(slave, 1, "Download segment request:\n"); ec_print_data(data, data_size); } @@ -1403,16 +1390,15 @@ if (datagram->state != EC_DATAGRAM_RECEIVED) { fsm->state = ec_fsm_coe_error; - EC_ERR("Failed to receive CoE download response datagram from" - " slave %u: ", slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to receive CoE download" + " response datagram: "); ec_datagram_print_state(datagram); return; } if (datagram->working_counter != 1) { fsm->state = ec_fsm_coe_error; - EC_ERR("Reception of CoE download response failed on slave %u: ", - slave->ring_position); + EC_SLAVE_ERR(slave, "Reception of CoE download response failed: "); ec_datagram_print_wc_error(datagram); return; } @@ -1425,7 +1411,8 @@ if (mbox_prot != 0x03) { // CoE fsm->state = ec_fsm_coe_error; - EC_ERR("Received mailbox protocol 0x%02X as response.\n", mbox_prot); + EC_SLAVE_ERR(slave, "Received mailbox protocol 0x%02X as response.\n", + mbox_prot); return; } @@ -1438,13 +1425,14 @@ } if (slave->master->debug_level) { - EC_DBG("Download response:\n"); + EC_SLAVE_DBG(slave, 1, "Download response:\n"); ec_print_data(data, rec_size); } if (rec_size < 6) { fsm->state = ec_fsm_coe_error; - EC_ERR("Received data are too small (%zu bytes):\n", rec_size); + EC_SLAVE_ERR(slave, "Received data are too small (%zu bytes):\n", + rec_size); ec_print_data(data, rec_size); return; } @@ -1458,15 +1446,14 @@ } else { sprintf(subidxstr, ":%02X", request->subindex); } - EC_ERR("SDO download 0x%04X%s (%zu bytes) aborted on slave %u.\n", - request->index, subidxstr, request->data_size, - slave->ring_position); + EC_SLAVE_ERR(slave, "SDO download 0x%04X%s (%zu bytes) aborted.\n", + request->index, subidxstr, request->data_size); if (rec_size < 10) { - EC_ERR("Incomplete abort command:\n"); + EC_SLAVE_ERR(slave, "Incomplete abort command:\n"); ec_print_data(data, rec_size); } else { fsm->request->abort_code = EC_READ_U32(data + 6); - ec_canopen_abort_msg(fsm->request->abort_code); + ec_canopen_abort_msg(slave, fsm->request->abort_code); } return; } @@ -1476,8 +1463,8 @@ EC_READ_U16(data + 3) != request->index || // index EC_READ_U8 (data + 5) != request->subindex) { // subindex if (slave->master->debug_level) { - EC_DBG("Invalid SDO download response at slave %u! Retrying...\n", - slave->ring_position); + EC_SLAVE_DBG(slave, 1, "Invalid SDO download response!" + " Retrying...\n"); ec_print_data(data, rec_size); } // check for CoE response again @@ -1511,16 +1498,15 @@ if (datagram->state != EC_DATAGRAM_RECEIVED) { fsm->state = ec_fsm_coe_error; - EC_ERR("Failed to receive CoE mailbox check datagram for slave %u: ", - slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check datagram: "); ec_datagram_print_state(datagram); return; } if (datagram->working_counter != 1) { fsm->state = ec_fsm_coe_error; - EC_ERR("Reception of CoE mailbox segment check" - " datagram failed on slave %u: ", slave->ring_position); + EC_SLAVE_ERR(slave, "Reception of CoE mailbox segment check" + " datagram failed: "); ec_datagram_print_wc_error(datagram); return; } @@ -1530,8 +1516,8 @@ (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; if (diff_ms >= fsm->request->response_timeout) { fsm->state = ec_fsm_coe_error; - EC_ERR("Timeout while waiting for SDO download segment response " - "on slave %u.\n", slave->ring_position); + EC_SLAVE_ERR(slave, "Timeout while waiting for SDO download" + " segment response.\n"); return; } @@ -1568,16 +1554,15 @@ if (datagram->state != EC_DATAGRAM_RECEIVED) { fsm->state = ec_fsm_coe_error; - EC_ERR("Failed to receive CoE download response datagram from" - " slave %u: ", slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to receive CoE download response" + " datagram: "); ec_datagram_print_state(datagram); return; } if (datagram->working_counter != 1) { fsm->state = ec_fsm_coe_error; - EC_ERR("Reception of CoE download response failed on slave %u: ", - slave->ring_position); + EC_SLAVE_ERR(slave, "Reception of CoE download response failed: "); ec_datagram_print_wc_error(datagram); return; } @@ -1590,7 +1575,8 @@ if (mbox_prot != 0x03) { // CoE fsm->state = ec_fsm_coe_error; - EC_ERR("Received mailbox protocol 0x%02X as response.\n", mbox_prot); + EC_SLAVE_ERR(slave, "Received mailbox protocol 0x%02X as response.\n", + mbox_prot); return; } @@ -1603,13 +1589,14 @@ } if (slave->master->debug_level) { - EC_DBG("Download response:\n"); + EC_SLAVE_DBG(slave, 1, "Download response:\n"); ec_print_data(data, rec_size); } if (rec_size < 6) { fsm->state = ec_fsm_coe_error; - EC_ERR("Received data are too small (%zu bytes):\n", rec_size); + EC_SLAVE_ERR(slave, "Received data are too small (%zu bytes):\n", + rec_size); ec_print_data(data, rec_size); return; } @@ -1623,15 +1610,14 @@ } else { sprintf(subidxstr, ":%02X", request->subindex); } - EC_ERR("SDO download 0x%04X%s (%zu bytes) aborted on slave %u.\n", - request->index, subidxstr, request->data_size, - slave->ring_position); + EC_SLAVE_ERR(slave, "SDO download 0x%04X%s (%zu bytes) aborted.\n", + request->index, subidxstr, request->data_size); if (rec_size < 10) { - EC_ERR("Incomplete abort command:\n"); + EC_SLAVE_ERR(slave, "Incomplete abort command:\n"); ec_print_data(data, rec_size); } else { fsm->request->abort_code = EC_READ_U32(data + 6); - ec_canopen_abort_msg(fsm->request->abort_code); + ec_canopen_abort_msg(slave, fsm->request->abort_code); } return; } @@ -1639,8 +1625,8 @@ if (EC_READ_U16(data) >> 12 != 0x3 || ((EC_READ_U8(data + 2) >> 5) != 0x01)) { // segment response if (slave->master->debug_level) { - EC_DBG("Invalid SDO download response at slave %u! Retrying...\n", - slave->ring_position); + EC_SLAVE_DBG(slave, 1, "Invalid SDO download response!" + " Retrying...\n"); ec_print_data(data, rec_size); } // check for CoE response again @@ -1651,7 +1637,8 @@ } if (((EC_READ_U8(data + 2) >> 4) & 0x01) != fsm->toggle) { - EC_ERR("Invalid toggle received during segmented download:\n"); + EC_SLAVE_ERR(slave, "Invalid toggle received during" + " segmented download:\n"); ec_print_data(data, rec_size); fsm->state = ec_fsm_coe_error; return; @@ -1679,12 +1666,11 @@ ec_sdo_request_t *request = fsm->request; uint8_t *data; - if (master->debug_level) - EC_DBG("Uploading SDO 0x%04X:%02X from slave %u.\n", - request->index, request->subindex, slave->ring_position); + EC_SLAVE_DBG(slave, 1, "Uploading SDO 0x%04X:%02X.\n", + request->index, request->subindex); if (!(slave->sii.mailbox_protocols & EC_MBOX_COE)) { - EC_ERR("Slave %u does not support CoE!\n", slave->ring_position); + EC_SLAVE_ERR(slave, "Slave does not support CoE!\n"); fsm->state = ec_fsm_coe_error; return; } @@ -1702,7 +1688,7 @@ memset(data + 6, 0x00, 4); if (master->debug_level) { - EC_DBG("Upload request:\n"); + EC_SLAVE_DBG(slave, 1, "Upload request:\n"); ec_print_data(data, 10); } @@ -1729,8 +1715,7 @@ if (datagram->state != EC_DATAGRAM_RECEIVED) { fsm->state = ec_fsm_coe_error; - EC_ERR("Failed to receive CoE upload request for slave %u: ", - slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to receive CoE upload request: "); ec_datagram_print_state(datagram); return; } @@ -1741,30 +1726,26 @@ if (!datagram->working_counter) { if (diff_ms < fsm->request->response_timeout) { #if DEBUG_RETRIES - if (fsm->slave->master->debug_level) { - EC_DBG("Slave %u did not respond to SDO upload request. " - "Retrying after %u ms...\n", - slave->ring_position, (u32) diff_ms); - } + EC_SLAVE_DBG(slave, 1, "Slave did not respond to" + " SDO upload request. Retrying after %u ms...\n", + (u32) diff_ms); #endif // no response; send request datagram again return; } } fsm->state = ec_fsm_coe_error; - EC_ERR("Reception of CoE upload request for SDO 0x%04x:%x failed" - " with timeout after %u ms on slave %u: ", - fsm->request->index, fsm->request->subindex, (u32) diff_ms, - fsm->slave->ring_position); + EC_SLAVE_ERR(slave, "Reception of CoE upload request for" + " SDO 0x%04x:%x failed with timeout after %u ms: ", + fsm->request->index, fsm->request->subindex, (u32) diff_ms); ec_datagram_print_wc_error(datagram); return; } #if DEBUG_LONG if (diff_ms > 200) { - EC_WARN("SDO 0x%04x:%x upload took %u ms on slave %u.\n", - fsm->request->index, fsm->request->subindex, (u32) diff_ms, - fsm->slave->ring_position); + EC_SLAVE_WARN(slave, "SDO 0x%04x:%x upload took %u ms.\n", + fsm->request->index, fsm->request->subindex, (u32) diff_ms); } #endif @@ -1791,16 +1772,15 @@ if (datagram->state != EC_DATAGRAM_RECEIVED) { fsm->state = ec_fsm_coe_error; - EC_ERR("Failed to receive CoE mailbox check datagram from slave %u: ", - slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check datagram: "); ec_datagram_print_state(datagram); return; } if (datagram->working_counter != 1) { fsm->state = ec_fsm_coe_error; - EC_ERR("Reception of CoE mailbox check" - " datagram failed on slave %u: ", slave->ring_position); + EC_SLAVE_ERR(slave, "Reception of CoE mailbox check" + " datagram failed: "); ec_datagram_print_wc_error(datagram); return; } @@ -1810,10 +1790,9 @@ (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; if (diff_ms >= fsm->request->response_timeout) { fsm->state = ec_fsm_coe_error; - EC_ERR("Timeout after %u ms while waiting for SDO 0x%04x:%x" - " upload response on slave %u.\n", (u32) diff_ms, - fsm->request->index, fsm->request->subindex, - slave->ring_position); + EC_SLAVE_ERR(slave, "Timeout after %u ms while waiting for" + " SDO 0x%04x:%x upload response.\n", (u32) diff_ms, + fsm->request->index, fsm->request->subindex); return; } @@ -1849,7 +1828,7 @@ memset(data + 3, 0x00, 7); if (fsm->slave->master->debug_level) { - EC_DBG("Upload segment request:\n"); + EC_SLAVE_DBG(fsm->slave, 1, "Upload segment request:\n"); ec_print_data(data, 10); } } @@ -1877,16 +1856,15 @@ if (datagram->state != EC_DATAGRAM_RECEIVED) { fsm->state = ec_fsm_coe_error; - EC_ERR("Failed to receive CoE upload response datagram for" - " slave %u: ", slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to receive CoE upload response" + " datagram: "); ec_datagram_print_state(datagram); return; } if (datagram->working_counter != 1) { fsm->state = ec_fsm_coe_error; - EC_ERR("Reception of CoE upload response failed on slave %u: ", - slave->ring_position); + EC_SLAVE_ERR(slave, "Reception of CoE upload response failed: "); ec_datagram_print_wc_error(datagram); return; } @@ -1898,13 +1876,14 @@ } if (master->debug_level) { - EC_DBG("Upload response:\n"); + EC_SLAVE_DBG(slave, 1, "Upload response:\n"); ec_print_data(data, rec_size); } if (mbox_prot != 0x03) { // CoE fsm->state = ec_fsm_coe_error; - EC_WARN("Received mailbox protocol 0x%02X as response.\n", mbox_prot); + EC_SLAVE_WARN(slave, "Received mailbox protocol 0x%02X" + " as response.\n", mbox_prot); return; } @@ -1918,20 +1897,21 @@ if (rec_size < 6) { fsm->state = ec_fsm_coe_error; - EC_ERR("Received currupted SDO upload response (%zu bytes)!\n", rec_size); + EC_SLAVE_ERR(slave, "Received currupted SDO upload response" + " (%zu bytes)!\n", rec_size); ec_print_data(data, rec_size); return; } if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request EC_READ_U8(data + 2) >> 5 == 0x4) { // abort SDO transfer request - EC_ERR("SDO upload 0x%04X:%02X aborted on slave %u.\n", - request->index, request->subindex, slave->ring_position); + EC_SLAVE_ERR(slave, "SDO upload 0x%04X:%02X aborted.\n", + request->index, request->subindex); if (rec_size >= 10) { request->abort_code = EC_READ_U32(data + 6); - ec_canopen_abort_msg(request->abort_code); + ec_canopen_abort_msg(slave, request->abort_code); } else { - EC_ERR("No abort message.\n"); + EC_SLAVE_ERR(slave, "No abort message.\n"); } fsm->state = ec_fsm_coe_error; return; @@ -1939,9 +1919,9 @@ if (EC_READ_U16(data) >> 12 != 0x3 || // SDO response EC_READ_U8(data + 2) >> 5 != 0x2) { // upload response - EC_ERR("Received unknown response while uploading SDO 0x%04X:%02X" - " from slave %u.\n", request->index, request->subindex, - slave->ring_position); + EC_SLAVE_ERR(slave, "Received unknown response while" + " uploading SDO 0x%04X:%02X.\n", + request->index, request->subindex); ec_print_data(data, rec_size); fsm->state = ec_fsm_coe_error; return; @@ -1951,10 +1931,9 @@ rec_subindex = EC_READ_U8(data + 5); if (rec_index != request->index || rec_subindex != request->subindex) { - EC_ERR("Received upload response for wrong SDO (0x%04X:%02X," - " requested: 0x%04X:%02X) from slave %u.\n", - rec_index, rec_subindex, request->index, request->subindex, - slave->ring_position); + EC_SLAVE_ERR(slave, "Received upload response for wrong SDO" + " (0x%04X:%02X, requested: 0x%04X:%02X).\n", + rec_index, rec_subindex, request->index, request->subindex); ec_print_data(data, rec_size); // check for CoE response again @@ -1977,7 +1956,7 @@ if (rec_size < 6 + fsm->complete_size) { fsm->state = ec_fsm_coe_error; - EC_ERR("Received corrupted SDO expedited upload" + EC_SLAVE_ERR(slave, "Received corrupted SDO expedited upload" " response (only %zu bytes)!\n", rec_size); ec_print_data(data, rec_size); return; @@ -1990,7 +1969,7 @@ } else { // normal if (rec_size < 10) { fsm->state = ec_fsm_coe_error; - EC_ERR("Received currupted SDO normal upload" + EC_SLAVE_ERR(slave, "Received currupted SDO normal upload" " response (only %zu bytes)!\n", rec_size); ec_print_data(data, rec_size); return; @@ -2001,7 +1980,7 @@ if (!fsm->complete_size) { fsm->state = ec_fsm_coe_error; - EC_ERR("No complete size supplied!\n"); + EC_SLAVE_ERR(slave, "No complete size supplied!\n"); ec_print_data(data, rec_size); return; } @@ -2019,10 +1998,8 @@ fsm->toggle = 0; if (data_size < fsm->complete_size) { - if (master->debug_level) - EC_DBG("SDO data incomplete (%zu / %u). Segmenting...\n", - data_size, fsm->complete_size); - + EC_SLAVE_DBG(slave, 1, "SDO data incomplete (%zu / %u)." + " Segmenting...\n", data_size, fsm->complete_size); ec_fsm_coe_up_prepare_segment_request(fsm); fsm->retries = EC_FSM_RETRIES; fsm->state = ec_fsm_coe_up_seg_request; @@ -2031,7 +2008,7 @@ } if (master->debug_level) { - EC_DBG("Uploaded data:\n"); + EC_SLAVE_DBG(slave, 1, "Uploaded data:\n"); ec_print_data(request->data, request->data_size); } @@ -2055,16 +2032,16 @@ if (datagram->state != EC_DATAGRAM_RECEIVED) { fsm->state = ec_fsm_coe_error; - EC_ERR("Failed to receive CoE upload segment request datagram for" - " slave %u: ", slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to receive CoE upload segment" + " request datagram: "); ec_datagram_print_state(datagram); return; } if (datagram->working_counter != 1) { fsm->state = ec_fsm_coe_error; - EC_ERR("Reception of CoE upload segment" - " request failed on slave %u: ", slave->ring_position); + EC_SLAVE_ERR(slave, "Reception of CoE upload segment" + " request failed: "); ec_datagram_print_wc_error(datagram); return; } @@ -2092,16 +2069,16 @@ if (datagram->state != EC_DATAGRAM_RECEIVED) { fsm->state = ec_fsm_coe_error; - EC_ERR("Failed to receive CoE mailbox check datagram for slave %u: ", - slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check" + " datagram: "); ec_datagram_print_state(datagram); return; } if (datagram->working_counter != 1) { fsm->state = ec_fsm_coe_error; - EC_ERR("Reception of CoE mailbox check" - " datagram failed on slave %u: ", slave->ring_position); + EC_SLAVE_ERR(slave, "Reception of CoE mailbox check datagram" + " failed: "); ec_datagram_print_wc_error(datagram); return; } @@ -2111,8 +2088,8 @@ (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; if (diff_ms >= fsm->request->response_timeout) { fsm->state = ec_fsm_coe_error; - EC_ERR("Timeout while waiting for SDO upload segment response " - "on slave %u.\n", slave->ring_position); + EC_SLAVE_ERR(slave, "Timeout while waiting for SDO upload" + " segment response.\n"); return; } @@ -2149,16 +2126,16 @@ if (datagram->state != EC_DATAGRAM_RECEIVED) { fsm->state = ec_fsm_coe_error; - EC_ERR("Failed to receive CoE upload segment response datagram for" - " slave %u: ", slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to receive CoE upload segment" + " response datagram: "); ec_datagram_print_state(datagram); return; } if (datagram->working_counter != 1) { fsm->state = ec_fsm_coe_error; - EC_ERR("Reception of CoE upload segment" - " response failed on slave %u: ", slave->ring_position); + EC_SLAVE_ERR(slave, "Reception of CoE upload segment" + " response failed: "); ec_datagram_print_wc_error(datagram); return; } @@ -2170,12 +2147,13 @@ } if (master->debug_level) { - EC_DBG("Upload segment response:\n"); + EC_SLAVE_DBG(slave, 1, "Upload segment response:\n"); ec_print_data(data, rec_size); } if (mbox_prot != 0x03) { // CoE - EC_ERR("Received mailbox protocol 0x%02X as response.\n", mbox_prot); + EC_SLAVE_ERR(slave, "Received mailbox protocol 0x%02X as response.\n", + mbox_prot); fsm->state = ec_fsm_coe_error; return; } @@ -2189,7 +2167,8 @@ } if (rec_size < 10) { - EC_ERR("Received currupted SDO upload segment response!\n"); + EC_SLAVE_ERR(slave, "Received currupted SDO upload" + " segment response!\n"); ec_print_data(data, rec_size); fsm->state = ec_fsm_coe_error; return; @@ -2197,10 +2176,10 @@ if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort SDO transfer request - EC_ERR("SDO upload 0x%04X:%02X aborted on slave %u.\n", - request->index, request->subindex, slave->ring_position); + EC_SLAVE_ERR(slave, "SDO upload 0x%04X:%02X aborted.\n", + request->index, request->subindex); request->abort_code = EC_READ_U32(data + 6); - ec_canopen_abort_msg(request->abort_code); + ec_canopen_abort_msg(slave, request->abort_code); fsm->state = ec_fsm_coe_error; return; } @@ -2208,8 +2187,7 @@ if (EC_READ_U16(data) >> 12 != 0x3 || // SDO response EC_READ_U8 (data + 2) >> 5 != 0x0) { // upload segment response if (fsm->slave->master->debug_level) { - EC_DBG("Invalid SDO upload segment response at slave %u!\n", - slave->ring_position); + EC_SLAVE_DBG(slave, 1, "Invalid SDO upload segment response!\n"); ec_print_data(data, rec_size); } // check for CoE response again @@ -2227,9 +2205,9 @@ } if (request->data_size + data_size > fsm->complete_size) { - EC_ERR("SDO upload 0x%04X:%02X failed on slave %u: Fragment" + EC_SLAVE_ERR(slave, "SDO upload 0x%04X:%02X failed: Fragment" " exceeding complete size!\n", - request->index, request->subindex, slave->ring_position); + request->index, request->subindex); fsm->state = ec_fsm_coe_error; return; } @@ -2247,14 +2225,14 @@ } if (request->data_size != fsm->complete_size) { - EC_WARN("SDO upload 0x%04X:%02X on slave %u: Assembled data" + EC_SLAVE_WARN(slave, "SDO upload 0x%04X:%02X: Assembled data" " size (%zu) does not match complete size (%u)!\n", - request->index, request->subindex, slave->ring_position, + request->index, request->subindex, request->data_size, fsm->complete_size); } if (master->debug_level) { - EC_DBG("Uploaded data:\n"); + EC_SLAVE_DBG(slave, 1, "Uploaded data:\n"); ec_print_data(request->data, request->data_size); }