# HG changeset patch # User Martin Troxler # Date 1273579078 -7200 # Node ID 6aa393418fb37c68cbfba9e3c118b030036914fd # Parent ea38efeeb7b3b53099fa584cc0dfeb31ad670a75# Parent 365a90e931616c979a6d14a68bb12f7b74ef7f8a merged diff -r ea38efeeb7b3 -r 6aa393418fb3 TODO --- a/TODO Fri May 07 15:26:26 2010 +0200 +++ b/TODO Tue May 11 13:57:58 2010 +0200 @@ -18,9 +18,7 @@ * Rescan command. * Change SDO index at runtime for SDO request. * Output skipped datagrams again. -* Output warning on unmatched slave configuration. * Output warning when send_ext() is called in illegal context. -* Add master index to log messages. * Implement CompleteAccess for SDO uploads. * Check for Enable SDO Complete Access flag. * Remove allow_scanning flag. @@ -42,8 +40,8 @@ * recompile tool/CommandVersion.cpp if revision changes. * Log SoE IDNs with real name ([SP]-x-yyyy). * Output SoE IDN configurations in 'ethercat config'. -* Output AL status error flag correctly in 'ethercat slaves'. * Fix casting away constness during expected WC calculation. +* Read AL status code on spontaneous state change. Future issues: diff -r ea38efeeb7b3 -r 6aa393418fb3 documentation/Makefile --- a/documentation/Makefile Fri May 07 15:26:26 2010 +0200 +++ b/documentation/Makefile Tue May 11 13:57:58 2010 +0200 @@ -30,6 +30,8 @@ sii_read \ sii_write \ slaves \ + soe_read \ + soe_write \ states \ upload \ version \ diff -r ea38efeeb7b3 -r 6aa393418fb3 documentation/ethercat_doc.tex --- a/documentation/ethercat_doc.tex Fri May 07 15:26:26 2010 +0200 +++ b/documentation/ethercat_doc.tex Tue May 11 13:57:58 2010 +0200 @@ -85,9 +85,9 @@ \begin{center} \rule{\textwidth}{1.5mm} - {\Huge\bf IgH \includegraphics[height=2.4ex]{images/ethercat} - Master \masterversion\\[1ex] - Documentation} + {\Huge\sf\textbf{IgH \includegraphics[height=2.4ex]{images/ethercat} + Master \masterversion}\\[1ex] + \textbf{Documentation}} \vspace{1ex} \rule{\textwidth}{1.5mm} @@ -293,6 +293,21 @@ \end{itemize} +\item Servo Profile over EtherCAT (SoE) + + \begin{itemize} + + \item Implemented according to IEC 61800-7 \cite{soespec}. + + \item Storing IDN configurations, that are written to the slave during + startup. + + \item Accessing IDNs via the command-line tool. + + \item Accessing IDNs at runtime via the the user-space library. + + \end{itemize} + \item Userspace command-line-tool ``ethercat'' (see sec.~\ref{sec:tool}) \begin{itemize} @@ -305,6 +320,7 @@ \item Viewing process data. \item SDO download/upload; listing SDO dictionaries. \item Loading and storing files via FoE. + \item SoE IDN access. \item Access to slave registers. \item Slave SII (EEPROM) access. \item Controlling application-layer states. @@ -614,6 +630,8 @@ % PDO entry registration % SDO configuration % SDO access +% IDN configurations +% IDN access The application interface provides functions and data structures for applications to access an EtherCAT master. The complete documentation of the @@ -919,7 +937,7 @@ sec.~\ref{sec:regaccess}): \begin{lstlisting} -$ `\textbf{watch -n0 "ethercat reg\_read -p4 -tint32 0x92c"}` +$ `\textbf{watch -n0 "ethercat reg\_read -p4 -tsm32 0x92c"}` \end{lstlisting} \paragraph{Sync Signals} Synchronous clocks are only the prerequisite for @@ -1959,8 +1977,10 @@ \chapter{Mailbox Protocol Implementations} \index{Mailbox} -The EtherCAT master implements the EoE and the CoE mailbox -protocols. See the below section for details. +The EtherCAT master implements the CANopen over EtherCAT (CoE), Ethernet over +EtherCAT (EoE), File-access over EtherCAT (FoE), Vendor-specific over EtherCAT +(VoE) and Servo Profile over EtherCAT (SoE) mailbox protocols. See the below +sections for details. %------------------------------------------------------------------------------ @@ -2139,8 +2159,6 @@ % SDO Info Services % -\ldots - \paragraph{SDO Download State Machine} The best time to apply SDO configurations is during the slave's PREOP state, @@ -2218,6 +2236,40 @@ %------------------------------------------------------------------------------ +\section{Servo Profile over EtherCAT (SoE)} +\label{sec:soe} +\index{SoE} + +The SoE protocol implements the Service Channel layer, specified in IEC +61800-7 \cite{soespec} via EtherCAT mailboxes. + +The SoE protocol is quite similar to the CoE protocol (see +sec.~\ref{sec:coe}). Instead of SDO indices and subindices, so-called +identification numbers (IDNs) identify parameters. + +The implementation covers the ``SCC Read'' and ``SCC Write'' primitives, each +with the ability to fragment data. + +There are several ways to use the SoE implementation: + +\begin{itemize} + +\item Reading and writing IDNs via the command-line tool (see +sec.~\ref{sec:soeaccess}). + +\item Storing configurations for arbitrary IDNs via the application interface +(see chap.~\ref{chap:api}, i.\,e.~\lstinline+ecrt_slave_config_idn()+). These +configurations are written to the slave during configuration in PREOP state, +before going to SAFEOP. + +\item The user-space library (see sec.~\ref{sec:userlib}), offers functions to +read/write IDNs in blocking mode (\lstinline+ecrt_master_read_idn()+, +\lstinline+ecrt_master_write_idn()+). + +\end{itemize} + +%------------------------------------------------------------------------------ + \chapter{Userspace Interfaces} \label{sec:user} \index{Userspace} @@ -2436,6 +2488,15 @@ %------------------------------------------------------------------------------ +\subsection{SoE IDN Access} +\label{sec:soeaccess} + +\lstinputlisting[basicstyle=\ttfamily\footnotesize]{external/ethercat_soe_read} + +\lstinputlisting[basicstyle=\ttfamily\footnotesize]{external/ethercat_soe_write} + +%------------------------------------------------------------------------------ + \subsection{Requesting Application-Layer States} \lstinputlisting[basicstyle=\ttfamily\footnotesize]{external/ethercat_states} @@ -3184,6 +3245,11 @@ \bibitem{autoconf} Autoconf -- GNU Project -- Free Software Foundation (FSF). \url{http://www.gnu.org/software/autoconf}, 2010. +\bibitem{soespec} IEC 61800-7-304: Adjustable speed electrical power drive +systems - Part 7-300: Generic interface and use of profiles for power drive +systems - Mapping of profiles to network technologies. International +Electrotechnical Commission (IEC), 2007. + \end{thebibliography} \printnomenclature diff -r ea38efeeb7b3 -r 6aa393418fb3 documentation/graphs/fsm_master.dot --- a/documentation/graphs/fsm_master.dot Fri May 07 15:26:26 2010 +0200 +++ b/documentation/graphs/fsm_master.dot Tue May 11 13:57:58 2010 +0200 @@ -5,14 +5,14 @@ size="7,9" center=1 ratio=fill + node [fontname="Helvetica"] - start [fontname="Helvetica"] start -> broadcast [weight=10] - broadcast [fontname="Helvetica"] broadcast -> start broadcast -> clear_addresses - broadcast -> read_state [weight=10] + broadcast -> read_state + broadcast -> write_system_times action_process_sii [shape=point,label=""] action_process_sii -> write_sii @@ -20,10 +20,14 @@ action_process_sdo [shape=point,label=""] action_process_sdo -> sdo_request + action_process_register [shape=point,label=""] + action_process_register -> reg_request + action_idle [shape=point,label=""] action_idle -> action_process_sdo action_idle -> sdo_dictionary action_idle -> action_process_sii + action_idle -> action_process_register action_idle -> start action_next_slave_state [shape=point,label=""] @@ -34,34 +38,30 @@ action_configure -> configure_slave [weight=10] action_configure -> action_next_slave_state - read_state [fontname="Helvetica"] read_state -> acknowledge [weight=10] read_state -> action_configure read_state -> action_next_slave_state - acknowledge [fontname="Helvetica"] acknowledge -> action_configure [weight=10] - clear_addresses [fontname="Helvetica"] clear_addresses -> dc_measure_delays [weight=10] - dc_measure_delays [fontname="Helvetica"] dc_measure_delays -> scan_slave [weight=10] - scan_slave [fontname="Helvetica"] - scan_slave -> start + scan_slave -> write_system_times - configure_slave [fontname="Helvetica"] + write_system_times -> start + configure_slave -> action_next_slave_state - write_sii [fontname="Helvetica"] write_sii -> action_process_sii write_sii -> start - sdo_dictionary [fontname="Helvetica"] sdo_dictionary -> start - sdo_request [fontname="Helvetica"] sdo_request -> action_process_sdo sdo_request -> start + + reg_request -> action_process_register + reg_request -> start } diff -r ea38efeeb7b3 -r 6aa393418fb3 master/cdev.c --- a/master/cdev.c Fri May 07 15:26:26 2010 +0200 +++ b/master/cdev.c Tue May 11 13:57:58 2010 +0200 @@ -123,7 +123,7 @@ ret = cdev_add(&cdev->cdev, MKDEV(MAJOR(dev_num), master->index), 1); if (ret) { - EC_ERR("Failed to add character device!\n"); + EC_MASTER_ERR(master, "Failed to add character device!\n"); } return ret; @@ -277,7 +277,7 @@ if (!(slave = ec_master_find_slave_const( master, 0, data.position))) { up(&master->master_sem); - EC_ERR("Slave %u does not exist!\n", data.position); + EC_MASTER_ERR(master, "Slave %u does not exist!\n", data.position); return -EINVAL; } @@ -361,14 +361,15 @@ if (!(slave = ec_master_find_slave_const( master, 0, data.slave_position))) { up(&master->master_sem); - EC_ERR("Slave %u does not exist!\n", data.slave_position); + EC_MASTER_ERR(master, "Slave %u does not exist!\n", + data.slave_position); return -EINVAL; } if (data.sync_index >= slave->sii.sync_count) { up(&master->master_sem); - EC_ERR("Sync manager %u does not exist in slave %u!\n", - data.sync_index, data.slave_position); + EC_SLAVE_ERR(slave, "Sync manager %u does not exist!\n", + data.sync_index); return -EINVAL; } @@ -412,14 +413,15 @@ if (!(slave = ec_master_find_slave_const( master, 0, data.slave_position))) { up(&master->master_sem); - EC_ERR("Slave %u does not exist!\n", data.slave_position); + EC_MASTER_ERR(master, "Slave %u does not exist!\n", + data.slave_position); return -EINVAL; } if (data.sync_index >= slave->sii.sync_count) { up(&master->master_sem); - EC_ERR("Sync manager %u does not exist in slave %u!\n", - data.sync_index, data.slave_position); + EC_SLAVE_ERR(slave, "Sync manager %u does not exist!\n", + data.sync_index); return -EINVAL; } @@ -427,9 +429,8 @@ if (!(pdo = ec_pdo_list_find_pdo_by_pos_const( &sync->pdos, data.pdo_pos))) { up(&master->master_sem); - EC_ERR("Sync manager %u does not contain a PDO with " - "position %u in slave %u!\n", data.sync_index, - data.pdo_pos, data.slave_position); + EC_SLAVE_ERR(slave, "Sync manager %u does not contain a PDO with " + "position %u!\n", data.sync_index, data.pdo_pos); return -EINVAL; } @@ -470,14 +471,15 @@ if (!(slave = ec_master_find_slave_const( master, 0, data.slave_position))) { up(&master->master_sem); - EC_ERR("Slave %u does not exist!\n", data.slave_position); + EC_MASTER_ERR(master, "Slave %u does not exist!\n", + data.slave_position); return -EINVAL; } if (data.sync_index >= slave->sii.sync_count) { up(&master->master_sem); - EC_ERR("Sync manager %u does not exist in slave %u!\n", - data.sync_index, data.slave_position); + EC_SLAVE_ERR(slave, "Sync manager %u does not exist!\n", + data.sync_index); return -EINVAL; } @@ -485,18 +487,16 @@ if (!(pdo = ec_pdo_list_find_pdo_by_pos_const( &sync->pdos, data.pdo_pos))) { up(&master->master_sem); - EC_ERR("Sync manager %u does not contain a PDO with " - "position %u in slave %u!\n", data.sync_index, - data.pdo_pos, data.slave_position); + EC_SLAVE_ERR(slave, "Sync manager %u does not contain a PDO with " + "position %u!\n", data.sync_index, data.pdo_pos); return -EINVAL; } if (!(entry = ec_pdo_find_entry_by_pos_const( pdo, data.entry_pos))) { up(&master->master_sem); - EC_ERR("PDO 0x%04X does not contain an entry with " - "position %u in slave %u!\n", data.pdo_pos, - data.entry_pos, data.slave_position); + EC_SLAVE_ERR(slave, "PDO 0x%04X does not contain an entry with " + "position %u!\n", data.pdo_pos, data.entry_pos); return -EINVAL; } @@ -534,7 +534,7 @@ if (!(domain = ec_master_find_domain_const(master, data.index))) { up(&master->master_sem); - EC_ERR("Domain %u does not exist!\n", data.index); + EC_MASTER_ERR(master, "Domain %u does not exist!\n", data.index); return -EINVAL; } @@ -575,13 +575,15 @@ if (!(domain = ec_master_find_domain_const(master, data.domain_index))) { up(&master->master_sem); - EC_ERR("Domain %u does not exist!\n", data.domain_index); + EC_MASTER_ERR(master, "Domain %u does not exist!\n", + data.domain_index); return -EINVAL; } if (!(fmmu = ec_domain_find_fmmu(domain, data.fmmu_index))) { up(&master->master_sem); - EC_ERR("Domain %u has less than %u fmmu configurations.\n", + EC_MASTER_ERR(master, "Domain %u has less than %u" + " fmmu configurations.\n", data.domain_index, data.fmmu_index + 1); return -EINVAL; } @@ -623,13 +625,14 @@ if (!(domain = ec_master_find_domain_const(master, data.domain_index))) { up(&master->master_sem); - EC_ERR("Domain %u does not exist!\n", data.domain_index); + EC_MASTER_ERR(master, "Domain %u does not exist!\n", + data.domain_index); return -EINVAL; } if (domain->data_size != data.data_size) { up(&master->master_sem); - EC_ERR("Data size mismatch %u/%zu!\n", + EC_MASTER_ERR(master, "Data size mismatch %u/%zu!\n", data.data_size, domain->data_size); return -EFAULT; } @@ -678,7 +681,8 @@ if (!(slave = ec_master_find_slave( master, 0, data.slave_position))) { up(&master->master_sem); - EC_ERR("Slave %u does not exist!\n", data.slave_position); + EC_MASTER_ERR(master, "Slave %u does not exist!\n", + data.slave_position); return -EINVAL; } @@ -711,15 +715,15 @@ if (!(slave = ec_master_find_slave_const( master, 0, data.slave_position))) { up(&master->master_sem); - EC_ERR("Slave %u does not exist!\n", data.slave_position); + EC_MASTER_ERR(master, "Slave %u does not exist!\n", + data.slave_position); return -EINVAL; } if (!(sdo = ec_slave_get_sdo_by_pos_const( slave, data.sdo_position))) { up(&master->master_sem); - EC_ERR("SDO %u does not exist in slave %u!\n", - data.sdo_position, data.slave_position); + EC_SLAVE_ERR(slave, "SDO %u does not exist!\n", data.sdo_position); return -EINVAL; } @@ -759,7 +763,8 @@ if (!(slave = ec_master_find_slave_const( master, 0, data.slave_position))) { up(&master->master_sem); - EC_ERR("Slave %u does not exist!\n", data.slave_position); + EC_MASTER_ERR(master, "Slave %u does not exist!\n", + data.slave_position); return -EINVAL; } @@ -767,16 +772,15 @@ if (!(sdo = ec_slave_get_sdo_by_pos_const( slave, -data.sdo_spec))) { up(&master->master_sem); - EC_ERR("SDO %u does not exist in slave %u!\n", - -data.sdo_spec, data.slave_position); + EC_SLAVE_ERR(slave, "SDO %u does not exist!\n", -data.sdo_spec); return -EINVAL; } } else { if (!(sdo = ec_slave_get_sdo_const( slave, data.sdo_spec))) { up(&master->master_sem); - EC_ERR("SDO 0x%04X does not exist in slave %u!\n", - data.sdo_spec, data.slave_position); + EC_SLAVE_ERR(slave, "SDO 0x%04X does not exist!\n", + data.sdo_spec); return -EINVAL; } } @@ -784,9 +788,8 @@ if (!(entry = ec_sdo_get_entry_const( sdo, data.sdo_entry_subindex))) { up(&master->master_sem); - EC_ERR("SDO entry 0x%04X:%02X does not exist " - "in slave %u!\n", sdo->index, - data.sdo_entry_subindex, data.slave_position); + EC_SLAVE_ERR(slave, "SDO entry 0x%04X:%02X does not exist!\n", + sdo->index, data.sdo_entry_subindex); return -EINVAL; } @@ -843,13 +846,13 @@ master, 0, data.slave_position))) { up(&master->master_sem); ec_sdo_request_clear(&request.req); - EC_ERR("Slave %u does not exist!\n", data.slave_position); - return -EINVAL; - } - - if (master->debug_level) - EC_DBG("Schedule SDO upload request for slave %u\n", - request.slave->ring_position); + EC_MASTER_ERR(master, "Slave %u does not exist!\n", + data.slave_position); + return -EINVAL; + } + + EC_SLAVE_DBG(request.slave, 1, "Schedule SDO upload request.\n"); + // schedule request. list_add_tail(&request.list, &request.slave->slave_sdo_requests); @@ -874,9 +877,7 @@ wait_event(request.slave->sdo_queue, request.req.state != EC_INT_REQUEST_BUSY); - if (master->debug_level) - EC_DBG("Scheduled SDO upload request for slave %u done\n", - request.slave->ring_position); + EC_SLAVE_DBG(request.slave, 1, "Finished SDO upload request.\n"); data.abort_code = request.req.abort_code; @@ -885,7 +886,7 @@ retval = -EIO; } else { if (request.req.data_size > data.target_size) { - EC_ERR("Buffer too small.\n"); + EC_MASTER_ERR(master, "Buffer too small.\n"); ec_sdo_request_clear(&request.req); return -EOVERFLOW; } @@ -926,7 +927,7 @@ // copy data to download if (!data.data_size) { - EC_ERR("Zero data size!\n"); + EC_MASTER_ERR(master, "Zero data size!\n"); return -EINVAL; } @@ -951,14 +952,14 @@ if (!(request.slave = ec_master_find_slave( master, 0, data.slave_position))) { up(&master->master_sem); - EC_ERR("Slave %u does not exist!\n", data.slave_position); + EC_MASTER_ERR(master, "Slave %u does not exist!\n", + data.slave_position); ec_sdo_request_clear(&request.req); return -EINVAL; } - if (master->debug_level) - EC_DBG("Schedule SDO download request for slave %u\n", - request.slave->ring_position); + EC_SLAVE_DBG(request.slave, 1, "Schedule SDO download request.\n"); + // schedule request. list_add_tail(&request.list, &request.slave->slave_sdo_requests); @@ -983,9 +984,7 @@ wait_event(request.slave->sdo_queue, request.req.state != EC_INT_REQUEST_BUSY); - if (master->debug_level) - EC_DBG("Scheduled SDO download request for slave %u done\n", - request.slave->ring_position); + EC_SLAVE_DBG(request.slave, 1, "Finished SDO download request.\n"); data.abort_code = request.req.abort_code; @@ -1022,16 +1021,16 @@ if (!(slave = ec_master_find_slave_const( master, 0, data.slave_position))) { up(&master->master_sem); - EC_ERR("Slave %u does not exist!\n", data.slave_position); + EC_MASTER_ERR(master, "Slave %u does not exist!\n", + data.slave_position); return -EINVAL; } if (!data.nwords || data.offset + data.nwords > slave->sii_nwords) { up(&master->master_sem); - EC_ERR("Invalid SII read offset/size %u/%u for slave " - "SII size %zu!\n", data.offset, - data.nwords, slave->sii_nwords); + EC_SLAVE_ERR(slave, "Invalid SII read offset/size %u/%u for slave SII" + " size %zu!\n", data.offset, data.nwords, slave->sii_nwords); return -EINVAL; } @@ -1069,8 +1068,8 @@ byte_size = sizeof(uint16_t) * data.nwords; if (!(words = kmalloc(byte_size, GFP_KERNEL))) { - EC_ERR("Failed to allocate %u bytes for SII contents.\n", - byte_size); + EC_MASTER_ERR(master, "Failed to allocate %u bytes" + " for SII contents.\n", byte_size); return -ENOMEM; } @@ -1086,7 +1085,8 @@ if (!(slave = ec_master_find_slave( master, 0, data.slave_position))) { up(&master->master_sem); - EC_ERR("Slave %u does not exist!\n", data.slave_position); + EC_MASTER_ERR(master, "Slave %u does not exist!\n", + data.slave_position); kfree(words); return -EINVAL; } @@ -1149,8 +1149,8 @@ return 0; if (!(contents = kmalloc(data.length, GFP_KERNEL))) { - EC_ERR("Failed to allocate %u bytes for register data.\n", - data.length); + EC_MASTER_ERR(master, "Failed to allocate %u bytes" + " for register data.\n", data.length); return -ENOMEM; } @@ -1160,7 +1160,8 @@ if (!(slave = ec_master_find_slave( master, 0, data.slave_position))) { up(&master->master_sem); - EC_ERR("Slave %u does not exist!\n", data.slave_position); + EC_MASTER_ERR(master, "Slave %u does not exist!\n", + data.slave_position); return -EINVAL; } @@ -1227,8 +1228,8 @@ return 0; if (!(contents = kmalloc(data.length, GFP_KERNEL))) { - EC_ERR("Failed to allocate %u bytes for register data.\n", - data.length); + EC_MASTER_ERR(master, "Failed to allocate %u bytes" + " for register data.\n", data.length); return -ENOMEM; } @@ -1243,7 +1244,8 @@ if (!(slave = ec_master_find_slave( master, 0, data.slave_position))) { up(&master->master_sem); - EC_ERR("Slave %u does not exist!\n", data.slave_position); + EC_MASTER_ERR(master, "Slave %u does not exist!\n", + data.slave_position); kfree(contents); return -EINVAL; } @@ -1308,7 +1310,7 @@ if (!(sc = ec_master_get_config_const( master, data.config_index))) { up(&master->master_sem); - EC_ERR("Slave config %u does not exist!\n", + EC_MASTER_ERR(master, "Slave config %u does not exist!\n", data.config_index); return -EINVAL; } @@ -1358,7 +1360,7 @@ } if (data.sync_index >= EC_MAX_SYNC_MANAGERS) { - EC_ERR("Invalid sync manager index %u!\n", + EC_MASTER_ERR(master, "Invalid sync manager index %u!\n", data.sync_index); return -EINVAL; } @@ -1369,7 +1371,7 @@ if (!(sc = ec_master_get_config_const( master, data.config_index))) { up(&master->master_sem); - EC_ERR("Slave config %u does not exist!\n", + EC_MASTER_ERR(master, "Slave config %u does not exist!\n", data.config_index); return -EINVAL; } @@ -1378,7 +1380,7 @@ &sc->sync_configs[data.sync_index].pdos, data.pdo_pos))) { up(&master->master_sem); - EC_ERR("Invalid PDO position!\n"); + EC_MASTER_ERR(master, "Invalid PDO position!\n"); return -EINVAL; } @@ -1413,7 +1415,7 @@ } if (data.sync_index >= EC_MAX_SYNC_MANAGERS) { - EC_ERR("Invalid sync manager index %u!\n", + EC_MASTER_ERR(master, "Invalid sync manager index %u!\n", data.sync_index); return -EINVAL; } @@ -1424,7 +1426,7 @@ if (!(sc = ec_master_get_config_const( master, data.config_index))) { up(&master->master_sem); - EC_ERR("Slave config %u does not exist!\n", + EC_MASTER_ERR(master, "Slave config %u does not exist!\n", data.config_index); return -EINVAL; } @@ -1433,14 +1435,14 @@ &sc->sync_configs[data.sync_index].pdos, data.pdo_pos))) { up(&master->master_sem); - EC_ERR("Invalid PDO position!\n"); + EC_MASTER_ERR(master, "Invalid PDO position!\n"); return -EINVAL; } if (!(entry = ec_pdo_find_entry_by_pos_const( pdo, data.entry_pos))) { up(&master->master_sem); - EC_ERR("Entry not found!\n"); + EC_MASTER_ERR(master, "Entry not found!\n"); return -EINVAL; } @@ -1480,7 +1482,7 @@ if (!(sc = ec_master_get_config_const( master, data.config_index))) { up(&master->master_sem); - EC_ERR("Slave config %u does not exist!\n", + EC_MASTER_ERR(master, "Slave config %u does not exist!\n", data.config_index); return -EINVAL; } @@ -1488,7 +1490,7 @@ if (!(req = ec_slave_config_get_sdo_by_pos_const( sc, data.sdo_pos))) { up(&master->master_sem); - EC_ERR("Invalid SDO position!\n"); + EC_MASTER_ERR(master, "Invalid SDO position!\n"); return -EINVAL; } @@ -1529,7 +1531,8 @@ if (!(eoe = ec_master_get_eoe_handler_const(master, data.eoe_index))) { up(&master->master_sem); - EC_ERR("EoE handler %u does not exist!\n", data.eoe_index); + EC_MASTER_ERR(master, "EoE handler %u does not exist!\n", + data.eoe_index); return -EINVAL; } @@ -2809,7 +2812,7 @@ return -EFAULT; if (!data.size) { - EC_ERR("Sdo download: Data size may not be zero!\n"); + EC_MASTER_ERR(master, "SDO download: Data size may not be zero!\n"); return -EINVAL; } @@ -3222,7 +3225,8 @@ master, 0, data.slave_position))) { up(&master->master_sem); ec_foe_request_clear(&request.req); - EC_ERR("Slave %u does not exist!\n", data.slave_position); + EC_MASTER_ERR(master, "Slave %u does not exist!\n", + data.slave_position); return -EINVAL; } @@ -3231,10 +3235,7 @@ up(&master->master_sem); - if (master->debug_level) { - EC_DBG("Scheduled FoE read request on slave %u.\n", - request.slave->ring_position); - } + EC_SLAVE_DBG(request.slave, 1, "Scheduled FoE read request.\n"); // wait for processing through FSM if (wait_event_interruptible(request.slave->foe_queue, @@ -3258,17 +3259,15 @@ data.result = request.req.result; data.error_code = request.req.error_code; - if (master->debug_level) { - EC_DBG("Read %zd bytes via FoE (result = 0x%x).\n", - request.req.data_size, request.req.result); - } + EC_SLAVE_DBG(request.slave, 1, "Read %zd bytes via FoE" + " (result = 0x%x).\n", request.req.data_size, request.req.result); if (request.req.state != EC_INT_REQUEST_SUCCESS) { data.data_size = 0; retval = -EIO; } else { if (request.req.data_size > data.buffer_size) { - EC_ERR("Buffer too small.\n"); + EC_MASTER_ERR(master, "Buffer too small.\n"); ec_foe_request_clear(&request.req); return -EOVERFLOW; } @@ -3285,9 +3284,7 @@ retval = -EFAULT; } - if (master->debug_level) - EC_DBG("FoE read request finished on slave %u.\n", - request.slave->ring_position); + EC_SLAVE_DBG(request.slave, 1, "Finished FoE read request.\n"); ec_foe_request_clear(&request.req); @@ -3333,14 +3330,13 @@ if (!(request.slave = ec_master_find_slave( master, 0, data.slave_position))) { up(&master->master_sem); - EC_ERR("Slave %u does not exist!\n", data.slave_position); + EC_MASTER_ERR(master, "Slave %u does not exist!\n", + data.slave_position); ec_foe_request_clear(&request.req); return -EINVAL; } - if (master->debug_level) { - EC_DBG("Scheduling FoE write request.\n"); - } + EC_SLAVE_DBG(request.slave, 1, "Scheduling FoE write request.\n"); // schedule FoE write request. list_add_tail(&request.list, &request.slave->foe_requests); @@ -3377,9 +3373,7 @@ ec_foe_request_clear(&request.req); - if (master->debug_level) { - EC_DBG("Finished FoE writing.\n"); - } + EC_SLAVE_DBG(request.slave, 1, "Finished FoE write request.\n"); return retval; } @@ -3412,7 +3406,8 @@ master, 0, ioctl.slave_position))) { up(&master->master_sem); ec_soe_request_clear(&request.req); - EC_ERR("Slave %u does not exist!\n", ioctl.slave_position); + EC_MASTER_ERR(master, "Slave %u does not exist!\n", + ioctl.slave_position); return -EINVAL; } @@ -3421,10 +3416,7 @@ up(&master->master_sem); - if (master->debug_level) { - EC_DBG("Scheduled SoE read request on slave %u.\n", - request.slave->ring_position); - } + EC_SLAVE_DBG(request.slave, 1, "Scheduled SoE read request.\n"); // wait for processing through FSM if (wait_event_interruptible(request.slave->soe_queue, @@ -3447,16 +3439,15 @@ ioctl.error_code = request.req.error_code; - if (master->debug_level) { - EC_DBG("Read %zd bytes via SoE.\n", request.req.data_size); - } + EC_SLAVE_DBG(request.slave, 1, "Read %zd bytes via SoE.\n", + request.req.data_size); if (request.req.state != EC_INT_REQUEST_SUCCESS) { ioctl.data_size = 0; retval = -EIO; } else { if (request.req.data_size > ioctl.mem_size) { - EC_ERR("Buffer too small.\n"); + EC_MASTER_ERR(master, "Buffer too small.\n"); ec_soe_request_clear(&request.req); return -EOVERFLOW; } @@ -3473,9 +3464,7 @@ retval = -EFAULT; } - if (master->debug_level) - EC_DBG("SoE read request finished on slave %u.\n", - request.slave->ring_position); + EC_SLAVE_DBG(request.slave, 1, "Finished SoE read request.\n"); ec_soe_request_clear(&request.req); @@ -3522,14 +3511,13 @@ if (!(request.slave = ec_master_find_slave( master, 0, ioctl.slave_position))) { up(&master->master_sem); - EC_ERR("Slave %u does not exist!\n", ioctl.slave_position); + EC_MASTER_ERR(master, "Slave %u does not exist!\n", + ioctl.slave_position); ec_soe_request_clear(&request.req); return -EINVAL; } - if (master->debug_level) { - EC_DBG("Scheduling SoE write request.\n"); - } + EC_SLAVE_DBG(request.slave, 1, "Scheduling SoE write request.\n"); // schedule SoE write request. list_add_tail(&request.list, &request.slave->soe_requests); @@ -3564,9 +3552,7 @@ ec_soe_request_clear(&request.req); - if (master->debug_level) { - EC_DBG("Finished SoE writing.\n"); - } + EC_SLAVE_DBG(request.slave, 1, "Finished SoE write request.\n"); return retval; } @@ -3584,7 +3570,8 @@ priv = kmalloc(sizeof(ec_cdev_priv_t), GFP_KERNEL); if (!priv) { - EC_ERR("Failed to allocate memory for private data structure.\n"); + EC_MASTER_ERR(cdev->master, + "Failed to allocate memory for private data structure.\n"); return -ENOMEM; } @@ -3596,7 +3583,7 @@ filp->private_data = priv; #if DEBUG_IOCTL - EC_DBG("File opened.\n"); + EC_MASTER_DBG(cdev->master, "File opened.\n"); #endif return 0; } @@ -3617,7 +3604,7 @@ vfree(priv->process_data); #if DEBUG_IOCTL - EC_DBG("File closed.\n"); + EC_MASTER_DBG(master, "File closed.\n"); #endif kfree(priv); @@ -3634,8 +3621,9 @@ ec_master_t *master = priv->cdev->master; #if DEBUG_IOCTL - EC_DBG("ioctl(filp = 0x%x, cmd = 0x%08x (0x%02x), arg = 0x%x)\n", - (u32) filp, (u32) cmd, (u32) _IOC_NR(cmd), (u32) arg); + EC_MASTER_DBG(master, "ioctl(filp = 0x%x, cmd = 0x%08x (0x%02x)," + " arg = 0x%x)\n", (u32) filp, (u32) cmd, (u32) _IOC_NR(cmd), + (u32) arg); #endif switch (cmd) { @@ -3872,7 +3860,7 @@ case EC_IOCTL_SET_SEND_INTERVAL: if (!(filp->f_mode & FMODE_WRITE)) return -EPERM; - return ec_cdev_ioctl_set_send_interval(master,arg,priv); + return ec_cdev_ioctl_set_send_interval(master, arg, priv); default: return -ENOTTY; } @@ -3892,8 +3880,7 @@ { ec_cdev_priv_t *priv = (ec_cdev_priv_t *) filp->private_data; - if (priv->cdev->master->debug_level) - EC_DBG("mmap()\n"); + EC_MASTER_DBG(priv->cdev->master, 1, "mmap()\n"); vma->vm_ops = &eccdev_vm_ops; vma->vm_flags |= VM_RESERVED; /* Pages will not be swapped out */ @@ -3930,9 +3917,8 @@ get_page(page); vmf->page = page; - if (priv->cdev->master->debug_level) - EC_DBG("Vma fault, virtual_address = %p, offset = %lu, page = %p\n", - vmf->virtual_address, offset, page); + EC_MASTER_DBG(priv->cdev->master, 1, "Vma fault, virtual_address = %p," + " offset = %lu, page = %p\n", vmf->virtual_address, offset, page); return 0; } @@ -3962,9 +3948,8 @@ page = vmalloc_to_page(priv->process_data + offset); - if (priv->cdev->master->debug_level) - EC_DBG("Nopage fault vma, address = %#lx, offset = %#lx, page = %p\n", - address, offset, page); + EC_MASTER_DBG(master, "Nopage fault vma, address = %#lx," + " offset = %#lx, page = %p\n", address, offset, page); get_page(page); if (type) diff -r ea38efeeb7b3 -r 6aa393418fb3 master/debug.c --- a/master/debug.c Fri May 07 15:26:26 2010 +0200 +++ b/master/debug.c Tue May 11 13:57:58 2010 +0200 @@ -39,6 +39,7 @@ #include #include "globals.h" +#include "master.h" #include "debug.h" /*****************************************************************************/ @@ -71,10 +72,12 @@ * \retval <0 Error code. */ int ec_debug_init( - ec_debug_t *dbg, /**< debug object */ - const char *name /**< interface name */ - ) -{ + ec_debug_t *dbg, /**< Debug object. */ + ec_device_t *device, /**< EtherCAT device. */ + const char *name /**< Interface name. */ + ) +{ + dbg->device = device; dbg->registered = 0; dbg->opened = 0; @@ -82,7 +85,8 @@ if (!(dbg->dev = alloc_netdev(sizeof(ec_debug_t *), name, ether_setup))) { - EC_ERR("Unable to allocate net_device for debug object!\n"); + EC_MASTER_ERR(device->master, "Unable to allocate net_device" + " for debug object!\n"); return -ENODEV; } @@ -134,7 +138,8 @@ // connect the net_device to the kernel if ((result = register_netdev(dbg->dev))) { - EC_WARN("Unable to register net_device: error %i\n", result); + EC_MASTER_WARN(dbg->device->master, "Unable to register net_device:" + " error %i\n", result); } else { dbg->registered = 1; } @@ -202,7 +207,8 @@ { ec_debug_t *dbg = *((ec_debug_t **) netdev_priv(dev)); dbg->opened = 1; - EC_INFO("Debug interface %s opened.\n", dev->name); + EC_MASTER_INFO(dbg->device->master, "Debug interface %s opened.\n", + dev->name); return 0; } @@ -216,7 +222,8 @@ { ec_debug_t *dbg = *((ec_debug_t **) netdev_priv(dev)); dbg->opened = 0; - EC_INFO("Debug interface %s stopped.\n", dev->name); + EC_MASTER_INFO(dbg->device->master, "Debug interface %s stopped.\n", + dev->name); return 0; } diff -r ea38efeeb7b3 -r 6aa393418fb3 master/debug.h --- a/master/debug.h Fri May 07 15:26:26 2010 +0200 +++ b/master/debug.h Tue May 11 13:57:58 2010 +0200 @@ -34,16 +34,18 @@ /*****************************************************************************/ -#include +#ifndef __EC_DEBUG_H__ +#define __EC_DEBUG_H__ + +#include "../devices/ecdev.h" /*****************************************************************************/ -/** - Debugging network interface. -*/ - +/** Debugging network interface. + */ typedef struct { + ec_device_t *device; /**< Parent device. */ struct net_device *dev; /**< net_device for virtual ethernet device */ struct net_device_stats stats; /**< device statistics */ uint8_t registered; /**< net_device is opened */ @@ -53,10 +55,12 @@ /*****************************************************************************/ -int ec_debug_init(ec_debug_t *, const char *); +int ec_debug_init(ec_debug_t *, ec_device_t *, const char *); void ec_debug_clear(ec_debug_t *); void ec_debug_register(ec_debug_t *, const struct net_device *); void ec_debug_unregister(ec_debug_t *); void ec_debug_send(ec_debug_t *, const uint8_t *, size_t); +#endif + /*****************************************************************************/ diff -r ea38efeeb7b3 -r 6aa393418fb3 master/device.c --- a/master/device.c Fri May 07 15:26:26 2010 +0200 +++ b/master/device.c Tue May 11 13:57:58 2010 +0200 @@ -94,9 +94,9 @@ sprintf(ifname, "ecdbg%c%u", mb, master->index); - ret = ec_debug_init(&device->dbg, ifname); + ret = ec_debug_init(&device->dbg, device, ifname); if (ret < 0) { - EC_ERR("Failed to init debug device!\n"); + EC_MASTER_ERR(master, "Failed to init debug device!\n"); goto out_return; } #endif @@ -106,7 +106,7 @@ for (i = 0; i < EC_TX_RING_SIZE; i++) { if (!(device->tx_skb[i] = dev_alloc_skb(ETH_FRAME_LEN))) { - EC_ERR("Error allocating device socket buffer!\n"); + EC_MASTER_ERR(master, "Error allocating device socket buffer!\n"); ret = -ENOMEM; goto out_tx_ring; } @@ -220,12 +220,12 @@ int ret; if (!device->dev) { - EC_ERR("No net_device to open!\n"); + EC_MASTER_ERR(device->master, "No net_device to open!\n"); return -ENODEV; } if (device->open) { - EC_WARN("Device already opened!\n"); + EC_MASTER_WARN(device->master, "Device already opened!\n"); return 0; } @@ -257,12 +257,12 @@ int ret; if (!device->dev) { - EC_ERR("No device to close!\n"); + EC_MASTER_ERR(device->master, "No device to close!\n"); return -ENODEV; } if (!device->open) { - EC_WARN("Device already closed!\n"); + EC_MASTER_WARN(device->master, "Device already closed!\n"); return 0; } @@ -337,7 +337,7 @@ skb->len = ETH_HLEN + size; if (unlikely(device->master->debug_level > 1)) { - EC_DBG("Sending frame:\n"); + EC_MASTER_DBG(device->master, 2, "Sending frame:\n"); ec_print_data(skb->data, ETH_HLEN + size); } @@ -434,7 +434,7 @@ % EC_DEBUG_RING_SIZE; t0 = device->debug_frames[ring_index].t; - EC_DBG("Debug ring %u:\n", ring_index); + EC_MASTER_DBG(device->master, 1, "Debug ring %u:\n", ring_index); // calculate index of the oldest frame in the ring ring_index = (device->debug_frame_index + EC_DEBUG_RING_SIZE @@ -444,7 +444,7 @@ df = &device->debug_frames[ring_index]; timersub(&t0, &df->t, &diff); - EC_DBG("Frame %u, dt=%u.%06u s, %s:\n", + EC_MASTER_DBG(device->master, 1, "Frame %u, dt=%u.%06u s, %s:\n", i + 1 - device->debug_frame_count, (unsigned int) diff.tv_sec, (unsigned int) diff.tv_usec, @@ -499,7 +499,7 @@ char str[20]; ec_mac_print(device->dev->dev_addr, str); - EC_INFO("Master %u releasing main device %s.\n", master->index, str); + EC_MASTER_INFO(master, "Releasing main device %s.\n", str); down(&master->device_sem); ec_device_detach(device); @@ -519,13 +519,13 @@ ret = ec_device_open(device); if (ret) { - EC_ERR("Failed to open device!\n"); + EC_MASTER_ERR(device->master, "Failed to open device!\n"); return ret; } ret = ec_master_enter_idle_phase(device->master); if (ret) { - EC_ERR("Failed to enter IDLE phase!\n"); + EC_MASTER_ERR(device->master, "Failed to enter IDLE phase!\n"); return ret; } @@ -544,7 +544,7 @@ ec_master_leave_idle_phase(device->master); if (ec_device_close(device)) - EC_WARN("Failed to close device!\n"); + EC_MASTER_WARN(device->master, "Failed to close device!\n"); } /*****************************************************************************/ @@ -566,14 +566,15 @@ size_t ec_size = size - ETH_HLEN; if (unlikely(!data)) { - EC_WARN("%s() called with NULL data.\n", __func__); + EC_MASTER_WARN(device->master, "%s() called with NULL data.\n", + __func__); return; } device->rx_count++; if (unlikely(device->master->debug_level > 1)) { - EC_DBG("Received frame:\n"); + EC_MASTER_DBG(device->master, 2, "Received frame:\n"); ec_print_data(data, size); } @@ -602,13 +603,14 @@ ) { if (unlikely(!device)) { - EC_WARN("ecdev_set_link(): No device!\n"); + EC_MASTER_WARN(device->master, "ecdev_set_link(): No device!\n"); return; } if (likely(state != device->link_state)) { device->link_state = state; - EC_INFO("Link state changed to %s.\n", (state ? "UP" : "DOWN")); + EC_MASTER_INFO(device->master, + "Link state changed to %s.\n", (state ? "UP" : "DOWN")); } } @@ -623,7 +625,7 @@ ) { if (unlikely(!device)) { - EC_WARN("ecdev_get_link(): No device!\n"); + EC_MASTER_WARN(device->master, "ecdev_get_link(): No device!\n"); return 0; } diff -r ea38efeeb7b3 -r 6aa393418fb3 master/domain.c --- a/master/domain.c Fri May 07 15:26:26 2010 +0200 +++ b/master/domain.c Tue May 11 13:57:58 2010 +0200 @@ -117,9 +117,9 @@ domain->tx_size += fmmu->tx_size; list_add_tail(&fmmu->list, &domain->fmmu_configs); - if (domain->master->debug_level) - EC_DBG("Domain %u: Added %u bytes, total %zu.\n", domain->index, - fmmu->data_size, domain->data_size); + EC_MASTER_DBG(domain->master, 1, "Domain %u:" + " Added %u bytes, total %zu.\n", + domain->index, fmmu->data_size, domain->data_size); } /*****************************************************************************/ @@ -144,7 +144,8 @@ int ret; if (!(datagram = kmalloc(sizeof(ec_datagram_t), GFP_KERNEL))) { - EC_ERR("Failed to allocate domain datagram!\n"); + EC_MASTER_ERR(domain->master, + "Failed to allocate domain datagram!\n"); return -ENOMEM; } @@ -215,8 +216,9 @@ if (domain->data_size && domain->data_origin == EC_ORIG_INTERNAL) { if (!(domain->data = (uint8_t *) kmalloc(domain->data_size, GFP_KERNEL))) { - EC_ERR("Failed to allocate %zu bytes internal memory for" - " domain %u!\n", domain->data_size, domain->index); + EC_MASTER_ERR(domain->master, "Failed to allocate %zu bytes" + " internal memory for domain %u!\n", + domain->data_size, domain->index); return -ENOMEM; } } @@ -284,14 +286,15 @@ datagram_count++; } - EC_INFO("Domain%u: Logical address 0x%08x, %zu byte, " - "expected working counter %u.\n", domain->index, + EC_MASTER_INFO(domain->master, "Domain%u: Logical address 0x%08x," + " %zu byte, expected working counter %u.\n", domain->index, domain->logical_base_address, domain->data_size, domain->expected_working_counter); list_for_each_entry(datagram, &domain->datagrams, list) { - EC_INFO(" Datagram %s: Logical offset 0x%08x, %zu byte, type %s.\n", - datagram->name, EC_READ_U32(datagram->address), - datagram->data_size, ec_datagram_type_string(datagram)); + EC_MASTER_INFO(domain->master, " Datagram %s: Logical offset 0x%08x," + " %zu byte, type %s.\n", datagram->name, + EC_READ_U32(datagram->address), datagram->data_size, + ec_datagram_type_string(datagram)); } return 0; @@ -344,9 +347,8 @@ ec_slave_config_t *sc; int ret; - if (domain->master->debug_level) - EC_DBG("ecrt_domain_reg_pdo_entry_list(domain = 0x%p, regs = 0x%p)\n", - domain, regs); + EC_MASTER_DBG(domain->master, 1, "ecrt_domain_reg_pdo_entry_list(" + "domain = 0x%p, regs = 0x%p)\n", domain, regs); for (reg = regs; reg->index; reg++) { sc = ecrt_master_slave_config_err(domain->master, reg->alias, @@ -376,9 +378,8 @@ void ecrt_domain_external_memory(ec_domain_t *domain, uint8_t *mem) { - if (domain->master->debug_level) - EC_DBG("ecrt_domain_external_memory(domain = 0x%p, mem = 0x%p)\n", - domain, mem); + EC_MASTER_DBG(domain->master, 1, "ecrt_domain_external_memory(" + "domain = 0x%p, mem = 0x%p)\n", domain, mem); down(&domain->master->master_sem); @@ -421,13 +422,14 @@ jiffies - domain->notify_jiffies > HZ) { domain->notify_jiffies = jiffies; if (domain->working_counter_changes == 1) { - EC_INFO("Domain %u: Working counter changed to %u/%u.\n", - domain->index, domain->working_counter, + EC_MASTER_INFO(domain->master, "Domain %u: Working counter" + " changed to %u/%u.\n", domain->index, + domain->working_counter, domain->expected_working_counter); + } else { + EC_MASTER_INFO(domain->master, "Domain %u: %u working counter" + " changes - now %u/%u.\n", domain->index, + domain->working_counter_changes, domain->working_counter, domain->expected_working_counter); - } else { - EC_INFO("Domain %u: %u working counter changes - now %u/%u.\n", - domain->index, domain->working_counter_changes, - domain->working_counter, domain->expected_working_counter); } domain->working_counter_changes = 0; } diff -r ea38efeeb7b3 -r 6aa393418fb3 master/ethernet.c --- a/master/ethernet.c Fri May 07 15:26:26 2010 +0200 +++ b/master/ethernet.c Tue May 11 13:57:58 2010 +0200 @@ -147,7 +147,8 @@ snprintf(eoe->datagram.name, EC_DATAGRAM_NAME_SIZE, name); if (!(eoe->dev = alloc_netdev(sizeof(ec_eoe_t *), name, ether_setup))) { - EC_ERR("Unable to allocate net_device %s for EoE handler!\n", name); + EC_SLAVE_ERR(slave, "Unable to allocate net_device %s" + " for EoE handler!\n", name); ret = -ENODEV; goto out_return; } @@ -180,7 +181,8 @@ // connect the net_device to the kernel ret = register_netdev(eoe->dev); if (ret) { - EC_ERR("Unable to register net_device: error %i\n", ret); + EC_SLAVE_ERR(slave, "Unable to register net_device:" + " error %i\n", ret); goto out_free; } @@ -273,19 +275,20 @@ } #if EOE_DEBUG_LEVEL >= 2 - EC_DBG("EoE %s TX sending fragment %u%s with %u octets (%u)." - " %u frames queued.\n", eoe->dev->name, eoe->tx_fragment_number, - last_fragment ? "" : "+", current_size, complete_offset, - eoe->tx_queued_frames); + EC_SLAVE_DBG(slave, 0, "EoE %s TX sending fragment %u%s" + " with %u octets (%u). %u frames queued.\n", + eoe->dev->name, eoe->tx_fragment_number, + last_fragment ? "" : "+", current_size, complete_offset, + eoe->tx_queued_frames); #endif #if EOE_DEBUG_LEVEL >= 3 - EC_DBG(""); + EC_SLAVE_DBG(master, 0, ""); for (i = 0; i < current_size; i++) { printk("%02X ", eoe->tx_frame->skb->data[eoe->tx_offset + i]); if ((i + 1) % 16 == 0) { printk("\n"); - EC_DBG(""); + EC_SLAVE_DBG(master, 0, ""); } } printk("\n"); @@ -408,8 +411,8 @@ if (eoe->datagram.state != EC_DATAGRAM_RECEIVED) { eoe->stats.rx_errors++; #if EOE_DEBUG_LEVEL >= 1 - EC_WARN("Failed to receive mbox check datagram for %s.\n", - eoe->dev->name); + EC_SLAVE_WARN(eoe->slave, "Failed to receive mbox" + " check datagram for %s.\n", eoe->dev->name); #endif eoe->state = ec_eoe_state_tx_start; return; @@ -447,8 +450,8 @@ if (eoe->datagram.state != EC_DATAGRAM_RECEIVED) { eoe->stats.rx_errors++; #if EOE_DEBUG_LEVEL >= 1 - EC_WARN("Failed to receive mbox fetch datagram for %s.\n", - eoe->dev->name); + EC_SLAVE_WARN(eoe->slave, "Failed to receive mbox" + " fetch datagram for %s.\n", eoe->dev->name); #endif eoe->state = ec_eoe_state_tx_start; return; @@ -459,7 +462,7 @@ if (IS_ERR(data)) { eoe->stats.rx_errors++; #if EOE_DEBUG_LEVEL >= 1 - EC_WARN("Invalid mailbox response for %s.\n", + EC_SLAVE_WARN(eoe->slave, "Invalid mailbox response for %s.\n", eoe->dev->name); #endif eoe->state = ec_eoe_state_tx_start; @@ -469,7 +472,7 @@ if (mbox_prot != 0x02) { // EoE FIXME mailbox handler necessary eoe->stats.rx_errors++; #if EOE_DEBUG_LEVEL >= 1 - EC_WARN("Other mailbox protocol response for %s.\n", + EC_SLAVE_WARN(eoe->slave, "Other mailbox protocol response for %s.\n", eoe->dev->name); #endif eoe->state = ec_eoe_state_tx_start; @@ -480,7 +483,8 @@ if (frame_type != 0x00) { #if EOE_DEBUG_LEVEL >= 1 - EC_WARN("%s: Other frame received. Dropping.\n", eoe->dev->name); + EC_SLAVE_WARN(eoe->slave, "%s: Other frame received." + " Dropping.\n", eoe->dev->name); #endif eoe->stats.rx_dropped++; eoe->state = ec_eoe_state_tx_start; @@ -496,20 +500,20 @@ frame_number = (EC_READ_U16(data + 2) >> 12) & 0x000F; #if EOE_DEBUG_LEVEL >= 2 - EC_DBG("EoE %s RX fragment %u%s, offset %u, frame %u%s," - " %u octets\n", eoe->dev->name, fragment_number, + EC_SLAVE_DBG(eoe->slave, 0, "EoE %s RX fragment %u%s, offset %u," + " frame %u%s, %u octets\n", eoe->dev->name, fragment_number, last_fragment ? "" : "+", fragment_offset, frame_number, time_appended ? ", + timestamp" : "", time_appended ? rec_size - 8 : rec_size - 4); #endif #if EOE_DEBUG_LEVEL >= 3 - EC_DBG(""); + EC_SLAVE_DBG(eoe->slave, 0, ""); for (i = 0; i < rec_size - 4; i++) { printk("%02X ", data[i + 4]); if ((i + 1) % 16 == 0) { printk("\n"); - EC_DBG(""); + EC_SLAVE_DBG(eoe->slave, 0, ""); } } printk("\n"); @@ -519,14 +523,15 @@ if (!fragment_number) { if (eoe->rx_skb) { - EC_WARN("EoE RX freeing old socket buffer...\n"); + EC_SLAVE_WARN(eoe->slave, "EoE RX freeing old socket buffer.\n"); dev_kfree_skb(eoe->rx_skb); } // new socket buffer if (!(eoe->rx_skb = dev_alloc_skb(fragment_offset * 32))) { if (printk_ratelimit()) - EC_WARN("EoE RX low on mem. frame dropped.\n"); + EC_SLAVE_WARN(eoe->slave, "EoE RX low on mem," + " frame dropped.\n"); eoe->stats.rx_dropped++; eoe->state = ec_eoe_state_tx_start; return; @@ -551,7 +556,8 @@ eoe->rx_skb = NULL; eoe->stats.rx_errors++; #if EOE_DEBUG_LEVEL >= 1 - EC_WARN("Fragmenting error at %s.\n", eoe->dev->name); + EC_SLAVE_WARN(eoe->slave, "Fragmenting error at %s.\n", + eoe->dev->name); #endif eoe->state = ec_eoe_state_tx_start; return; @@ -569,8 +575,8 @@ eoe->rx_counter += eoe->rx_skb->len; #if EOE_DEBUG_LEVEL >= 2 - EC_DBG("EoE %s RX frame completed with %u octets.\n", - eoe->dev->name, eoe->rx_skb->len); + EC_SLAVE_DBG(eoe->slave, 0, "EoE %s RX frame completed" + " with %u octets.\n", eoe->dev->name, eoe->rx_skb->len); #endif // pass socket buffer to network stack @@ -578,7 +584,7 @@ eoe->rx_skb->protocol = eth_type_trans(eoe->rx_skb, eoe->dev); eoe->rx_skb->ip_summed = CHECKSUM_UNNECESSARY; if (netif_rx(eoe->rx_skb)) { - EC_WARN("EoE RX netif_rx failed.\n"); + EC_SLAVE_WARN(eoe->slave, "EoE RX netif_rx failed.\n"); } eoe->rx_skb = NULL; @@ -587,7 +593,7 @@ else { eoe->rx_expected_fragment++; #if EOE_DEBUG_LEVEL >= 2 - EC_DBG("EoE %s RX expecting fragment %u\n", + EC_SLAVE_DBG(eoe->slave, 0, "EoE %s RX expecting fragment %u\n", eoe->dev->name, eoe->rx_expected_fragment); #endif eoe->state = ec_eoe_state_rx_start; @@ -654,14 +660,15 @@ eoe->stats.tx_errors++; eoe->state = ec_eoe_state_rx_start; #if EOE_DEBUG_LEVEL >= 1 - EC_WARN("Send error at %s.\n", eoe->dev->name); + EC_SLAVE_WARN(eoe->slave, "Send error at %s.\n", eoe->dev->name); #endif return; } #if EOE_DEBUG_LEVEL >= 2 if (wakeup) - EC_DBG("EoE %s waking up TX queue...\n", eoe->dev->name); + EC_SLAVE_DBG(eoe->slave, 0, "EoE %s waking up TX queue...\n", + eoe->dev->name); #endif eoe->tries = EC_EOE_TRIES; @@ -684,7 +691,8 @@ } else { eoe->stats.tx_errors++; #if EOE_DEBUG_LEVEL >= 1 - EC_WARN("Failed to receive send datagram for %s after %u tries.\n", + EC_SLAVE_WARN(eoe->slave, "Failed to receive send" + " datagram for %s after %u tries.\n", eoe->dev->name, EC_EOE_TRIES); #endif eoe->state = ec_eoe_state_rx_start; @@ -699,7 +707,8 @@ } else { eoe->stats.tx_errors++; #if EOE_DEBUG_LEVEL >= 1 - EC_WARN("No sending response for %s after %u tries.\n", + EC_SLAVE_WARN(eoe->slave, "No sending response" + " for %s after %u tries.\n", eoe->dev->name, EC_EOE_TRIES); #endif eoe->state = ec_eoe_state_rx_start; @@ -724,7 +733,7 @@ eoe->tx_frame = NULL; eoe->stats.tx_errors++; #if EOE_DEBUG_LEVEL >= 1 - EC_WARN("Send error at %s.\n", eoe->dev->name); + EC_SLAVE_WARN(eoe->slave, "Send error at %s.\n", eoe->dev->name); #endif eoe->state = ec_eoe_state_rx_start; } @@ -747,7 +756,7 @@ netif_start_queue(dev); eoe->tx_queue_active = 1; #if EOE_DEBUG_LEVEL >= 2 - EC_DBG("%s opened.\n", dev->name); + EC_SLAVE_DBG(eoe->slave, 0, "%s opened.\n", dev->name); #endif ec_slave_request_state(eoe->slave, EC_SLAVE_STATE_OP); return 0; @@ -767,7 +776,7 @@ eoe->opened = 0; ec_eoe_flush(eoe); #if EOE_DEBUG_LEVEL >= 2 - EC_DBG("%s stopped.\n", dev->name); + EC_SLAVE_DBG(eoe->slave, 0, "%s stopped.\n", dev->name); #endif ec_slave_request_state(eoe->slave, EC_SLAVE_STATE_PREOP); return 0; @@ -786,7 +795,8 @@ #if 0 if (skb->len > eoe->slave->configured_tx_mailbox_size - 10) { - EC_WARN("EoE TX frame (%u octets) exceeds MTU. dropping.\n", skb->len); + EC_SLAVE_WARN(eoe->slave, "EoE TX frame (%u octets)" + " exceeds MTU. dropping.\n", skb->len); dev_kfree_skb(skb); eoe->stats.tx_dropped++; return 0; @@ -796,7 +806,7 @@ if (!(frame = (ec_eoe_frame_t *) kmalloc(sizeof(ec_eoe_frame_t), GFP_ATOMIC))) { if (printk_ratelimit()) - EC_WARN("EoE TX: low on mem. frame dropped.\n"); + EC_SLAVE_WARN(eoe->slave, "EoE TX: low on mem. frame dropped.\n"); return 1; } @@ -812,10 +822,11 @@ up(&eoe->tx_queue_sem); #if EOE_DEBUG_LEVEL >= 2 - EC_DBG("EoE %s TX queued frame with %u octets (%u frames queued).\n", - eoe->dev->name, skb->len, eoe->tx_queued_frames); + EC_SLAVE_DBG(eoe->slave, 0, "EoE %s TX queued frame" + " with %u octets (%u frames queued).\n", + eoe->dev->name, skb->len, eoe->tx_queued_frames); if (!eoe->tx_queue_active) - EC_WARN("EoE TX queue is now full.\n"); + EC_SLAVE_WARN(eoe->slave, "EoE TX queue is now full.\n"); #endif return 0; diff -r ea38efeeb7b3 -r 6aa393418fb3 master/fmmu_config.c --- a/master/fmmu_config.c Fri May 07 15:26:26 2010 +0200 +++ b/master/fmmu_config.c Tue May 11 13:57:58 2010 +0200 @@ -95,12 +95,13 @@ uint8_t *data /**> Configuration page memory. */ ) { - if (fmmu->sc->master->debug_level) { - EC_DBG("FMMU: LogAddr 0x%08X, DomAddr 0x%08X, Size %3u, Tx %3u, PhysAddr 0x%04X, SM%u, " - "Dir %s\n", fmmu->logical_start_address, fmmu->domain_address, fmmu->data_size, - fmmu->tx_size, sync->physical_start_address, fmmu->sync_index, - fmmu->dir == EC_DIR_INPUT ? "in" : "out"); - } + EC_CONFIG_DBG(fmmu->sc, 1, "FMMU: LogAddr 0x%08X, DomAddr 0x%08X," + " Size %3u, Tx %3u" + " PhysAddr 0x%04X, SM%u, Dir %s\n", + fmmu->logical_start_address, fmmu->domain_address, + fmmu->data_size, 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 diff -r ea38efeeb7b3 -r 6aa393418fb3 master/fsm_change.c --- a/master/fsm_change.c Fri May 07 15:26:26 2010 +0200 +++ b/master/fsm_change.c Tue May 11 13:57:58 2010 +0200 @@ -182,8 +182,7 @@ if (datagram->state != EC_DATAGRAM_RECEIVED) { fsm->state = ec_fsm_change_state_error; - EC_ERR("Failed to receive state datagram from slave %u: ", - fsm->slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to receive state datagram: "); ec_datagram_print_state(datagram); return; } @@ -198,8 +197,7 @@ char state_str[EC_STATE_STRING_SIZE]; ec_state_string(fsm->requested_state, state_str, 0); fsm->state = ec_fsm_change_state_error; - EC_ERR("Failed to set state %s on slave %u: ", - state_str, fsm->slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to set state %s: ", state_str); ec_datagram_print_wc_error(datagram); return; } @@ -215,8 +213,7 @@ char state_str[EC_STATE_STRING_SIZE]; ec_state_string(fsm->requested_state, state_str, 0); fsm->state = ec_fsm_change_state_error; - EC_ERR("Failed to set state %s on slave %u: ", - state_str, fsm->slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to set state %s: ", state_str); ec_datagram_print_wc_error(datagram); return; } @@ -248,8 +245,7 @@ if (datagram->state != EC_DATAGRAM_RECEIVED) { fsm->state = ec_fsm_change_state_error; - EC_ERR("Failed to receive state checking datagram from slave %u: ", - slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to receive state checking datagram: "); ec_datagram_print_state(datagram); return; } @@ -258,8 +254,7 @@ char req_state[EC_STATE_STRING_SIZE]; ec_state_string(fsm->requested_state, req_state, 0); fsm->state = ec_fsm_change_state_error; - EC_ERR("Failed to check state %s on slave %u: ", - req_state, slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to check state %s: ", req_state); ec_datagram_print_wc_error(datagram); return; } @@ -288,8 +283,8 @@ // state change fsm->spontaneous_change = 1; fsm->old_state = slave->current_state; - EC_WARN("Slave %u changed to %s in the meantime.\n", - slave->ring_position, cur_state); + EC_SLAVE_WARN(slave, "Changed to %s in the meantime.\n", + cur_state); goto check_again; } @@ -298,8 +293,8 @@ slave->error_flag = 1; ec_state_string(fsm->requested_state, req_state, 0); - EC_ERR("Failed to set %s state, slave %u refused state change (%s).\n", - req_state, slave->ring_position, cur_state); + EC_SLAVE_ERR(slave, "Failed to set %s state, slave refused state" + " change (%s).\n", req_state, cur_state); // fetch AL status error code ec_datagram_fprd(datagram, slave->station_address, 0x0134, 2); ec_datagram_zero(datagram); @@ -316,8 +311,7 @@ char state_str[EC_STATE_STRING_SIZE]; ec_state_string(fsm->requested_state, state_str, 0); fsm->state = ec_fsm_change_state_error; - EC_ERR("Timeout while setting state %s on slave %u.\n", - state_str, slave->ring_position); + EC_SLAVE_ERR(slave, "Timeout while setting state %s.\n", state_str); return; } @@ -406,25 +400,27 @@ if (datagram->state != EC_DATAGRAM_RECEIVED) { fsm->state = ec_fsm_change_state_error; - EC_ERR("Failed to receive AL status code datagram from slave %u: ", - fsm->slave->ring_position); + EC_SLAVE_ERR(fsm->slave, "Failed to receive" + " AL status code datagram: "); ec_datagram_print_state(datagram); return; } if (datagram->working_counter != 1) { - EC_WARN("Reception of AL status code datagram failed: "); + EC_SLAVE_WARN(fsm->slave, "Reception of AL status code" + " datagram failed: "); ec_datagram_print_wc_error(datagram); } else { code = EC_READ_U16(datagram->data); for (al_msg = al_status_messages; al_msg->code != 0xffff; al_msg++) { if (al_msg->code != code) continue; - EC_ERR("AL status message 0x%04X: \"%s\".\n", + EC_SLAVE_ERR(fsm->slave, "AL status message 0x%04X: \"%s\".\n", al_msg->code, al_msg->message); break; } if (!al_msg->code) - EC_ERR("Unknown AL status code 0x%04X.\n", code); + EC_SLAVE_ERR(fsm->slave, "Unknown AL status code 0x%04X.\n", + code); } // acknowledge "old" slave state @@ -465,16 +461,14 @@ if (datagram->state != EC_DATAGRAM_RECEIVED) { fsm->state = ec_fsm_change_state_error; - EC_ERR("Failed to receive state ack datagram for slave %u: ", - slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to receive state ack datagram: "); ec_datagram_print_state(datagram); return; } if (datagram->working_counter != 1) { fsm->state = ec_fsm_change_state_error; - EC_ERR("Reception of state ack datagram failed on slave %u: ", - slave->ring_position); + EC_SLAVE_ERR(slave, "Reception of state ack datagram failed: "); ec_datagram_print_wc_error(datagram); return; } @@ -505,16 +499,14 @@ if (datagram->state != EC_DATAGRAM_RECEIVED) { fsm->state = ec_fsm_change_state_error; - EC_ERR("Failed to receive state ack check datagram from slave %u: ", - slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to receive state ack check datagram: "); ec_datagram_print_state(datagram); return; } if (datagram->working_counter != 1) { fsm->state = ec_fsm_change_state_error; - EC_ERR("Reception of state ack check datagram failed on slave %u: ", - slave->ring_position); + EC_SLAVE_ERR(slave, "Reception of state ack check datagram failed: "); ec_datagram_print_wc_error(datagram); return; } @@ -535,8 +527,7 @@ else { // EC_FSM_CHANGE_MODE_ACK_ONLY fsm->state = ec_fsm_change_state_end; } - EC_INFO("Acknowledged state %s on slave %u.\n", - state_str, slave->ring_position); + EC_SLAVE_INFO(slave, "Acknowledged state %s.\n", state_str); return; } @@ -546,8 +537,8 @@ char state_str[EC_STATE_STRING_SIZE]; ec_state_string(slave->current_state, state_str, 0); fsm->state = ec_fsm_change_state_error; - EC_ERR("Timeout while acknowledging state %s on slave %u.\n", - state_str, slave->ring_position); + EC_SLAVE_ERR(slave, "Timeout while acknowledging state %s.\n", + state_str); return; } diff -r ea38efeeb7b3 -r 6aa393418fb3 master/fsm_coe.c --- a/master/fsm_coe.c Fri May 07 15:26:26 2010 +0200 +++ b/master/fsm_coe.c Tue May 11 13:57:58 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; } @@ -530,11 +523,13 @@ } 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. + if (fragments_left) { + 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); } diff -r ea38efeeb7b3 -r 6aa393418fb3 master/fsm_foe.c --- a/master/fsm_foe.c Fri May 07 15:26:26 2010 +0200 +++ b/master/fsm_foe.c Tue May 11 13:57:58 2010 +0200 @@ -288,7 +288,7 @@ if (!(slave->sii.mailbox_protocols & EC_MBOX_FOE)) { ec_foe_set_tx_error(fsm, FOE_MBOX_PROT_ERROR); - EC_ERR("Slave %u does not support FoE!\n", slave->ring_position); + EC_SLAVE_ERR(slave, "Slave does not support FoE!\n"); return; } @@ -317,8 +317,7 @@ if (datagram->state != EC_DATAGRAM_RECEIVED) { ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR); - EC_ERR("Failed to receive FoE mailbox check datagram for slave %u: ", - slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to receive FoE mailbox check datagram: "); ec_datagram_print_state(datagram); return; } @@ -326,8 +325,8 @@ if (datagram->working_counter != 1) { // slave did not put anything in the mailbox yet ec_foe_set_rx_error(fsm, FOE_WC_ERROR); - EC_ERR("Reception of FoE mailbox check datagram failed on slave %u: ", - slave->ring_position); + EC_SLAVE_ERR(slave, "Reception of FoE mailbox check datagram" + " failed: "); ec_datagram_print_wc_error(datagram); return; } @@ -337,8 +336,7 @@ (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; if (diff_ms >= EC_FSM_FOE_TIMEOUT) { ec_foe_set_tx_error(fsm, FOE_TIMEOUT_ERROR); - EC_ERR("Timeout while waiting for ack response " - "on slave %u.\n", slave->ring_position); + EC_SLAVE_ERR(slave, "Timeout while waiting for ack response.\n"); return; } @@ -374,16 +372,14 @@ if (datagram->state != EC_DATAGRAM_RECEIVED) { ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR); - EC_ERR("Failed to receive FoE ack response datagram for" - " slave %u: ", slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to receive FoE ack response datagram: "); ec_datagram_print_state(datagram); return; } if (datagram->working_counter != 1) { ec_foe_set_rx_error(fsm, FOE_WC_ERROR); - EC_ERR("Reception of FoE ack response failed on slave %u: ", - slave->ring_position); + EC_SLAVE_ERR(slave, "Reception of FoE ack response failed: "); ec_datagram_print_wc_error(datagram); return; } @@ -396,7 +392,8 @@ if (mbox_prot != EC_MBOX_TYPE_FILEACCESS) { // FoE ec_foe_set_tx_error(fsm, FOE_MBOX_PROT_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; } @@ -406,7 +403,7 @@ // slave not ready if (ec_foe_prepare_data_send(fsm)) { ec_foe_set_tx_error(fsm, FOE_PROT_ERROR); - EC_ERR("Slave is busy.\n"); + EC_SLAVE_ERR(slave, "Slave is busy.\n"); return; } fsm->state = ec_fsm_foe_state_data_sent; @@ -452,8 +449,7 @@ if (datagram->state != EC_DATAGRAM_RECEIVED) { ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR); - EC_ERR("Failed to send FoE WRQ for slave %u: ", - slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to send FoE WRQ: "); ec_datagram_print_state(datagram); return; } @@ -461,8 +457,7 @@ if (datagram->working_counter != 1) { // slave did not put anything in the mailbox yet ec_foe_set_rx_error(fsm, FOE_WC_ERROR); - EC_ERR("Reception of FoE WRQ failed on slave %u: ", - slave->ring_position); + EC_SLAVE_ERR(slave, "Reception of FoE WRQ failed: "); ec_datagram_print_wc_error(datagram); return; } @@ -495,16 +490,14 @@ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) { ec_foe_set_tx_error(fsm, FOE_RECEIVE_ERROR); - EC_ERR("Failed to receive FoE ack response datagram for" - " slave %u: ", slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to receive FoE ack response datagram: "); ec_datagram_print_state(datagram); return; } if (fsm->datagram->working_counter != 1) { ec_foe_set_tx_error(fsm, FOE_WC_ERROR); - EC_ERR("Reception of FoE data send failed on slave %u: ", - slave->ring_position); + EC_SLAVE_ERR(slave, "Reception of FoE data send failed: "); ec_datagram_print_wc_error(datagram); return; } @@ -536,7 +529,7 @@ memcpy(data + EC_FOE_HEADER_SIZE, fsm->rx_filename, current_size); if (fsm->slave->master->debug_level) { - EC_DBG("FoE Read Request:\n"); + EC_SLAVE_DBG(fsm->slave, 1, "FoE Read Request:\n"); ec_print_data(data, current_size + EC_FOE_HEADER_SIZE); } @@ -584,8 +577,7 @@ if (datagram->state != EC_DATAGRAM_RECEIVED) { ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR); - EC_ERR("Failed to send FoE RRQ for slave %u" ": ", - slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to send FoE RRQ: "); ec_datagram_print_state(datagram); return; } @@ -593,8 +585,7 @@ if (datagram->working_counter != 1) { // slave did not put anything in the mailbox yet ec_foe_set_rx_error(fsm, FOE_WC_ERROR); - EC_ERR("Reception of FoE RRQ failed on slave %u: ", - slave->ring_position); + EC_SLAVE_ERR(slave, "Reception of FoE RRQ failed: "); ec_datagram_print_wc_error(datagram); return; } @@ -646,7 +637,7 @@ if (!(slave->sii.mailbox_protocols & EC_MBOX_FOE)) { ec_foe_set_tx_error(fsm, FOE_MBOX_PROT_ERROR); - EC_ERR("Slave %u does not support FoE!\n", slave->ring_position); + EC_SLAVE_ERR(slave, "Slave does not support FoE!\n"); return; } @@ -675,16 +666,14 @@ if (datagram->state != EC_DATAGRAM_RECEIVED) { ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR); - EC_ERR("Failed to send FoE DATA READ for slave %u: ", - slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to send FoE DATA READ: "); ec_datagram_print_state(datagram); return; } if (datagram->working_counter != 1) { ec_foe_set_rx_error(fsm, FOE_WC_ERROR); - EC_ERR("Reception of FoE DATA READ on slave %u: ", - slave->ring_position); + EC_SLAVE_ERR(slave, "Reception of FoE DATA READ: "); ec_datagram_print_wc_error(datagram); return; } @@ -694,8 +683,7 @@ (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; if (diff_ms >= EC_FSM_FOE_TIMEOUT) { ec_foe_set_tx_error(fsm, FOE_TIMEOUT_ERROR); - EC_ERR("Timeout while waiting for ack response " - "on slave %u.\n", slave->ring_position); + EC_SLAVE_ERR(slave, "Timeout while waiting for ack response.\n"); return; } @@ -732,16 +720,14 @@ if (datagram->state != EC_DATAGRAM_RECEIVED) { ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR); - EC_ERR("Failed to receive FoE DATA READ datagram for" - " slave %u: ", slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to receive FoE DATA READ datagram: "); ec_datagram_print_state(datagram); return; } if (datagram->working_counter != 1) { ec_foe_set_rx_error(fsm, FOE_WC_ERROR); - EC_ERR("Reception of FoE DATA READ failed on slave %u: ", - slave->ring_position); + EC_SLAVE_ERR(slave, "Reception of FoE DATA READ failed: "); ec_datagram_print_wc_error(datagram); return; } @@ -752,7 +738,8 @@ } if (mbox_prot != EC_MBOX_TYPE_FILEACCESS) { // FoE - 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); ec_foe_set_rx_error(fsm, FOE_PROT_ERROR); return; } @@ -768,20 +755,20 @@ if (opCode == EC_FOE_OPCODE_ERR) { fsm->request->error_code = EC_READ_U32(data + 2); - EC_ERR("Received FoE Error Request (code 0x%08x) on slave %u.\n", - fsm->request->error_code, slave->ring_position); + EC_SLAVE_ERR(slave, "Received FoE Error Request (code 0x%08x).\n", + fsm->request->error_code); if (rec_size > 6) { uint8_t text[1024]; strncpy(text, data + 6, min(rec_size - 6, sizeof(text))); - EC_ERR("FoE Error Text: %s\n", text); + EC_SLAVE_ERR(slave, "FoE Error Text: %s\n", text); } ec_foe_set_rx_error(fsm, FOE_OPCODE_ERROR); return; } if (opCode != EC_FOE_OPCODE_DATA) { - EC_ERR("Received OPCODE %x, expected %x on slave %u.\n", - opCode, EC_FOE_OPCODE_DATA, slave->ring_position); + EC_SLAVE_ERR(slave, "Received OPCODE %x, expected %x.\n", + opCode, EC_FOE_OPCODE_DATA); fsm->request->error_code = 0x00000000; ec_foe_set_rx_error(fsm, FOE_OPCODE_ERROR); return; @@ -789,8 +776,7 @@ packet_no = EC_READ_U16(data + 2); if (packet_no != fsm->rx_expected_packet_no) { - EC_ERR("Received unexpected packet number on slave %u.\n", - slave->ring_position); + EC_SLAVE_ERR(slave, "Received unexpected packet number.\n"); ec_foe_set_rx_error(fsm, FOE_PACKETNO_ERROR); return; } @@ -854,8 +840,7 @@ if (datagram->state != EC_DATAGRAM_RECEIVED) { ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR); - EC_ERR("Failed to send FoE ACK for slave %u: ", - slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to send FoE ACK: "); ec_datagram_print_state(datagram); return; } @@ -863,8 +848,7 @@ if (datagram->working_counter != 1) { // slave did not put anything into the mailbox yet ec_foe_set_rx_error(fsm, FOE_WC_ERROR); - EC_ERR("Reception of FoE ACK failed on slave %u: ", - slave->ring_position); + EC_SLAVE_ERR(slave, "Reception of FoE ACK failed: "); ec_datagram_print_wc_error(datagram); return; } diff -r ea38efeeb7b3 -r 6aa393418fb3 master/fsm_master.c --- a/master/fsm_master.c Fri May 07 15:26:26 2010 +0200 +++ b/master/fsm_master.c Tue May 11 13:57:58 2010 +0200 @@ -46,6 +46,12 @@ /*****************************************************************************/ +/** Time difference [ns] to tolerate without setting a new system time offset. + */ +#define EC_SYSTEM_TIME_TOLERANCE_NS 100000000 + +/*****************************************************************************/ + void ec_fsm_master_state_start(ec_fsm_master_t *); void ec_fsm_master_state_broadcast(ec_fsm_master_t *); void ec_fsm_master_state_read_state(ec_fsm_master_t *); @@ -54,11 +60,14 @@ void ec_fsm_master_state_clear_addresses(ec_fsm_master_t *); void ec_fsm_master_state_dc_measure_delays(ec_fsm_master_t *); void ec_fsm_master_state_scan_slave(ec_fsm_master_t *); +void ec_fsm_master_state_dc_read_offset(ec_fsm_master_t *); +void ec_fsm_master_state_dc_write_offset(ec_fsm_master_t *); void ec_fsm_master_state_write_sii(ec_fsm_master_t *); void ec_fsm_master_state_sdo_dictionary(ec_fsm_master_t *); void ec_fsm_master_state_sdo_request(ec_fsm_master_t *); void ec_fsm_master_state_reg_request(ec_fsm_master_t *); +void ec_fsm_master_enter_write_system_times(ec_fsm_master_t *); /*****************************************************************************/ @@ -194,14 +203,13 @@ if (datagram->working_counter != fsm->slaves_responding) { fsm->topology_change_pending = 1; fsm->slaves_responding = datagram->working_counter; - EC_INFO("%u slave(s) responding.\n", fsm->slaves_responding); + EC_MASTER_INFO(master, "%u slave(s) responding.\n", + fsm->slaves_responding); } if (fsm->link_state && !master->main_device.link_state) { // link went down - if (master->debug_level) { - EC_DBG("Master state machine detected " - "link down. Clearing slave list.\n"); - } + EC_MASTER_DBG(master, 1, "Master state machine detected " + "link down. Clearing slave list.\n"); #ifdef EC_EOE ec_master_eoe_stop(master); @@ -223,7 +231,7 @@ char state_str[EC_STATE_STRING_SIZE]; fsm->slave_states = states; ec_state_string(fsm->slave_states, state_str, 1); - EC_INFO("Slave states: %s.\n", state_str); + EC_MASTER_INFO(master, "Slave states: %s.\n", state_str); } } else { fsm->slave_states = 0x00; @@ -261,8 +269,8 @@ size = sizeof(ec_slave_t) * master->slave_count; if (!(master->slaves = (ec_slave_t *) kmalloc(size, GFP_KERNEL))) { - EC_ERR("Failed to allocate %u bytes of slave memory!\n", - size); + EC_MASTER_ERR(master, "Failed to allocate %u bytes" + " of slave memory!\n", size); master->slave_count = 0; // TODO avoid retrying scan! master->scan_busy = 0; wake_up_interruptible(&master->scan_queue); @@ -291,13 +299,25 @@ } if (master->slave_count) { - // fetch state from first slave - fsm->slave = master->slaves; - ec_datagram_fprd(fsm->datagram, fsm->slave->station_address, - 0x0130, 2); - ec_datagram_zero(datagram); - fsm->retries = EC_FSM_RETRIES; - fsm->state = ec_fsm_master_state_read_state; + + // application applied configurations + if (master->config_changed) { + master->config_changed = 0; + + EC_MASTER_DBG(master, 1, "Configuration changed.\n"); + + fsm->slave = master->slaves; // begin with first slave + ec_fsm_master_enter_write_system_times(fsm); + + } else { + // fetch state from first slave + fsm->slave = master->slaves; + ec_datagram_fprd(fsm->datagram, fsm->slave->station_address, + 0x0130, 2); + ec_datagram_zero(datagram); + fsm->retries = EC_FSM_RETRIES; + fsm->state = ec_fsm_master_state_read_state; + } } else { ec_fsm_master_restart(fsm); } @@ -328,9 +348,7 @@ request->state = EC_INT_REQUEST_BUSY; // found pending SII write operation. execute it! - if (master->debug_level) - EC_DBG("Writing SII data to slave %u...\n", - request->slave->ring_position); + EC_SLAVE_DBG(request->slave, 1, "Writing SII data...\n"); fsm->sii_request = request; fsm->sii_index = 0; ec_fsm_sii_write(&fsm->fsm_sii, request->slave, request->offset, @@ -366,14 +384,12 @@ request->state = EC_INT_REQUEST_BUSY; // found pending request; process it! - if (master->debug_level) - EC_DBG("Processing register request for slave %u, " - "offset 0x%04x, length %zu...\n", - request->slave->ring_position, - request->offset, request->length); + EC_SLAVE_DBG(request->slave, 1, "Processing register request, " + "offset 0x%04x, length %zu...\n", + request->offset, request->length); if (request->length > fsm->datagram->mem_size) { - EC_ERR("Request length (%zu) exceeds maximum " + EC_MASTER_ERR(master, "Request length (%zu) exceeds maximum " "datagram size (%zu)!\n", request->length, fsm->datagram->mem_size); request->state = EC_INT_REQUEST_FAILURE; @@ -425,9 +441,8 @@ if (ec_sdo_request_timed_out(req)) { req->state = EC_INT_REQUEST_FAILURE; - if (master->debug_level) - EC_DBG("Internal SDO request for slave %u" - " timed out...\n", slave->ring_position); + EC_SLAVE_DBG(slave, 1, "Internal SDO request" + " timed out.\n"); continue; } @@ -437,10 +452,8 @@ } req->state = EC_INT_REQUEST_BUSY; - if (master->debug_level) - EC_DBG("Processing internal SDO request for slave %u...\n", - slave->ring_position); - + EC_SLAVE_DBG(slave, 1, "Processing internal" + " SDO request...\n"); fsm->idle = 0; fsm->sdo_request = req; fsm->slave = slave; @@ -492,10 +505,7 @@ || jiffies - slave->jiffies_preop < EC_WAIT_SDO_DICT * HZ ) continue; - if (master->debug_level) { - EC_DBG("Fetching SDO dictionary from slave %u.\n", - slave->ring_position); - } + EC_SLAVE_DBG(slave, 1, "Fetching SDO dictionary.\n"); slave->sdo_dictionary_fetched = 1; @@ -574,8 +584,8 @@ new_state[EC_STATE_STRING_SIZE]; ec_state_string(slave->current_state, old_state, 0); ec_state_string(slave->requested_state, new_state, 0); - EC_DBG("Changing state of slave %u from %s to %s%s.\n", - slave->ring_position, old_state, new_state, + EC_SLAVE_DBG(slave, 1, "Changing state from %s to %s%s.\n", + old_state, new_state, slave->force_config ? " (forced)" : ""); } @@ -608,8 +618,7 @@ return; if (datagram->state != EC_DATAGRAM_RECEIVED) { - EC_ERR("Failed to receive AL state datagram for slave %u: ", - slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to receive AL state datagram: "); ec_datagram_print_state(datagram); ec_fsm_master_restart(fsm); return; @@ -619,9 +628,7 @@ if (datagram->working_counter != 1) { if (!slave->error_flag) { slave->error_flag = 1; - if (fsm->master->debug_level) - EC_DBG("Slave %u did not respond to state query.\n", - fsm->slave->ring_position); + EC_SLAVE_DBG(slave, 1, "Slave did not respond to state query.\n"); } fsm->topology_change_pending = 1; ec_fsm_master_restart(fsm); @@ -665,8 +672,7 @@ if (!ec_fsm_change_success(&fsm->fsm_change)) { fsm->slave->error_flag = 1; - EC_ERR("Failed to acknowledge state change on slave %u.\n", - slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to acknowledge state change.\n"); } ec_fsm_master_action_configure(fsm); @@ -687,8 +693,9 @@ return; if (datagram->state != EC_DATAGRAM_RECEIVED) { - EC_ERR("Failed to receive address clearing datagram (state %u).\n", - datagram->state); + EC_MASTER_ERR(master, "Failed to receive address" + " clearing datagram: "); + ec_datagram_print_state(datagram); master->scan_busy = 0; wake_up_interruptible(&master->scan_queue); ec_fsm_master_restart(fsm); @@ -696,12 +703,13 @@ } if (datagram->working_counter != master->slave_count) { - EC_WARN("Failed to clear all station addresses: Cleared %u of %u", + EC_MASTER_WARN(master, "Failed to clear all station addresses:" + " Cleared %u of %u", datagram->working_counter, master->slave_count); } - if (master->debug_level) - EC_DBG("Sending broadcast-write to measure transmission delays.\n"); + EC_MASTER_DBG(master, 1, "Sending broadcast-write" + " to measure transmission delays.\n"); ec_datagram_bwr(datagram, 0x0900, 1); ec_datagram_zero(datagram); @@ -724,19 +732,18 @@ return; if (datagram->state != EC_DATAGRAM_RECEIVED) { - EC_ERR("Failed to receive delay measuring datagram (state %u).\n", - datagram->state); + EC_MASTER_ERR(master, "Failed to receive delay measuring datagram: "); + ec_datagram_print_state(datagram); master->scan_busy = 0; wake_up_interruptible(&master->scan_queue); ec_fsm_master_restart(fsm); return; } - if (master->debug_level) - EC_DBG("%u slaves responded to delay measuring.\n", - datagram->working_counter); - - EC_INFO("Scanning bus.\n"); + EC_MASTER_DBG(master, 1, "%u slaves responded to delay measuring.\n", + datagram->working_counter); + + EC_MASTER_INFO(master, "Scanning bus.\n"); // begin scanning of slaves fsm->slave = master->slaves; @@ -767,11 +774,9 @@ // create EoE handler for this slave ec_eoe_t *eoe; if (!(eoe = kmalloc(sizeof(ec_eoe_t), GFP_KERNEL))) { - EC_ERR("Failed to allocate EoE handler memory for slave %u!\n", - slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to allocate EoE handler memory!\n"); } else if (ec_eoe_init(eoe, slave)) { - EC_ERR("Failed to init EoE handler for slave %u!\n", - slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to init EoE handler!\n"); kfree(eoe); } else { list_add_tail(&eoe->list, &master->eoe_handlers); @@ -787,7 +792,7 @@ return; } - EC_INFO("Bus scanning completed in %u ms.\n", + EC_MASTER_INFO(master, "Bus scanning completed in %u ms.\n", (u32) (jiffies - fsm->scan_jiffies) * 1000 / HZ); master->scan_busy = 0; @@ -803,7 +808,12 @@ ec_master_eoe_start(master); #endif - ec_fsm_master_restart(fsm); + if (master->slave_count) { + fsm->slave = master->slaves; // begin with first slave + ec_fsm_master_enter_write_system_times(fsm); + } else { + ec_fsm_master_restart(fsm); + } } /*****************************************************************************/ @@ -837,6 +847,219 @@ /*****************************************************************************/ +/** Start writing DC system times. + */ +void ec_fsm_master_enter_write_system_times( + ec_fsm_master_t *fsm /**< Master state machine. */ + ) +{ + ec_master_t *master = fsm->master; + + EC_MASTER_DBG(master, 1, "Writing system time offsets...\n"); + + if (master->has_app_time) { + while (fsm->slave < master->slaves + master->slave_count) { + if (!fsm->slave->base_dc_supported + || !fsm->slave->has_dc_system_time) { + fsm->slave++; + continue; + } + + // read DC system time (0x0910, 64 bit) + // gap (64 bit) + // and time offset (0x0920, 64 bit) + ec_datagram_fprd(fsm->datagram, fsm->slave->station_address, + 0x0910, 24); + fsm->retries = EC_FSM_RETRIES; + fsm->state = ec_fsm_master_state_dc_read_offset; + return; + } + } else { + EC_MASTER_DBG(master, 1, "No app_time received up to now.\n"); + } + + ec_master_request_op(master); + ec_fsm_master_restart(fsm); +} + +/*****************************************************************************/ + +/** Configure 32 bit time offset. + */ +u64 ec_fsm_master_dc_offset32( + ec_fsm_master_t *fsm, /**< Master state machine. */ + u64 system_time, /**< System time register. */ + u64 old_offset, /**< Time offset register. */ + u64 correction /**< Correction. */ + ) +{ + ec_slave_t *slave = fsm->slave; + u32 correction32, system_time32, old_offset32, new_offset; + s32 time_diff; + + system_time32 = (u32) system_time; + // correct read system time by elapsed time between read operation + // and app_time set time + correction32 = (u32)correction; + system_time32 -= correction32; + old_offset32 = (u32) old_offset; + + time_diff = (u32) slave->master->app_time - system_time32; + + EC_SLAVE_DBG(slave, 1, "DC system time offset calculation:" + " system_time=%u (corrected with %u)," + " app_time=%u, diff=%i\n", + system_time32, correction32, + (u32) slave->master->app_time, time_diff); + + if (EC_ABS(time_diff) > EC_SYSTEM_TIME_TOLERANCE_NS) { + new_offset = time_diff + old_offset32; + EC_SLAVE_DBG(slave, 1, "Setting time offset to %u (was %u)\n", + new_offset, old_offset32); + return (u64) new_offset; + } else { + EC_SLAVE_DBG(slave, 1, "Not touching time offset.\n"); + return old_offset; + } +} + +/*****************************************************************************/ + +/** Configure 64 bit time offset. + */ +u64 ec_fsm_master_dc_offset64( + ec_fsm_master_t *fsm, /**< Master state machine. */ + u64 system_time, /**< System time register. */ + u64 old_offset, /**< Time offset register. */ + u64 correction /**< Correction. */ + ) +{ + ec_slave_t *slave = fsm->slave; + u64 new_offset; + s64 time_diff; + + // correct read system time by elapsed time between read operation + // and app_time set time + system_time -= correction; + time_diff = fsm->slave->master->app_time - system_time; + + EC_SLAVE_DBG(slave, 1, "DC system time offset calculation:" + " system_time=%llu (corrected with %llu)," + " app_time=%llu, diff=%lli\n", + system_time, correction, + slave->master->app_time, time_diff); + + if (EC_ABS(time_diff) > EC_SYSTEM_TIME_TOLERANCE_NS) { + new_offset = time_diff + old_offset; + EC_SLAVE_DBG(slave, 1, "Setting time offset to %llu (was %llu)\n", + new_offset, old_offset); + } else { + new_offset = old_offset; + EC_SLAVE_DBG(slave, 1, "Not touching time offset.\n"); + } + + return new_offset; +} + +/*****************************************************************************/ + +/** Master state: DC READ OFFSET. + */ +void ec_fsm_master_state_dc_read_offset( + ec_fsm_master_t *fsm /**< Master state machine. */ + ) +{ + ec_datagram_t *datagram = fsm->datagram; + ec_slave_t *slave = fsm->slave; + u64 system_time, old_offset, new_offset, correction; + + if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) + return; + + if (datagram->state != EC_DATAGRAM_RECEIVED) { + EC_SLAVE_ERR(slave, "Failed to receive DC times datagram: "); + ec_datagram_print_state(datagram); + fsm->slave++; + ec_fsm_master_enter_write_system_times(fsm); + return; + } + + if (datagram->working_counter != 1) { + EC_SLAVE_WARN(slave, "Failed to get DC times: "); + ec_datagram_print_wc_error(datagram); + fsm->slave++; + ec_fsm_master_enter_write_system_times(fsm); + return; + } + + system_time = EC_READ_U64(datagram->data); // 0x0910 + old_offset = EC_READ_U64(datagram->data + 16); // 0x0920 + /* correct read system time by elapsed time since read operation + and the app_time set time */ +#ifdef EC_HAVE_CYCLES + correction = + (datagram->cycles_sent - slave->master->dc_cycles_app_time) + * 1000000LL; + do_div(correction,cpu_khz); +#else + correction = + (u64) ((datagram->jiffies_sent-slave->master->dc_jiffies_app_time) * 1000 / HZ) + * 1000000; +#endif + + if (slave->base_dc_range == EC_DC_32) { + new_offset = ec_fsm_master_dc_offset32(fsm, + system_time, old_offset, correction); + } else { + new_offset = ec_fsm_master_dc_offset64(fsm, + system_time, old_offset, correction); + } + + // set DC system time offset and transmission delay + ec_datagram_fpwr(datagram, slave->station_address, 0x0920, 12); + EC_WRITE_U64(datagram->data, new_offset); + EC_WRITE_U32(datagram->data + 8, slave->transmission_delay); + fsm->retries = EC_FSM_RETRIES; + fsm->state = ec_fsm_master_state_dc_write_offset; +} + +/*****************************************************************************/ + +/** Master state: DC WRITE OFFSET. + */ +void ec_fsm_master_state_dc_write_offset( + ec_fsm_master_t *fsm /**< Master state machine. */ + ) +{ + ec_datagram_t *datagram = fsm->datagram; + ec_slave_t *slave = fsm->slave; + + if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) + return; + + if (datagram->state != EC_DATAGRAM_RECEIVED) { + EC_SLAVE_ERR(slave, + "Failed to receive DC system time offset datagram: "); + ec_datagram_print_state(datagram); + fsm->slave++; + ec_fsm_master_enter_write_system_times(fsm); + return; + } + + if (datagram->working_counter != 1) { + EC_SLAVE_ERR(slave, "Failed to set DC system time offset: "); + ec_datagram_print_wc_error(datagram); + fsm->slave++; + ec_fsm_master_enter_write_system_times(fsm); + return; + } + + fsm->slave++; + ec_fsm_master_enter_write_system_times(fsm); +} + +/*****************************************************************************/ + /** Master state: WRITE SII. */ void ec_fsm_master_state_write_sii( @@ -850,8 +1073,7 @@ if (ec_fsm_sii_exec(&fsm->fsm_sii)) return; if (!ec_fsm_sii_success(&fsm->fsm_sii)) { - EC_ERR("Failed to write SII data to slave %u.\n", - slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to write SII data.\n"); request->state = EC_INT_REQUEST_FAILURE; wake_up(&master->sii_queue); ec_fsm_master_restart(fsm); @@ -869,9 +1091,8 @@ } // finished writing SII - if (master->debug_level) - EC_DBG("Finished writing %zu words of SII data to slave %u.\n", - request->nwords, slave->ring_position); + EC_SLAVE_DBG(slave, 1, "Finished writing %zu words of SII data.\n", + request->nwords); if (request->offset <= 4 && request->offset + request->nwords > 4) { // alias was written @@ -914,8 +1135,8 @@ if (master->debug_level) { unsigned int sdo_count, entry_count; ec_slave_sdo_dict_info(slave, &sdo_count, &entry_count); - EC_DBG("Fetched %u SDOs and %u entries from slave %u.\n", - sdo_count, entry_count, slave->ring_position); + EC_SLAVE_DBG(slave, 1, "Fetched %u SDOs and %u entries.\n", + sdo_count, entry_count); } // attach pdo names from dictionary @@ -932,14 +1153,13 @@ ec_fsm_master_t *fsm /**< Master state machine. */ ) { - ec_master_t *master = fsm->master; ec_sdo_request_t *request = fsm->sdo_request; if (ec_fsm_coe_exec(&fsm->fsm_coe)) return; if (!ec_fsm_coe_success(&fsm->fsm_coe)) { - EC_DBG("Failed to process internal SDO request for slave %u.\n", - fsm->slave->ring_position); + EC_SLAVE_DBG(fsm->slave, 1, + "Failed to process internal SDO request.\n"); request->state = EC_INT_REQUEST_FAILURE; wake_up(&fsm->slave->sdo_queue); ec_fsm_master_restart(fsm); @@ -950,9 +1170,7 @@ request->state = EC_INT_REQUEST_SUCCESS; wake_up(&fsm->slave->sdo_queue); - if (master->debug_level) - EC_DBG("Finished internal SDO request for slave %u.\n", - fsm->slave->ring_position); + EC_SLAVE_DBG(fsm->slave, 1, "Finished internal SDO request.\n"); // check for another SDO request if (ec_fsm_master_action_process_sdo(fsm)) @@ -974,8 +1192,9 @@ ec_reg_request_t *request = fsm->reg_request; if (datagram->state != EC_DATAGRAM_RECEIVED) { - EC_ERR("Failed to receive register request datagram (state %u).\n", - datagram->state); + EC_MASTER_ERR(master, "Failed to receive register" + " request datagram: "); + ec_datagram_print_state(datagram); request->state = EC_INT_REQUEST_FAILURE; wake_up(&master->reg_queue); ec_fsm_master_restart(fsm); @@ -988,8 +1207,8 @@ kfree(request->data); request->data = kmalloc(request->length, GFP_KERNEL); if (!request->data) { - EC_ERR("Failed to allocate %zu bytes of memory for" - " register data.\n", request->length); + EC_MASTER_ERR(master, "Failed to allocate %zu bytes" + " of memory for register data.\n", request->length); request->state = EC_INT_REQUEST_FAILURE; wake_up(&master->reg_queue); ec_fsm_master_restart(fsm); @@ -999,12 +1218,10 @@ } request->state = EC_INT_REQUEST_SUCCESS; - if (master->debug_level) { - EC_DBG("Register request successful.\n"); - } + EC_SLAVE_DBG(request->slave, 1, "Register request successful.\n"); } else { request->state = EC_INT_REQUEST_FAILURE; - EC_ERR("Register request failed.\n"); + EC_MASTER_ERR(master, "Register request failed.\n"); } wake_up(&master->reg_queue); diff -r ea38efeeb7b3 -r 6aa393418fb3 master/fsm_pdo.c --- a/master/fsm_pdo.c Fri May 07 15:26:26 2010 +0200 +++ b/master/fsm_pdo.c Tue May 11 13:57:58 2010 +0200 @@ -213,8 +213,8 @@ if (!(fsm->sync = ec_slave_get_sync(slave, fsm->sync_index))) continue; - if (slave->master->debug_level) - EC_DBG("Reading PDO assignment of SM%u.\n", fsm->sync_index); + EC_SLAVE_DBG(slave, 1, "Reading PDO assignment of SM%u.\n", + fsm->sync_index); ec_pdo_list_clear_pdos(&fsm->pdos); @@ -226,8 +226,7 @@ return; } - if (slave->master->debug_level) - EC_DBG("Reading of PDO configuration finished.\n"); + EC_SLAVE_DBG(slave, 1, "Reading of PDO configuration finished.\n"); ec_pdo_list_clear_pdos(&fsm->pdos); fsm->state = ec_fsm_pdo_state_end; @@ -244,24 +243,22 @@ if (ec_fsm_coe_exec(fsm->fsm_coe)) return; if (!ec_fsm_coe_success(fsm->fsm_coe)) { - EC_ERR("Failed to read number of assigned PDOs for SM%u" - " of slave %u.\n", fsm->sync_index, fsm->slave->ring_position); + EC_SLAVE_ERR(fsm->slave, "Failed to read number of assigned PDOs" + " for SM%u.\n", fsm->sync_index); ec_fsm_pdo_read_action_next_sync(fsm); return; } if (fsm->request.data_size != sizeof(uint8_t)) { - EC_ERR("Invalid data size %zu returned when uploading SDO 0x%04X:%02X " - "from slave %u.\n", fsm->request.data_size, - fsm->request.index, fsm->request.subindex, - fsm->slave->ring_position); + EC_SLAVE_ERR(fsm->slave, "Invalid data size %zu returned" + " when uploading SDO 0x%04X:%02X.\n", fsm->request.data_size, + fsm->request.index, fsm->request.subindex); ec_fsm_pdo_read_action_next_sync(fsm); return; } fsm->pdo_count = EC_READ_U8(fsm->request.data); - if (fsm->slave->master->debug_level) - EC_DBG("%u PDOs assigned.\n", fsm->pdo_count); + EC_SLAVE_DBG(fsm->slave, 1, "%u PDOs assigned.\n", fsm->pdo_count); // read first PDO fsm->pdo_pos = 1; @@ -306,24 +303,24 @@ if (ec_fsm_coe_exec(fsm->fsm_coe)) return; if (!ec_fsm_coe_success(fsm->fsm_coe)) { - EC_ERR("Failed to read index of assigned PDO %u from SM%u.\n", + EC_SLAVE_ERR(fsm->slave, "Failed to read index of" + " assigned PDO %u from SM%u.\n", fsm->pdo_pos, fsm->sync_index); ec_fsm_pdo_read_action_next_sync(fsm); return; } if (fsm->request.data_size != sizeof(uint16_t)) { - EC_ERR("Invalid data size %zu returned when uploading SDO 0x%04X:%02X " - "from slave %u.\n", fsm->request.data_size, - fsm->request.index, fsm->request.subindex, - fsm->slave->ring_position); + EC_SLAVE_ERR(fsm->slave, "Invalid data size %zu returned" + " when uploading SDO 0x%04X:%02X.\n", fsm->request.data_size, + fsm->request.index, fsm->request.subindex); ec_fsm_pdo_read_action_next_sync(fsm); return; } if (!(fsm->pdo = (ec_pdo_t *) kmalloc(sizeof(ec_pdo_t), GFP_KERNEL))) { - EC_ERR("Failed to allocate PDO.\n"); + EC_SLAVE_ERR(fsm->slave, "Failed to allocate PDO.\n"); ec_fsm_pdo_read_action_next_sync(fsm); return; } @@ -332,8 +329,7 @@ fsm->pdo->index = EC_READ_U16(fsm->request.data); fsm->pdo->sync_index = fsm->sync_index; - if (fsm->slave->master->debug_level) - EC_DBG("PDO 0x%04X.\n", fsm->pdo->index); + EC_SLAVE_DBG(fsm->slave, 1, "PDO 0x%04X.\n", fsm->pdo->index); list_add_tail(&fsm->pdo->list, &fsm->pdos.list); @@ -354,8 +350,8 @@ return; if (!ec_fsm_pdo_entry_success(&fsm->fsm_pdo_entry)) { - EC_ERR("Failed to read mapped PDO entries for PDO 0x%04X.\n", - fsm->pdo->index); + EC_SLAVE_ERR(fsm->slave, "Failed to read mapped PDO entries" + " for PDO 0x%04X.\n", fsm->pdo->index); ec_fsm_pdo_read_action_next_sync(fsm); return; } @@ -410,22 +406,25 @@ fsm->sync_index++; for (; fsm->sync_index < EC_MAX_SYNC_MANAGERS; fsm->sync_index++) { - if (!fsm->slave->config) { // slave configuration removed in the meantime + if (!fsm->slave->config) { + // slave configuration removed in the meantime fsm->state = ec_fsm_pdo_state_error; return; } if (ec_pdo_list_copy(&fsm->pdos, - &fsm->slave->config->sync_configs[fsm->sync_index].pdos)) { + &fsm->slave->config->sync_configs[fsm->sync_index].pdos)) + { fsm->state = ec_fsm_pdo_state_error; return; } if (!(fsm->sync = ec_slave_get_sync(fsm->slave, fsm->sync_index))) { if (!list_empty(&fsm->pdos.list)) - EC_WARN("PDOs configured for SM%u, but slave %u does not " - "provide the sync manager information!\n", - fsm->sync_index, fsm->slave->ring_position); + EC_SLAVE_WARN(fsm->slave, "PDOs configured for SM%u," + " but slave does not provide the" + " sync manager information!\n", + fsm->sync_index); continue; } @@ -462,10 +461,8 @@ } if (list_empty(&fsm->slave_pdo.entries)) { - - if (fsm->slave->master->debug_level) - EC_DBG("Reading mapping of PDO 0x%04X.\n", - fsm->pdo->index); + EC_SLAVE_DBG(fsm->slave, 1, "Reading mapping of PDO 0x%04X.\n", + fsm->pdo->index); // pdo mapping is unknown; start loading it ec_fsm_pdo_entry_start_reading(&fsm->fsm_pdo_entry, fsm->slave, @@ -491,7 +488,8 @@ return; if (!ec_fsm_pdo_entry_success(&fsm->fsm_pdo_entry)) - EC_WARN("Failed to read mapped PDO entries for PDO 0x%04X.\n", + EC_SLAVE_WARN(fsm->slave, + "Failed to read PDO entries for PDO 0x%04X.\n", fsm->pdo->index); // check if the mapping must be re-configured @@ -509,9 +507,8 @@ ) { if (ec_pdo_equal_entries(fsm->pdo, &fsm->slave_pdo)) { - if (fsm->slave->master->debug_level) - EC_DBG("Mapping of PDO 0x%04X is already configured correctly.\n", - fsm->pdo->index); + EC_SLAVE_DBG(fsm->slave, 1, "Mapping of PDO 0x%04X" + " is already configured correctly.\n", fsm->pdo->index); ec_fsm_pdo_conf_action_next_pdo_mapping(fsm); return; } @@ -534,7 +531,8 @@ return; if (!ec_fsm_pdo_entry_success(&fsm->fsm_pdo_entry)) - EC_WARN("Failed to configure mapping of PDO 0x%04X.\n", + EC_SLAVE_WARN(fsm->slave, + "Failed to configure mapping of PDO 0x%04X.\n", fsm->pdo->index); ec_fsm_pdo_conf_action_next_pdo_mapping(fsm); @@ -568,27 +566,24 @@ { // check if assignment has to be re-configured if (ec_pdo_list_equal(&fsm->sync->pdos, &fsm->pdos)) { - - if (fsm->slave->master->debug_level) - EC_DBG("PDO assignment for SM%u is already configured " - "correctly.\n", fsm->sync_index); - + EC_SLAVE_DBG(fsm->slave, 1, "PDO assignment for SM%u" + " is already configured correctly.\n", fsm->sync_index); ec_fsm_pdo_conf_action_next_sync(fsm); return; } if (fsm->slave->master->debug_level) { - EC_DBG("PDO assignment of SM%u differs:\n", fsm->sync_index); - EC_DBG(""); ec_fsm_pdo_print(fsm); + EC_SLAVE_DBG(fsm->slave, 1, "PDO assignment of SM%u differs:\n", + fsm->sync_index); + EC_SLAVE_DBG(fsm->slave, 1, ""); ec_fsm_pdo_print(fsm); } // PDO assignment has to be changed. Does the slave support this? if (!(fsm->slave->sii.mailbox_protocols & EC_MBOX_COE) || (fsm->slave->sii.has_general && !fsm->slave->sii.coe_details.enable_pdo_assign)) { - EC_WARN("Slave %u does not support assigning PDOs!\n", - fsm->slave->ring_position); - EC_WARN(""); ec_fsm_pdo_print(fsm); + EC_SLAVE_WARN(fsm->slave, "Slave does not support assigning PDOs!\n"); + EC_SLAVE_WARN(fsm->slave, ""); ec_fsm_pdo_print(fsm); ec_fsm_pdo_conf_action_next_sync(fsm); return; } @@ -604,8 +599,7 @@ ec_sdo_request_address(&fsm->request, 0x1C10 + fsm->sync_index, 0); ecrt_sdo_request_write(&fsm->request); - if (fsm->slave->master->debug_level) - EC_DBG("Setting number of assigned PDOs to zero.\n"); + EC_SLAVE_DBG(fsm->slave, 1, "Setting number of assigned PDOs to zero.\n"); fsm->state = ec_fsm_pdo_conf_state_zero_pdo_count; ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request); @@ -624,8 +618,9 @@ return; if (!ec_fsm_coe_success(fsm->fsm_coe)) { - EC_WARN("Failed to clear PDO assignment of SM%u.\n", fsm->sync_index); - EC_WARN(""); ec_fsm_pdo_print(fsm); + EC_SLAVE_WARN(fsm->slave, "Failed to clear PDO assignment of SM%u.\n", + fsm->sync_index); + EC_SLAVE_WARN(fsm->slave, ""); ec_fsm_pdo_print(fsm); fsm->state = ec_fsm_pdo_state_error; return; } @@ -637,9 +632,7 @@ // find first PDO if (!(fsm->pdo = ec_fsm_pdo_conf_action_next_pdo(fsm, &fsm->pdos.list))) { - - if (fsm->slave->master->debug_level) - EC_DBG("No PDOs to assign.\n"); + EC_SLAVE_DBG(fsm->slave, 1, "No PDOs to assign.\n"); // check for mapping to be altered ec_fsm_pdo_conf_action_next_sync(fsm); @@ -665,9 +658,8 @@ 0x1C10 + fsm->sync_index, fsm->pdo_pos); ecrt_sdo_request_write(&fsm->request); - if (fsm->slave->master->debug_level) - EC_DBG("Assigning PDO 0x%04X at position %u.\n", - fsm->pdo->index, fsm->pdo_pos); + EC_SLAVE_DBG(fsm->slave, 1, "Assigning PDO 0x%04X at position %u.\n", + fsm->pdo->index, fsm->pdo_pos); fsm->state = ec_fsm_pdo_conf_state_assign_pdo; ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request); @@ -685,9 +677,10 @@ if (ec_fsm_coe_exec(fsm->fsm_coe)) return; if (!ec_fsm_coe_success(fsm->fsm_coe)) { - EC_WARN("Failed to assign PDO 0x%04X at position %u of SM%u.\n", + EC_SLAVE_WARN(fsm->slave, "Failed to assign PDO 0x%04X at position %u" + " of SM%u.\n", fsm->pdo->index, fsm->pdo_pos, fsm->sync_index); - EC_WARN(""); ec_fsm_pdo_print(fsm); + EC_SLAVE_WARN(fsm->slave, ""); ec_fsm_pdo_print(fsm); fsm->state = ec_fsm_pdo_state_error; return; } @@ -701,8 +694,9 @@ ec_sdo_request_address(&fsm->request, 0x1C10 + fsm->sync_index, 0); ecrt_sdo_request_write(&fsm->request); - if (fsm->slave->master->debug_level) - EC_DBG("Setting number of assigned PDOs to %u.\n", fsm->pdo_pos); + EC_SLAVE_DBG(fsm->slave, 1, + "Setting number of assigned PDOs to %u.\n", + fsm->pdo_pos); fsm->state = ec_fsm_pdo_conf_state_set_pdo_count; ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request); @@ -726,9 +720,9 @@ if (ec_fsm_coe_exec(fsm->fsm_coe)) return; if (!ec_fsm_coe_success(fsm->fsm_coe)) { - EC_WARN("Failed to set number of assigned PDOs of SM%u.\n", - fsm->sync_index); - EC_WARN(""); ec_fsm_pdo_print(fsm); + EC_SLAVE_WARN(fsm->slave, "Failed to set number of" + " assigned PDOs of SM%u.\n", fsm->sync_index); + EC_SLAVE_WARN(fsm->slave, ""); ec_fsm_pdo_print(fsm); fsm->state = ec_fsm_pdo_state_error; return; } @@ -736,9 +730,8 @@ // PDOs have been configured ec_pdo_list_copy(&fsm->sync->pdos, &fsm->pdos); - if (fsm->slave->master->debug_level) - EC_DBG("Successfully configured PDO assignment of SM%u.\n", - fsm->sync_index); + EC_SLAVE_DBG(fsm->slave, 1, "Successfully configured" + " PDO assignment of SM%u.\n", fsm->sync_index); // check if PDO mapping has to be altered ec_fsm_pdo_conf_action_next_sync(fsm); diff -r ea38efeeb7b3 -r 6aa393418fb3 master/fsm_pdo_entry.c --- a/master/fsm_pdo_entry.c Fri May 07 15:26:26 2010 +0200 +++ b/master/fsm_pdo_entry.c Tue May 11 13:57:58 2010 +0200 @@ -131,8 +131,9 @@ fsm->cur_pdo = cur_pdo; if (fsm->slave->master->debug_level) { - EC_DBG("Changing mapping of PDO 0x%04X.\n", pdo->index); - EC_DBG(""); ec_fsm_pdo_entry_print(fsm); + EC_SLAVE_DBG(slave, 1, "Changing mapping of PDO 0x%04X.\n", + pdo->index); + EC_SLAVE_DBG(slave, 1, ""); ec_fsm_pdo_entry_print(fsm); } fsm->state = ec_fsm_pdo_entry_conf_state_start; @@ -209,13 +210,15 @@ return; if (!ec_fsm_coe_success(fsm->fsm_coe)) { - EC_ERR("Failed to read number of mapped PDO entries.\n"); + EC_SLAVE_ERR(fsm->slave, + "Failed to read number of mapped PDO entries.\n"); fsm->state = ec_fsm_pdo_entry_state_error; return; } if (fsm->request.data_size != sizeof(uint8_t)) { - EC_ERR("Invalid data size %zu at uploading SDO 0x%04X:%02X.\n", + EC_SLAVE_ERR(fsm->slave, "Invalid data size %zu at uploading" + " SDO 0x%04X:%02X.\n", fsm->request.data_size, fsm->request.index, fsm->request.subindex); fsm->state = ec_fsm_pdo_entry_state_error; @@ -224,8 +227,7 @@ fsm->entry_count = EC_READ_U8(fsm->request.data); - if (fsm->slave->master->debug_level) - EC_DBG("%u PDO entries mapped.\n", fsm->entry_count); + EC_SLAVE_DBG(fsm->slave, 1, "%u PDO entries mapped.\n", fsm->entry_count); // read first PDO entry fsm->entry_pos = 1; @@ -264,13 +266,14 @@ if (ec_fsm_coe_exec(fsm->fsm_coe)) return; if (!ec_fsm_coe_success(fsm->fsm_coe)) { - EC_ERR("Failed to read mapped PDO entry.\n"); + EC_SLAVE_ERR(fsm->slave, "Failed to read mapped PDO entry.\n"); fsm->state = ec_fsm_pdo_entry_state_error; return; } if (fsm->request.data_size != sizeof(uint32_t)) { - EC_ERR("Invalid data size %zu at uploading SDO 0x%04X:%02X.\n", + EC_SLAVE_ERR(fsm->slave, "Invalid data size %zu at" + " uploading SDO 0x%04X:%02X.\n", fsm->request.data_size, fsm->request.index, fsm->request.subindex); fsm->state = ec_fsm_pdo_entry_state_error; @@ -282,7 +285,7 @@ if (!(pdo_entry = (ec_pdo_entry_t *) kmalloc(sizeof(ec_pdo_entry_t), GFP_KERNEL))) { - EC_ERR("Failed to allocate PDO entry.\n"); + EC_SLAVE_ERR(fsm->slave, "Failed to allocate PDO entry.\n"); fsm->state = ec_fsm_pdo_entry_state_error; return; } @@ -301,12 +304,11 @@ } } - if (fsm->slave->master->debug_level) { - EC_DBG("PDO entry 0x%04X:%02X, %u bit, \"%s\".\n", - pdo_entry->index, pdo_entry->subindex, - pdo_entry->bit_length, - pdo_entry->name ? pdo_entry->name : "???"); - } + EC_SLAVE_DBG(fsm->slave, 1, + "PDO entry 0x%04X:%02X, %u bit, \"%s\".\n", + pdo_entry->index, pdo_entry->subindex, + pdo_entry->bit_length, + pdo_entry->name ? pdo_entry->name : "???"); list_add_tail(&pdo_entry->list, &fsm->target_pdo->entries); @@ -330,9 +332,9 @@ if (!(fsm->slave->sii.mailbox_protocols & EC_MBOX_COE) || (fsm->slave->sii.has_general && !fsm->slave->sii.coe_details.enable_pdo_configuration)) { - EC_WARN("Slave %u does not support changing the PDO mapping!\n", - fsm->slave->ring_position); - EC_WARN(""); ec_fsm_pdo_entry_print(fsm); + EC_SLAVE_WARN(fsm->slave, "Slave does not support" + " changing the PDO mapping!\n"); + EC_SLAVE_WARN(fsm->slave, ""); ec_fsm_pdo_entry_print(fsm); fsm->state = ec_fsm_pdo_entry_state_error; return; } @@ -348,8 +350,7 @@ ec_sdo_request_address(&fsm->request, fsm->source_pdo->index, 0); ecrt_sdo_request_write(&fsm->request); - if (fsm->slave->master->debug_level) - EC_DBG("Setting entry count to zero.\n"); + EC_SLAVE_DBG(fsm->slave, 1, "Setting entry count to zero.\n"); fsm->state = ec_fsm_pdo_entry_conf_state_zero_entry_count; ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request); @@ -383,8 +384,8 @@ return; if (!ec_fsm_coe_success(fsm->fsm_coe)) { - EC_WARN("Failed to clear PDO mapping.\n"); - EC_WARN(""); ec_fsm_pdo_entry_print(fsm); + EC_SLAVE_WARN(fsm->slave, "Failed to clear PDO mapping.\n"); + EC_SLAVE_WARN(fsm->slave, ""); ec_fsm_pdo_entry_print(fsm); fsm->state = ec_fsm_pdo_entry_state_error; return; } @@ -393,8 +394,7 @@ if (!(fsm->entry = ec_fsm_pdo_entry_conf_next_entry( fsm, &fsm->source_pdo->entries))) { - if (fsm->slave->master->debug_level) - EC_DBG("No entries to map.\n"); + EC_SLAVE_DBG(fsm->slave, 1, "No entries to map.\n"); fsm->state = ec_fsm_pdo_entry_state_end; // finished return; @@ -415,10 +415,10 @@ { uint32_t value; - if (fsm->slave->master->debug_level) - EC_DBG("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); + 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; @@ -443,10 +443,11 @@ if (ec_fsm_coe_exec(fsm->fsm_coe)) return; if (!ec_fsm_coe_success(fsm->fsm_coe)) { - EC_WARN("Failed to map PDO entry 0x%04X:%02X (%u bit) to " - "position %u.\n", fsm->entry->index, fsm->entry->subindex, + EC_SLAVE_WARN(fsm->slave, "Failed to map PDO entry" + " 0x%04X:%02X (%u bit) to position %u.\n", + fsm->entry->index, fsm->entry->subindex, fsm->entry->bit_length, fsm->entry_pos); - EC_WARN(""); ec_fsm_pdo_entry_print(fsm); + EC_SLAVE_WARN(fsm->slave, ""); ec_fsm_pdo_entry_print(fsm); fsm->state = ec_fsm_pdo_entry_state_error; return; } @@ -461,8 +462,8 @@ ec_sdo_request_address(&fsm->request, fsm->source_pdo->index, 0); ecrt_sdo_request_write(&fsm->request); - if (fsm->slave->master->debug_level) - EC_DBG("Setting number of PDO entries to %u.\n", fsm->entry_pos); + EC_SLAVE_DBG(fsm->slave, 1, "Setting number of PDO entries to %u.\n", + fsm->entry_pos); fsm->state = ec_fsm_pdo_entry_conf_state_set_entry_count; ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request); @@ -486,15 +487,14 @@ if (ec_fsm_coe_exec(fsm->fsm_coe)) return; if (!ec_fsm_coe_success(fsm->fsm_coe)) { - EC_WARN("Failed to set number of entries.\n"); - EC_WARN(""); ec_fsm_pdo_entry_print(fsm); - fsm->state = ec_fsm_pdo_entry_state_error; - return; - } - - if (fsm->slave->master->debug_level) - EC_DBG("Successfully configured mapping for PDO 0x%04X.\n", - fsm->source_pdo->index); + EC_SLAVE_WARN(fsm->slave, "Failed to set number of entries.\n"); + EC_SLAVE_WARN(fsm->slave, ""); ec_fsm_pdo_entry_print(fsm); + fsm->state = ec_fsm_pdo_entry_state_error; + return; + } + + EC_SLAVE_DBG(fsm->slave, 1, "Successfully configured" + " mapping for PDO 0x%04X.\n", fsm->source_pdo->index); fsm->state = ec_fsm_pdo_entry_state_end; // finished } diff -r ea38efeeb7b3 -r 6aa393418fb3 master/fsm_sii.c --- a/master/fsm_sii.c Fri May 07 15:26:26 2010 +0200 +++ b/master/fsm_sii.c Tue May 11 13:57:58 2010 +0200 @@ -184,7 +184,7 @@ EC_WRITE_U16(datagram->data + 2, fsm->word_offset); #ifdef SII_DEBUG - EC_DBG("reading SII data:\n"); + EC_SLAVE_DBG(slave, 0, "reading SII data:\n"); ec_print_data(datagram->data, 4); #endif @@ -210,16 +210,14 @@ if (datagram->state != EC_DATAGRAM_RECEIVED) { fsm->state = ec_fsm_sii_state_error; - EC_ERR("Failed to receive SII read datagram from slave %u: ", - fsm->slave->ring_position); + EC_SLAVE_ERR(fsm->slave, "Failed to receive SII read datagram: "); ec_datagram_print_state(datagram); return; } if (datagram->working_counter != 1) { fsm->state = ec_fsm_sii_state_error; - EC_ERR("Reception of SII read datagram failed on slave %u: ", - fsm->slave->ring_position); + EC_SLAVE_ERR(fsm->slave, "Reception of SII read datagram failed: "); ec_datagram_print_wc_error(datagram); return; } @@ -259,27 +257,27 @@ if (datagram->state != EC_DATAGRAM_RECEIVED) { fsm->state = ec_fsm_sii_state_error; - EC_ERR("Failed to receive SII check/fetch datagram from slave %u: ", - fsm->slave->ring_position); + EC_SLAVE_ERR(fsm->slave, + "Failed to receive SII check/fetch datagram: "); ec_datagram_print_state(datagram); return; } if (datagram->working_counter != 1) { fsm->state = ec_fsm_sii_state_error; - EC_ERR("Reception of SII check/fetch datagram failed on slave %u: ", - fsm->slave->ring_position); + EC_SLAVE_ERR(fsm->slave, + "Reception of SII check/fetch datagram failed: "); ec_datagram_print_wc_error(datagram); return; } #ifdef SII_DEBUG - EC_DBG("checking SII read state:\n"); + EC_SLAVE_DBG(fsm->slave, 0, "checking SII read state:\n"); ec_print_data(datagram->data, 10); #endif if (EC_READ_U8(datagram->data + 1) & 0x20) { - EC_ERR("SII: Error on last SII command!\n"); + EC_SLAVE_ERR(fsm->slave, "SII: Error on last SII command!\n"); fsm->state = ec_fsm_sii_state_error; return; } @@ -294,7 +292,7 @@ if (fsm->check_once_more) { fsm->check_once_more = 0; } else { - EC_ERR("SII: Read timeout.\n"); + EC_SLAVE_ERR(fsm->slave, "SII: Read timeout.\n"); fsm->state = ec_fsm_sii_state_error; return; } @@ -333,7 +331,7 @@ memcpy(datagram->data + 6, fsm->value, 2); #ifdef SII_DEBUG - EC_DBG("writing SII data:\n"); + EC_SLAVE_DBG(fsm->slave, 0, "writing SII data:\n"); ec_print_data(datagram->data, 8); #endif @@ -358,16 +356,14 @@ if (datagram->state != EC_DATAGRAM_RECEIVED) { fsm->state = ec_fsm_sii_state_error; - EC_ERR("Failed to receive SII write datagram for slave %u: ", - fsm->slave->ring_position); + EC_SLAVE_ERR(fsm->slave, "Failed to receive SII write datagram: "); ec_datagram_print_state(datagram); return; } if (datagram->working_counter != 1) { fsm->state = ec_fsm_sii_state_error; - EC_ERR("Reception of SII write datagram failed on slave %u: ", - fsm->slave->ring_position); + EC_SLAVE_ERR(fsm->slave, "Reception of SII write datagram failed: "); ec_datagram_print_wc_error(datagram); return; } @@ -400,27 +396,27 @@ if (datagram->state != EC_DATAGRAM_RECEIVED) { fsm->state = ec_fsm_sii_state_error; - EC_ERR("Failed to receive SII write check datagram from slave %u: ", - fsm->slave->ring_position); + EC_SLAVE_ERR(fsm->slave, + "Failed to receive SII write check datagram: "); ec_datagram_print_state(datagram); return; } if (datagram->working_counter != 1) { fsm->state = ec_fsm_sii_state_error; - EC_ERR("Reception of SII write check datagram failed on slave %u: ", - fsm->slave->ring_position); + EC_SLAVE_ERR(fsm->slave, + "Reception of SII write check datagram failed: "); ec_datagram_print_wc_error(datagram); return; } #ifdef SII_DEBUG - EC_DBG("checking SII write state:\n"); + EC_SLAVE_DBG(fsm->slave, 0, "checking SII write state:\n"); ec_print_data(datagram->data, 2); #endif if (EC_READ_U8(datagram->data + 1) & 0x20) { - EC_ERR("SII: Error on last SII command!\n"); + EC_SLAVE_ERR(fsm->slave, "SII: Error on last SII command!\n"); fsm->state = ec_fsm_sii_state_error; return; } @@ -430,7 +426,7 @@ diff_ms = (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; if (diff_ms < SII_INHIBIT) { #ifdef SII_DEBUG - EC_DBG("too early.\n"); + EC_SLAVE_DBG(fsm->slave, 0, "too early.\n"); #endif // issue check datagram again fsm->retries = EC_FSM_RETRIES; @@ -444,7 +440,7 @@ if (fsm->check_once_more) { fsm->check_once_more = 0; } else { - EC_ERR("SII: Write timeout.\n"); + EC_SLAVE_ERR(fsm->slave, "SII: Write timeout.\n"); fsm->state = ec_fsm_sii_state_error; return; } @@ -456,7 +452,7 @@ } if (EC_READ_U8(datagram->data + 1) & 0x40) { - EC_ERR("SII: Write operation failed!\n"); + EC_SLAVE_ERR(fsm->slave, "SII: Write operation failed!\n"); fsm->state = ec_fsm_sii_state_error; return; } diff -r ea38efeeb7b3 -r 6aa393418fb3 master/fsm_slave.c --- a/master/fsm_slave.c Fri May 07 15:26:26 2010 +0200 +++ b/master/fsm_slave.c Tue May 11 13:57:58 2010 +0200 @@ -64,8 +64,7 @@ fsm->datagram = datagram; fsm->datagram->data_size = 0; - if (slave->master->debug_level) - EC_DBG("Init FSM for slave %u...\n", slave->ring_position); + EC_SLAVE_DBG(slave, 1, "Init FSM.\n"); fsm->state = ec_fsm_slave_state_idle; @@ -119,10 +118,7 @@ ) { if (fsm->state == ec_fsm_slave_state_idle) { - if (fsm->slave->master->debug_level) { - EC_DBG("Slave %u ready for requests.\n", - fsm->slave->ring_position); - } + EC_SLAVE_DBG(fsm->slave, 1, "Ready for requests.\n"); fsm->state = ec_fsm_slave_state_ready; } } @@ -173,7 +169,6 @@ ) { ec_slave_t *slave = fsm->slave; - ec_master_t *master = slave->master; ec_master_sdo_request_t *request, *next; // search the first external request to be processed @@ -181,8 +176,8 @@ list_del_init(&request->list); // dequeue if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) { - EC_WARN("Aborting SDO request, slave %u has ERROR.\n", - slave->ring_position); + EC_SLAVE_WARN(slave, "Aborting SDO request," + " slave has error flag set.\n"); request->req.state = EC_INT_REQUEST_FAILURE; wake_up(&slave->sdo_queue); fsm->sdo_request = NULL; @@ -191,8 +186,7 @@ } if (slave->current_state == EC_SLAVE_STATE_INIT) { - EC_WARN("Aborting SDO request, slave %u is in INIT.\n", - slave->ring_position); + EC_SLAVE_WARN(slave, "Aborting SDO request, slave is in INIT.\n"); request->req.state = EC_INT_REQUEST_FAILURE; wake_up(&slave->sdo_queue); fsm->sdo_request = NULL; @@ -203,9 +197,7 @@ request->req.state = EC_INT_REQUEST_BUSY; // Found pending SDO request. Execute it! - if (master->debug_level) - EC_DBG("Processing SDO request for slave %u...\n", - slave->ring_position); + EC_SLAVE_DBG(slave, 1, "Processing SDO request...\n"); // Start SDO transfer fsm->sdo_request = &request->req; @@ -227,7 +219,6 @@ ) { ec_slave_t *slave = fsm->slave; - ec_master_t *master = slave->master; ec_sdo_request_t *request = fsm->sdo_request; if (ec_fsm_coe_exec(&fsm->fsm_coe)) @@ -236,8 +227,7 @@ return; } if (!ec_fsm_coe_success(&fsm->fsm_coe)) { - EC_ERR("Failed to process SDO request for slave %u.\n", - fsm->slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to process SDO request.\n"); request->state = EC_INT_REQUEST_FAILURE; wake_up(&slave->sdo_queue); fsm->sdo_request = NULL; @@ -245,9 +235,7 @@ return; } - if (master->debug_level) - EC_DBG("Finished SDO request for slave %u.\n", - fsm->slave->ring_position); + EC_SLAVE_DBG(slave, 1, "Finished SDO request.\n"); // SDO request finished request->state = EC_INT_REQUEST_SUCCESS; @@ -268,14 +256,13 @@ ) { ec_slave_t *slave = fsm->slave; - ec_master_t *master = slave->master; ec_master_foe_request_t *request, *next; // search the first request to be processed list_for_each_entry_safe(request, next, &slave->foe_requests, list) { if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) { - EC_WARN("Aborting FOE request, slave %u has ERROR.\n", - slave->ring_position); + EC_SLAVE_WARN(slave, "Aborting FOE request," + " slave has error flag set.\n"); request->req.state = EC_INT_REQUEST_FAILURE; wake_up(&slave->sdo_queue); fsm->sdo_request = NULL; @@ -285,9 +272,7 @@ list_del_init(&request->list); // dequeue request->req.state = EC_INT_REQUEST_BUSY; - if (master->debug_level) - EC_DBG("Processing FOE request for slave %u.\n", - slave->ring_position); + EC_SLAVE_DBG(slave, 1, "Processing FoE request.\n"); fsm->foe_request = &request->req; fsm->state = ec_fsm_slave_state_foe_request; @@ -308,7 +293,6 @@ ) { ec_slave_t *slave = fsm->slave; - ec_master_t *master = slave->master; ec_foe_request_t *request = fsm->foe_request; if (ec_fsm_foe_exec(&fsm->fsm_foe)) @@ -318,8 +302,7 @@ } if (!ec_fsm_foe_success(&fsm->fsm_foe)) { - EC_ERR("Failed to handle FoE request to slave %u.\n", - slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to handle FoE request.\n"); request->state = EC_INT_REQUEST_FAILURE; wake_up(&slave->foe_queue); fsm->foe_request = NULL; @@ -328,9 +311,8 @@ } // finished transferring FoE - if (master->debug_level) - EC_DBG("Successfully transferred %u bytes of FoE data from/to" - " slave %u.\n", request->data_size, slave->ring_position); + EC_SLAVE_DBG(slave, 1, "Successfully transferred %u bytes of FoE" + " data.\n", request->data_size); request->state = EC_INT_REQUEST_SUCCESS; wake_up(&slave->foe_queue); @@ -350,7 +332,6 @@ ) { ec_slave_t *slave = fsm->slave; - ec_master_t *master = slave->master; ec_master_soe_request_t *req, *next; // search the first request to be processed @@ -358,8 +339,8 @@ list_del_init(&req->list); // dequeue if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) { - EC_WARN("Aborting SoE request, slave %u has ERROR.\n", - slave->ring_position); + EC_SLAVE_WARN(slave, "Aborting SoE request," + " slave has error flag set.\n"); req->req.state = EC_INT_REQUEST_FAILURE; wake_up(&slave->soe_queue); fsm->state = ec_fsm_slave_state_idle; @@ -367,8 +348,7 @@ } if (slave->current_state == EC_SLAVE_STATE_INIT) { - EC_WARN("Aborting SoE request, slave %u is in INIT.\n", - slave->ring_position); + EC_SLAVE_WARN(slave, "Aborting SoE request, slave is in INIT.\n"); req->req.state = EC_INT_REQUEST_FAILURE; wake_up(&slave->soe_queue); fsm->state = ec_fsm_slave_state_idle; @@ -378,9 +358,7 @@ req->req.state = EC_INT_REQUEST_BUSY; // Found pending request. Execute it! - if (master->debug_level) - EC_DBG("Processing SoE request for slave %u...\n", - slave->ring_position); + EC_SLAVE_DBG(slave, 1, "Processing SoE request...\n"); // Start SoE transfer fsm->soe_request = &req->req; @@ -402,7 +380,6 @@ ) { ec_slave_t *slave = fsm->slave; - ec_master_t *master = slave->master; ec_soe_request_t *request = fsm->soe_request; if (ec_fsm_soe_exec(&fsm->fsm_soe)) { @@ -411,8 +388,7 @@ } if (!ec_fsm_soe_success(&fsm->fsm_soe)) { - EC_ERR("Failed to process SoE request for slave %u.\n", - fsm->slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to process SoE request.\n"); request->state = EC_INT_REQUEST_FAILURE; wake_up(&slave->soe_queue); fsm->soe_request = NULL; @@ -420,9 +396,7 @@ return; } - if (master->debug_level) - EC_DBG("Finished SoE request for slave %u.\n", - fsm->slave->ring_position); + EC_SLAVE_DBG(slave, 1, "Finished SoE request.\n"); // SoE request finished request->state = EC_INT_REQUEST_SUCCESS; diff -r ea38efeeb7b3 -r 6aa393418fb3 master/fsm_slave_config.c --- a/master/fsm_slave_config.c Fri May 07 15:26:26 2010 +0200 +++ b/master/fsm_slave_config.c Tue May 11 13:57:58 2010 +0200 @@ -44,9 +44,20 @@ /*****************************************************************************/ -/** Time difference [ns] to tolerate without setting a new system time offset. - */ -#define EC_SYSTEM_TIME_TOLERANCE_NS 100000000 +/** Maximum clock difference (in ns) before going to SAFEOP. + * + * Wait for DC time difference to drop under this absolute value before + * requesting SAFEOP. + */ +#define EC_DC_MAX_SYNC_DIFF_NS 5000 + +/** Maximum time (in ms) to wait for clock discipline. + */ +#define EC_DC_SYNC_WAIT_MS 5000 + +/** Time offset (in ns), that is added to cyclic start time. + */ +#define EC_DC_START_OFFSET 100000000ULL /*****************************************************************************/ @@ -55,8 +66,6 @@ void ec_fsm_slave_config_state_clear_fmmus(ec_fsm_slave_config_t *); void ec_fsm_slave_config_state_clear_sync(ec_fsm_slave_config_t *); void ec_fsm_slave_config_state_dc_clear_assign(ec_fsm_slave_config_t *); -void ec_fsm_slave_config_state_dc_read_offset(ec_fsm_slave_config_t *); -void ec_fsm_slave_config_state_dc_write_offset(ec_fsm_slave_config_t *); void ec_fsm_slave_config_state_mbox_sync(ec_fsm_slave_config_t *); void ec_fsm_slave_config_state_boot_preop(ec_fsm_slave_config_t *); void ec_fsm_slave_config_state_sdo_conf(ec_fsm_slave_config_t *); @@ -67,6 +76,7 @@ void ec_fsm_slave_config_state_pdo_conf(ec_fsm_slave_config_t *); void ec_fsm_slave_config_state_fmmu(ec_fsm_slave_config_t *); void ec_fsm_slave_config_state_dc_cycle(ec_fsm_slave_config_t *); +void ec_fsm_slave_config_state_dc_sync_check(ec_fsm_slave_config_t *); void ec_fsm_slave_config_state_dc_start(ec_fsm_slave_config_t *); void ec_fsm_slave_config_state_dc_assign(ec_fsm_slave_config_t *); void ec_fsm_slave_config_state_safeop(ec_fsm_slave_config_t *); @@ -196,10 +206,7 @@ ec_fsm_slave_config_t *fsm /**< slave state machine */ ) { - if (fsm->slave->master->debug_level) { - EC_DBG("Configuring slave %u...\n", fsm->slave->ring_position); - } - + EC_SLAVE_DBG(fsm->slave, 1, "Configuring...\n"); ec_fsm_slave_config_enter_init(fsm); } @@ -224,7 +231,6 @@ ec_fsm_slave_config_t *fsm /**< slave state machine */ ) { - ec_master_t *master = fsm->slave->master; ec_slave_t *slave = fsm->slave; ec_datagram_t *datagram = fsm->datagram; @@ -237,18 +243,14 @@ return; } - if (master->debug_level) { - EC_DBG("Slave %u is now in INIT.\n", slave->ring_position); - } + EC_SLAVE_DBG(slave, 1, "Now in INIT.\n"); if (!slave->base_fmmu_count) { // skip FMMU configuration ec_fsm_slave_config_enter_clear_sync(fsm); return; } - if (master->debug_level) - EC_DBG("Clearing FMMU configurations of slave %u...\n", - slave->ring_position); + EC_SLAVE_DBG(slave, 1, "Clearing FMMU configurations...\n"); // clear FMMU configurations ec_datagram_fpwr(datagram, slave->station_address, @@ -273,16 +275,14 @@ if (datagram->state != EC_DATAGRAM_RECEIVED) { fsm->state = ec_fsm_slave_config_state_error; - EC_ERR("Failed receive FMMU clearing datagram for slave %u.\n", - fsm->slave->ring_position); + EC_SLAVE_ERR(fsm->slave, "Failed receive FMMU clearing datagram.\n"); return; } if (datagram->working_counter != 1) { fsm->slave->error_flag = 1; fsm->state = ec_fsm_slave_config_state_error; - EC_ERR("Failed to clear FMMUs of slave %u: ", - fsm->slave->ring_position); + EC_SLAVE_ERR(fsm->slave, "Failed to clear FMMUs: "); ec_datagram_print_wc_error(datagram); return; } @@ -308,9 +308,7 @@ return; } - if (slave->master->debug_level) - EC_DBG("Clearing sync manager configurations of slave %u...\n", - slave->ring_position); + EC_SLAVE_DBG(slave, 1, "Clearing sync manager configurations...\n"); sync_size = EC_SYNC_PAGE_SIZE * slave->base_sync_count; @@ -336,16 +334,16 @@ if (datagram->state != EC_DATAGRAM_RECEIVED) { fsm->state = ec_fsm_slave_config_state_error; - EC_ERR("Failed receive sync manager clearing datagram" - " for slave %u.\n", fsm->slave->ring_position); + EC_SLAVE_ERR(fsm->slave, "Failed receive sync manager" + " clearing datagram.\n"); return; } if (datagram->working_counter != 1) { fsm->slave->error_flag = 1; fsm->state = ec_fsm_slave_config_state_error; - EC_ERR("Failed to clear sync manager configurations of slave %u: ", - fsm->slave->ring_position); + EC_SLAVE_ERR(fsm->slave, + "Failed to clear sync manager configurations: "); ec_datagram_print_wc_error(datagram); return; } @@ -369,9 +367,7 @@ return; } - if (slave->master->debug_level) - EC_DBG("Clearing DC assignment of slave %u...\n", - slave->ring_position); + EC_SLAVE_DBG(slave, 1, "Clearing DC assignment...\n"); ec_datagram_fpwr(datagram, slave->station_address, 0x0980, 2); ec_datagram_zero(datagram); @@ -394,202 +390,17 @@ if (datagram->state != EC_DATAGRAM_RECEIVED) { fsm->state = ec_fsm_slave_config_state_error; - EC_ERR("Failed receive DC assignment clearing datagram" - " for slave %u.\n", fsm->slave->ring_position); - return; - } - - if (fsm->slave->master->debug_level && datagram->working_counter != 1) { + EC_SLAVE_ERR(fsm->slave, "Failed receive DC assignment" + " clearing datagram.\n"); + return; + } + + if (datagram->working_counter != 1) { // clearing the DC assignment does not succeed on simple slaves - EC_DBG("Failed to clear DC assignment of slave %u: ", - fsm->slave->ring_position); + EC_SLAVE_DBG(fsm->slave, 1, "Failed to clear DC assignment: "); ec_datagram_print_wc_error(datagram); } - // read DC system time (0x0910, 64 bit) - // gap (64 bit) - // and time offset (0x0920, 64 bit) - ec_datagram_fprd(fsm->datagram, fsm->slave->station_address, 0x0910, 24); - fsm->retries = EC_FSM_RETRIES; - fsm->state = ec_fsm_slave_config_state_dc_read_offset; -} - -/*****************************************************************************/ - -/** Configure 32 bit time offset. - */ -u64 ec_fsm_slave_config_dc_offset32( - ec_fsm_slave_config_t *fsm, /**< slave state machine */ - u64 system_time, /**< System time register. */ - u64 old_offset, /**< Time offset register. */ - u64 correction /**< Correction. */ - ) -{ - ec_slave_t *slave = fsm->slave; - u32 correction32, system_time32, old_offset32, new_offset; - s32 time_diff; - - system_time32 = (u32) system_time; - old_offset32 = (u32) old_offset; - - // correct read system time by elapsed time since read operation - correction32 = (u32)correction; - system_time32 -= correction32; - time_diff = (u32) slave->master->app_time - system_time32; - - if (slave->master->debug_level) - EC_DBG("Slave %u: system_time=%u (corrected with %u)," - " app_time=%u, diff=%i\n", - slave->ring_position, system_time32, correction32, - (u32) slave->master->app_time, time_diff); - - if (EC_ABS(time_diff) > EC_SYSTEM_TIME_TOLERANCE_NS) { - new_offset = time_diff + old_offset32; - if (slave->master->debug_level) - EC_DBG("Slave %u: Setting time offset to %u (was %u)\n", - slave->ring_position, new_offset, old_offset32); - return (u64) new_offset; - } else { - if (slave->master->debug_level) - EC_DBG("Slave %u: Not touching time offset.\n", - slave->ring_position); - return old_offset; - } -} - -/*****************************************************************************/ - -/** Configure 64 bit time offset. - */ -u64 ec_fsm_slave_config_dc_offset64( - ec_fsm_slave_config_t *fsm, /**< slave state machine */ - u64 system_time, /**< System time register. */ - u64 old_offset, /**< Time offset register. */ - u64 correction /**< Correction. */ - ) -{ - ec_slave_t *slave = fsm->slave; - u64 new_offset; - s64 time_diff; - - system_time -= correction; - time_diff = fsm->slave->master->app_time - system_time; - - if (slave->master->debug_level) - EC_DBG("Slave %u: system_time=%llu (corrected with %llu)," - " app_time=%llu, diff=%lli\n", - slave->ring_position, system_time, correction, - slave->master->app_time, time_diff); - - - if (EC_ABS(time_diff) > EC_SYSTEM_TIME_TOLERANCE_NS) { - new_offset = time_diff + old_offset; - if (slave->master->debug_level) - EC_DBG("Slave %u: Setting time offset to %llu (was %llu)\n", - slave->ring_position, new_offset, old_offset); - } else { - new_offset = old_offset; - if (slave->master->debug_level) - EC_DBG("Slave %u: Not touching time offset.\n", - slave->ring_position); - } - - return new_offset; -} - -/*****************************************************************************/ - -/** Slave configuration state: DC READ OFFSET. - */ -void ec_fsm_slave_config_state_dc_read_offset( - ec_fsm_slave_config_t *fsm /**< slave state machine */ - ) -{ - ec_datagram_t *datagram = fsm->datagram; - ec_slave_t *slave = fsm->slave; - u64 system_time, old_offset, new_offset, correction; - if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) - return; - - if (datagram->state != EC_DATAGRAM_RECEIVED) { - fsm->state = ec_fsm_slave_config_state_error; - EC_ERR("Failed to receive DC times datagram for slave %u: ", - slave->ring_position); - ec_datagram_print_state(datagram); - return; - } - - if (datagram->working_counter != 1) { - slave->error_flag = 1; - fsm->state = ec_fsm_slave_config_state_error; - EC_ERR("Failed to get DC times of slave %u: ", - slave->ring_position); - ec_datagram_print_wc_error(datagram); - return; - } - - system_time = EC_READ_U64(datagram->data); // 0x0910 - old_offset = EC_READ_U64(datagram->data + 16); // 0x0920 - /* correct read system time by elapsed time since read operation - and the app_time set time */ -#ifdef EC_HAVE_CYCLES - correction = - (datagram->cycles_sent - slave->master->dc_cycles_app_time) - * 1000000LL; - do_div(correction,cpu_khz); -#else - correction = - (u64) ((datagram->jiffies_sent-slave->master->dc_jiffies_app_time) * 1000 / HZ) - * 1000000; -#endif - - if (slave->base_dc_range == EC_DC_32) { - new_offset = ec_fsm_slave_config_dc_offset32(fsm, - system_time, old_offset, correction); - } else { - new_offset = ec_fsm_slave_config_dc_offset64(fsm, - system_time, old_offset, correction); - } - - // set DC system time offset and transmission delay - ec_datagram_fpwr(datagram, slave->station_address, 0x0920, 12); - EC_WRITE_U64(datagram->data, new_offset); - EC_WRITE_U32(datagram->data + 8, slave->transmission_delay); - fsm->retries = EC_FSM_RETRIES; - fsm->state = ec_fsm_slave_config_state_dc_write_offset; -} - -/*****************************************************************************/ - -/** Slave configuration state: DC WRITE OFFSET. - */ -void ec_fsm_slave_config_state_dc_write_offset( - ec_fsm_slave_config_t *fsm /**< slave state machine */ - ) -{ - ec_datagram_t *datagram = fsm->datagram; - ec_slave_t *slave = fsm->slave; - - if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) - return; - - if (datagram->state != EC_DATAGRAM_RECEIVED) { - fsm->state = ec_fsm_slave_config_state_error; - EC_ERR("Failed to receive DC system time offset datagram for" - " slave %u: ", slave->ring_position); - ec_datagram_print_state(datagram); - return; - } - - if (datagram->working_counter != 1) { - slave->error_flag = 1; - fsm->state = ec_fsm_slave_config_state_error; - EC_ERR("Failed to set DC system time offset of slave %u: ", - slave->ring_position); - ec_datagram_print_wc_error(datagram); - return; - } - ec_fsm_slave_config_enter_mbox_sync(fsm); } @@ -601,7 +412,6 @@ ec_fsm_slave_config_t *fsm /**< slave state machine */ ) { - ec_master_t *master = fsm->slave->master; ec_slave_t *slave = fsm->slave; ec_datagram_t *datagram = fsm->datagram; unsigned int i; @@ -609,26 +419,19 @@ // slave is now in INIT if (slave->current_state == slave->requested_state) { fsm->state = ec_fsm_slave_config_state_end; // successful - if (master->debug_level) { - EC_DBG("Finished configuration of slave %u.\n", - slave->ring_position); - } + EC_SLAVE_DBG(slave, 1, "Finished configuration.\n"); return; } if (!slave->sii.mailbox_protocols) { // no mailbox protocols supported - if (master->debug_level) - EC_DBG("Slave %u does not support mailbox communication.\n", - slave->ring_position); + EC_SLAVE_DBG(slave, 1, "Slave does not support" + " mailbox communication.\n"); ec_fsm_slave_config_enter_boot_preop(fsm); return; } - if (master->debug_level) { - EC_DBG("Configuring mailbox sync managers of slave %u.\n", - slave->ring_position); - } + EC_SLAVE_DBG(slave, 1, "Configuring mailbox sync managers...\n"); if (slave->requested_state == EC_SLAVE_STATE_BOOT) { ec_sync_t sync; @@ -684,10 +487,8 @@ } else { // no mailbox sync manager configurations provided ec_sync_t sync; - if (master->debug_level) - EC_DBG("Slave %u does not provide" - " mailbox sync manager configurations.\n", - slave->ring_position); + EC_SLAVE_DBG(slave, 1, "Slave does not provide" + " mailbox sync manager configurations.\n"); ec_datagram_fpwr(datagram, slave->station_address, 0x0800, EC_SYNC_PAGE_SIZE * 2); @@ -742,8 +543,8 @@ if (datagram->state != EC_DATAGRAM_RECEIVED) { fsm->state = ec_fsm_slave_config_state_error; - EC_ERR("Failed to receive sync manager configuration datagram for" - " slave %u: ", slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to receive sync manager" + " configuration datagram: "); ec_datagram_print_state(datagram); return; } @@ -764,12 +565,11 @@ if (diff >= HZ) { slave->error_flag = 1; fsm->state = ec_fsm_slave_config_state_error; - EC_ERR("Timeout while configuring mailbox sync managers of" - " slave %u.\n", slave->ring_position); + EC_SLAVE_ERR(slave, "Timeout while configuring" + " mailbox sync managers.\n"); return; - } - else if (slave->master->debug_level) { - EC_DBG("Resending after %u ms...\n", + } else { + EC_SLAVE_DBG(slave, 1, "Resending after %u ms...\n", (unsigned int) diff * 1000 / HZ); } @@ -780,8 +580,7 @@ else if (datagram->working_counter != 1) { slave->error_flag = 1; fsm->state = ec_fsm_slave_config_state_error; - EC_ERR("Failed to set sync managers of slave %u: ", - slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to set sync managers: "); ec_datagram_print_wc_error(datagram); return; } @@ -817,7 +616,6 @@ ) { ec_slave_t *slave = fsm->slave; - ec_master_t *master = fsm->slave->master; if (ec_fsm_change_exec(fsm->fsm_change)) return; @@ -831,18 +629,12 @@ // slave is now in BOOT or PREOP slave->jiffies_preop = fsm->datagram->jiffies_received; - if (master->debug_level) { - EC_DBG("Slave %u is now in %s.\n", slave->ring_position, - slave->requested_state != EC_SLAVE_STATE_BOOT - ? "PREOP" : "BOOT"); - } + EC_SLAVE_DBG(slave, 1, "Now in %s.\n", + slave->requested_state != EC_SLAVE_STATE_BOOT ? "PREOP" : "BOOT"); if (slave->current_state == slave->requested_state) { fsm->state = ec_fsm_slave_config_state_end; // successful - if (master->debug_level) { - EC_DBG("Finished configuration of slave %u.\n", - slave->ring_position); - } + EC_SLAVE_DBG(slave, 1, "Finished configuration.\n"); return; } @@ -891,8 +683,7 @@ if (ec_fsm_coe_exec(fsm->fsm_coe)) return; if (!ec_fsm_coe_success(fsm->fsm_coe)) { - EC_ERR("SDO configuration failed for slave %u.\n", - fsm->slave->ring_position); + EC_SLAVE_ERR(fsm->slave, "SDO configuration failed.\n"); fsm->slave->error_flag = 1; fsm->state = ec_fsm_slave_config_state_error; return; @@ -968,8 +759,7 @@ } if (!ec_fsm_soe_success(fsm_soe)) { - EC_ERR("SoE configuration failed for slave %u.\n", - fsm->slave->ring_position); + EC_SLAVE_ERR(slave, "SoE configuration failed.\n"); fsm->slave->error_flag = 1; fsm->state = ec_fsm_slave_config_state_error; return; @@ -1029,8 +819,7 @@ } if (!ec_fsm_pdo_success(fsm->fsm_pdo)) { - EC_WARN("PDO configuration failed on slave %u.\n", - fsm->slave->ring_position); + EC_SLAVE_WARN(fsm->slave, "PDO configuration failed.\n"); } ec_fsm_slave_config_enter_watchdog_divider(fsm); @@ -1046,13 +835,11 @@ { ec_slave_t *slave = fsm->slave; ec_datagram_t *datagram = fsm->datagram; - ec_master_t *master = slave->master; ec_slave_config_t *config = slave->config; if (config && config->watchdog_divider) { - if (master->debug_level) - EC_DBG("Setting watchdog divider to %u.\n", - config->watchdog_divider); + EC_SLAVE_DBG(slave, 1, "Setting watchdog divider to %u.\n", + config->watchdog_divider); ec_datagram_fpwr(datagram, slave->station_address, 0x0400, 2); EC_WRITE_U16(datagram->data, config->watchdog_divider); @@ -1079,16 +866,15 @@ if (datagram->state != EC_DATAGRAM_RECEIVED) { fsm->state = ec_fsm_slave_config_state_error; - EC_ERR("Failed to receive watchdog divider configuration datagram for" - " slave %u: ", slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to receive watchdog divider" + " configuration datagram: "); ec_datagram_print_state(datagram); return; } if (datagram->working_counter != 1) { slave->error_flag = 1; - EC_WARN("Failed to set watchdog divider of slave %u: ", - slave->ring_position); + EC_SLAVE_WARN(slave, "Failed to set watchdog divider: "); ec_datagram_print_wc_error(datagram); return; } @@ -1109,9 +895,8 @@ ec_slave_config_t *config = slave->config; if (config && config->watchdog_intervals) { - if (slave->master->debug_level) - EC_DBG("Setting process data watchdog intervals to %u.\n", - config->watchdog_intervals); + EC_SLAVE_DBG(slave, 1, "Setting process data" + " watchdog intervals to %u.\n", config->watchdog_intervals); ec_datagram_fpwr(datagram, slave->station_address, 0x0420, 2); EC_WRITE_U16(datagram->data, config->watchdog_intervals); @@ -1140,15 +925,15 @@ if (datagram->state != EC_DATAGRAM_RECEIVED) { fsm->state = ec_fsm_slave_config_state_error; - EC_ERR("Failed to receive sync manager watchdog configuration " - "datagram for slave %u: ", slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to receive sync manager" + " watchdog configuration datagram: "); ec_datagram_print_state(datagram); return; } if (datagram->working_counter != 1) { - EC_WARN("Failed to set process data watchdog intervals of slave %u: ", - slave->ring_position); + EC_SLAVE_WARN(slave, "Failed to set process data" + " watchdog intervals: "); ec_datagram_print_wc_error(datagram); } @@ -1227,8 +1012,8 @@ if (datagram->state != EC_DATAGRAM_RECEIVED) { fsm->state = ec_fsm_slave_config_state_error; - EC_ERR("Failed to receive process data sync manager configuration" - " datagram for slave %u: ", slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to receive process data sync" + " manager configuration datagram: "); ec_datagram_print_state(datagram); return; } @@ -1236,8 +1021,7 @@ if (datagram->working_counter != 1) { slave->error_flag = 1; fsm->state = ec_fsm_slave_config_state_error; - EC_ERR("Failed to set process data sync managers of slave %u: ", - slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to set process data sync managers: "); ec_datagram_print_wc_error(datagram); return; } @@ -1267,8 +1051,8 @@ if (slave->base_fmmu_count < slave->config->used_fmmus) { slave->error_flag = 1; fsm->state = ec_fsm_slave_config_state_error; - EC_ERR("Slave %u has less FMMUs (%u) than requested (%u).\n", - slave->ring_position, slave->base_fmmu_count, + EC_SLAVE_ERR(slave, "Slave has less FMMUs (%u)" + " than requested (%u).\n", slave->base_fmmu_count, slave->config->used_fmmus); return; } @@ -1287,8 +1071,8 @@ if (!(sync = ec_slave_get_sync(slave, fmmu->sync_index))) { slave->error_flag = 1; fsm->state = ec_fsm_slave_config_state_error; - EC_ERR("Failed to determine PDO sync manager for FMMU on slave" - " %u!\n", slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to determine PDO sync manager" + " for FMMU!\n"); return; } ec_fmmu_config_page(fmmu, sync, @@ -1315,8 +1099,7 @@ if (datagram->state != EC_DATAGRAM_RECEIVED) { fsm->state = ec_fsm_slave_config_state_error; - EC_ERR("Failed to receive FMMUs datagram for slave %u: ", - slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to receive FMMUs datagram: "); ec_datagram_print_state(datagram); return; } @@ -1324,8 +1107,7 @@ if (datagram->working_counter != 1) { slave->error_flag = 1; fsm->state = ec_fsm_slave_config_state_error; - EC_ERR("Failed to set FMMUs of slave %u: ", - slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to set FMMUs: "); ec_datagram_print_wc_error(datagram); return; } @@ -1352,15 +1134,12 @@ if (config->dc_assign_activate) { if (!slave->base_dc_supported || !slave->has_dc_system_time) { - EC_WARN("Slave %u seems not to support distributed clocks!\n", - slave->ring_position); + EC_SLAVE_WARN(slave, "Slave seems not to support" + " distributed clocks!\n"); } - if (slave->master->debug_level) - EC_DBG("Slave %u: Setting DC cycle times to %u / %u.\n", - slave->ring_position, - config->dc_sync[0].cycle_time, - config->dc_sync[1].cycle_time); + EC_SLAVE_DBG(slave, 1, "Setting DC cycle times to %u / %u.\n", + config->dc_sync[0].cycle_time, config->dc_sync[1].cycle_time); // set DC cycle times ec_datagram_fpwr(datagram, slave->station_address, 0x09A0, 8); @@ -1384,8 +1163,53 @@ { ec_datagram_t *datagram = fsm->datagram; ec_slave_t *slave = fsm->slave; + ec_slave_config_t *config = slave->config; + + if (!config) { // config removed in the meantime + ec_fsm_slave_config_reconfigure(fsm); + return; + } + + if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) + return; + + if (datagram->state != EC_DATAGRAM_RECEIVED) { + fsm->state = ec_fsm_slave_config_state_error; + EC_SLAVE_ERR(slave, "Failed to receive DC cycle times datagram: "); + ec_datagram_print_state(datagram); + return; + } + + if (datagram->working_counter != 1) { + slave->error_flag = 1; + fsm->state = ec_fsm_slave_config_state_error; + EC_SLAVE_ERR(slave, "Failed to set DC cycle times: "); + ec_datagram_print_wc_error(datagram); + return; + } + + EC_SLAVE_DBG(slave, 1, "Checking for synchrony.\n"); + + fsm->jiffies_start = jiffies; + ec_datagram_fprd(datagram, slave->station_address, 0x092c, 4); + fsm->retries = EC_FSM_RETRIES; + fsm->state = ec_fsm_slave_config_state_dc_sync_check; +} + +/*****************************************************************************/ + +/** Slave configuration state: DC SYNC CHECK. + */ +void ec_fsm_slave_config_state_dc_sync_check( + ec_fsm_slave_config_t *fsm /**< slave state machine */ + ) +{ + ec_datagram_t *datagram = fsm->datagram; + ec_slave_t *slave = fsm->slave; ec_master_t *master = slave->master; ec_slave_config_t *config = slave->config; + uint32_t abs_sync_diff; + unsigned long diff_ms; ec_sync_signal_t *sync0 = &config->dc_sync[0]; u64 start_time; @@ -1399,8 +1223,7 @@ if (datagram->state != EC_DATAGRAM_RECEIVED) { fsm->state = ec_fsm_slave_config_state_error; - EC_ERR("Failed to receive DC cycle times datagram for slave %u: ", - slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to receive DC sync check datagram: "); ec_datagram_print_state(datagram); return; } @@ -1408,19 +1231,40 @@ if (datagram->working_counter != 1) { slave->error_flag = 1; fsm->state = ec_fsm_slave_config_state_error; - EC_ERR("Failed to set DC cycle times of slave %u: ", - slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to check DC synchrony: "); ec_datagram_print_wc_error(datagram); return; } + abs_sync_diff = EC_READ_U32(datagram->data) & 0x7fffffff; + diff_ms = (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; + + if (abs_sync_diff > EC_DC_MAX_SYNC_DIFF_NS) { + + if (diff_ms >= EC_DC_SYNC_WAIT_MS) { + EC_SLAVE_WARN(slave, "Slave did not sync after %u ms.\n", + (u32) diff_ms); + } else { + EC_SLAVE_DBG(slave, 1, "Sync after %4u ms: %10u ns\n", + (u32) diff_ms, abs_sync_diff); + + // check synchrony again + ec_datagram_fprd(datagram, slave->station_address, 0x092c, 4); + fsm->retries = EC_FSM_RETRIES; + return; + } + } else { + EC_SLAVE_DBG(slave, 1, "%u ns difference after %u ms.\n", + abs_sync_diff, (u32) diff_ms); + } + // set DC start time - start_time = master->app_time + 100000000ULL; // now + X ns + start_time = master->app_time + EC_DC_START_OFFSET; // now + X ns // FIXME use slave's local system time here? if (sync0->cycle_time) { // find correct phase - if (master->has_start_time) { + if (master->has_app_time) { u64 diff, start; u32 remainder; @@ -1430,24 +1274,22 @@ start = start_time + sync0->cycle_time - remainder + sync0->shift_time; - if (master->debug_level) { - EC_DBG("app_start_time=%llu\n", master->app_start_time); - EC_DBG(" start_time=%llu\n", start_time); - EC_DBG(" cycle_time=%u\n", sync0->cycle_time); - EC_DBG(" shift_time=%u\n", sync0->shift_time); - EC_DBG(" remainder=%u\n", remainder); - EC_DBG(" start=%llu\n", start); - } + EC_SLAVE_DBG(slave, 1, "app_start_time=%llu\n", + master->app_start_time); + EC_SLAVE_DBG(slave, 1, " start_time=%llu\n", start_time); + EC_SLAVE_DBG(slave, 1, " cycle_time=%u\n", sync0->cycle_time); + EC_SLAVE_DBG(slave, 1, " shift_time=%u\n", sync0->shift_time); + EC_SLAVE_DBG(slave, 1, " remainder=%u\n", remainder); + EC_SLAVE_DBG(slave, 1, " start=%llu\n", start); start_time = start; } else { - EC_WARN("No application time supplied. Cyclic start time will " - "not be in phase for slave %u.\n", slave->ring_position); + EC_SLAVE_WARN(slave, "No application time supplied." + " Cyclic start time will not be in phase.\n"); } } - if (master->debug_level) - EC_DBG("Slave %u: Setting DC cyclic operation start time to %llu.\n", - slave->ring_position, start_time); + EC_SLAVE_DBG(slave, 1, "Setting DC cyclic operation" + " start time to %llu.\n", start_time); ec_datagram_fpwr(datagram, slave->station_address, 0x0990, 8); EC_WRITE_U64(datagram->data, start_time); @@ -1477,8 +1319,7 @@ if (datagram->state != EC_DATAGRAM_RECEIVED) { fsm->state = ec_fsm_slave_config_state_error; - EC_ERR("Failed to receive DC start time datagram for slave %u: ", - slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to receive DC start time datagram: "); ec_datagram_print_state(datagram); return; } @@ -1486,15 +1327,13 @@ if (datagram->working_counter != 1) { slave->error_flag = 1; fsm->state = ec_fsm_slave_config_state_error; - EC_ERR("Failed to set DC start time of slave %u: ", - slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to set DC start time: "); ec_datagram_print_wc_error(datagram); return; } - if (slave->master->debug_level) - EC_DBG("Slave %u: Setting DC AssignActivate to 0x%04x.\n", - slave->ring_position, config->dc_assign_activate); + EC_SLAVE_DBG(slave, 1, "Setting DC AssignActivate to 0x%04x.\n", + config->dc_assign_activate); // assign sync unit to EtherCAT or PDI ec_datagram_fpwr(datagram, slave->station_address, 0x0980, 2); @@ -1519,8 +1358,7 @@ if (datagram->state != EC_DATAGRAM_RECEIVED) { fsm->state = ec_fsm_slave_config_state_error; - EC_ERR("Failed to receive DC activation datagram for slave %u: ", - slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to receive DC activation datagram: "); ec_datagram_print_state(datagram); return; } @@ -1528,8 +1366,7 @@ if (datagram->working_counter != 1) { slave->error_flag = 1; fsm->state = ec_fsm_slave_config_state_error; - EC_ERR("Failed to set DC cyclic operation state of slave %u: ", - slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to activate DC: "); ec_datagram_print_wc_error(datagram); return; } @@ -1558,7 +1395,6 @@ ec_fsm_slave_config_t *fsm /**< slave state machine */ ) { - ec_master_t *master = fsm->slave->master; ec_slave_t *slave = fsm->slave; if (ec_fsm_change_exec(fsm->fsm_change)) return; @@ -1572,16 +1408,11 @@ // slave is now in SAFEOP - if (master->debug_level) { - EC_DBG("Slave %u is now in SAFEOP.\n", slave->ring_position); - } + EC_SLAVE_DBG(slave, 1, "Now in SAFEOP.\n"); if (fsm->slave->current_state == fsm->slave->requested_state) { fsm->state = ec_fsm_slave_config_state_end; // successful - if (master->debug_level) { - EC_DBG("Finished configuration of slave %u.\n", - slave->ring_position); - } + EC_SLAVE_DBG(slave, 1, "Finished configuration.\n"); return; } @@ -1599,7 +1430,6 @@ ec_fsm_slave_config_t *fsm /**< slave state machine */ ) { - ec_master_t *master = fsm->slave->master; ec_slave_t *slave = fsm->slave; if (ec_fsm_change_exec(fsm->fsm_change)) return; @@ -1613,10 +1443,7 @@ // slave is now in OP - if (master->debug_level) { - EC_DBG("Slave %u is now in OP.\n", slave->ring_position); - EC_DBG("Finished configuration of slave %u.\n", slave->ring_position); - } + EC_SLAVE_DBG(slave, 1, "Now in OP. Finished configuration.\n"); fsm->state = ec_fsm_slave_config_state_end; // successful } @@ -1629,10 +1456,8 @@ ec_fsm_slave_config_t *fsm /**< slave state machine */ ) { - if (fsm->slave->master->debug_level) { - EC_DBG("Slave configuration for slave %u detached during " - "configuration. Reconfiguring.", fsm->slave->ring_position); - } + EC_SLAVE_DBG(fsm->slave, 1, "Slave configuration detached during " + "configuration. Reconfiguring."); ec_fsm_slave_config_enter_init(fsm); // reconfigure } diff -r ea38efeeb7b3 -r 6aa393418fb3 master/fsm_slave_scan.c --- a/master/fsm_slave_scan.c Fri May 07 15:26:26 2010 +0200 +++ b/master/fsm_slave_scan.c Tue May 11 13:57:58 2010 +0200 @@ -190,8 +190,8 @@ if (datagram->state != EC_DATAGRAM_RECEIVED) { fsm->state = ec_fsm_slave_scan_state_error; - EC_ERR("Failed to receive station address datagram for slave %u: ", - fsm->slave->ring_position); + EC_SLAVE_ERR(fsm->slave, + "Failed to receive station address datagram: "); ec_datagram_print_state(datagram); return; } @@ -199,8 +199,7 @@ if (datagram->working_counter != 1) { fsm->slave->error_flag = 1; fsm->state = ec_fsm_slave_scan_state_error; - EC_ERR("Failed to write station address on slave %u: ", - fsm->slave->ring_position); + EC_SLAVE_ERR(fsm->slave, "Failed to write station address: "); ec_datagram_print_wc_error(datagram); return; } @@ -230,8 +229,7 @@ if (datagram->state != EC_DATAGRAM_RECEIVED) { fsm->state = ec_fsm_slave_scan_state_error; - EC_ERR("Failed to receive AL state datagram from slave %u: ", - fsm->slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to receive AL state datagram: "); ec_datagram_print_state(datagram); return; } @@ -239,8 +237,7 @@ if (datagram->working_counter != 1) { fsm->slave->error_flag = 1; fsm->state = ec_fsm_slave_scan_state_error; - EC_ERR("Failed to read AL state of slave %u: ", - fsm->slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to read AL state: "); ec_datagram_print_wc_error(datagram); return; } @@ -249,8 +246,8 @@ if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) { char state_str[EC_STATE_STRING_SIZE]; ec_state_string(slave->current_state, state_str, 0); - EC_WARN("Slave %u has state error bit set (%s)!\n", - slave->ring_position, state_str); + EC_SLAVE_WARN(slave, "Slave has state error bit set (%s)!\n", + state_str); } // read base data @@ -278,8 +275,7 @@ if (datagram->state != EC_DATAGRAM_RECEIVED) { fsm->state = ec_fsm_slave_scan_state_error; - EC_ERR("Failed to receive base data datagram for slave %u: ", - slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to receive base data datagram: "); ec_datagram_print_state(datagram); return; } @@ -287,8 +283,7 @@ if (datagram->working_counter != 1) { fsm->slave->error_flag = 1; fsm->state = ec_fsm_slave_scan_state_error; - EC_ERR("Failed to read base data from slave %u: ", - slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to read base data: "); ec_datagram_print_wc_error(datagram); return; } @@ -299,16 +294,15 @@ slave->base_fmmu_count = EC_READ_U8 (datagram->data + 4); if (slave->base_fmmu_count > EC_MAX_FMMUS) { - EC_WARN("Slave %u has more FMMUs (%u) than the master can" - " handle (%u).\n", slave->ring_position, - slave->base_fmmu_count, EC_MAX_FMMUS); + EC_SLAVE_WARN(slave, "Slave has more FMMUs (%u) than the master can" + " handle (%u).\n", slave->base_fmmu_count, EC_MAX_FMMUS); slave->base_fmmu_count = EC_MAX_FMMUS; } slave->base_sync_count = EC_READ_U8(datagram->data + 5); if (slave->base_sync_count > EC_MAX_SYNC_MANAGERS) { - EC_WARN("Slave %u provides more sync managers (%u) than the master can" - " handle (%u).\n", slave->ring_position, + EC_SLAVE_WARN(slave, "Slave provides more sync managers (%u)" + " than the master can handle (%u).\n", slave->base_sync_count, EC_MAX_SYNC_MANAGERS); slave->base_sync_count = EC_MAX_SYNC_MANAGERS; } @@ -353,28 +347,22 @@ if (datagram->state != EC_DATAGRAM_RECEIVED) { fsm->state = ec_fsm_slave_scan_state_error; - EC_ERR("Failed to receive system time datagram for slave %u: ", - slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to receive system time datagram: "); ec_datagram_print_state(datagram); return; } if (datagram->working_counter == 1) { slave->has_dc_system_time = 1; - if (slave->master->debug_level) { - EC_DBG("Slave %u has the System Time register.\n", - slave->ring_position); - } + EC_SLAVE_DBG(slave, 1, "Slave has the System Time register.\n"); } else if (datagram->working_counter == 0) { - if (slave->master->debug_level) { - EC_DBG("Slave %u has no System Time register; delay " - "measurement only.\n", slave->ring_position); - } + EC_SLAVE_DBG(slave, 1, "Slave has no System Time register; delay " + "measurement only.\n"); } else { fsm->slave->error_flag = 1; fsm->state = ec_fsm_slave_scan_state_error; - EC_ERR("Failed to determine, if system time register is " - "supported by slave %u: ", slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to determine, if system time register is " + "supported: "); ec_datagram_print_wc_error(datagram); return; } @@ -405,8 +393,7 @@ if (datagram->state != EC_DATAGRAM_RECEIVED) { fsm->state = ec_fsm_slave_scan_state_error; - EC_ERR("Failed to receive system time datagram for slave %u: ", - slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to receive system time datagram: "); ec_datagram_print_state(datagram); return; } @@ -414,8 +401,7 @@ if (datagram->working_counter != 1) { fsm->slave->error_flag = 1; fsm->state = ec_fsm_slave_scan_state_error; - EC_ERR("Failed to get DC receive times of slave %u: ", - slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to get DC receive times: "); ec_datagram_print_wc_error(datagram); return; } @@ -465,8 +451,7 @@ if (datagram->state != EC_DATAGRAM_RECEIVED) { fsm->state = ec_fsm_slave_scan_state_error; - EC_ERR("Failed to receive DL status datagram from slave %u: ", - slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to receive DL status datagram: "); ec_datagram_print_state(datagram); return; } @@ -474,17 +459,19 @@ if (datagram->working_counter != 1) { fsm->slave->error_flag = 1; fsm->state = ec_fsm_slave_scan_state_error; - EC_ERR("Failed to read DL status from slave %u: ", - slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to read DL status: "); ec_datagram_print_wc_error(datagram); return; } dl_status = EC_READ_U16(datagram->data); for (i = 0; i < EC_MAX_PORTS; i++) { - slave->ports[i].link.link_up = dl_status & (1 << (4 + i)) ? 1 : 0; - slave->ports[i].link.loop_closed = dl_status & (1 << (8 + i * 2)) ? 1 : 0; - slave->ports[i].link.signal_detected = dl_status & (1 << (9 + i * 2)) ? 1 : 0; + slave->ports[i].link.link_up = + dl_status & (1 << (4 + i)) ? 1 : 0; + slave->ports[i].link.loop_closed = + dl_status & (1 << (8 + i * 2)) ? 1 : 0; + slave->ports[i].link.signal_detected = + dl_status & (1 << (9 + i * 2)) ? 1 : 0; } // Start fetching SII size @@ -512,8 +499,7 @@ if (!ec_fsm_sii_success(&fsm->fsm_sii)) { fsm->slave->error_flag = 1; fsm->state = ec_fsm_slave_scan_state_error; - EC_ERR("Failed to read SII size of slave %u.\n", - slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to read SII size.\n"); return; } @@ -523,9 +509,8 @@ if (cat_type != 0xFFFF) { // not the last category off_t next_offset = 2UL + fsm->sii_offset + cat_size; if (next_offset >= EC_MAX_SII_SIZE) { - EC_WARN("SII size of slave %u exceeds" - " %u words (0xffff limiter missing?).\n", - slave->ring_position, EC_MAX_SII_SIZE); + EC_SLAVE_WARN(slave, "SII size exceeds %u words" + " (0xffff limiter missing?).\n", EC_MAX_SII_SIZE); // cut off category data... slave->sii_nwords = EC_FIRST_SII_CATEGORY_OFFSET; goto alloc_sii; @@ -541,15 +526,14 @@ alloc_sii: if (slave->sii_words) { - EC_WARN("Freeing old SII data on slave %u...\n", - slave->ring_position); + EC_SLAVE_WARN(slave, "Freeing old SII data...\n"); kfree(slave->sii_words); } if (!(slave->sii_words = (uint16_t *) kmalloc(slave->sii_nwords * 2, GFP_KERNEL))) { - EC_ERR("Failed to allocate %zu words of SII data for slave %u.\n", - slave->sii_nwords, slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to allocate %zu words of SII data.\n", + slave->sii_nwords); slave->sii_nwords = 0; slave->error_flag = 1; fsm->state = ec_fsm_slave_scan_state_error; @@ -581,8 +565,7 @@ if (!ec_fsm_sii_success(&fsm->fsm_sii)) { fsm->slave->error_flag = 1; fsm->state = ec_fsm_slave_scan_state_error; - EC_ERR("Failed to fetch SII contents of slave %u.\n", - slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to fetch SII contents.\n"); return; } @@ -644,9 +627,8 @@ } if (slave->sii_nwords < EC_FIRST_SII_CATEGORY_OFFSET + 1) { - EC_ERR("Unexpected end of SII data in slave %u:" - " First category header missing.\n", - slave->ring_position); + EC_SLAVE_ERR(slave, "Unexpected end of SII data:" + " First category header missing.\n"); goto end; } @@ -656,9 +638,8 @@ // type and size words must fit if (cat_word + 2 - slave->sii_words > slave->sii_nwords) { - EC_ERR("Unexpected end of SII data in slave %u:" - " Category header incomplete.\n", - slave->ring_position); + EC_SLAVE_ERR(slave, "Unexpected end of SII data:" + " Category header incomplete.\n"); goto end; } @@ -667,9 +648,8 @@ cat_word += 2; if (cat_word + cat_size - slave->sii_words > slave->sii_nwords) { - EC_WARN("Unexpected end of SII data in slave %u:" - " Category data incomplete.\n", - slave->ring_position); + EC_SLAVE_WARN(slave, "Unexpected end of SII data:" + " Category data incomplete.\n"); goto end; } @@ -702,16 +682,14 @@ goto end; break; default: - if (fsm->slave->master->debug_level) - EC_WARN("Unknown category type 0x%04X in slave %u.\n", - cat_type, slave->ring_position); + EC_SLAVE_DBG(slave, 1, "Unknown category type 0x%04X.\n", + cat_type); } cat_word += cat_size; if (cat_word - slave->sii_words >= slave->sii_nwords) { - EC_WARN("Unexpected end of SII data in slave %u:" - " Next category header missing.\n", - slave->ring_position); + EC_SLAVE_WARN(slave, "Unexpected end of SII data:" + " Next category header missing.\n"); goto end; } } @@ -720,7 +698,7 @@ return; end: - EC_ERR("Failed to analyze category data.\n"); + EC_SLAVE_ERR(slave, "Failed to analyze category data.\n"); fsm->slave->error_flag = 1; fsm->state = ec_fsm_slave_scan_state_error; } @@ -740,9 +718,7 @@ ec_slave_t *slave = fsm->slave; // read alias from register - if (slave->master->debug_level) - EC_DBG("Reading alias from register of slave %u.\n", - slave->ring_position); + EC_SLAVE_DBG(slave, 1, "Reading alias from register.\n"); ec_datagram_fprd(datagram, slave->station_address, 0x0012, 2); ec_datagram_zero(datagram); fsm->retries = EC_FSM_RETRIES; @@ -766,20 +742,17 @@ if (datagram->state != EC_DATAGRAM_RECEIVED) { fsm->state = ec_fsm_slave_scan_state_error; - EC_ERR("Failed to receive get reg alias datagram from slave %u: ", - fsm->slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to receive register alias datagram: "); ec_datagram_print_state(datagram); return; } if (datagram->working_counter != 1) { - EC_WARN("Failed to read reg alias of slave %u.\n", - fsm->slave->ring_position); + EC_SLAVE_DBG(slave, 1, "Failed to read register alias.\n"); } else { slave->effective_alias = EC_READ_U16(datagram->data); - if (slave->master->debug_level) - EC_DBG("Read alias %u from register of slave %u.\n", - slave->effective_alias, slave->ring_position); + EC_SLAVE_DBG(slave, 1, "Read alias %u from register.\n", + slave->effective_alias); } if (slave->sii.mailbox_protocols & EC_MBOX_COE) { ec_fsm_slave_scan_enter_preop(fsm); @@ -805,8 +778,8 @@ if (slave->master->debug_level) { char str[EC_STATE_STRING_SIZE]; ec_state_string(current_state, str, 0); - EC_DBG("Slave %u is not in the state to do mailbox com (%s)," - " setting to PREOP.\n", slave->ring_position, str); + EC_SLAVE_DBG(slave, 0, "Slave is not in the state" + " to do mailbox com (%s), setting to PREOP.\n", str); } fsm->state = ec_fsm_slave_scan_state_preop; @@ -814,9 +787,8 @@ ec_fsm_slave_config_start(fsm->fsm_slave_config, slave); ec_fsm_slave_config_exec(fsm->fsm_slave_config); } else { - if (slave->master->debug_level) - EC_DBG("Reading mailbox syncmanager configuration of slave %u.\n", - slave->ring_position); + EC_SLAVE_DBG(slave, 1, "Reading mailbox" + " sync manager configuration.\n"); /* Scan current sync manager configuration to get configured mailbox * sizes. */ @@ -862,8 +834,8 @@ if (datagram->state != EC_DATAGRAM_RECEIVED) { fsm->state = ec_fsm_slave_scan_state_error; - EC_ERR("Failed to receive sync manager configuration datagram" - " from slave %u: ", slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to receive sync manager" + " configuration datagram: "); ec_datagram_print_state(datagram); return; } @@ -871,8 +843,7 @@ if (datagram->working_counter != 1) { fsm->slave->error_flag = 1; fsm->state = ec_fsm_slave_scan_state_error; - EC_ERR("Failed to read DL status from slave %u: ", - slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to read DL status: "); ec_datagram_print_wc_error(datagram); return; } @@ -882,15 +853,13 @@ slave->configured_tx_mailbox_offset = EC_READ_U16(datagram->data + 8); slave->configured_tx_mailbox_size = EC_READ_U16(datagram->data + 10); - if (slave->master->debug_level) { - EC_DBG("Mailbox configuration of slave %u:\n", slave->ring_position); - EC_DBG(" RX offset=0x%04x size=%u\n", - slave->configured_rx_mailbox_offset, - slave->configured_rx_mailbox_size); - EC_DBG(" TX offset=0x%04x size=%u\n", - slave->configured_tx_mailbox_offset, - slave->configured_tx_mailbox_size); - } + EC_SLAVE_DBG(slave, 1, "Mailbox configuration:\n"); + EC_SLAVE_DBG(slave, 1, " RX offset=0x%04x size=%u\n", + slave->configured_rx_mailbox_offset, + slave->configured_rx_mailbox_size); + EC_SLAVE_DBG(slave, 1, " TX offset=0x%04x size=%u\n", + slave->configured_tx_mailbox_offset, + slave->configured_tx_mailbox_size); ec_fsm_slave_scan_enter_pdos(fsm); } @@ -905,9 +874,7 @@ { ec_slave_t *slave = fsm->slave; - if (slave->master->debug_level) - EC_DBG("Scanning PDO assignment and mapping of slave %u.\n", - slave->ring_position); + EC_SLAVE_DBG(slave, 1, "Scanning PDO assignment and mapping.\n"); fsm->state = ec_fsm_slave_scan_state_pdos; ec_fsm_pdo_start_reading(fsm->fsm_pdo, slave); ec_fsm_pdo_exec(fsm->fsm_pdo); // execute immediately diff -r ea38efeeb7b3 -r 6aa393418fb3 master/fsm_soe.c --- a/master/fsm_soe.c Fri May 07 15:26:26 2010 +0200 +++ b/master/fsm_soe.c Tue May 11 13:57:58 2010 +0200 @@ -85,19 +85,19 @@ /** Outputs an SoE error code. */ -void ec_print_soe_error(uint16_t error_code) +void ec_print_soe_error(const ec_slave_t *slave, uint16_t error_code) { const ec_code_msg_t *error_msg; for (error_msg = soe_error_codes; error_msg->code; error_msg++) { if (error_msg->code == error_code) { - EC_ERR("SoE error 0x%04X: \"%s\".\n", + EC_SLAVE_ERR(slave, "SoE error 0x%04X: \"%s\".\n", error_msg->code, error_msg->message); return; } } - EC_ERR("Unknown SoE error 0x%04X.\n", error_code); + EC_SLAVE_ERR(slave, "Unknown SoE error 0x%04X.\n", error_code); } /*****************************************************************************/ @@ -176,7 +176,7 @@ { ec_soe_request_t *request = fsm->request; - EC_ERR(""); + EC_SLAVE_ERR(fsm->slave, ""); if (request->dir == EC_DIR_OUTPUT) { printk("Writing"); @@ -184,8 +184,7 @@ printk("Reading"); } - printk(" IDN 0x%04X failed on slave %u.\n", - request->idn, fsm->slave->ring_position); + printk(" IDN 0x%04X failed.\n", request->idn); } /****************************************************************************** @@ -202,12 +201,10 @@ ec_soe_request_t *request = fsm->request; uint8_t *data; - if (master->debug_level) - EC_DBG("Reading IDN 0x%04X from slave %u.\n", - request->idn, slave->ring_position); + EC_SLAVE_DBG(slave, 1, "Reading IDN 0x%04X.\n", request->idn); if (!(slave->sii.mailbox_protocols & EC_MBOX_SOE)) { - EC_ERR("Slave does not support SoE!\n"); + EC_SLAVE_ERR(slave, "Slave does not support SoE!\n"); fsm->state = ec_fsm_soe_error; ec_fsm_soe_print_error(fsm); return; @@ -226,7 +223,7 @@ EC_WRITE_U16(data + 2, request->idn); if (master->debug_level) { - EC_DBG("SCC read request:\n"); + EC_SLAVE_DBG(slave, 0, "SCC read request:\n"); ec_print_data(data, EC_SOE_SIZE); } @@ -251,7 +248,7 @@ if (datagram->state != EC_DATAGRAM_RECEIVED) { fsm->state = ec_fsm_soe_error; - EC_ERR("Failed to receive SoE read request: "); + EC_SLAVE_ERR(slave, "Failed to receive SoE read request: "); ec_datagram_print_state(datagram); ec_fsm_soe_print_error(fsm); return; @@ -267,8 +264,8 @@ } } fsm->state = ec_fsm_soe_error; - EC_ERR("Reception of SoE read request failed after %u ms: ", - (u32) diff_ms); + EC_SLAVE_ERR(slave, "Reception of SoE read request" + " failed after %u ms: ", (u32) diff_ms); ec_datagram_print_wc_error(datagram); ec_fsm_soe_print_error(fsm); return; @@ -294,7 +291,7 @@ if (datagram->state != EC_DATAGRAM_RECEIVED) { fsm->state = ec_fsm_soe_error; - EC_ERR("Failed to receive SoE mailbox check datagram: "); + EC_SLAVE_ERR(slave, "Failed to receive SoE mailbox check datagram: "); ec_datagram_print_state(datagram); ec_fsm_soe_print_error(fsm); return; @@ -302,7 +299,8 @@ if (datagram->working_counter != 1) { fsm->state = ec_fsm_soe_error; - EC_ERR("Reception of SoE mailbox check datagram failed: "); + EC_SLAVE_ERR(slave, "Reception of SoE mailbox check" + " datagram failed: "); ec_datagram_print_wc_error(datagram); ec_fsm_soe_print_error(fsm); return; @@ -313,8 +311,8 @@ (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; if (diff_ms >= EC_SOE_RESPONSE_TIMEOUT) { fsm->state = ec_fsm_soe_error; - EC_ERR("Timeout after %u ms while waiting for read response.\n", - (u32) diff_ms); + EC_SLAVE_ERR(slave, "Timeout after %u ms while waiting for" + " read response.\n", (u32) diff_ms); ec_fsm_soe_print_error(fsm); return; } @@ -349,7 +347,7 @@ if (datagram->state != EC_DATAGRAM_RECEIVED) { fsm->state = ec_fsm_soe_error; - EC_ERR("Failed to receive SoE read response datagram: "); + EC_SLAVE_ERR(slave, "Failed to receive SoE read response datagram: "); ec_datagram_print_state(datagram); ec_fsm_soe_print_error(fsm); return; @@ -357,7 +355,7 @@ if (datagram->working_counter != 1) { fsm->state = ec_fsm_soe_error; - EC_ERR("Reception of SoE read response failed: "); + EC_SLAVE_ERR(slave, "Reception of SoE read response failed: "); ec_datagram_print_wc_error(datagram); ec_fsm_soe_print_error(fsm); return; @@ -371,20 +369,21 @@ } if (master->debug_level) { - EC_DBG("SCC read response:\n"); + EC_SLAVE_DBG(slave, 0, "SCC read response:\n"); ec_print_data(data, rec_size); } if (mbox_prot != EC_MBOX_TYPE_SOE) { fsm->state = ec_fsm_soe_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); ec_fsm_soe_print_error(fsm); return; } if (rec_size < EC_SOE_SIZE) { fsm->state = ec_fsm_soe_error; - EC_ERR("Received currupted SoE read response" + EC_SLAVE_ERR(slave, "Received currupted SoE read response" " (%zu bytes)!\n", rec_size); ec_print_data(data, rec_size); ec_fsm_soe_print_error(fsm); @@ -397,7 +396,8 @@ error_flag = (header >> 4) & 1; if (opcode != OPCODE_READ_RESPONSE) { - EC_ERR("Received no read response (opcode %x).\n", opcode); + EC_SLAVE_ERR(slave, "Received no read response (opcode %x).\n", + opcode); ec_print_data(data, rec_size); ec_fsm_soe_print_error(fsm); fsm->state = ec_fsm_soe_error; @@ -406,8 +406,8 @@ if (error_flag) { req->error_code = EC_READ_U16(data + rec_size - 2); - EC_ERR("Received error response:\n"); - ec_print_soe_error(req->error_code); + EC_SLAVE_ERR(slave, "Received error response:\n"); + ec_print_soe_error(slave, req->error_code); ec_fsm_soe_print_error(fsm); fsm->state = ec_fsm_soe_error; return; @@ -417,7 +417,7 @@ value_included = (EC_READ_U8(data + 1) >> 6) & 1; if (!value_included) { - EC_ERR("No value included!\n"); + EC_SLAVE_ERR(slave, "No value included!\n"); ec_fsm_soe_print_error(fsm); fsm->state = ec_fsm_soe_error; return; @@ -432,17 +432,15 @@ } if (incomplete) { - if (master->debug_level) { - EC_DBG("SoE data incomplete. Waiting for fragment" - " at offset %zu.\n", req->data_size); - } + EC_SLAVE_DBG(slave, 1, "SoE data incomplete. Waiting for fragment" + " at offset %zu.\n", req->data_size); fsm->jiffies_start = datagram->jiffies_sent; ec_slave_mbox_prepare_check(slave, datagram); // can not fail. fsm->retries = EC_FSM_RETRIES; fsm->state = ec_fsm_soe_read_check; } else { if (master->debug_level) { - EC_DBG("IDN data:\n"); + EC_SLAVE_DBG(slave, 0, "IDN data:\n"); ec_print_data(req->data, req->data_size); } @@ -470,7 +468,7 @@ header_size = EC_MBOX_HEADER_SIZE + EC_SOE_SIZE; if (slave->configured_rx_mailbox_size <= header_size) { - EC_ERR("Mailbox size (%u) too small for SoE write.\n", + EC_SLAVE_ERR(slave, "Mailbox size (%u) too small for SoE write.\n", slave->configured_rx_mailbox_size); fsm->state = ec_fsm_soe_error; ec_fsm_soe_print_error(fsm); @@ -501,7 +499,7 @@ fsm->offset += fragment_size; if (master->debug_level) { - EC_DBG("SCC write request:\n"); + EC_SLAVE_DBG(slave, 0, "SCC write request:\n"); ec_print_data(data, EC_SOE_SIZE + fragment_size); } @@ -517,15 +515,13 @@ void ec_fsm_soe_write_start(ec_fsm_soe_t *fsm /**< finite state machine */) { ec_slave_t *slave = fsm->slave; - ec_master_t *master = slave->master; ec_soe_request_t *req = fsm->request; - if (master->debug_level) - EC_DBG("Writing IDN 0x%04X (%zu byte) to slave %u.\n", - req->idn, req->data_size, slave->ring_position); + EC_SLAVE_DBG(slave, 1, "Writing IDN 0x%04X (%zu byte).\n", + req->idn, req->data_size); if (!(slave->sii.mailbox_protocols & EC_MBOX_SOE)) { - EC_ERR("Slave does not support SoE!\n"); + EC_SLAVE_ERR(slave, "Slave does not support SoE!\n"); fsm->state = ec_fsm_soe_error; ec_fsm_soe_print_error(fsm); return; @@ -550,7 +546,7 @@ if (datagram->state != EC_DATAGRAM_RECEIVED) { fsm->state = ec_fsm_soe_error; - EC_ERR("Failed to receive SoE write request: "); + EC_SLAVE_ERR(slave, "Failed to receive SoE write request: "); ec_datagram_print_state(datagram); ec_fsm_soe_print_error(fsm); return; @@ -566,8 +562,8 @@ } } fsm->state = ec_fsm_soe_error; - EC_ERR("Reception of SoE write request failed after %u ms: ", - (u32) diff_ms); + EC_SLAVE_ERR(slave, "Reception of SoE write request" + " failed after %u ms: ", (u32) diff_ms); ec_datagram_print_wc_error(datagram); ec_fsm_soe_print_error(fsm); return; @@ -595,7 +591,7 @@ if (datagram->state != EC_DATAGRAM_RECEIVED) { fsm->state = ec_fsm_soe_error; - EC_ERR("Failed to receive SoE write request datagram: "); + EC_SLAVE_ERR(slave, "Failed to receive SoE write request datagram: "); ec_datagram_print_state(datagram); ec_fsm_soe_print_error(fsm); return; @@ -603,7 +599,7 @@ if (datagram->working_counter != 1) { fsm->state = ec_fsm_soe_error; - EC_ERR("Reception of SoE write request datagram: "); + EC_SLAVE_ERR(slave, "Reception of SoE write request datagram: "); ec_datagram_print_wc_error(datagram); ec_fsm_soe_print_error(fsm); return; @@ -617,7 +613,7 @@ (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; if (diff_ms >= EC_SOE_RESPONSE_TIMEOUT) { fsm->state = ec_fsm_soe_error; - EC_ERR("Timeout after %u ms while waiting" + EC_SLAVE_ERR(slave, "Timeout after %u ms while waiting" " for write response.\n", (u32) diff_ms); ec_fsm_soe_print_error(fsm); return; @@ -654,7 +650,8 @@ if (datagram->state != EC_DATAGRAM_RECEIVED) { fsm->state = ec_fsm_soe_error; - EC_ERR("Failed to receive SoE write response datagram: "); + EC_SLAVE_ERR(slave, "Failed to receive SoE write" + " response datagram: "); ec_datagram_print_state(datagram); ec_fsm_soe_print_error(fsm); return; @@ -662,7 +659,7 @@ if (datagram->working_counter != 1) { fsm->state = ec_fsm_soe_error; - EC_ERR("Reception of SoE write response failed: "); + EC_SLAVE_ERR(slave, "Reception of SoE write response failed: "); ec_datagram_print_wc_error(datagram); ec_fsm_soe_print_error(fsm); return; @@ -676,21 +673,22 @@ } if (master->debug_level) { - EC_DBG("SCC write response:\n"); + EC_SLAVE_DBG(slave, 0, "SCC write response:\n"); ec_print_data(data, rec_size); } if (mbox_prot != EC_MBOX_TYPE_SOE) { fsm->state = ec_fsm_soe_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); ec_fsm_soe_print_error(fsm); return; } if (rec_size < EC_SOE_SIZE) { fsm->state = ec_fsm_soe_error; - EC_ERR("Received currupted SoE write response (%zu bytes)!\n", - rec_size); + EC_SLAVE_ERR(slave, "Received currupted SoE write response" + " (%zu bytes)!\n", rec_size); ec_print_data(data, rec_size); ec_fsm_soe_print_error(fsm); return; @@ -698,7 +696,8 @@ opcode = EC_READ_U8(data) & 0x7; if (opcode != OPCODE_WRITE_RESPONSE) { - EC_ERR("Received no write response (opcode %x).\n", opcode); + EC_SLAVE_ERR(slave, "Received no write response" + " (opcode %x).\n", opcode); ec_print_data(data, rec_size); ec_fsm_soe_print_error(fsm); fsm->state = ec_fsm_soe_error; @@ -707,7 +706,8 @@ idn = EC_READ_U16(data + 2); if (idn != req->idn) { - EC_ERR("Received response for wrong IDN 0x%04x.\n", idn); + EC_SLAVE_ERR(slave, "Received response for" + " wrong IDN 0x%04x.\n", idn); ec_print_data(data, rec_size); ec_fsm_soe_print_error(fsm); fsm->state = ec_fsm_soe_error; @@ -717,12 +717,13 @@ error_flag = (EC_READ_U8(data) >> 4) & 1; if (error_flag) { if (rec_size < EC_SOE_SIZE + 2) { - EC_ERR("Received corrupted error response - error flag set," - " but received size is %zu.\n", rec_size); + EC_SLAVE_ERR(slave, "Received corrupted error response" + " - error flag set, but received size is %zu.\n", + rec_size); } else { req->error_code = EC_READ_U16(data + EC_SOE_SIZE); - EC_ERR("Received error response:\n"); - ec_print_soe_error(req->error_code); + EC_SLAVE_ERR(slave, "Received error response:\n"); + ec_print_soe_error(slave, req->error_code); } ec_print_data(data, rec_size); ec_fsm_soe_print_error(fsm); diff -r ea38efeeb7b3 -r 6aa393418fb3 master/mailbox.c --- a/master/mailbox.c Fri May 07 15:26:26 2010 +0200 +++ b/master/mailbox.c Tue May 11 13:57:58 2010 +0200 @@ -58,15 +58,15 @@ int ret; if (unlikely(!slave->sii.mailbox_protocols)) { - EC_ERR("Slave %u does not support mailbox communication!\n", - slave->ring_position); + EC_SLAVE_ERR(slave, "Slave does not support mailbox" + " communication!\n"); return ERR_PTR(-EPROTONOSUPPORT); } total_size = EC_MBOX_HEADER_SIZE + size; if (unlikely(total_size > slave->configured_rx_mailbox_size)) { - EC_ERR("Data size (%zu) does not fit in mailbox (%u)!\n", + EC_SLAVE_ERR(slave, "Data size (%zu) does not fit in mailbox (%u)!\n", total_size, slave->configured_rx_mailbox_size); return ERR_PTR(-EOVERFLOW); } @@ -173,8 +173,7 @@ data_size = EC_READ_U16(datagram->data); if (data_size + EC_MBOX_HEADER_SIZE > slave->configured_tx_mailbox_size) { - EC_ERR("Corrupt mailbox response received from slave %u!\n", - slave->ring_position); + EC_SLAVE_ERR(slave, "Corrupt mailbox response received!\n"); ec_print_data(datagram->data, slave->configured_tx_mailbox_size); return ERR_PTR(-EPROTO); } @@ -186,8 +185,7 @@ const ec_code_msg_t *mbox_msg; uint16_t code = EC_READ_U16(datagram->data + 8); - EC_ERR("Mailbox error response received from slave %u - ", - slave->ring_position); + EC_SLAVE_ERR(slave, "Mailbox error response received - "); for (mbox_msg = mbox_error_messages; mbox_msg->code; mbox_msg++) { if (mbox_msg->code != code) diff -r ea38efeeb7b3 -r 6aa393418fb3 master/master.c --- a/master/master.c Fri May 07 15:26:26 2010 +0200 +++ b/master/master.c Tue May 11 13:57:58 2010 +0200 @@ -139,6 +139,7 @@ master->phase = EC_ORPHANED; master->active = 0; + master->config_changed = 0; master->injection_seq_fsm = 0; master->injection_seq_rt = 0; @@ -153,7 +154,7 @@ #endif master->dc_jiffies_app_time = 0; master->app_start_time = 0ULL; - master->has_start_time = 0; + master->has_app_time = 0; master->scan_busy = 0; master->allow_scan = 1; @@ -220,7 +221,7 @@ ret = ec_datagram_prealloc(&master->fsm_datagram, EC_MAX_DATA_SIZE); if (ret < 0) { ec_datagram_clear(&master->fsm_datagram); - EC_ERR("Failed to allocate FSM datagram.\n"); + EC_MASTER_ERR(master, "Failed to allocate FSM datagram.\n"); goto out_clear_backup; } @@ -233,7 +234,8 @@ ret = ec_datagram_apwr(&master->ref_sync_datagram, 0, 0x0910, 8); if (ret < 0) { ec_datagram_clear(&master->ref_sync_datagram); - EC_ERR("Failed to allocate reference synchronisation datagram.\n"); + EC_MASTER_ERR(master, "Failed to allocate reference" + " synchronisation datagram.\n"); goto out_clear_fsm; } @@ -243,7 +245,8 @@ ret = ec_datagram_prealloc(&master->sync_datagram, 4); if (ret < 0) { ec_datagram_clear(&master->sync_datagram); - EC_ERR("Failed to allocate synchronisation datagram.\n"); + EC_MASTER_ERR(master, "Failed to allocate" + " synchronisation datagram.\n"); goto out_clear_ref_sync; } @@ -253,7 +256,8 @@ ret = ec_datagram_brd(&master->sync_mon_datagram, 0x092c, 4); if (ret < 0) { ec_datagram_clear(&master->sync_mon_datagram); - EC_ERR("Failed to allocate sync monitoring datagram.\n"); + EC_MASTER_ERR(master, "Failed to allocate sync" + " monitoring datagram.\n"); goto out_clear_sync; } @@ -282,7 +286,7 @@ "EtherCAT%u", master->index); #endif if (IS_ERR(master->class_device)) { - EC_ERR("Failed to create class device!\n"); + EC_MASTER_ERR(master, "Failed to create class device!\n"); ret = PTR_ERR(master->class_device); goto out_clear_cdev; } @@ -386,33 +390,24 @@ // external requests are obsolete, so we wake pending waiters and remove // them from the list - // - // SII requests - while (1) { - ec_sii_write_request_t *request; - if (list_empty(&master->sii_requests)) - break; - // get first request - request = list_entry(master->sii_requests.next, - ec_sii_write_request_t, list); + + while (!list_empty(&master->sii_requests)) { + ec_sii_write_request_t *request = + list_entry(master->sii_requests.next, + ec_sii_write_request_t, list); list_del_init(&request->list); // dequeue - EC_INFO("Discarding SII request, slave %u does not exist anymore.\n", - request->slave->ring_position); + EC_MASTER_WARN(master, "Discarding SII request, slave %u about" + " to be deleted.\n", request->slave->ring_position); request->state = EC_INT_REQUEST_FAILURE; wake_up(&master->sii_queue); } - // Register requests - while (1) { - ec_reg_request_t *request; - if (list_empty(&master->reg_requests)) - break; - // get first request - request = list_entry(master->reg_requests.next, - ec_reg_request_t, list); + while (!list_empty(&master->reg_requests)) { + ec_reg_request_t *request = + list_entry(master->reg_requests.next, ec_reg_request_t, list); list_del_init(&request->list); // dequeue - EC_INFO("Discarding Reg request, slave %u does not exist anymore.\n", - request->slave->ring_position); + EC_MASTER_WARN(master, "Discarding register request, slave %u" + " about to be deleted.\n", request->slave->ring_position); request->state = EC_INT_REQUEST_FAILURE; wake_up(&master->reg_queue); } @@ -420,51 +415,6 @@ for (slave = master->slaves; slave < master->slaves + master->slave_count; slave++) { - // SDO requests - while (1) { - ec_master_sdo_request_t *request; - if (list_empty(&slave->slave_sdo_requests)) - break; - // get first request - request = list_entry(slave->slave_sdo_requests.next, - ec_master_sdo_request_t, list); - list_del_init(&request->list); // dequeue - EC_INFO("Discarding SDO request," - " slave %u does not exist anymore.\n", - slave->ring_position); - request->req.state = EC_INT_REQUEST_FAILURE; - wake_up(&slave->sdo_queue); - } - // FoE requests - while (1) { - ec_master_foe_request_t *request; - if (list_empty(&slave->foe_requests)) - break; - // get first request - request = list_entry(slave->foe_requests.next, - ec_master_foe_request_t, list); - list_del_init(&request->list); // dequeue - EC_INFO("Discarding FoE request," - " slave %u does not exist anymore.\n", - slave->ring_position); - request->req.state = EC_INT_REQUEST_FAILURE; - wake_up(&slave->foe_queue); - } - // SoE requests - while (1) { - ec_master_soe_request_t *request; - if (list_empty(&slave->soe_requests)) - break; - // get first request - request = list_entry(slave->soe_requests.next, - ec_master_soe_request_t, list); - list_del_init(&request->list); // dequeue - EC_INFO("Discarding SoE request," - " slave %u does not exist anymore.\n", - slave->ring_position); - request->req.state = EC_INT_REQUEST_FAILURE; - wake_up(&slave->soe_queue); - } ec_slave_clear(slave); } @@ -532,11 +482,12 @@ const char *name /**< Thread name. */ ) { - EC_INFO("Starting %s thread.\n", name); + EC_MASTER_INFO(master, "Starting %s thread.\n", name); master->thread = kthread_run(thread_func, master, name); if (IS_ERR(master->thread)) { int err = (int) PTR_ERR(master->thread); - EC_ERR("Failed to start master thread (error %i)!\n", err); + EC_MASTER_ERR(master, "Failed to start master thread (error %i)!\n", + err); master->thread = NULL; return err; } @@ -555,16 +506,15 @@ unsigned long sleep_jiffies; if (!master->thread) { - EC_WARN("ec_master_thread_stop(): Already finished!\n"); + EC_MASTER_WARN(master, "%s(): Already finished!\n", __func__); return; } - if (master->debug_level) - EC_DBG("Stopping master thread.\n"); + EC_MASTER_DBG(master, 1, "Stopping master thread.\n"); kthread_stop(master->thread); master->thread = NULL; - EC_INFO("Master thread exited.\n"); + EC_MASTER_INFO(master, "Master thread exited.\n"); if (master->fsm_datagram.state != EC_DATAGRAM_SENT) return; @@ -584,8 +534,7 @@ { int ret; - if (master->debug_level) - EC_DBG("ORPHANED -> IDLE.\n"); + EC_MASTER_DBG(master, 1, "ORPHANED -> IDLE.\n"); master->send_cb = ec_master_internal_send_cb; master->receive_cb = ec_master_internal_receive_cb; @@ -606,8 +555,7 @@ */ void ec_master_leave_idle_phase(ec_master_t *master /**< EtherCAT master */) { - if (master->debug_level) - EC_DBG("IDLE -> ORPHANED.\n"); + EC_MASTER_DBG(master, 1, "IDLE -> ORPHANED.\n"); master->phase = EC_ORPHANED; @@ -633,8 +581,7 @@ ec_eoe_t *eoe; #endif - if (master->debug_level) - EC_DBG("IDLE -> OPERATION.\n"); + EC_MASTER_DBG(master, 1, "IDLE -> OPERATION.\n"); down(&master->config_sem); master->allow_config = 0; // temporarily disable slave configuration @@ -645,12 +592,13 @@ ret = wait_event_interruptible(master->config_queue, !master->config_busy); if (ret) { - EC_INFO("Finishing slave configuration interrupted by signal.\n"); + EC_MASTER_INFO(master, "Finishing slave configuration" + " interrupted by signal.\n"); goto out_allow; } - if (master->debug_level) - EC_DBG("Waiting for pending slave configuration returned.\n"); + EC_MASTER_DBG(master, 1, "Waiting for pending slave" + " configuration returned.\n"); } else { up(&master->config_sem); } @@ -665,12 +613,13 @@ // wait for slave scan to complete ret = wait_event_interruptible(master->scan_queue, !master->scan_busy); if (ret) { - EC_INFO("Waiting for slave scan interrupted by signal.\n"); + EC_MASTER_INFO(master, "Waiting for slave scan" + " interrupted by signal.\n"); goto out_allow; } - if (master->debug_level) - EC_DBG("Waiting for pending slave scan returned.\n"); + EC_MASTER_DBG(master, 1, "Waiting for pending" + " slave scan returned.\n"); } // set states for all slaves @@ -711,8 +660,7 @@ if (master->active) ecrt_master_deactivate(master); - if (master->debug_level) - EC_DBG("OPERATION -> IDLE.\n"); + EC_MASTER_DBG(master, 1, "OPERATION -> IDLE.\n"); master->phase = EC_IDLE; } @@ -739,11 +687,9 @@ if (queue_size <= master->max_queue_size) { list_del_init(&datagram->queue); #if DEBUG_INJECT - if (master->debug_level) { - EC_DBG("Injecting external datagram %08x size=%u," - " queue_size=%u\n", (unsigned int) datagram, - datagram->data_size, queue_size); - } + EC_MASTER_DBG(master, 0, "Injecting external datagram %08x" + " size=%u, queue_size=%u\n", (unsigned int) datagram, + datagram->data_size, queue_size); #endif #ifdef EC_HAVE_CYCLES datagram->cycles_sent = 0; @@ -755,7 +701,7 @@ if (datagram->data_size > master->max_queue_size) { list_del_init(&datagram->queue); datagram->state = EC_DATAGRAM_ERROR; - EC_ERR("External datagram %p is too large," + EC_MASTER_ERR(master, "External datagram %p is too large," " size=%u, max_queue_size=%u\n", datagram, datagram->data_size, master->max_queue_size); @@ -782,14 +728,15 @@ time_us = (unsigned int) ((jiffies - datagram->jiffies_sent) * 1000000 / HZ); #endif - EC_ERR("Timeout %u us: injecting external datagram %p" - " size=%u, max_queue_size=%u\n", - time_us, datagram, + EC_MASTER_ERR(master, "Timeout %u us: Injecting" + " external datagram %p size=%u," + " max_queue_size=%u\n", time_us, datagram, datagram->data_size, master->max_queue_size); } #if DEBUG_INJECT - else if (master->debug_level) { - EC_DBG("Deferred injecting of external datagram %p" + else { + EC_MASTER_DBG(master, 0, "Deferred injecting" + " of external datagram %p" " size=%u, queue_size=%u\n", datagram, datagram->data_size, queue_size); } @@ -838,10 +785,8 @@ } #if DEBUG_INJECT - if (master->debug_level) { - EC_DBG("Requesting external datagram %p size=%u\n", - datagram, datagram->data_size); - } + EC_MASTER_DBG(master, 0, "Requesting external datagram %p size=%u\n", + datagram, datagram->data_size); #endif list_add_tail(&datagram->queue, &master->external_datagram_queue); @@ -872,8 +817,7 @@ list_for_each_entry(queued_datagram, &master->datagram_queue, queue) { if (queued_datagram == datagram) { datagram->skip_count++; - if (master->debug_level) - EC_DBG("skipping datagram %p.\n", datagram); + EC_MASTER_DBG(master, 1, "skipping datagram %p.\n", datagram); datagram->state = EC_DATAGRAM_QUEUED; return; } @@ -922,8 +866,7 @@ frame_count = 0; INIT_LIST_HEAD(&sent_datagrams); - if (unlikely(master->debug_level > 1)) - EC_DBG("ec_master_send_datagrams\n"); + EC_MASTER_DBG(master, 2, "ec_master_send_datagrams\n"); do { // fetch pointer to transmit socket buffer @@ -947,8 +890,8 @@ list_add_tail(&datagram->sent, &sent_datagrams); datagram->index = master->datagram_index++; - if (unlikely(master->debug_level > 1)) - EC_DBG("adding datagram 0x%02X\n", datagram->index); + EC_MASTER_DBG(master, 2, "adding datagram 0x%02X\n", + datagram->index); // set "datagram following" flag in previous frame if (follows_word) @@ -994,8 +937,7 @@ } if (list_empty(&sent_datagrams)) { - if (unlikely(master->debug_level > 1)) - EC_DBG("nothing to send.\n"); + EC_MASTER_DBG(master, 2, "nothing to send.\n"); break; } @@ -1007,8 +949,7 @@ while (cur_data - frame_data < ETH_ZLEN - ETH_HLEN) EC_WRITE_U8(cur_data++, 0x00); - if (unlikely(master->debug_level > 1)) - EC_DBG("frame size: %zu\n", cur_data - frame_data); + EC_MASTER_DBG(master, 2, "frame size: %zu\n", cur_data - frame_data); // send frame ec_device_send(&master->main_device, cur_data - frame_data); @@ -1034,8 +975,8 @@ #ifdef EC_HAVE_CYCLES if (unlikely(master->debug_level > 1)) { cycles_end = get_cycles(); - EC_DBG("ec_master_send_datagrams sent %u frames in %uus.\n", - frame_count, + EC_MASTER_DBG(master, 0, "ec_master_send_datagrams" + " sent %u frames in %uus.\n", frame_count, (unsigned int) (cycles_end - cycles_start) * 1000 / cpu_khz); } #endif @@ -1063,7 +1004,8 @@ if (unlikely(size < EC_FRAME_HEADER_SIZE)) { if (master->debug_level) { - EC_DBG("Corrupted frame received (size %zu < %u byte):\n", + EC_MASTER_DBG(master, 0, "Corrupted frame received" + " (size %zu < %u byte):\n", size, EC_FRAME_HEADER_SIZE); ec_print_data(frame_data, size); } @@ -1080,7 +1022,8 @@ if (unlikely(frame_size > size)) { if (master->debug_level) { - EC_DBG("Corrupted frame received (invalid frame size %zu for " + EC_MASTER_DBG(master, 0, "Corrupted frame received" + " (invalid frame size %zu for " "received size %zu):\n", frame_size, size); ec_print_data(frame_data, size); } @@ -1101,8 +1044,8 @@ if (unlikely(cur_data - frame_data + data_size + EC_DATAGRAM_FOOTER_SIZE > size)) { if (master->debug_level) { - EC_DBG("Corrupted frame received (invalid data size %zu):\n", - data_size); + EC_MASTER_DBG(master, 0, "Corrupted frame received" + " (invalid data size %zu):\n", data_size); ec_print_data(frame_data, size); } master->stats.corrupted++; @@ -1128,7 +1071,7 @@ ec_master_output_stats(master); if (unlikely(master->debug_level > 0)) { - EC_DBG("UNMATCHED datagram:\n"); + EC_MASTER_DBG(master, 0, "UNMATCHED datagram:\n"); ec_print_data(cur_data - EC_DATAGRAM_HEADER_SIZE, EC_DATAGRAM_HEADER_SIZE + data_size + EC_DATAGRAM_FOOTER_SIZE); @@ -1192,17 +1135,20 @@ master->stats.output_jiffies = jiffies; if (master->stats.timeouts) { - EC_WARN("%u datagram%s TIMED OUT!\n", master->stats.timeouts, + EC_MASTER_WARN(master, "%u datagram%s TIMED OUT!\n", + master->stats.timeouts, master->stats.timeouts == 1 ? "" : "s"); master->stats.timeouts = 0; } if (master->stats.corrupted) { - EC_WARN("%u frame%s CORRUPTED!\n", master->stats.corrupted, + EC_MASTER_WARN(master, "%u frame%s CORRUPTED!\n", + master->stats.corrupted, master->stats.corrupted == 1 ? "" : "s"); master->stats.corrupted = 0; } if (master->stats.unmatched) { - EC_WARN("%u datagram%s UNMATCHED!\n", master->stats.unmatched, + EC_MASTER_WARN(master, "%u datagram%s UNMATCHED!\n", + master->stats.unmatched, master->stats.unmatched == 1 ? "" : "s"); master->stats.unmatched = 0; } @@ -1299,10 +1245,9 @@ // send interval in IDLE phase ec_master_set_send_interval(master, 1000000 / HZ); - if (master->debug_level) - EC_DBG("Idle thread running with send interval = %d us," - " max data size=%d\n", master->send_interval, - master->max_queue_size); + EC_MASTER_DBG(master, 1, "Idle thread running with send interval = %d us," + " max data size=%d\n", master->send_interval, + master->max_queue_size); while (!kthread_should_stop()) { ec_datagram_output_stats(&master->fsm_datagram); @@ -1351,8 +1296,7 @@ } } - if (master->debug_level) - EC_DBG("Master IDLE thread exiting...\n"); + EC_MASTER_DBG(master, 1, "Master IDLE thread exiting...\n"); return 0; } @@ -1367,11 +1311,9 @@ ec_slave_t *slave = NULL; int fsm_exec; - if (master->debug_level) - EC_DBG("Operation thread running with fsm interval = %d us," - " max data size=%d\n", - master->send_interval, - master->max_queue_size); + EC_MASTER_DBG(master, 1, "Operation thread running" + " with fsm interval = %d us, max data size=%d\n", + master->send_interval, master->max_queue_size); while (!kthread_should_stop()) { ec_datagram_output_stats(&master->fsm_datagram); @@ -1411,8 +1353,7 @@ #endif } - if (master->debug_level) - EC_DBG("Master OP thread exiting...\n"); + EC_MASTER_DBG(master, 1, "Master OP thread exiting...\n"); return 0; } @@ -1426,7 +1367,7 @@ struct sched_param param = { .sched_priority = 0 }; if (master->eoe_thread) { - EC_WARN("EoE already running!\n"); + EC_MASTER_WARN(master, "EoE already running!\n"); return; } @@ -1434,16 +1375,18 @@ return; if (!master->send_cb || !master->receive_cb) { - EC_WARN("No EoE processing because of missing callbacks!\n"); + EC_MASTER_WARN(master, "No EoE processing" + " because of missing callbacks!\n"); return; } - EC_INFO("Starting EoE thread.\n"); + EC_MASTER_INFO(master, "Starting EoE thread.\n"); master->eoe_thread = kthread_run(ec_master_eoe_thread, master, "EtherCAT-EoE"); if (IS_ERR(master->eoe_thread)) { int err = (int) PTR_ERR(master->eoe_thread); - EC_ERR("Failed to start EoE thread (error %i)!\n", err); + EC_MASTER_ERR(master, "Failed to start EoE thread (error %i)!\n", + err); master->eoe_thread = NULL; return; } @@ -1459,11 +1402,11 @@ void ec_master_eoe_stop(ec_master_t *master /**< EtherCAT master */) { if (master->eoe_thread) { - EC_INFO("Stopping EoE thread.\n"); + EC_MASTER_INFO(master, "Stopping EoE thread.\n"); kthread_stop(master->eoe_thread); master->eoe_thread = NULL; - EC_INFO("EoE thread exited.\n"); + EC_MASTER_INFO(master, "EoE thread exited.\n"); } } @@ -1477,8 +1420,7 @@ ec_eoe_t *eoe; unsigned int none_open, sth_to_send, all_idle; - if (master->debug_level) - EC_DBG("EoE thread running.\n"); + EC_MASTER_DBG(master, 1, "EoE thread running.\n"); while (!kthread_should_stop()) { none_open = 1; @@ -1527,8 +1469,7 @@ } } - if (master->debug_level) - EC_DBG("EoE thread exiting...\n"); + EC_MASTER_DBG(master, 1, "EoE thread exiting...\n"); return 0; } #endif @@ -1802,13 +1743,14 @@ ) { if (level > 2) { - EC_ERR("Invalid debug level %u!\n", level); + EC_MASTER_ERR(master, "Invalid debug level %u!\n", level); return -EINVAL; } if (level != master->debug_level) { master->debug_level = level; - EC_INFO("Master debug level set to %u.\n", master->debug_level); + EC_MASTER_INFO(master, "Master debug level set to %u.\n", + master->debug_level); } return 0; @@ -1888,7 +1830,7 @@ return; if (ec_master_calc_topology_rec(master, NULL, &slave_position)) - EC_ERR("Failed to calculate bus topology.\n"); + EC_MASTER_ERR(master, "Failed to calculate bus topology.\n"); } /*****************************************************************************/ @@ -1930,6 +1872,37 @@ ec_master_calc_transmission_delays(master); } +/*****************************************************************************/ + +/** Request OP state for configured slaves. + */ +void ec_master_request_op( + ec_master_t *master /**< EtherCAT master. */ + ) +{ + unsigned int i; + ec_slave_t *slave; + + if (!master->active) + return; + + EC_MASTER_DBG(master, 1, "Requesting OP...\n"); + + // request OP for all configured slaves + for (i = 0; i < master->slave_count; i++) { + slave = master->slaves + i; + if (slave->config) { + ec_slave_request_state(slave, EC_SLAVE_STATE_OP); + } + } + + // always set DC reference clock to OP + if (master->dc_ref_clock) { + ec_slave_request_state(master->dc_ref_clock, + EC_SLAVE_STATE_OP); + } +} + /****************************************************************************** * Application interface *****************************************************************************/ @@ -1943,11 +1916,11 @@ ec_domain_t *domain, *last_domain; unsigned int index; - if (master->debug_level) - EC_DBG("ecrt_master_create_domain(master = 0x%p)\n", master); + EC_MASTER_DBG(master, 1, "ecrt_master_create_domain(master = 0x%p)\n", + master); if (!(domain = (ec_domain_t *) kmalloc(sizeof(ec_domain_t), GFP_KERNEL))) { - EC_ERR("Error allocating domain memory!\n"); + EC_MASTER_ERR(master, "Error allocating domain memory!\n"); return ERR_PTR(-ENOMEM); } @@ -1965,8 +1938,7 @@ up(&master->master_sem); - if (master->debug_level) - EC_DBG("Created domain %u.\n", domain->index); + EC_MASTER_DBG(master, 1, "Created domain %u.\n", domain->index); return domain; } @@ -1992,11 +1964,10 @@ int eoe_was_running; #endif - if (master->debug_level) - EC_DBG("ecrt_master_activate(master = 0x%p)\n", master); + EC_MASTER_DBG(master, 1, "ecrt_master_activate(master = 0x%p)\n", master); if (master->active) { - EC_WARN("%s: Master already active!\n", __func__); + EC_MASTER_WARN(master, "%s: Master already active!\n", __func__); return 0; } @@ -2008,17 +1979,12 @@ ret = ec_domain_finish(domain, domain_offset); if (ret < 0) { up(&master->master_sem); - EC_ERR("Failed to finish domain 0x%p!\n", domain); + EC_MASTER_ERR(master, "Failed to finish domain 0x%p!\n", domain); return ret; } domain_offset += domain->data_size; } - // always set DC reference clock to OP - if (master->dc_ref_clock) { - ec_slave_request_state(master->dc_ref_clock, EC_SLAVE_STATE_OP); - } - up(&master->master_sem); // restart EoE process and master thread with new locking @@ -2029,8 +1995,7 @@ ec_master_eoe_stop(master); #endif - if (master->debug_level) - EC_DBG("FSM datagram is %p.\n", &master->fsm_datagram); + EC_MASTER_DBG(master, 1, "FSM datagram is %p.\n", &master->fsm_datagram); master->injection_seq_fsm = 0; master->injection_seq_rt = 0; @@ -2047,13 +2012,17 @@ ret = ec_master_thread_start(master, ec_master_operation_thread, "EtherCAT-OP"); if (ret < 0) { - EC_ERR("Failed to start master thread!\n"); + EC_MASTER_ERR(master, "Failed to start master thread!\n"); return ret; } master->allow_config = 1; // request the current configuration master->allow_scan = 1; // allow re-scanning on topology change master->active = 1; + + // notify state machine, that the configuration shall now be applied + master->config_changed = 1; + return 0; } @@ -2067,11 +2036,11 @@ int eoe_was_running; #endif - if (master->debug_level) - EC_DBG("ecrt_master_deactivate(master = 0x%x)\n", (u32) master); + EC_MASTER_DBG(master, 1, "ecrt_master_deactivate(master = 0x%x)\n", + (u32) master); if (!master->active) { - EC_WARN("%s: Master not active.\n", __func__); + EC_MASTER_WARN(master, "%s: Master not active.\n", __func__); return; } @@ -2113,7 +2082,7 @@ master->app_time = 0ULL; master->app_start_time = 0ULL; - master->has_start_time = 0; + master->has_app_time = 0; #ifdef EC_EOE if (eoe_was_running) { @@ -2122,7 +2091,7 @@ #endif if (ec_master_thread_start(master, ec_master_idle_thread, "EtherCAT-IDLE")) - EC_WARN("Failed to restart master thread!\n"); + EC_MASTER_WARN(master, "Failed to restart master thread!\n"); master->allow_scan = 1; master->allow_config = 1; @@ -2195,7 +2164,8 @@ time_us = (unsigned int) ((master->main_device.jiffies_poll - datagram->jiffies_sent) * 1000000 / HZ); #endif - EC_DBG("TIMED OUT datagram %p, index %02X waited %u us.\n", + EC_MASTER_DBG(master, 0, "TIMED OUT datagram %p," + " index %02X waited %u us.\n", datagram, datagram->index, time_us); } } @@ -2229,10 +2199,10 @@ unsigned int found = 0; - if (master->debug_level) - EC_DBG("ecrt_master_slave_config(master = 0x%p, alias = %u, " - "position = %u, vendor_id = 0x%08x, product_code = 0x%08x)\n", - master, alias, position, vendor_id, product_code); + EC_MASTER_DBG(master, 1, "ecrt_master_slave_config(master = 0x%p," + " alias = %u, position = %u, vendor_id = 0x%08x," + " product_code = 0x%08x)\n", + master, alias, position, vendor_id, product_code); list_for_each_entry(sc, &master->configs, list) { if (sc->alias == alias && sc->position == position) { @@ -2243,20 +2213,21 @@ if (found) { // config with same alias/position already existing if (sc->vendor_id != vendor_id || sc->product_code != product_code) { - EC_ERR("Slave type mismatch. Slave was configured as" - " 0x%08X/0x%08X before. Now configuring with" - " 0x%08X/0x%08X.\n", sc->vendor_id, sc->product_code, + EC_MASTER_ERR(master, "Slave type mismatch. Slave was" + " configured as 0x%08X/0x%08X before. Now configuring" + " with 0x%08X/0x%08X.\n", sc->vendor_id, sc->product_code, vendor_id, product_code); return ERR_PTR(-ENOENT); } } else { - if (master->debug_level) - EC_DBG("Creating slave configuration for %u:%u, 0x%08X/0x%08X.\n", - alias, position, vendor_id, product_code); + EC_MASTER_DBG(master, 1, "Creating slave configuration for %u:%u," + " 0x%08X/0x%08X.\n", + alias, position, vendor_id, product_code); if (!(sc = (ec_slave_config_t *) kmalloc(sizeof(ec_slave_config_t), GFP_KERNEL))) { - EC_ERR("Failed to allocate memory for slave configuration.\n"); + EC_MASTER_ERR(master, "Failed to allocate memory" + " for slave configuration.\n"); return ERR_PTR(-ENOMEM); } @@ -2291,9 +2262,8 @@ int ecrt_master(ec_master_t *master, ec_master_info_t *master_info) { - if (master->debug_level) - EC_DBG("ecrt_master(master = 0x%p, master_info = 0x%p)\n", - master, master_info); + EC_MASTER_DBG(master, 1, "ecrt_master(master = 0x%p," + " master_info = 0x%p)\n", master, master_info); master_info->slave_count = master->slave_count; master_info->link_up = master->main_device.link_state; @@ -2342,10 +2312,9 @@ void ecrt_master_callbacks(ec_master_t *master, void (*send_cb)(void *), void (*receive_cb)(void *), void *cb_data) { - if (master->debug_level) - EC_DBG("ecrt_master_callbacks(master = 0x%p, send_cb = 0x%p, " - " receive_cb = 0x%p, cb_data = 0x%p)\n", master, - send_cb, receive_cb, cb_data); + EC_MASTER_DBG(master, 1, "ecrt_master_callbacks(master = 0x%p," + " send_cb = 0x%p, receive_cb = 0x%p, cb_data = 0x%p)\n", + master, send_cb, receive_cb, cb_data); master->app_send_cb = send_cb; master->app_receive_cb = receive_cb; @@ -2390,9 +2359,10 @@ #endif master->dc_jiffies_app_time = jiffies; - if (unlikely(!master->has_start_time)) { - master->app_start_time = app_time; - master->has_start_time = 1; + if (unlikely(!master->has_app_time)) { + EC_MASTER_DBG(master, 1, "set application start time = %llu\n",app_time); + master->app_start_time = app_time; + master->has_app_time = 1; } } diff -r ea38efeeb7b3 -r 6aa393418fb3 master/master.h --- a/master/master.h Fri May 07 15:26:26 2010 +0200 +++ b/master/master.h Tue May 11 13:57:58 2010 +0200 @@ -57,6 +57,61 @@ /*****************************************************************************/ +/** Convenience macro for printing master-specific information to syslog. + * + * This will print the message in \a fmt with a prefixed "EtherCAT : ", + * where INDEX is the master index. + * + * \param master EtherCAT master + * \param fmt format string (like in printf()) + * \param args arguments (optional) + */ +#define EC_MASTER_INFO(master, fmt, args...) \ + printk(KERN_INFO "EtherCAT %u: " fmt, master->index, ##args) + +/** Convenience macro for printing master-specific errors to syslog. + * + * This will print the message in \a fmt with a prefixed "EtherCAT : ", + * where INDEX is the master index. + * + * \param master EtherCAT master + * \param fmt format string (like in printf()) + * \param args arguments (optional) + */ +#define EC_MASTER_ERR(master, fmt, args...) \ + printk(KERN_ERR "EtherCAT ERROR %u: " fmt, master->index, ##args) + +/** Convenience macro for printing master-specific warnings to syslog. + * + * This will print the message in \a fmt with a prefixed "EtherCAT : ", + * where INDEX is the master index. + * + * \param master EtherCAT master + * \param fmt format string (like in printf()) + * \param args arguments (optional) + */ +#define EC_MASTER_WARN(master, fmt, args...) \ + printk(KERN_WARNING "EtherCAT WARNING %u: " fmt, master->index, ##args) + +/** Convenience macro for printing master-specific debug messages to syslog. + * + * This will print the message in \a fmt with a prefixed "EtherCAT : ", + * where INDEX is the master index. + * + * \param master EtherCAT master + * \param fmt format string (like in printf()) + * \param args arguments (optional) + */ +#define EC_MASTER_DBG(master, level, fmt, args...) \ + do { \ + if (master->debug_level >= level) { \ + printk(KERN_DEBUG "EtherCAT DEBUG %u: " fmt, \ + master->index, ##args); \ + } \ + } while (0) + +/*****************************************************************************/ + /** EtherCAT master phase. */ typedef enum { @@ -109,6 +164,7 @@ ec_datagram_t fsm_datagram; /**< Datagram used for state machines. */ ec_master_phase_t phase; /**< Master phase. */ unsigned int active; /**< Master has been activated. */ + unsigned int config_changed; /**< The configuration changed. */ unsigned int injection_seq_fsm; /**< Datagram injection sequence number for the FSM side. */ unsigned int injection_seq_rt; /**< Datagram injection sequence number @@ -122,7 +178,7 @@ u64 app_time; /**< Time of the last ecrt_master_sync() call. */ u64 app_start_time; /**< Application start time. */ - u8 has_start_time; /**< Start time already taken. */ + u8 has_app_time; /**< Application time is valid. */ ec_datagram_t ref_sync_datagram; /**< Datagram used for synchronizing the reference clock to the master clock. */ ec_datagram_t sync_datagram; /**< Datagram used for DC drift @@ -255,6 +311,7 @@ uint16_t, uint32_t, uint32_t); void ec_master_calc_dc(ec_master_t *); +void ec_master_request_op(ec_master_t *); void ec_master_internal_send_cb(void *); void ec_master_internal_receive_cb(void *); diff -r ea38efeeb7b3 -r 6aa393418fb3 master/module.c --- a/master/module.c Fri May 07 15:26:26 2010 +0200 +++ b/master/module.c Tue May 11 13:57:58 2010 +0200 @@ -104,7 +104,8 @@ sema_init(&master_sem, 1); if (master_count) { - if (alloc_chrdev_region(&device_number, 0, master_count, "EtherCAT")) { + if (alloc_chrdev_region(&device_number, + 0, master_count, "EtherCAT")) { EC_ERR("Failed to obtain device number(s)!\n"); ret = -EBUSY; goto out_return; @@ -140,7 +141,8 @@ if (master_count) { if (!(masters = kmalloc(sizeof(ec_master_t) * master_count, GFP_KERNEL))) { - EC_ERR("Failed to allocate memory for EtherCAT masters.\n"); + EC_ERR("Failed to allocate memory" + " for EtherCAT masters.\n"); ret = -ENOMEM; goto out_class; } @@ -489,7 +491,8 @@ if (master->debug_level) { ec_mac_print(net_dev->dev_addr, str); - EC_DBG("Master %u declined device %s.\n", master->index, str); + EC_MASTER_DBG(master, 0, "Master declined device %s.\n", + str); } } } @@ -527,7 +530,7 @@ if (master->reserved) { up(&master_sem); - EC_ERR("Master %u is already in use!\n", master_index); + EC_MASTER_ERR(master, "Master already in use!\n"); errptr = ERR_PTR(-EBUSY); goto out_return; } @@ -541,7 +544,7 @@ if (master->phase != EC_IDLE) { up(&master->device_sem); - EC_ERR("Master %u still waiting for devices!\n", master_index); + EC_MASTER_ERR(master, "Master still waiting for devices!\n"); errptr = ERR_PTR(-ENODEV); goto out_release; } @@ -556,7 +559,7 @@ up(&master->device_sem); if (ec_master_enter_operation_phase(master)) { - EC_ERR("Failed to enter OPERATION phase!\n"); + EC_MASTER_ERR(master, "Failed to enter OPERATION phase!\n"); errptr = ERR_PTR(-EIO); goto out_module_put; } @@ -584,10 +587,11 @@ void ecrt_release_master(ec_master_t *master) { - EC_INFO("Releasing master %u...\n", master->index); + EC_MASTER_INFO(master, "Releasing master...\n"); if (!master->reserved) { - EC_WARN("Master %u was was not requested!\n", master->index); + EC_MASTER_WARN(master, "%s(): Master was was not requested!\n", + __func__); return; } @@ -596,7 +600,7 @@ module_put(master->main_device.module); master->reserved = 0; - EC_INFO("Released master %u.\n", master->index); + EC_MASTER_INFO(master, "Released.\n"); } /*****************************************************************************/ diff -r ea38efeeb7b3 -r 6aa393418fb3 master/slave.c --- a/master/slave.c Fri May 07 15:26:26 2010 +0200 +++ b/master/slave.c Tue May 11 13:57:58 2010 +0200 @@ -161,17 +161,17 @@ // init state machine datagram ec_datagram_init(&slave->fsm_datagram); - snprintf(slave->fsm_datagram.name, EC_DATAGRAM_NAME_SIZE, "slave%u-fsm",slave->ring_position); + snprintf(slave->fsm_datagram.name, EC_DATAGRAM_NAME_SIZE, + "slave%u-fsm", slave->ring_position); ret = ec_datagram_prealloc(&slave->fsm_datagram, EC_MAX_DATA_SIZE); if (ret < 0) { ec_datagram_clear(&slave->fsm_datagram); - EC_ERR("Failed to allocate Slave %u FSM datagram.\n",slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to allocate FSM datagram.\n"); return; } // create state machine object ec_fsm_slave_init(&slave->fsm, slave, &slave->fsm_datagram); - } /*****************************************************************************/ @@ -187,6 +187,41 @@ unsigned int i; ec_pdo_t *pdo, *next_pdo; + // abort all pending requests + + while (!list_empty(&slave->slave_sdo_requests)) { + ec_master_sdo_request_t *request = + list_entry(slave->slave_sdo_requests.next, + ec_master_sdo_request_t, list); + list_del_init(&request->list); // dequeue + EC_SLAVE_WARN(slave, "Discarding SDO request," + " slave about to be deleted.\n"); + request->req.state = EC_INT_REQUEST_FAILURE; + wake_up(&slave->sdo_queue); + } + + while (!list_empty(&slave->foe_requests)) { + ec_master_foe_request_t *request = + list_entry(slave->foe_requests.next, + ec_master_foe_request_t, list); + list_del_init(&request->list); // dequeue + EC_SLAVE_WARN(slave, "Discarding FoE request," + " slave about to be deleted.\n"); + request->req.state = EC_INT_REQUEST_FAILURE; + wake_up(&slave->foe_queue); + } + + while (!list_empty(&slave->soe_requests)) { + ec_master_soe_request_t *request = + list_entry(slave->soe_requests.next, + ec_master_soe_request_t, list); + list_del_init(&request->list); // dequeue + EC_SLAVE_WARN(slave, "Discarding SoE request," + " slave about to be deleted.\n"); + request->req.state = EC_INT_REQUEST_FAILURE; + wake_up(&slave->soe_queue); + } + if (slave->config) ec_slave_config_detach(slave->config); @@ -218,7 +253,6 @@ kfree(slave->sii_words); ec_fsm_slave_clear(&slave->fsm); ec_datagram_clear(&slave->fsm_datagram); - } /*****************************************************************************/ @@ -254,8 +288,7 @@ cur_state[EC_STATE_STRING_SIZE]; ec_state_string(slave->current_state, old_state, 0); ec_state_string(new_state, cur_state, 0); - EC_DBG("Slave %u: %s -> %s.\n", - slave->ring_position, old_state, cur_state); + EC_SLAVE_DBG(slave, 0, "%s -> %s.\n", old_state, cur_state); } slave->current_state = new_state; } @@ -299,7 +332,7 @@ if (!(slave->sii.strings = kmalloc(sizeof(char *) * slave->sii.string_count, GFP_KERNEL))) { - EC_ERR("Failed to allocate string array memory.\n"); + EC_SLAVE_ERR(slave, "Failed to allocate string array memory.\n"); err = -ENOMEM; goto out_zero; } @@ -310,7 +343,7 @@ // allocate memory for string structure and data at a single blow if (!(slave->sii.strings[i] = kmalloc(sizeof(char) * size + 1, GFP_KERNEL))) { - EC_ERR("Failed to allocate string memory.\n"); + EC_SLAVE_ERR(slave, "Failed to allocate string memory.\n"); err = -ENOMEM; goto out_free; } @@ -349,8 +382,8 @@ uint8_t flags; if (data_size != 32) { - EC_ERR("Wrong size of general category (%zu/32) in slave %u.\n", - data_size, slave->ring_position); + EC_SLAVE_ERR(slave, "Wrong size of general category (%zu/32).\n", + data_size); return -EINVAL; } @@ -404,8 +437,8 @@ // one sync manager struct is 4 words long if (data_size % 8) { - EC_ERR("Invalid SII sync manager category size %zu in slave %u.\n", - data_size, slave->ring_position); + EC_SLAVE_ERR(slave, "Invalid SII sync manager category size %zu.\n", + data_size); return -EINVAL; } @@ -414,13 +447,14 @@ if (count) { total_count = count + slave->sii.sync_count; if (total_count > EC_MAX_SYNC_MANAGERS) { - EC_ERR("Exceeded maximum number of sync managers!\n"); + EC_SLAVE_ERR(slave, "Exceeded maximum number of" + " sync managers!\n"); return -EOVERFLOW; } memsize = sizeof(ec_sync_t) * total_count; if (!(syncs = kmalloc(memsize, GFP_KERNEL))) { - EC_ERR("Failed to allocate %zu bytes for sync managers.\n", - memsize); + EC_SLAVE_ERR(slave, "Failed to allocate %zu bytes" + " for sync managers.\n", memsize); return -ENOMEM; } @@ -469,7 +503,7 @@ while (data_size >= 8) { if (!(pdo = kmalloc(sizeof(ec_pdo_t), GFP_KERNEL))) { - EC_ERR("Failed to allocate PDO memory.\n"); + EC_SLAVE_ERR(slave, "Failed to allocate PDO memory.\n"); return -ENOMEM; } @@ -491,7 +525,7 @@ for (i = 0; i < entry_count; i++) { if (!(entry = kmalloc(sizeof(ec_pdo_entry_t), GFP_KERNEL))) { - EC_ERR("Failed to allocate PDO entry memory.\n"); + EC_SLAVE_ERR(slave, "Failed to allocate PDO entry memory.\n"); return -ENOMEM; } @@ -517,8 +551,8 @@ ec_sync_t *sync; if (!(sync = ec_slave_get_sync(slave, pdo->sync_index))) { - EC_ERR("Invalid SM index %i for PDO 0x%04X in slave %u.", - pdo->sync_index, pdo->index, slave->ring_position); + EC_SLAVE_ERR(slave, "Invalid SM index %i for PDO 0x%04X.", + pdo->sync_index, pdo->index); return -ENOENT; } @@ -547,9 +581,7 @@ return NULL; if (index >= slave->sii.string_count) { - if (slave->master->debug_level) - EC_WARN("String %u not found in slave %u.\n", - index, slave->ring_position); + EC_SLAVE_DBG(slave, 1, "String %u not found.\n", index); return NULL; } @@ -848,7 +880,8 @@ next_dc->ports[0].delay_to_next_dc = (rtt - next_rtt_sum) / 2; #if 0 - EC_DBG("delay %u:%u rtt=%u next_rtt_sum=%u delay=%u\n", + EC_SLAVE_DBG(slave, 1, "delay %u:%u rtt=%u" + " next_rtt_sum=%u delay=%u\n", slave->ring_position, i, rtt, next_rtt_sum, slave->ports[i].delay_to_next_dc); #endif @@ -868,7 +901,7 @@ ec_slave_t *next, *next_dc; #if 0 - EC_DBG("%u: %u\n", slave->ring_position, *delay); + EC_SLAVE_DBG(slave, 1, "%u\n", *delay); #endif slave->transmission_delay = *delay; @@ -884,7 +917,7 @@ *delay = *delay + port->delay_to_next_dc; #if 0 - EC_DBG("%u:%u %u\n", slave->ring_position, i, *delay); + EC_SLAVE_DBG(slave, 1, "%u:%u %u\n", slave->ring_position, i, *delay); #endif ec_slave_calc_transmission_delays_rec(next_dc, delay); } diff -r ea38efeeb7b3 -r 6aa393418fb3 master/slave.h --- a/master/slave.h Fri May 07 15:26:26 2010 +0200 +++ b/master/slave.h Tue May 11 13:57:58 2010 +0200 @@ -49,6 +49,68 @@ /*****************************************************************************/ +/** Convenience macro for printing slave-specific information to syslog. + * + * This will print the message in \a fmt with a prefixed + * "EtherCAT -: ", where INDEX is the master index and + * POSITION is the slave's ring position. + * + * \param slave EtherCAT slave + * \param fmt format string (like in printf()) + * \param args arguments (optional) + */ +#define EC_SLAVE_INFO(slave, fmt, args...) \ + printk(KERN_INFO "EtherCAT %u-%u: " fmt, slave->master->index, \ + slave->ring_position, ##args) + +/** Convenience macro for printing slave-specific errors to syslog. + * + * This will print the message in \a fmt with a prefixed + * "EtherCAT -: ", where INDEX is the master index and + * POSITION is the slave's ring position. + * + * \param slave EtherCAT slave + * \param fmt format string (like in printf()) + * \param args arguments (optional) + */ +#define EC_SLAVE_ERR(slave, fmt, args...) \ + printk(KERN_ERR "EtherCAT ERROR %u-%u: " fmt, slave->master->index, \ + slave->ring_position, ##args) + +/** Convenience macro for printing slave-specific warnings to syslog. + * + * This will print the message in \a fmt with a prefixed + * "EtherCAT -: ", where INDEX is the master index and + * POSITION is the slave's ring position. + * + * \param slave EtherCAT slave + * \param fmt format string (like in printf()) + * \param args arguments (optional) + */ +#define EC_SLAVE_WARN(slave, fmt, args...) \ + printk(KERN_WARNING "EtherCAT WARNING %u-%u: " fmt, \ + slave->master->index, slave->ring_position, ##args) + +/** Convenience macro for printing slave-specific debug messages to syslog. + * + * This will print the message in \a fmt with a prefixed + * "EtherCAT -: ", where INDEX is the master index and + * POSITION is the slave's ring position. + * + * \param slave EtherCAT slave + * \param fmt format string (like in printf()) + * \param args arguments (optional) + */ +#define EC_SLAVE_DBG(slave, level, fmt, args...) \ + do { \ + if (slave->master->debug_level >= level) { \ + printk(KERN_DEBUG "EtherCAT DEBUG %u-%u: " fmt, \ + slave->master->index, slave->ring_position, ##args); \ + } \ + } while (0) + +/*****************************************************************************/ + /** Slave port. */ typedef struct { diff -r ea38efeeb7b3 -r 6aa393418fb3 master/slave_config.c --- a/master/slave_config.c Fri May 07 15:26:26 2010 +0200 +++ b/master/slave_config.c Tue May 11 13:57:58 2010 +0200 @@ -176,8 +176,7 @@ } if (sc->used_fmmus == EC_MAX_FMMUS) { - EC_ERR("FMMU limit reached for slave configuration %u:%u!\n", - sc->alias, sc->position); + EC_CONFIG_ERR(sc, "FMMU limit reached!\n"); return -EOVERFLOW; } @@ -220,28 +219,22 @@ if (!(slave = ec_master_find_slave( sc->master, sc->alias, sc->position))) { - if (sc->master->debug_level) - EC_DBG("Failed to find slave for configuration %u:%u.\n", - sc->alias, sc->position); + EC_CONFIG_DBG(sc, 1, "Failed to find slave for configuration.\n"); return -ENOENT; } if (slave->config) { - if (sc->master->debug_level) - EC_DBG("Failed to attach slave configuration %u:%u. Slave %u" - " already has a configuration!\n", sc->alias, - sc->position, slave->ring_position); + EC_CONFIG_DBG(sc, 1, "Failed to attach configuration. Slave %u" + " already has a configuration!\n", slave->ring_position); return -EEXIST; } if (slave->sii.vendor_id != sc->vendor_id || slave->sii.product_code != sc->product_code) { - if (sc->master->debug_level) - EC_DBG("Slave %u has an invalid type (0x%08X/0x%08X) for" - " configuration %u:%u (0x%08X/0x%08X).\n", - slave->ring_position, slave->sii.vendor_id, - slave->sii.product_code, sc->alias, sc->position, - sc->vendor_id, sc->product_code); + EC_CONFIG_DBG(sc, 1, "Slave %u has an invalid type (0x%08X/0x%08X)" + " for configuration (0x%08X/0x%08X).\n", + slave->ring_position, slave->sii.vendor_id, + slave->sii.product_code, sc->vendor_id, sc->product_code); return -EINVAL; } @@ -251,9 +244,7 @@ ec_slave_request_state(slave, EC_SLAVE_STATE_OP); - if (sc->master->debug_level) - EC_DBG("Attached slave %u to config %u:%u.\n", - slave->ring_position, sc->alias, sc->position); + EC_CONFIG_DBG(sc, 1, "Attached slave %u.\n", slave->ring_position); return 0; } @@ -290,8 +281,8 @@ if ((sync = ec_slave_get_sync(sc->slave, sync_index))) { sync_config->dir = ec_sync_default_direction(sync); if (sync_config->dir == EC_DIR_INVALID) - EC_WARN("SM%u of slave %u has an invalid direction field!\n", - sync_index, sc->slave->ring_position); + EC_SLAVE_WARN(sc->slave, + "SM%u has an invalid direction field!\n", sync_index); ec_pdo_list_copy(&sync_config->pdos, &sync->pdos); } } @@ -313,9 +304,8 @@ if (!sc->slave) return; - if (sc->master->debug_level) - EC_DBG("Loading default mapping for PDO 0x%04X in config %u:%u.\n", - pdo->index, sc->alias, sc->position); + EC_CONFIG_DBG(sc, 1, "Loading default mapping for PDO 0x%04X.\n", + pdo->index); // find PDO in any sync manager (it could be reassigned later) for (i = 0; i < sc->slave->sii.sync_count; i++) { @@ -326,8 +316,8 @@ continue; if (default_pdo->name) { - if (sc->master->debug_level) - EC_DBG("Found PDO name \"%s\".\n", default_pdo->name); + EC_CONFIG_DBG(sc, 1, "Found PDO name \"%s\".\n", + default_pdo->name); // take PDO name from assigned one ec_pdo_set_name(pdo, default_pdo->name); @@ -340,7 +330,7 @@ if (sc->master->debug_level) { const ec_pdo_entry_t *entry; list_for_each_entry(entry, &pdo->entries, list) { - EC_DBG("Entry 0x%04X:%02X.\n", + EC_CONFIG_DBG(sc, 1, "Entry 0x%04X:%02X.\n", entry->index, entry->subindex); } } @@ -349,8 +339,7 @@ } } - if (sc->master->debug_level) - EC_DBG("No default mapping found.\n"); + EC_CONFIG_DBG(sc, 1, "No default mapping found.\n"); } /*****************************************************************************/ @@ -444,18 +433,17 @@ { ec_sync_config_t *sync_config; - if (sc->master->debug_level) - EC_DBG("ecrt_slave_config_sync_manager(sc = 0x%p, sync_index = %u, " - "dir = %i, watchdog_mode = %i)\n", - sc, sync_index, dir, watchdog_mode); + EC_CONFIG_DBG(sc, 1, "ecrt_slave_config_sync_manager(sc = 0x%p," + " sync_index = %u, dir = %i, watchdog_mode = %i)\n", + sc, sync_index, dir, watchdog_mode); if (sync_index >= EC_MAX_SYNC_MANAGERS) { - EC_ERR("Invalid sync manager index %u!\n", sync_index); + EC_CONFIG_ERR(sc, "Invalid sync manager index %u!\n", sync_index); return -ENOENT; } if (dir != EC_DIR_OUTPUT && dir != EC_DIR_INPUT) { - EC_ERR("Invalid direction %u!\n", (u32) dir); + EC_CONFIG_ERR(sc, "Invalid direction %u!\n", (u32) dir); return -EINVAL; } @@ -470,9 +458,8 @@ void ecrt_slave_config_watchdog(ec_slave_config_t *sc, uint16_t divider, uint16_t intervals) { - if (sc->master->debug_level) - EC_DBG("%s(sc = 0x%p, divider = %u, intervals = %u)\n", - __func__, sc, divider, intervals); + EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, divider = %u, intervals = %u)\n", + __func__, sc, divider, intervals); sc->watchdog_divider = divider; sc->watchdog_intervals = intervals; @@ -497,12 +484,11 @@ { ec_pdo_t *pdo; - if (sc->master->debug_level) - EC_DBG("ecrt_slave_config_pdo_assign_add(sc = 0x%p, sync_index = %u, " - "pdo_index = 0x%04X)\n", sc, sync_index, pdo_index); + EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, sync_index = %u, " + "pdo_index = 0x%04X)\n", __func__, sc, sync_index, pdo_index); if (sync_index >= EC_MAX_SYNC_MANAGERS) { - EC_ERR("Invalid sync manager index %u!\n", sync_index); + EC_CONFIG_ERR(sc, "Invalid sync manager index %u!\n", sync_index); return -EINVAL; } @@ -526,12 +512,11 @@ void ecrt_slave_config_pdo_assign_clear(ec_slave_config_t *sc, uint8_t sync_index) { - if (sc->master->debug_level) - EC_DBG("ecrt_slave_config_pdo_assign_clear(sc = 0x%p, " - "sync_index = %u)\n", sc, sync_index); + EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, sync_index = %u)\n", + __func__, sc, sync_index); if (sync_index >= EC_MAX_SYNC_MANAGERS) { - EC_ERR("Invalid sync manager index %u!\n", sync_index); + EC_CONFIG_ERR(sc, "Invalid sync manager index %u!\n", sync_index); return; } @@ -551,12 +536,11 @@ ec_pdo_entry_t *entry; int retval = 0; - if (sc->master->debug_level) - EC_DBG("ecrt_slave_config_pdo_mapping_add(sc = 0x%p, " - "pdo_index = 0x%04X, entry_index = 0x%04X, " - "entry_subindex = 0x%02X, entry_bit_length = %u)\n", - sc, pdo_index, entry_index, entry_subindex, - entry_bit_length); + EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, " + "pdo_index = 0x%04X, entry_index = 0x%04X, " + "entry_subindex = 0x%02X, entry_bit_length = %u)\n", + __func__, sc, pdo_index, entry_index, entry_subindex, + entry_bit_length); for (sync_index = 0; sync_index < EC_MAX_SYNC_MANAGERS; sync_index++) if ((pdo = ec_pdo_list_find_pdo( @@ -571,8 +555,7 @@ if (IS_ERR(entry)) retval = PTR_ERR(entry); } else { - EC_ERR("PDO 0x%04X is not assigned in config %u:%u.\n", - pdo_index, sc->alias, sc->position); + EC_CONFIG_ERR(sc, "PDO 0x%04X is not assigned.\n", pdo_index); retval = -ENOENT; } @@ -587,9 +570,8 @@ uint8_t sync_index; ec_pdo_t *pdo = NULL; - if (sc->master->debug_level) - EC_DBG("ecrt_slave_config_pdo_mapping_clear(sc = 0x%p, " - "pdo_index = 0x%04X)\n", sc, pdo_index); + EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, pdo_index = 0x%04X)\n", + __func__, sc, pdo_index); for (sync_index = 0; sync_index < EC_MAX_SYNC_MANAGERS; sync_index++) if ((pdo = ec_pdo_list_find_pdo( @@ -601,8 +583,7 @@ ec_pdo_clear_entries(pdo); up(&sc->master->master_sem); } else { - EC_WARN("PDO 0x%04X is not assigned in config %u:%u.\n", - pdo_index, sc->alias, sc->position); + EC_CONFIG_WARN(sc, "PDO 0x%04X is not assigned.\n", pdo_index); } } @@ -617,9 +598,8 @@ const ec_pdo_info_t *pdo_info; const ec_pdo_entry_info_t *entry_info; - if (sc->master->debug_level) - EC_DBG("ecrt_slave_config_pdos(sc = 0x%p, n_syncs = %u, " - "syncs = 0x%p)\n", sc, n_syncs, syncs); + EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, n_syncs = %u, syncs = 0x%p)\n", + __func__, sc, n_syncs, syncs); if (!syncs) return 0; @@ -631,7 +611,8 @@ break; if (sync_info->index >= EC_MAX_SYNC_MANAGERS) { - EC_ERR("Invalid sync manager index %u!\n", sync_info->index); + EC_CONFIG_ERR(sc, "Invalid sync manager index %u!\n", + sync_info->index); return -ENOENT; } @@ -689,10 +670,9 @@ ec_pdo_entry_t *entry; int sync_offset; - if (sc->master->debug_level) - EC_DBG("ecrt_slave_config_reg_pdo_entry(sc = 0x%p, index = 0x%04X, " - "subindex = 0x%02X, domain = 0x%p, bit_position = 0x%p)\n", - sc, index, subindex, domain, bit_position); + EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, index = 0x%04X, " + "subindex = 0x%02X, domain = 0x%p, bit_position = 0x%p)\n", + __func__, sc, index, subindex, domain, bit_position); for (sync_index = 0; sync_index < EC_MAX_SYNC_MANAGERS; sync_index++) { sync_config = &sc->sync_configs[sync_index]; @@ -707,9 +687,8 @@ if (bit_position) { *bit_position = bit_pos; } else if (bit_pos) { - EC_ERR("PDO entry 0x%04X:%02X does not byte-align " - "in config %u:%u.\n", index, subindex, - sc->alias, sc->position); + EC_CONFIG_ERR(sc, "PDO entry 0x%04X:%02X does" + " not byte-align.\n", index, subindex); return -EFAULT; } @@ -724,8 +703,8 @@ } } - EC_ERR("PDO entry 0x%04X:%02X is not mapped in slave config %u:%u.\n", - index, subindex, sc->alias, sc->position); + EC_CONFIG_ERR(sc, "PDO entry 0x%04X:%02X is not mapped.\n", + index, subindex); return -ENOENT; } @@ -751,19 +730,18 @@ ec_sdo_request_t *req; int ret; - if (sc->master->debug_level) - EC_DBG("ecrt_slave_config_sdo(sc = 0x%p, index = 0x%04X, " - "subindex = 0x%02X, data = 0x%p, size = %zu)\n", sc, - index, subindex, data, size); + EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, index = 0x%04X, " + "subindex = 0x%02X, data = 0x%p, size = %zu)\n", + __func__, sc, index, subindex, data, size); if (slave && !(slave->sii.mailbox_protocols & EC_MBOX_COE)) { - EC_ERR("Slave %u does not support CoE!\n", slave->ring_position); - return -EPROTONOSUPPORT; // protocol not supported + EC_CONFIG_WARN(sc, "Attached slave does not support CoE!\n"); } if (!(req = (ec_sdo_request_t *) kmalloc(sizeof(ec_sdo_request_t), GFP_KERNEL))) { - EC_ERR("Failed to allocate memory for SDO configuration!\n"); + EC_CONFIG_ERR(sc, "Failed to allocate memory for" + " SDO configuration!\n"); return -ENOMEM; } @@ -790,10 +768,9 @@ { uint8_t data[1]; - if (sc->master->debug_level) - EC_DBG("ecrt_slave_config_sdo8(sc = 0x%p, index = 0x%04X, " - "subindex = 0x%02X, value = %u)\n", sc, - index, subindex, (u32) value); + EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, index = 0x%04X, " + "subindex = 0x%02X, value = %u)\n", + __func__, sc, index, subindex, (u32) value); EC_WRITE_U8(data, value); return ecrt_slave_config_sdo(sc, index, subindex, data, 1); @@ -806,10 +783,9 @@ { uint8_t data[2]; - if (sc->master->debug_level) - EC_DBG("ecrt_slave_config_sdo16(sc = 0x%p, index = 0x%04X, " - "subindex = 0x%02X, value = %u)\n", sc, - index, subindex, value); + EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, index = 0x%04X, " + "subindex = 0x%02X, value = %u)\n", + __func__, sc, index, subindex, value); EC_WRITE_U16(data, value); return ecrt_slave_config_sdo(sc, index, subindex, data, 2); @@ -822,10 +798,9 @@ { uint8_t data[4]; - if (sc->master->debug_level) - EC_DBG("ecrt_slave_config_sdo32(sc = 0x%p, index = 0x%04X, " - "subindex = 0x%02X, value = %u)\n", sc, - index, subindex, value); + EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, index = 0x%04X, " + "subindex = 0x%02X, value = %u)\n", + __func__, sc, index, subindex, value); EC_WRITE_U32(data, value); return ecrt_slave_config_sdo(sc, index, subindex, data, 4); @@ -840,18 +815,17 @@ ec_sdo_request_t *req; int ret; - if (sc->master->debug_level) - EC_DBG("ecrt_slave_config_complete_sdo(sc = 0x%p, index = 0x%04X, " - "data = 0x%p, size = %zu)\n", sc, index, data, size); + EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, index = 0x%04X, " + "data = 0x%p, size = %zu)\n", __func__, sc, index, data, size); if (slave && !(slave->sii.mailbox_protocols & EC_MBOX_COE)) { - EC_ERR("Slave %u does not support CoE!\n", slave->ring_position); - return -EPROTONOSUPPORT; // protocol not supported + EC_CONFIG_WARN(sc, "Attached slave does not support CoE!\n"); } if (!(req = (ec_sdo_request_t *) kmalloc(sizeof(ec_sdo_request_t), GFP_KERNEL))) { - EC_ERR("Failed to allocate memory for SDO configuration!\n"); + EC_CONFIG_ERR(sc, "Failed to allocate memory for" + " SDO configuration!\n"); return -ENOMEM; } @@ -883,14 +857,13 @@ ec_sdo_request_t *req; int ret; - if (sc->master->debug_level) - EC_DBG("ecrt_slave_config_create_sdo_request(sc = 0x%p, " - "index = 0x%04X, subindex = 0x%02X, size = %zu)\n", sc, - index, subindex, size); + EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, " + "index = 0x%04X, subindex = 0x%02X, size = %zu)\n", + __func__, sc, index, subindex, size); if (!(req = (ec_sdo_request_t *) kmalloc(sizeof(ec_sdo_request_t), GFP_KERNEL))) { - EC_ERR("Failed to allocate SDO request memory!\n"); + EC_CONFIG_ERR(sc, "Failed to allocate SDO request memory!\n"); return ERR_PTR(-ENOMEM); } @@ -936,13 +909,11 @@ ec_voe_handler_t *voe; int ret; - if (sc->master->debug_level) - EC_DBG("ecrt_slave_config_create_voe_handler(sc = 0x%p, size = %zu)\n", - sc, size); + EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, size = %zu)\n", __func__, sc, size); if (!(voe = (ec_voe_handler_t *) kmalloc(sizeof(ec_voe_handler_t), GFP_KERNEL))) { - EC_ERR("Failed to allocate VoE request memory!\n"); + EC_CONFIG_ERR(sc, "Failed to allocate VoE request memory!\n"); return ERR_PTR(-ENOMEM); } @@ -995,18 +966,17 @@ ec_soe_request_t *req; int ret; - if (sc->master->debug_level) - EC_DBG("ecrt_slave_config_idn(sc = 0x%p, idn = 0x%04X, " - "data = 0x%p, size = %zu)\n", sc, idn, data, size); + EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, idn = 0x%04X, " + "data = 0x%p, size = %zu)\n", __func__, sc, idn, data, size); if (slave && !(slave->sii.mailbox_protocols & EC_MBOX_SOE)) { - EC_ERR("Slave %u does not support SoE!\n", slave->ring_position); - return -EPROTONOSUPPORT; // protocol not supported + EC_CONFIG_WARN(sc, "Attached slave does not support SoE!\n"); } if (!(req = (ec_soe_request_t *) kmalloc(sizeof(ec_soe_request_t), GFP_KERNEL))) { - EC_ERR("Failed to allocate memory for IDN configuration!\n"); + EC_CONFIG_ERR(sc, "Failed to allocate memory for" + " IDN configuration!\n"); return -ENOMEM; } diff -r ea38efeeb7b3 -r 6aa393418fb3 master/slave_config.h --- a/master/slave_config.h Fri May 07 15:26:26 2010 +0200 +++ b/master/slave_config.h Tue May 11 13:57:58 2010 +0200 @@ -46,6 +46,70 @@ /*****************************************************************************/ +/** Convenience macro for printing configuration-specific information to + * syslog. + * + * This will print the message in \a fmt with a prefixed + * "EtherCAT :: ", where INDEX is the master index + * and ALIAS and POSITION identify the configuration. + * + * \param sc EtherCAT slave configuration + * \param fmt format string (like in printf()) + * \param args arguments (optional) + */ +#define EC_CONFIG_INFO(sc, fmt, args...) \ + printk(KERN_INFO "EtherCAT %u %u:%u: " fmt, sc->master->index, \ + sc->alias, sc->position, ##args) + +/** Convenience macro for printing configuration-specific errors to syslog. + * + * This will print the message in \a fmt with a prefixed + * "EtherCAT :: ", where INDEX is the master index + * and ALIAS and POSITION identify the configuration. + * + * \param sc EtherCAT slave configuration + * \param fmt format string (like in printf()) + * \param args arguments (optional) + */ +#define EC_CONFIG_ERR(sc, fmt, args...) \ + printk(KERN_ERR "EtherCAT ERROR %u %u:%u: " fmt, sc->master->index, \ + sc->alias, sc->position, ##args) + +/** Convenience macro for printing configuration-specific warnings to syslog. + * + * This will print the message in \a fmt with a prefixed + * "EtherCAT :: ", where INDEX is the master index + * and ALIAS and POSITION identify the configuration. + * + * \param sc EtherCAT slave configuration + * \param fmt format string (like in printf()) + * \param args arguments (optional) + */ +#define EC_CONFIG_WARN(sc, fmt, args...) \ + printk(KERN_WARNING "EtherCAT WARNING %u %u:%u: " fmt, \ + sc->master->index, sc->alias, sc->position, ##args) + +/** Convenience macro for printing configuration-specific debug messages to + * syslog. + * + * This will print the message in \a fmt with a prefixed + * "EtherCAT :: ", where INDEX is the master index + * and ALIAS and POSITION identify the configuration. + * + * \param sc EtherCAT slave configuration + * \param fmt format string (like in printf()) + * \param args arguments (optional) + */ +#define EC_CONFIG_DBG(sc, level, fmt, args...) \ + do { \ + if (sc->master->debug_level >= level) { \ + printk(KERN_DEBUG "EtherCAT DEBUG %u %u:%u: " fmt, \ + sc->master->index, sc->alias, sc->position, ##args); \ + } \ + } while (0) + +/*****************************************************************************/ + /** EtherCAT slave configuration. */ struct ec_slave_config { diff -r ea38efeeb7b3 -r 6aa393418fb3 master/sync.c --- a/master/sync.c Fri May 07 15:26:26 2010 +0200 +++ b/master/sync.c Tue May 11 13:57:58 2010 +0200 @@ -129,10 +129,10 @@ } } - if (sync->slave->master->debug_level) - EC_DBG("SM%u: Addr 0x%04X, Size %3u, Ctrl 0x%02X, En %u\n", - sync_index, sync->physical_start_address, - data_size, control, enable); + EC_SLAVE_DBG(sync->slave, 1, "SM%u: Addr 0x%04X, Size %3u," + " Ctrl 0x%02X, En %u\n", + sync_index, sync->physical_start_address, + data_size, control, enable); EC_WRITE_U16(data, sync->physical_start_address); EC_WRITE_U16(data + 2, data_size); diff -r ea38efeeb7b3 -r 6aa393418fb3 master/voe_handler.c --- a/master/voe_handler.c Fri May 07 15:26:26 2010 +0200 +++ b/master/voe_handler.c Tue May 11 13:57:58 2010 +0200 @@ -212,13 +212,13 @@ uint8_t *data; if (slave->master->debug_level) { - EC_DBG("Writing %zu bytes of VoE data to slave %u.\n", - voe->data_size, slave->ring_position); + EC_SLAVE_DBG(slave, 0, "Writing %zu bytes of VoE data.\n", + voe->data_size); ec_print_data(ecrt_voe_handler_data(voe), voe->data_size); } if (!(slave->sii.mailbox_protocols & EC_MBOX_VOE)) { - EC_ERR("Slave %u does not support VoE!\n", slave->ring_position); + EC_SLAVE_ERR(slave, "Slave does not support VoE!\n"); voe->state = ec_voe_handler_state_error; voe->request_state = EC_INT_REQUEST_FAILURE; return; @@ -256,8 +256,7 @@ if (datagram->state != EC_DATAGRAM_RECEIVED) { voe->state = ec_voe_handler_state_error; voe->request_state = EC_INT_REQUEST_FAILURE; - EC_ERR("Failed to receive VoE write request datagram for" - " slave %u: ", slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to receive VoE write request datagram: "); ec_datagram_print_state(datagram); return; } @@ -267,25 +266,21 @@ unsigned long diff_ms = (jiffies - voe->jiffies_start) * 1000 / HZ; if (diff_ms < EC_VOE_RESPONSE_TIMEOUT) { - if (slave->master->debug_level) { - EC_DBG("Slave %u did not respond to VoE write request. " - "Retrying after %u ms...\n", - slave->ring_position, (u32) diff_ms); - } + EC_SLAVE_DBG(slave, 1, "Slave did not respond to" + " VoE write request. Retrying after %u ms...\n", + (u32) diff_ms); // no response; send request datagram again return; } } voe->state = ec_voe_handler_state_error; voe->request_state = EC_INT_REQUEST_FAILURE; - EC_ERR("Reception of VoE write request failed on slave %u: ", - slave->ring_position); + EC_SLAVE_ERR(slave, "Reception of VoE write request failed: "); ec_datagram_print_wc_error(datagram); return; } - if (voe->config->master->debug_level) - EC_DBG("VoE write request successful.\n"); + EC_CONFIG_DBG(voe->config, 1, "VoE write request successful.\n"); voe->request_state = EC_INT_REQUEST_SUCCESS; voe->state = ec_voe_handler_state_end; @@ -300,11 +295,10 @@ ec_datagram_t *datagram = &voe->datagram; ec_slave_t *slave = voe->config->slave; - if (slave->master->debug_level) - EC_DBG("Reading VoE data to slave %u.\n", slave->ring_position); + EC_SLAVE_DBG(slave, 1, "Reading VoE data.\n"); if (!(slave->sii.mailbox_protocols & EC_MBOX_VOE)) { - EC_ERR("Slave %u does not support VoE!\n", slave->ring_position); + EC_SLAVE_ERR(slave, "Slave does not support VoE!\n"); voe->state = ec_voe_handler_state_error; voe->request_state = EC_INT_REQUEST_FAILURE; return; @@ -332,8 +326,7 @@ if (datagram->state != EC_DATAGRAM_RECEIVED) { voe->state = ec_voe_handler_state_error; voe->request_state = EC_INT_REQUEST_FAILURE; - EC_ERR("Failed to receive VoE mailbox check datagram from slave %u: ", - slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to receive VoE mailbox check datagram: "); ec_datagram_print_state(datagram); return; } @@ -341,8 +334,8 @@ if (datagram->working_counter != 1) { voe->state = ec_voe_handler_state_error; voe->request_state = EC_INT_REQUEST_FAILURE; - EC_ERR("Reception of VoE mailbox check" - " datagram failed on slave %u: ", slave->ring_position); + EC_SLAVE_ERR(slave, "Reception of VoE mailbox check" + " datagram failed: "); ec_datagram_print_wc_error(datagram); return; } @@ -353,8 +346,7 @@ if (diff_ms >= EC_VOE_RESPONSE_TIMEOUT) { voe->state = ec_voe_handler_state_error; voe->request_state = EC_INT_REQUEST_FAILURE; - EC_ERR("Timeout while waiting for VoE data on " - "slave %u.\n", slave->ring_position); + EC_SLAVE_ERR(slave, "Timeout while waiting for VoE data.\n"); return; } @@ -387,8 +379,7 @@ if (datagram->state != EC_DATAGRAM_RECEIVED) { voe->state = ec_voe_handler_state_error; voe->request_state = EC_INT_REQUEST_FAILURE; - EC_ERR("Failed to receive VoE read datagram for" - " slave %u: ", slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to receive VoE read datagram: "); ec_datagram_print_state(datagram); return; } @@ -396,8 +387,7 @@ if (datagram->working_counter != 1) { voe->state = ec_voe_handler_state_error; voe->request_state = EC_INT_REQUEST_FAILURE; - EC_ERR("Reception of VoE read response failed on slave %u: ", - slave->ring_position); + EC_SLAVE_ERR(slave, "Reception of VoE read response failed: "); ec_datagram_print_wc_error(datagram); return; } @@ -412,7 +402,8 @@ if (mbox_prot != EC_MBOX_TYPE_VOE) { voe->state = ec_voe_handler_state_error; voe->request_state = EC_INT_REQUEST_FAILURE; - 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); ec_print_data(data, rec_size); return; } @@ -420,12 +411,13 @@ if (rec_size < EC_VOE_HEADER_SIZE) { voe->state = ec_voe_handler_state_error; voe->request_state = EC_INT_REQUEST_FAILURE; - EC_ERR("Received VoE header is incomplete (%zu bytes)!\n", rec_size); + EC_SLAVE_ERR(slave, "Received VoE header is" + " incomplete (%zu bytes)!\n", rec_size); return; } if (master->debug_level) { - EC_DBG("VoE data:\n"); + EC_CONFIG_DBG(voe->config, 0, "VoE data:\n"); ec_print_data(data, rec_size); } @@ -443,11 +435,10 @@ ec_datagram_t *datagram = &voe->datagram; ec_slave_t *slave = voe->config->slave; - if (slave->master->debug_level) - EC_DBG("Reading VoE data to slave %u.\n", slave->ring_position); + EC_SLAVE_DBG(slave, 1, "Reading VoE data.\n"); if (!(slave->sii.mailbox_protocols & EC_MBOX_VOE)) { - EC_ERR("Slave %u does not support VoE!\n", slave->ring_position); + EC_SLAVE_ERR(slave, "Slave does not support VoE!\n"); voe->state = ec_voe_handler_state_error; voe->request_state = EC_INT_REQUEST_FAILURE; return; @@ -479,8 +470,7 @@ if (datagram->state != EC_DATAGRAM_RECEIVED) { voe->state = ec_voe_handler_state_error; voe->request_state = EC_INT_REQUEST_FAILURE; - EC_ERR("Failed to receive VoE read datagram for" - " slave %u: ", slave->ring_position); + EC_SLAVE_ERR(slave, "Failed to receive VoE read datagram: "); ec_datagram_print_state(datagram); return; } @@ -488,16 +478,14 @@ if (datagram->working_counter == 0) { voe->state = ec_voe_handler_state_error; voe->request_state = EC_INT_REQUEST_FAILURE; - if (master->debug_level) - EC_DBG("Slave %u did not send VoE data.\n", slave->ring_position); + EC_SLAVE_DBG(slave, 1, "Slave did not send VoE data.\n"); return; } if (datagram->working_counter != 1) { voe->state = ec_voe_handler_state_error; voe->request_state = EC_INT_REQUEST_FAILURE; - EC_WARN("Reception of VoE read response failed on slave %u: ", - slave->ring_position); + EC_SLAVE_WARN(slave, "Reception of VoE read response failed: "); ec_datagram_print_wc_error(datagram); return; } @@ -512,7 +500,8 @@ if (mbox_prot != EC_MBOX_TYPE_VOE) { voe->state = ec_voe_handler_state_error; voe->request_state = EC_INT_REQUEST_FAILURE; - 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); ec_print_data(data, rec_size); return; } @@ -520,12 +509,13 @@ if (rec_size < EC_VOE_HEADER_SIZE) { voe->state = ec_voe_handler_state_error; voe->request_state = EC_INT_REQUEST_FAILURE; - EC_ERR("Received VoE header is incomplete (%zu bytes)!\n", rec_size); + EC_SLAVE_ERR(slave, "Received VoE header is" + " incomplete (%zu bytes)!\n", rec_size); return; } if (master->debug_level) { - EC_DBG("VoE data:\n"); + EC_CONFIG_DBG(voe->config, 1, "VoE data:\n"); ec_print_data(data, rec_size); } diff -r ea38efeeb7b3 -r 6aa393418fb3 tool/Command.cpp --- a/tool/Command.cpp Fri May 07 15:26:26 2010 +0200 +++ b/tool/Command.cpp Tue May 11 13:57:58 2010 +0200 @@ -371,14 +371,22 @@ string Command::alStateString(uint8_t state) { - switch (state) { - case 1: return "INIT"; - case 2: return "PREOP"; - case 3: return "BOOT"; - case 4: return "SAFEOP"; - case 8: return "OP"; - default: return "???"; - } -} - -/****************************************************************************/ + string ret; + + switch (state & EC_SLAVE_STATE_MASK) { + case 1: ret = "INIT"; break; + case 2: ret = "PREOP"; break; + case 3: ret = "BOOT"; break; + case 4: ret = "SAFEOP"; break; + case 8: ret = "OP"; break; + default: ret = "???"; + } + + if (state & EC_SLAVE_STATE_ACK_ERR) { + ret += "+ERROR"; + } + + return ret; +} + +/****************************************************************************/ diff -r ea38efeeb7b3 -r 6aa393418fb3 tool/CommandConfig.cpp --- a/tool/CommandConfig.cpp Fri May 07 15:26:26 2010 +0200 +++ b/tool/CommandConfig.cpp Tue May 11 13:57:58 2010 +0200 @@ -330,10 +330,7 @@ str.clear(); str.str(""); - str << alStateString(slave.al_state); - info.state = str.str(); - str.clear(); - str.str(""); + info.state = alStateString(slave.al_state); } else { str << "-"; info.slavePos = str.str();