bkinnightskytw
/
ethercat
mirror of https://gitlab.com/bkinnightskytw/ethercat.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

merged

This commit is contained in:
Martin Troxler 2010-05-11 13:57:58 +02:00
parent 2812f5b36b 337f2c09ef
commit d135711521
34 changed files with 1903 additions and 1746 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
TODO
View File

@ -18,9 +18,7 @@ Version 1.5.0:
* 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 @@ Version 1.5.0:
* 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:

2
documentation/Makefile
View File

@ -30,6 +30,8 @@ COMMANDS := \
sii_read \
sii_write \
slaves \
soe_read \
soe_write \
states \
upload \
version \

82
documentation/ethercat_doc.tex
View File

@ -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 @@ EtherCAT functionality (see chap.~\ref{chap:api}).
\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 @@ EtherCAT functionality (see chap.~\ref{chap:api}).
\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 @@ physical memory to logical process data images.
% 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 @@ registers can also be cyclically read via the command-line-tool (see
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 @@ mapped PDO entry index, subindex and bit size.
\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 @@ exchange data objects on application level.
% 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 @@ the example applications provided in the \textit{examples/} subdirectory.
%------------------------------------------------------------------------------
\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 @@ $ `\textbf{ethercat slaves}`
%------------------------------------------------------------------------------
\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 @@ misunderstandings. In: IEE journal ``Computing and Control Engineering'',
\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

26
documentation/graphs/fsm_master.dot
View File

@ -5,14 +5,14 @@ digraph master {
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 @@ digraph master {
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 @@ digraph master {
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
write_system_times -> start
configure_slave [fontname="Helvetica"]
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
}

247
master/cdev.c
View File

@ -123,7 +123,7 @@ int ec_cdev_init(
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 @@ int ec_cdev_ioctl_slave(
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 @@ int ec_cdev_ioctl_slave_sync(
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 @@ int ec_cdev_ioctl_slave_sync_pdo(
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 @@ int ec_cdev_ioctl_slave_sync_pdo(
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 @@ int ec_cdev_ioctl_slave_sync_pdo_entry(
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 @@ int ec_cdev_ioctl_slave_sync_pdo_entry(
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 @@ int ec_cdev_ioctl_domain(
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 @@ int ec_cdev_ioctl_domain_fmmu(
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 @@ int ec_cdev_ioctl_domain_data(
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 @@ int ec_cdev_ioctl_slave_state(
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 @@ int ec_cdev_ioctl_slave_sdo(
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 @@ int ec_cdev_ioctl_slave_sdo_entry(
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 @@ int ec_cdev_ioctl_slave_sdo_entry(
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 @@ int ec_cdev_ioctl_slave_sdo_entry(
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 @@ int ec_cdev_ioctl_slave_sdo_upload(
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);
EC_MASTER_ERR(master, "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_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 @@ int ec_cdev_ioctl_slave_sdo_upload(
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 @@ int ec_cdev_ioctl_slave_sdo_upload(
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 @@ int ec_cdev_ioctl_slave_sdo_download(
// 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 @@ int ec_cdev_ioctl_slave_sdo_download(
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 @@ int ec_cdev_ioctl_slave_sdo_download(
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 @@ int ec_cdev_ioctl_slave_sii_read(
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 @@ int ec_cdev_ioctl_slave_sii_write(
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 @@ int ec_cdev_ioctl_slave_sii_write(
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 @@ int ec_cdev_ioctl_slave_reg_read(
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 @@ int ec_cdev_ioctl_slave_reg_read(
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 @@ int ec_cdev_ioctl_slave_reg_write(
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 @@ int ec_cdev_ioctl_slave_reg_write(
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 @@ int ec_cdev_ioctl_config(
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 @@ int ec_cdev_ioctl_config_pdo(
}
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 @@ int ec_cdev_ioctl_config_pdo(
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 @@ int ec_cdev_ioctl_config_pdo(
&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 @@ int ec_cdev_ioctl_config_pdo_entry(
}
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 @@ int ec_cdev_ioctl_config_pdo_entry(
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 @@ int ec_cdev_ioctl_config_pdo_entry(
&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 @@ int ec_cdev_ioctl_config_sdo(
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 @@ int ec_cdev_ioctl_config_sdo(
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 @@ int ec_cdev_ioctl_eoe_handler(
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 @@ int ec_cdev_ioctl_sdo_request_write(
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 @@ int ec_cdev_ioctl_slave_foe_read(
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 @@ int ec_cdev_ioctl_slave_foe_read(
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 @@ int ec_cdev_ioctl_slave_foe_read(
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 @@ int ec_cdev_ioctl_slave_foe_read(
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 @@ int ec_cdev_ioctl_slave_foe_write(
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 @@ int ec_cdev_ioctl_slave_foe_write(
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 @@ int ec_cdev_ioctl_slave_soe_read(
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 @@ int ec_cdev_ioctl_slave_soe_read(
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 @@ int ec_cdev_ioctl_slave_soe_read(
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 @@ int ec_cdev_ioctl_slave_soe_read(
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 @@ int ec_cdev_ioctl_slave_soe_write(
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 @@ int ec_cdev_ioctl_slave_soe_write(
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 @@ int eccdev_open(struct inode *inode, struct file *filp)
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 @@ int eccdev_open(struct inode *inode, struct file *filp)
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 @@ int eccdev_release(struct inode *inode, struct file *filp)
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 @@ long eccdev_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
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 @@ long eccdev_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
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 @@ int eccdev_mmap(
{
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 @@ static int eccdev_vma_fault(
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 @@ struct page *eccdev_vma_nopage(
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)

19
master/debug.c
View File

@ -39,6 +39,7 @@
#include <linux/etherdevice.h>
#include "globals.h"
#include "master.h"
#include "debug.h"
/*****************************************************************************/
@ -71,10 +72,12 @@ static const struct net_device_ops ec_dbg_netdev_ops =
* \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 @@ int ec_debug_init(
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 @@ void ec_debug_register(
// 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 @@ int ec_dbgdev_open(
{
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 @@ int ec_dbgdev_stop(
{
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;
}

16
master/debug.h
View File

@ -34,16 +34,18 @@
/*****************************************************************************/
#include <linux/netdevice.h>
#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 @@ ec_debug_t;
/*****************************************************************************/
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
/*****************************************************************************/

40
master/device.c
View File

@ -94,9 +94,9 @@ int ec_device_init(
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 @@ int ec_device_init(
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 ec_device_open(
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 ec_device_close(
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 @@ void ec_device_send(
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 @@ void ec_device_debug_ring_print(
% 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 @@ void ec_device_debug_ring_print(
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 @@ void ecdev_withdraw(ec_device_t *device /**< EtherCAT device */)
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 @@ int ecdev_open(ec_device_t *device /**< EtherCAT device */)
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 @@ void ecdev_close(ec_device_t *device /**< EtherCAT device */)
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 @@ void ecdev_receive(
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 @@ void ecdev_set_link(
)
{
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 @@ uint8_t ecdev_get_link(
)
{
if (unlikely(!device)) {
EC_WARN("ecdev_get_link(): No device!\n");
EC_MASTER_WARN(device->master, "ecdev_get_link(): No device!\n");
return 0;
}

48
master/domain.c
View File

@ -117,9 +117,9 @@ void ec_domain_add_fmmu_config(
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 ec_domain_add_datagram(
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 @@ int ec_domain_finish(
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 @@ int ec_domain_finish(
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 @@ int ecrt_domain_reg_pdo_entry_list(ec_domain_t *domain,
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 @@ size_t ecrt_domain_size(const ec_domain_t *domain)
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 @@ void ecrt_domain_process(ec_domain_t *domain)
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,
domain->expected_working_counter);
} else {
EC_INFO("Domain %u: %u working counter changes - now %u/%u.\n",
domain->index, domain->working_counter_changes,
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);
}
domain->working_counter_changes = 0;
}

87
master/ethernet.c
View File

@ -147,7 +147,8 @@ int ec_eoe_init(
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 @@ int ec_eoe_init(
// 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 @@ int ec_eoe_send(ec_eoe_t *eoe /**< EoE handler */)
}
#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 @@ void ec_eoe_state_rx_check(ec_eoe_t *eoe /**< EoE handler */)
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 @@ void ec_eoe_state_rx_fetch(ec_eoe_t *eoe /**< EoE handler */)
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 @@ void ec_eoe_state_rx_fetch(ec_eoe_t *eoe /**< EoE handler */)
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 @@ void ec_eoe_state_rx_fetch(ec_eoe_t *eoe /**< EoE handler */)
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 @@ void ec_eoe_state_rx_fetch(ec_eoe_t *eoe /**< EoE handler */)
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 @@ void ec_eoe_state_rx_fetch(ec_eoe_t *eoe /**< EoE handler */)
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 @@ void ec_eoe_state_rx_fetch(ec_eoe_t *eoe /**< EoE handler */)
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 @@ void ec_eoe_state_rx_fetch(ec_eoe_t *eoe /**< EoE handler */)
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 @@ void ec_eoe_state_rx_fetch(ec_eoe_t *eoe /**< EoE handler */)
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 @@ void ec_eoe_state_rx_fetch(ec_eoe_t *eoe /**< EoE handler */)
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 @@ void ec_eoe_state_rx_fetch(ec_eoe_t *eoe /**< EoE handler */)
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 @@ void ec_eoe_state_tx_start(ec_eoe_t *eoe /**< EoE handler */)
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 @@ void ec_eoe_state_tx_sent(ec_eoe_t *eoe /**< EoE handler */)
} 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 @@ void ec_eoe_state_tx_sent(ec_eoe_t *eoe /**< EoE handler */)
} 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 @@ void ec_eoe_state_tx_sent(ec_eoe_t *eoe /**< EoE handler */)
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 @@ int ec_eoedev_open(struct net_device *dev /**< EoE net_device */)
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 @@ int ec_eoedev_stop(struct net_device *dev /**< EoE net_device */)
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 @@ int ec_eoedev_tx(struct sk_buff *skb, /**< transmit socket buffer */
#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 @@ int ec_eoedev_tx(struct sk_buff *skb, /**< transmit socket buffer */
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 @@ int ec_eoedev_tx(struct sk_buff *skb, /**< transmit socket buffer */
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;

13
master/fmmu_config.c
View File

@ -95,12 +95,13 @@ void ec_fmmu_config_page(
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

57
master/fsm_change.c
View File

@ -182,8 +182,7 @@ void ec_fsm_change_state_check(ec_fsm_change_t *fsm
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 @@ void ec_fsm_change_state_check(ec_fsm_change_t *fsm
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 @@ void ec_fsm_change_state_check(ec_fsm_change_t *fsm
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 @@ void ec_fsm_change_state_status(ec_fsm_change_t *fsm
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 @@ void ec_fsm_change_state_status(ec_fsm_change_t *fsm
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 @@ void ec_fsm_change_state_status(ec_fsm_change_t *fsm
// 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 @@ void ec_fsm_change_state_status(ec_fsm_change_t *fsm
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 @@ void ec_fsm_change_state_status(ec_fsm_change_t *fsm
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 @@ void ec_fsm_change_state_code(ec_fsm_change_t *fsm
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 @@ void ec_fsm_change_state_ack(ec_fsm_change_t *fsm /**< finite state machine */)
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 @@ void ec_fsm_change_state_check_ack(ec_fsm_change_t *fsm
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 @@ void ec_fsm_change_state_check_ack(ec_fsm_change_t *fsm
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 @@ void ec_fsm_change_state_check_ack(ec_fsm_change_t *fsm
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;
}

476
master/fsm_coe.c
View File

File diff suppressed because it is too large Load Diff

82
master/fsm_foe.c
View File

@ -288,7 +288,7 @@ void ec_fsm_foe_write_start(ec_fsm_foe_t *fsm /**< finite state machine */)
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 @@ void ec_fsm_foe_state_ack_check(
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 @@ void ec_fsm_foe_state_ack_check(
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 @@ void ec_fsm_foe_state_ack_check(
(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 @@ void ec_fsm_foe_state_ack_read(
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 @@ void ec_fsm_foe_state_ack_read(
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 @@ void ec_fsm_foe_state_ack_read(
// 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 @@ void ec_fsm_foe_state_wrq_sent(
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 @@ void ec_fsm_foe_state_wrq_sent(
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 @@ void ec_fsm_foe_state_data_sent(
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 @@ int ec_foe_prepare_rrq_send(ec_fsm_foe_t *fsm)
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 @@ void ec_fsm_foe_state_rrq_sent(
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 @@ void ec_fsm_foe_state_rrq_sent(
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 @@ void ec_fsm_foe_read_start(
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 @@ void ec_fsm_foe_state_data_check(
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 @@ void ec_fsm_foe_state_data_check(
(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 @@ void ec_fsm_foe_state_data_read(
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 @@ void ec_fsm_foe_state_data_read(
}
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 @@ void ec_fsm_foe_state_data_read(
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 @@ void ec_fsm_foe_state_data_read(
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 @@ void ec_fsm_foe_state_sent_ack(
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 @@ void ec_fsm_foe_state_sent_ack(
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;
}

381
master/fsm_master.c
View File

@ -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_configure_slave(ec_fsm_master_t *);
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 @@ void ec_fsm_master_state_broadcast(
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 @@ void ec_fsm_master_state_broadcast(
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 @@ void ec_fsm_master_state_broadcast(
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 @@ void ec_fsm_master_state_broadcast(
}
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 @@ int ec_fsm_master_action_process_sii(
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 @@ int ec_fsm_master_action_process_register(
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 @@ int ec_fsm_master_action_process_sdo(
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 @@ int ec_fsm_master_action_process_sdo(
}
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 @@ void ec_fsm_master_action_idle(
|| 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 @@ void ec_fsm_master_action_configure(
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 @@ void ec_fsm_master_state_read_state(
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 @@ void ec_fsm_master_state_read_state(
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 @@ void ec_fsm_master_state_acknowledge(
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 @@ void ec_fsm_master_state_clear_addresses(
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 @@ void ec_fsm_master_state_clear_addresses(
}
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 @@ void ec_fsm_master_state_dc_measure_delays(
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_MASTER_DBG(master, 1, "%u slaves responded to delay measuring.\n",
datagram->working_counter);
EC_INFO("Scanning bus.\n");
EC_MASTER_INFO(master, "Scanning bus.\n");
// begin scanning of slaves
fsm->slave = master->slaves;
@ -767,11 +774,9 @@ void ec_fsm_master_state_scan_slave(
// 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 @@ void ec_fsm_master_state_scan_slave(
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 @@ void ec_fsm_master_state_scan_slave(
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 @@ void ec_fsm_master_state_configure_slave(
/*****************************************************************************/
/** 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 @@ void ec_fsm_master_state_write_sii(
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 @@ void ec_fsm_master_state_write_sii(
}
// 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 @@ void ec_fsm_master_state_sdo_dictionary(
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 @@ void ec_fsm_master_state_sdo_request(
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 @@ void ec_fsm_master_state_sdo_request(
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 @@ void ec_fsm_master_state_reg_request(
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 @@ void ec_fsm_master_state_reg_request(
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 @@ void ec_fsm_master_state_reg_request(
}
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);

125
master/fsm_pdo.c
View File

@ -213,8 +213,8 @@ void ec_fsm_pdo_read_action_next_sync(
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 @@ void ec_fsm_pdo_read_action_next_sync(
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 @@ void ec_fsm_pdo_read_state_pdo_count(
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 @@ void ec_fsm_pdo_read_state_pdo(
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 @@ void ec_fsm_pdo_read_state_pdo(
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 @@ void ec_fsm_pdo_read_state_pdo_entries(
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 @@ void ec_fsm_pdo_conf_action_next_sync(
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 @@ void ec_fsm_pdo_conf_action_pdo_mapping(
}
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 @@ void ec_fsm_pdo_conf_state_read_mapping(
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 @@ void ec_fsm_pdo_conf_action_check_mapping(
)
{
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 @@ void ec_fsm_pdo_conf_state_mapping(
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 @@ void ec_fsm_pdo_conf_action_check_assignment(
{
// 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 @@ void ec_fsm_pdo_conf_action_check_assignment(
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 @@ void ec_fsm_pdo_conf_state_zero_pdo_count(
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 @@ void ec_fsm_pdo_conf_state_zero_pdo_count(
// 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 @@ void ec_fsm_pdo_conf_action_assign_pdo(
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 @@ void ec_fsm_pdo_conf_state_assign_pdo(
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 @@ void ec_fsm_pdo_conf_state_assign_pdo(
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 @@ void ec_fsm_pdo_conf_state_set_pdo_count(
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 @@ void ec_fsm_pdo_conf_state_set_pdo_count(
// 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);

76
master/fsm_pdo_entry.c
View File

@ -131,8 +131,9 @@ void ec_fsm_pdo_entry_start_configuration(
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 @@ void ec_fsm_pdo_entry_read_state_count(
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 @@ void ec_fsm_pdo_entry_read_state_count(
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 @@ void ec_fsm_pdo_entry_read_state_entry(
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 @@ void ec_fsm_pdo_entry_read_state_entry(
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 @@ void ec_fsm_pdo_entry_read_state_entry(
}
}
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 @@ void ec_fsm_pdo_entry_conf_state_start(
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 @@ void ec_fsm_pdo_entry_conf_state_start(
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 @@ void ec_fsm_pdo_entry_conf_state_zero_entry_count(
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 @@ void ec_fsm_pdo_entry_conf_state_zero_entry_count(
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 @@ void ec_fsm_pdo_entry_conf_action_map(
{
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 @@ void ec_fsm_pdo_entry_conf_state_map_entry(
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 @@ void ec_fsm_pdo_entry_conf_state_map_entry(
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 @@ void ec_fsm_pdo_entry_conf_state_set_entry_count(
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);
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;
}
if (fsm->slave->master->debug_level)
EC_DBG("Successfully configured mapping for PDO 0x%04X.\n",
fsm->source_pdo->index);
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
}

48
master/fsm_sii.c
View File

@ -184,7 +184,7 @@ void ec_fsm_sii_state_start_reading(
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 @@ void ec_fsm_sii_state_read_check(
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 @@ void ec_fsm_sii_state_read_fetch(
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 @@ void ec_fsm_sii_state_read_fetch(
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 @@ void ec_fsm_sii_state_start_writing(
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 @@ void ec_fsm_sii_state_write_check(
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 @@ void ec_fsm_sii_state_write_check2(
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 @@ void ec_fsm_sii_state_write_check2(
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 @@ void ec_fsm_sii_state_write_check2(
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 @@ void ec_fsm_sii_state_write_check2(
}
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;
}

66
master/fsm_slave.c
View File

@ -64,8 +64,7 @@ void ec_fsm_slave_init(
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 @@ void ec_fsm_slave_ready(
)
{
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 @@ int ec_fsm_slave_action_process_sdo(
)
{
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 @@ int ec_fsm_slave_action_process_sdo(
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 @@ int ec_fsm_slave_action_process_sdo(
}
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 @@ int ec_fsm_slave_action_process_sdo(
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 @@ void ec_fsm_slave_state_sdo_request(
)
{
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 @@ void ec_fsm_slave_state_sdo_request(
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 @@ void ec_fsm_slave_state_sdo_request(
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 @@ int ec_fsm_slave_action_process_foe(
)
{
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 @@ int ec_fsm_slave_action_process_foe(
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 @@ void ec_fsm_slave_state_foe_request(
)
{
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 @@ void ec_fsm_slave_state_foe_request(
}
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 @@ void ec_fsm_slave_state_foe_request(
}
// 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 @@ int ec_fsm_slave_action_process_soe(
)
{
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 @@ int ec_fsm_slave_action_process_soe(
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 @@ int ec_fsm_slave_action_process_soe(
}
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 @@ int ec_fsm_slave_action_process_soe(
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 @@ void ec_fsm_slave_state_soe_request(
)
{
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 @@ void ec_fsm_slave_state_soe_request(
}
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 @@ void ec_fsm_slave_state_soe_request(
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;

505
master/fsm_slave_config.c
View File

@ -44,9 +44,20 @@
/*****************************************************************************/
/** Time difference [ns] to tolerate without setting a new system time offset.
/** 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_SYSTEM_TIME_TOLERANCE_NS 100000000
#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_init(ec_fsm_slave_config_t *);
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_sync(ec_fsm_slave_config_t *);
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 @@ void ec_fsm_slave_config_state_start(
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 @@ void ec_fsm_slave_config_state_init(
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 @@ void ec_fsm_slave_config_state_init(
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 @@ void ec_fsm_slave_config_state_clear_fmmus(
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 @@ void ec_fsm_slave_config_enter_clear_sync(
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 @@ void ec_fsm_slave_config_state_clear_sync(
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 @@ void ec_fsm_slave_config_enter_dc_clear_assign(
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 @@ void ec_fsm_slave_config_state_dc_clear_assign(
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);
EC_SLAVE_ERR(fsm->slave, "Failed receive DC assignment"
" clearing datagram.\n");
return;
}
if (fsm->slave->master->debug_level && datagram->working_counter != 1) {
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 @@ void ec_fsm_slave_config_enter_mbox_sync(
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 @@ void ec_fsm_slave_config_enter_mbox_sync(
// 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 @@ void ec_fsm_slave_config_enter_mbox_sync(
} 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 @@ void ec_fsm_slave_config_state_mbox_sync(
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 @@ void ec_fsm_slave_config_state_mbox_sync(
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 @@ void ec_fsm_slave_config_state_mbox_sync(
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 @@ void ec_fsm_slave_config_state_boot_preop(
)
{
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 @@ void ec_fsm_slave_config_state_boot_preop(
// 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 @@ void ec_fsm_slave_config_state_sdo_conf(
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 @@ void ec_fsm_slave_config_state_soe_conf(
}
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 @@ void ec_fsm_slave_config_state_pdo_conf(
}
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 @@ void ec_fsm_slave_config_enter_watchdog_divider(
{
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 @@ void ec_fsm_slave_config_state_watchdog_divider(
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 @@ void ec_fsm_slave_config_enter_watchdog(
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 @@ void ec_fsm_slave_config_state_watchdog(
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 @@ void ec_fsm_slave_config_state_pdo_sync(
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 @@ void ec_fsm_slave_config_state_pdo_sync(
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 @@ void ec_fsm_slave_config_enter_fmmu(
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 @@ void ec_fsm_slave_config_enter_fmmu(
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 @@ void ec_fsm_slave_config_state_fmmu(
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 @@ void ec_fsm_slave_config_state_fmmu(
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 @@ void ec_fsm_slave_config_enter_dc_cycle(
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);
@ -1381,11 +1160,56 @@ void ec_fsm_slave_config_enter_dc_cycle(
void ec_fsm_slave_config_state_dc_cycle(
ec_fsm_slave_config_t *fsm /**< slave state machine */
)
{
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 @@ void ec_fsm_slave_config_state_dc_cycle(
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 @@ void ec_fsm_slave_config_state_dc_cycle(
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 @@ void ec_fsm_slave_config_state_dc_cycle(
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 @@ void ec_fsm_slave_config_state_dc_start(
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 @@ void ec_fsm_slave_config_state_dc_start(
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 @@ void ec_fsm_slave_config_state_dc_assign(
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 @@ void ec_fsm_slave_config_state_dc_assign(
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 @@ void ec_fsm_slave_config_state_safeop(
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 @@ void ec_fsm_slave_config_state_safeop(
// 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 @@ void ec_fsm_slave_config_state_op(
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 @@ void ec_fsm_slave_config_state_op(
// 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 @@ void ec_fsm_slave_config_reconfigure(
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
}

167
master/fsm_slave_scan.c
View File

@ -190,8 +190,8 @@ void ec_fsm_slave_scan_state_address(
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 @@ void ec_fsm_slave_scan_state_address(
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 @@ void ec_fsm_slave_scan_state_state(
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 @@ void ec_fsm_slave_scan_state_state(
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 @@ void ec_fsm_slave_scan_state_state(
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 @@ void ec_fsm_slave_scan_state_base(
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 @@ void ec_fsm_slave_scan_state_base(
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 @@ void ec_fsm_slave_scan_state_base(
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 @@ void ec_fsm_slave_scan_state_dc_cap(
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 @@ void ec_fsm_slave_scan_state_dc_times(
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 @@ void ec_fsm_slave_scan_state_dc_times(
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 @@ void ec_fsm_slave_scan_state_datalink(ec_fsm_slave_scan_t *fsm /**< slave state
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 @@ void ec_fsm_slave_scan_state_datalink(ec_fsm_slave_scan_t *fsm /**< slave state
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 @@ void ec_fsm_slave_scan_state_sii_size(ec_fsm_slave_scan_t *fsm /**< slave state
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 @@ void ec_fsm_slave_scan_state_sii_size(ec_fsm_slave_scan_t *fsm /**< slave state
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 @@ void ec_fsm_slave_scan_state_sii_size(ec_fsm_slave_scan_t *fsm /**< slave state
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 @@ void ec_fsm_slave_scan_state_sii_data(ec_fsm_slave_scan_t *fsm /**< slave state
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 @@ void ec_fsm_slave_scan_state_sii_data(ec_fsm_slave_scan_t *fsm /**< slave state
}
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 @@ void ec_fsm_slave_scan_state_sii_data(ec_fsm_slave_scan_t *fsm /**< slave state
// 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 @@ void ec_fsm_slave_scan_state_sii_data(ec_fsm_slave_scan_t *fsm /**< slave state
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 @@ void ec_fsm_slave_scan_state_sii_data(ec_fsm_slave_scan_t *fsm /**< slave state
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 @@ void ec_fsm_slave_scan_state_sii_data(ec_fsm_slave_scan_t *fsm /**< slave state
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 @@ void ec_fsm_slave_scan_enter_regalias(
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 @@ void ec_fsm_slave_scan_state_regalias(
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 @@ void ec_fsm_slave_scan_enter_preop(
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 @@ void ec_fsm_slave_scan_enter_preop(
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 @@ void ec_fsm_slave_scan_state_sync(
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 @@ void ec_fsm_slave_scan_state_sync(
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 @@ void ec_fsm_slave_scan_state_sync(
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 @@ void ec_fsm_slave_scan_enter_pdos(
{
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

115
master/fsm_soe.c
View File

@ -85,19 +85,19 @@ extern const ec_code_msg_t soe_error_codes[];
/** 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 @@ void ec_fsm_soe_print_error(ec_fsm_soe_t *fsm /**< Finite state machine */)
{
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 @@ void ec_fsm_soe_print_error(ec_fsm_soe_t *fsm /**< Finite state machine */)
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 @@ void ec_fsm_soe_read_start(ec_fsm_soe_t *fsm /**< finite state machine */)
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 @@ void ec_fsm_soe_read_start(ec_fsm_soe_t *fsm /**< finite state machine */)
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 @@ void ec_fsm_soe_read_request(ec_fsm_soe_t *fsm /**< finite state machine */)
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 @@ void ec_fsm_soe_read_request(ec_fsm_soe_t *fsm /**< finite state machine */)
}
}
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 @@ void ec_fsm_soe_read_check(ec_fsm_soe_t *fsm /**< finite state machine */)
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 @@ void ec_fsm_soe_read_check(ec_fsm_soe_t *fsm /**< finite state machine */)
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 @@ void ec_fsm_soe_read_check(ec_fsm_soe_t *fsm /**< finite state machine */)
(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 @@ void ec_fsm_soe_read_response(ec_fsm_soe_t *fsm /**< finite state machine */)
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 @@ void ec_fsm_soe_read_response(ec_fsm_soe_t *fsm /**< finite state machine */)
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 @@ void ec_fsm_soe_read_response(ec_fsm_soe_t *fsm /**< finite state machine */)
}
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 @@ void ec_fsm_soe_read_response(ec_fsm_soe_t *fsm /**< finite state machine */)
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 @@ void ec_fsm_soe_read_response(ec_fsm_soe_t *fsm /**< finite state machine */)
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 @@ void ec_fsm_soe_read_response(ec_fsm_soe_t *fsm /**< finite state machine */)
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 @@ void ec_fsm_soe_read_response(ec_fsm_soe_t *fsm /**< finite state machine */)
}
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 @@ void ec_fsm_soe_write_next_fragment(
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 @@ void ec_fsm_soe_write_next_fragment(
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_next_fragment(
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 @@ void ec_fsm_soe_write_request(ec_fsm_soe_t *fsm /**< finite state machine */)
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 @@ void ec_fsm_soe_write_request(ec_fsm_soe_t *fsm /**< finite state machine */)
}
}
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 @@ void ec_fsm_soe_write_check(ec_fsm_soe_t *fsm /**< finite state machine */)
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 @@ void ec_fsm_soe_write_check(ec_fsm_soe_t *fsm /**< finite state machine */)
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 @@ void ec_fsm_soe_write_check(ec_fsm_soe_t *fsm /**< finite state machine */)
(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 @@ void ec_fsm_soe_write_response(ec_fsm_soe_t *fsm /**< finite state machine */)
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 @@ void ec_fsm_soe_write_response(ec_fsm_soe_t *fsm /**< finite state machine */)
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 @@ void ec_fsm_soe_write_response(ec_fsm_soe_t *fsm /**< finite state machine */)
}
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 @@ void ec_fsm_soe_write_response(ec_fsm_soe_t *fsm /**< finite state machine */)
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 @@ void ec_fsm_soe_write_response(ec_fsm_soe_t *fsm /**< finite state machine */)
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 @@ void ec_fsm_soe_write_response(ec_fsm_soe_t *fsm /**< finite state machine */)
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);

12
master/mailbox.c
View File

@ -58,15 +58,15 @@ uint8_t *ec_slave_mbox_prepare_send(const ec_slave_t *slave, /**< slave */
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 @@ uint8_t *ec_slave_mbox_fetch(const ec_slave_t *slave, /**< slave */
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 @@ uint8_t *ec_slave_mbox_fetch(const ec_slave_t *slave, /**< slave */
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)

364
master/master.c
View File

@ -139,6 +139,7 @@ int ec_master_init(ec_master_t *master, /**< EtherCAT master */
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 @@ int ec_master_init(ec_master_t *master, /**< EtherCAT master */
#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 @@ int ec_master_init(ec_master_t *master, /**< EtherCAT master */
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 @@ int ec_master_init(ec_master_t *master, /**< EtherCAT master */
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 @@ int ec_master_init(ec_master_t *master, /**< EtherCAT master */
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 @@ int ec_master_init(ec_master_t *master, /**< EtherCAT master */
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 @@ int ec_master_init(ec_master_t *master, /**< EtherCAT master */
"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 @@ void ec_master_clear_slaves(ec_master_t *master)
// 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 @@ void ec_master_clear_slaves(ec_master_t *master)
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 @@ int ec_master_thread_start(
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 @@ void ec_master_thread_stop(
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 ec_master_enter_idle_phase(
{
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 @@ int ec_master_enter_idle_phase(
*/
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 @@ int ec_master_enter_operation_phase(ec_master_t *master /**< EtherCAT master */)
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 @@ int ec_master_enter_operation_phase(ec_master_t *master /**< EtherCAT master */)
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 @@ int ec_master_enter_operation_phase(ec_master_t *master /**< EtherCAT master */)
// 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 @@ void ec_master_leave_operation_phase(
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 @@ void ec_master_inject_external_datagrams(
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 @@ void ec_master_inject_external_datagrams(
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 @@ void ec_master_inject_external_datagrams(
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 @@ void ec_master_queue_external_datagram(
}
#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 @@ void ec_master_queue_datagram(
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 @@ void ec_master_send_datagrams(ec_master_t *master /**< EtherCAT master */)
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 @@ void ec_master_send_datagrams(ec_master_t *master /**< EtherCAT master */)
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 @@ void ec_master_send_datagrams(ec_master_t *master /**< EtherCAT master */)
}
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 @@ void ec_master_send_datagrams(ec_master_t *master /**< EtherCAT master */)
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 @@ void ec_master_send_datagrams(ec_master_t *master /**< EtherCAT master */)
#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 @@ void ec_master_receive_datagrams(ec_master_t *master, /**< EtherCAT master */
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 @@ void ec_master_receive_datagrams(ec_master_t *master, /**< EtherCAT master */
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 @@ void ec_master_receive_datagrams(ec_master_t *master, /**< EtherCAT master */
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 @@ void ec_master_receive_datagrams(ec_master_t *master, /**< EtherCAT master */
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 @@ void ec_master_output_stats(ec_master_t *master /**< EtherCAT master */)
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 @@ static int ec_master_idle_thread(void *priv_data)
// 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 @@ static int ec_master_idle_thread(void *priv_data)
}
}
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 @@ static int ec_master_operation_thread(void *priv_data)
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 @@ static int ec_master_operation_thread(void *priv_data)
#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 @@ void ec_master_eoe_start(ec_master_t *master /**< EtherCAT master */)
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 @@ void ec_master_eoe_start(ec_master_t *master /**< EtherCAT master */)
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_start(ec_master_t *master /**< EtherCAT master */)
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 @@ static int ec_master_eoe_thread(void *priv_data)
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 @@ schedule:
}
}
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 @@ int ec_master_debug_level(
)
{
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 @@ void ec_master_calc_topology(
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 @@ void ec_master_calc_dc(
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 *ecrt_master_create_domain_err(
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 @@ ec_domain_t *ecrt_master_create_domain_err(
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 ecrt_master_activate(ec_master_t *master)
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 @@ int ecrt_master_activate(ec_master_t *master)
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 @@ int ecrt_master_activate(ec_master_t *master)
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 @@ int ecrt_master_activate(ec_master_t *master)
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 @@ void ecrt_master_deactivate(ec_master_t *master)
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 @@ void ecrt_master_deactivate(ec_master_t *master)
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 @@ void ecrt_master_deactivate(ec_master_t *master)
#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 @@ void ecrt_master_receive(ec_master_t *master)
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 @@ ec_slave_config_t *ecrt_master_slave_config_err(ec_master_t *master,
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 @@ ec_slave_config_t *ecrt_master_slave_config_err(ec_master_t *master,
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 @@ ec_slave_config_t *ecrt_master_slave_config(ec_master_t *master,
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 @@ int ecrt_master_get_slave(ec_master_t *master, uint16_t slave_position,
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 @@ void ecrt_master_application_time(ec_master_t *master, uint64_t app_time)
#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;
}
}

59
master/master.h
View File

@ -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 <INDEX>: ",
* 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 <INDEX>: ",
* 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 <INDEX>: ",
* 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 <INDEX>: ",
* 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 @@ struct ec_master {
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 @@ struct ec_master {
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 @@ ec_slave_config_t *ecrt_master_slave_config_err(ec_master_t *, uint16_t,
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 *);

22
master/module.c
View File

@ -104,7 +104,8 @@ int __init ec_init_module(void)
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 @@ int __init ec_init_module(void)
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 @@ ec_device_t *ecdev_offer(
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 @@ ec_master_t *ecrt_request_master_err(
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 @@ ec_master_t *ecrt_request_master_err(
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 @@ ec_master_t *ecrt_request_master_err(
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 @@ ec_master_t *ecrt_request_master(unsigned int master_index)
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 @@ void ecrt_release_master(ec_master_t *master)
module_put(master->main_device.module);
master->reserved = 0;
EC_INFO("Released master %u.\n", master->index);
EC_MASTER_INFO(master, "Released.\n");
}
/*****************************************************************************/

83
master/slave.c
View File

@ -161,17 +161,17 @@ void ec_slave_init(
// 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 @@ void ec_slave_clear(ec_slave_t *slave /**< EtherCAT slave */)
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 @@ void ec_slave_clear(ec_slave_t *slave /**< EtherCAT slave */)
kfree(slave->sii_words);
ec_fsm_slave_clear(&slave->fsm);
ec_datagram_clear(&slave->fsm_datagram);
}
/*****************************************************************************/
@ -254,8 +288,7 @@ void ec_slave_set_state(ec_slave_t *slave, /**< EtherCAT slave */
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 @@ int ec_slave_fetch_sii_strings(
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 @@ int ec_slave_fetch_sii_strings(
// 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 @@ int ec_slave_fetch_sii_general(
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 @@ int ec_slave_fetch_sii_syncs(
// 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 @@ int ec_slave_fetch_sii_syncs(
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 @@ int ec_slave_fetch_sii_pdos(
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 @@ int ec_slave_fetch_sii_pdos(
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 @@ int ec_slave_fetch_sii_pdos(
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 @@ char *ec_slave_sii_string(
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 @@ void ec_slave_calc_port_delays(
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 @@ void ec_slave_calc_transmission_delays_rec(
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 @@ void ec_slave_calc_transmission_delays_rec(
*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);
}

62
master/slave.h
View File

@ -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 <INDEX>-<POSITION>: ", 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 <INDEX>-<POSITION>: ", 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 <INDEX>-<POSITION>: ", 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 <INDEX>-<POSITION>: ", 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 {

192
master/slave_config.c
View File

@ -176,8 +176,7 @@ int ec_slave_config_prepare_fmmu(
}
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 @@ int ec_slave_config_attach(
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 @@ int ec_slave_config_attach(
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 @@ void ec_slave_config_load_default_sync_config(ec_slave_config_t *sc)
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 @@ void ec_slave_config_load_default_mapping(
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 @@ void ec_slave_config_load_default_mapping(
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 @@ void ec_slave_config_load_default_mapping(
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 @@ void ec_slave_config_load_default_mapping(
}
}
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 @@ int ecrt_slave_config_sync_manager(ec_slave_config_t *sc, uint8_t sync_index,
{
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 @@ int ecrt_slave_config_sync_manager(ec_slave_config_t *sc, uint8_t sync_index,
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 @@ int ecrt_slave_config_pdo_assign_add(ec_slave_config_t *sc,
{
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 @@ int ecrt_slave_config_pdo_assign_add(ec_slave_config_t *sc,
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 @@ int ecrt_slave_config_pdo_mapping_add(ec_slave_config_t *sc,
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 @@ int ecrt_slave_config_pdo_mapping_add(ec_slave_config_t *sc,
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 @@ void ecrt_slave_config_pdo_mapping_clear(ec_slave_config_t *sc,
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 @@ void ecrt_slave_config_pdo_mapping_clear(ec_slave_config_t *sc,
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 @@ int ecrt_slave_config_pdos(ec_slave_config_t *sc,
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 @@ int ecrt_slave_config_pdos(ec_slave_config_t *sc,
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 @@ int ecrt_slave_config_reg_pdo_entry(
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 @@ int ecrt_slave_config_reg_pdo_entry(
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 @@ int ecrt_slave_config_reg_pdo_entry(
}
}
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 @@ int ecrt_slave_config_sdo(ec_slave_config_t *sc, uint16_t index,
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 @@ int ecrt_slave_config_sdo8(ec_slave_config_t *sc, uint16_t index,
{
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 @@ int ecrt_slave_config_sdo16(ec_slave_config_t *sc, uint16_t index,
{
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 @@ int ecrt_slave_config_sdo32(ec_slave_config_t *sc, uint16_t index,
{
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 @@ int ecrt_slave_config_complete_sdo(ec_slave_config_t *sc, uint16_t index,
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 *ecrt_slave_config_create_sdo_request_err(
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 *ecrt_slave_config_create_voe_handler_err(
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 @@ int ecrt_slave_config_idn(ec_slave_config_t *sc, uint16_t idn,
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;
}

64
master/slave_config.h
View File

@ -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 <INDEX> <ALIAS>:<POSITION>: ", 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 <INDEX> <ALIAS>:<POSITION>: ", 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 <INDEX> <ALIAS>:<POSITION>: ", 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 <INDEX> <ALIAS>:<POSITION>: ", 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 {

8
master/sync.c
View File

@ -129,10 +129,10 @@ void ec_sync_page(
}
}
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);

74
master/voe_handler.c
View File

@ -212,13 +212,13 @@ void ec_voe_handler_state_write_start(ec_voe_handler_t *voe)
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 @@ void ec_voe_handler_state_write_response(ec_voe_handler_t *voe)
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 @@ void ec_voe_handler_state_write_response(ec_voe_handler_t *voe)
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 @@ void ec_voe_handler_state_read_start(ec_voe_handler_t *voe)
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 @@ void ec_voe_handler_state_read_check(ec_voe_handler_t *voe)
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 @@ void ec_voe_handler_state_read_check(ec_voe_handler_t *voe)
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 @@ void ec_voe_handler_state_read_check(ec_voe_handler_t *voe)
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 @@ void ec_voe_handler_state_read_response(ec_voe_handler_t *voe)
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 @@ void ec_voe_handler_state_read_response(ec_voe_handler_t *voe)
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 @@ void ec_voe_handler_state_read_response(ec_voe_handler_t *voe)
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 @@ void ec_voe_handler_state_read_response(ec_voe_handler_t *voe)
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 @@ void ec_voe_handler_state_read_nosync_start(ec_voe_handler_t *voe)
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 @@ void ec_voe_handler_state_read_nosync_response(ec_voe_handler_t *voe)
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 @@ void ec_voe_handler_state_read_nosync_response(ec_voe_handler_t *voe)
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 @@ void ec_voe_handler_state_read_nosync_response(ec_voe_handler_t *voe)
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 @@ void ec_voe_handler_state_read_nosync_response(ec_voe_handler_t *voe)
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);
}

22
tool/Command.cpp
View File

@ -371,14 +371,22 @@ Command::DomainList Command::selectedDomains(MasterDevice &m)
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;
}
/****************************************************************************/

5
tool/CommandConfig.cpp
View File

@ -330,10 +330,7 @@ void CommandConfig::listConfigs(
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();