etherlabmaster: master/fsm_coe.c@8b00e63fff90 (annotated)

436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1	/******************************************************************************
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	2	*
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	3	* $Id$
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	4	*
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	5	* Copyright (C) 2006 Florian Pose, Ingenieurgemeinschaft IgH
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	6	*
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	7	* This file is part of the IgH EtherCAT Master.
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	8	*
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	9	* The IgH EtherCAT Master is free software; you can redistribute it
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	10	* and/or modify it under the terms of the GNU General Public License
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	11	* as published by the Free Software Foundation; either version 2 of the
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	12	* License, or (at your option) any later version.
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	13	*
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	14	* The IgH EtherCAT Master is distributed in the hope that it will be
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	15	* useful, but WITHOUT ANY WARRANTY; without even the implied warranty of
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	16	* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	17	* GNU General Public License for more details.
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	18	*
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	19	* You should have received a copy of the GNU General Public License
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	20	* along with the IgH EtherCAT Master; if not, write to the Free Software
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	21	* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	22	*
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	23	* The right to use EtherCAT Technology is granted and comes free of
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	24	* charge under condition of compatibility of product made by
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	25	* Licensee. People intending to distribute/sell products based on the
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	26	* code, have to sign an agreement to guarantee that products using
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	27	* software based on IgH EtherCAT master stay compatible with the actual
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	28	* EtherCAT specification (which are released themselves as an open
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	29	* standard) as the (only) precondition to have the right to use EtherCAT
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	30	* Technology, IP and trade marks.
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	31	*
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	32	*****************************************************************************/
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	33
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	34	/**
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	35	\file
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	36	EtherCAT CoE state machines.
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	37	*/
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	38
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	39	/*****************************************************************************/
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	40
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	41	#include "globals.h"
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	42	#include "master.h"
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	43	#include "mailbox.h"
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	44	#include "fsm_coe.h"
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	45
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	46	/*****************************************************************************/
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	47
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	48	void ec_fsm_coe_dict_start(ec_fsm_coe_t *);
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	49	void ec_fsm_coe_dict_request(ec_fsm_coe_t *);
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	50	void ec_fsm_coe_dict_check(ec_fsm_coe_t *);
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	51	void ec_fsm_coe_dict_response(ec_fsm_coe_t *);
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	52	void ec_fsm_coe_dict_desc_request(ec_fsm_coe_t *);
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	53	void ec_fsm_coe_dict_desc_check(ec_fsm_coe_t *);
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	54	void ec_fsm_coe_dict_desc_response(ec_fsm_coe_t *);
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	55	void ec_fsm_coe_dict_entry_request(ec_fsm_coe_t *);
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	56	void ec_fsm_coe_dict_entry_check(ec_fsm_coe_t *);
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	57	void ec_fsm_coe_dict_entry_response(ec_fsm_coe_t *);
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	58
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	59	void ec_fsm_coe_down_start(ec_fsm_coe_t *);
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	60	void ec_fsm_coe_down_request(ec_fsm_coe_t *);
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	61	void ec_fsm_coe_down_check(ec_fsm_coe_t *);
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	62	void ec_fsm_coe_down_response(ec_fsm_coe_t *);
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	63
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	64	void ec_fsm_coe_up_start(ec_fsm_coe_t *);
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	65	void ec_fsm_coe_up_request(ec_fsm_coe_t *);
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	66	void ec_fsm_coe_up_check(ec_fsm_coe_t *);
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	67	void ec_fsm_coe_up_response(ec_fsm_coe_t *);
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	68	void ec_fsm_coe_up_seg_request(ec_fsm_coe_t *);
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	69	void ec_fsm_coe_up_seg_check(ec_fsm_coe_t *);
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	70	void ec_fsm_coe_up_seg_response(ec_fsm_coe_t *);
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	71
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	72	void ec_fsm_coe_end(ec_fsm_coe_t *);
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	73	void ec_fsm_coe_error(ec_fsm_coe_t *);
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	74
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	75	/*****************************************************************************/
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	76
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	77	/**
814 a51f857b1b2d Renamed [PS]DO to [PS]do. Florian Pose <fp@igh-essen.com> parents: 769 diff changeset	78	Sdo abort messages.
a51f857b1b2d Renamed [PS]DO to [PS]do. Florian Pose <fp@igh-essen.com> parents: 769 diff changeset	79	The "abort Sdo transfer request" supplies an abort code,
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	80	which can be translated to clear text. This table does
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	81	the mapping of the codes and messages.
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	82	*/
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	83
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	84	const ec_code_msg_t sdo_abort_messages[] = {
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	85	{0x05030000, "Toggle bit not changed"},
814 a51f857b1b2d Renamed [PS]DO to [PS]do. Florian Pose <fp@igh-essen.com> parents: 769 diff changeset	86	{0x05040000, "Sdo protocol timeout"},
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	87	{0x05040001, "Client/Server command specifier not valid or unknown"},
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	88	{0x05040005, "Out of memory"},
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	89	{0x06010000, "Unsupported access to an object"},
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	90	{0x06010001, "Attempt to read a write-only object"},
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	91	{0x06010002, "Attempt to write a read-only object"},
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	92	{0x06020000, "This object does not exist in the object directory"},
814 a51f857b1b2d Renamed [PS]DO to [PS]do. Florian Pose <fp@igh-essen.com> parents: 769 diff changeset	93	{0x06040041, "The object cannot be mapped into the Pdo"},
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	94	{0x06040042, "The number and length of the objects to be mapped would"
814 a51f857b1b2d Renamed [PS]DO to [PS]do. Florian Pose <fp@igh-essen.com> parents: 769 diff changeset	95	" exceed the Pdo length"},
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	96	{0x06040043, "General parameter incompatibility reason"},
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	97	{0x06040047, "Gerneral internal incompatibility in device"},
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	98	{0x06060000, "Access failure due to a hardware error"},
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	99	{0x06070010, "Data type does not match, length of service parameter does"
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	100	" not match"},
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	101	{0x06070012, "Data type does not match, length of service parameter too"
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	102	" high"},
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	103	{0x06070013, "Data type does not match, length of service parameter too"
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	104	" low"},
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	105	{0x06090011, "Subindex does not exist"},
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	106	{0x06090030, "Value range of parameter exceeded"},
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	107	{0x06090031, "Value of parameter written too high"},
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	108	{0x06090032, "Value of parameter written too low"},
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	109	{0x06090036, "Maximum value is less than minimum value"},
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	110	{0x08000000, "General error"},
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	111	{0x08000020, "Data cannot be transferred or stored to the application"},
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	112	{0x08000021, "Data cannot be transferred or stored to the application"
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	113	" because of local control"},
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	114	{0x08000022, "Data cannot be transferred or stored to the application"
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	115	" because of the present device state"},
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	116	{0x08000023, "Object dictionary dynamic generation fails or no object"
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	117	" dictionary is present"},
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	118	{}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	119	};
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	120
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	121	/*****************************************************************************/
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	122
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	123	/**
814 a51f857b1b2d Renamed [PS]DO to [PS]do. Florian Pose <fp@igh-essen.com> parents: 769 diff changeset	124	Outputs an Sdo abort message.
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	125	*/
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	126
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	127	void ec_canopen_abort_msg(uint32_t abort_code)
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	128	{
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	129	const ec_code_msg_t *abort_msg;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	130
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	131	for (abort_msg = sdo_abort_messages; abort_msg->code; abort_msg++) {
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	132	if (abort_msg->code == abort_code) {
814 a51f857b1b2d Renamed [PS]DO to [PS]do. Florian Pose <fp@igh-essen.com> parents: 769 diff changeset	133	EC_ERR("Sdo abort message 0x%08X: \"%s\".\n",
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	134	abort_msg->code, abort_msg->message);
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	135	return;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	136	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	137	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	138
814 a51f857b1b2d Renamed [PS]DO to [PS]do. Florian Pose <fp@igh-essen.com> parents: 769 diff changeset	139	EC_ERR("Unknown Sdo abort code 0x%08X.\n", abort_code);
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	140	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	141
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	142	/*****************************************************************************/
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	143
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	144	/**
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	145	Constructor.
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	146	*/
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	147
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	148	void ec_fsm_coe_init(ec_fsm_coe_t fsm, /< finite state machine /
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	149	ec_datagram_t datagram /< datagram /
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	150	)
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	151	{
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	152	fsm->state = NULL;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	153	fsm->datagram = datagram;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	154	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	155
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	156	/*****************************************************************************/
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	157
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	158	/**
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	159	Destructor.
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	160	*/
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	161
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	162	void ec_fsm_coe_clear(ec_fsm_coe_t fsm /< finite state machine /)
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	163	{
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	164	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	165
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	166	/*****************************************************************************/
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	167
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	168	/**
814 a51f857b1b2d Renamed [PS]DO to [PS]do. Florian Pose <fp@igh-essen.com> parents: 769 diff changeset	169	Starts reading a slaves' Sdo dictionary.
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	170	*/
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	171
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	172	void ec_fsm_coe_dictionary(ec_fsm_coe_t fsm, /< finite state machine /
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	173	ec_slave_t slave /< EtherCAT slave /
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	174	)
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	175	{
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	176	fsm->slave = slave;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	177	fsm->state = ec_fsm_coe_dict_start;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	178	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	179
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	180	/*****************************************************************************/
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	181
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	182	/**
859 233e32f428e1 Replaced ec_fsm_coe_$upload\\|download$() with ec_fsm_coe_transfer(); Florian Pose <fp@igh-essen.com> parents: 854 diff changeset	183	Starts to transfer an Sdo to/from a slave.
233e32f428e1 Replaced ec_fsm_coe_$upload\\|download$() with ec_fsm_coe_transfer(); Florian Pose <fp@igh-essen.com> parents: 854 diff changeset	184	*/
233e32f428e1 Replaced ec_fsm_coe_$upload\\|download$() with ec_fsm_coe_transfer(); Florian Pose <fp@igh-essen.com> parents: 854 diff changeset	185
233e32f428e1 Replaced ec_fsm_coe_$upload\\|download$() with ec_fsm_coe_transfer(); Florian Pose <fp@igh-essen.com> parents: 854 diff changeset	186	void ec_fsm_coe_transfer(
854 f4f53be425ac Removed ec_sdodata_t; CoE state machines work on ec_sdo_request_t. Florian Pose <fp@igh-essen.com> parents: 834 diff changeset	187	ec_fsm_coe_t fsm, /< State machine. /
f4f53be425ac Removed ec_sdodata_t; CoE state machines work on ec_sdo_request_t. Florian Pose <fp@igh-essen.com> parents: 834 diff changeset	188	ec_slave_t slave, /< EtherCAT slave. /
f4f53be425ac Removed ec_sdodata_t; CoE state machines work on ec_sdo_request_t. Florian Pose <fp@igh-essen.com> parents: 834 diff changeset	189	ec_sdo_request_t request /< Sdo request. /
f4f53be425ac Removed ec_sdodata_t; CoE state machines work on ec_sdo_request_t. Florian Pose <fp@igh-essen.com> parents: 834 diff changeset	190	)
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	191	{
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	192	fsm->slave = slave;
854 f4f53be425ac Removed ec_sdodata_t; CoE state machines work on ec_sdo_request_t. Florian Pose <fp@igh-essen.com> parents: 834 diff changeset	193	fsm->request = request;
859 233e32f428e1 Replaced ec_fsm_coe_$upload\\|download$() with ec_fsm_coe_transfer(); Florian Pose <fp@igh-essen.com> parents: 854 diff changeset	194	if (request->dir == EC_DIR_OUTPUT)
233e32f428e1 Replaced ec_fsm_coe_$upload\\|download$() with ec_fsm_coe_transfer(); Florian Pose <fp@igh-essen.com> parents: 854 diff changeset	195	fsm->state = ec_fsm_coe_down_start;
233e32f428e1 Replaced ec_fsm_coe_$upload\\|download$() with ec_fsm_coe_transfer(); Florian Pose <fp@igh-essen.com> parents: 854 diff changeset	196	else
233e32f428e1 Replaced ec_fsm_coe_$upload\\|download$() with ec_fsm_coe_transfer(); Florian Pose <fp@igh-essen.com> parents: 854 diff changeset	197	fsm->state = ec_fsm_coe_up_start;
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	198	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	199
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	200	/*****************************************************************************/
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	201
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	202	/**
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	203	Executes the current state of the state machine.
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	204	\return false, if state machine has terminated
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	205	*/
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	206
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	207	int ec_fsm_coe_exec(ec_fsm_coe_t fsm /< finite state machine /)
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	208	{
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	209	fsm->state(fsm);
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	210
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	211	return fsm->state != ec_fsm_coe_end && fsm->state != ec_fsm_coe_error;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	212	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	213
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	214	/*****************************************************************************/
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	215
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	216	/**
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	217	Returns, if the state machine terminated with success.
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	218	\return non-zero if successful.
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	219	*/
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	220
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	221	int ec_fsm_coe_success(ec_fsm_coe_t fsm /< Finite state machine /)
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	222	{
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	223	return fsm->state == ec_fsm_coe_end;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	224	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	225
895 5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	226	/*****************************************************************************/
5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	227
5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	228	/**
5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	229	*/
5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	230	int ec_fsm_coe_check_emergency(
5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	231	ec_fsm_coe_t fsm, /< Finite state machine /
5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	232	const uint8_t data, /< CoE mailbox data. /
5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	233	size_t size /*< CoE mailbox data size. /
5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	234	)
5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	235	{
5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	236	if (size < 2 \|\| ((EC_READ_U16(data) >> 12) & 0x0F) != 0x01)
5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	237	return 0;
5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	238
5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	239	if (size < 10) {
5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	240	EC_WARN("Received incomplete CoE Emergency request from slave %u:\n",
5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	241	fsm->slave->ring_position);
5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	242	ec_print_data(data, size);
5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	243	return 1;
5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	244	}
5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	245
5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	246	EC_INFO("CoE Emergency Request received from slave %u:\n",
5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	247	fsm->slave->ring_position);
5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	248	EC_INFO("Error code 0x%04X, Error register 0x%02X, data:\n",
5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	249	EC_READ_U16(data + 2), EC_READ_U8(data + 4));
5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	250	ec_print_data(data + 5, 5);
5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	251	return 1;
5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	252	}
5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	253
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	254	/******************************************************************************
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	255	* CoE dictionary state machine
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	256	*****************************************************************************/
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	257
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	258	/**
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	259	CoE state: DICT START.
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	260	*/
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	261
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	262	void ec_fsm_coe_dict_start(ec_fsm_coe_t fsm /< finite state machine /)
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	263	{
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	264	ec_datagram_t *datagram = fsm->datagram;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	265	ec_slave_t *slave = fsm->slave;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	266	uint8_t *data;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	267
834 0791aac03180 Separated SII data from slave. Florian Pose <fp@igh-essen.com> parents: 831 diff changeset	268	if (!(slave->sii.mailbox_protocols & EC_MBOX_COE)) {
825 3771f5a0ed90 CoE state machines raising error, if the slave does not support CoE. Florian Pose <fp@igh-essen.com> parents: 814 diff changeset	269	EC_ERR("Slave %u does not support CoE!\n", slave->ring_position);
3771f5a0ed90 CoE state machines raising error, if the slave does not support CoE. Florian Pose <fp@igh-essen.com> parents: 814 diff changeset	270	fsm->state = ec_fsm_coe_error;
3771f5a0ed90 CoE state machines raising error, if the slave does not support CoE. Florian Pose <fp@igh-essen.com> parents: 814 diff changeset	271	return;
3771f5a0ed90 CoE state machines raising error, if the slave does not support CoE. Florian Pose <fp@igh-essen.com> parents: 814 diff changeset	272	}
3771f5a0ed90 CoE state machines raising error, if the slave does not support CoE. Florian Pose <fp@igh-essen.com> parents: 814 diff changeset	273
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	274	if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 8))) {
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	275	fsm->state = ec_fsm_coe_error;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	276	return;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	277	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	278
814 a51f857b1b2d Renamed [PS]DO to [PS]do. Florian Pose <fp@igh-essen.com> parents: 769 diff changeset	279	EC_WRITE_U16(data, 0x8 << 12); // Sdo information
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	280	EC_WRITE_U8 (data + 2, 0x01); // Get OD List Request
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	281	EC_WRITE_U8 (data + 3, 0x00);
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	282	EC_WRITE_U16(data + 4, 0x0000);
814 a51f857b1b2d Renamed [PS]DO to [PS]do. Florian Pose <fp@igh-essen.com> parents: 769 diff changeset	283	EC_WRITE_U16(data + 6, 0x0001); // deliver all Sdos!
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	284
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	285	fsm->retries = EC_FSM_RETRIES;
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	286	fsm->state = ec_fsm_coe_dict_request;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	287	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	288
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	289	/*****************************************************************************/
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	290
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	291	/**
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	292	CoE state: DICT REQUEST.
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	293	\todo Timeout behavior
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	294	*/
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	295
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	296	void ec_fsm_coe_dict_request(ec_fsm_coe_t fsm /< finite state machine /)
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	297	{
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	298	ec_datagram_t *datagram = fsm->datagram;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	299	ec_slave_t *slave = fsm->slave;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	300
637 d5d04c868e0e Removed ecrt_master_run(), removed datagram queueing from state Florian Pose <fp@igh-essen.com> parents: 594 diff changeset	301	if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
d5d04c868e0e Removed ecrt_master_run(), removed datagram queueing from state Florian Pose <fp@igh-essen.com> parents: 594 diff changeset	302	return; // FIXME: request again?
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	303
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	304	if (datagram->state != EC_DATAGRAM_RECEIVED) {
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	305	fsm->state = ec_fsm_coe_error;
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	306	EC_ERR("Failed to receive CoE dictionary request datagram for"
895 5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	307	" slave %u (datagram state %i).\n",
594 07dd2a7df66e Added datagram state to "Failed to receive..." messages. Florian Pose <fp@igh-essen.com> parents: 505 diff changeset	308	slave->ring_position, datagram->state);
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	309	return;
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	310	}
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	311
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	312	if (datagram->working_counter != 1) {
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	313	fsm->state = ec_fsm_coe_error;
895 5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	314	EC_ERR("Reception of CoE dictionary request failed on slave %u: ",
713 ae41cadd25b6 Better handling of unexpected working counters; introduced Florian Pose <fp@igh-essen.com> parents: 637 diff changeset	315	slave->ring_position);
ae41cadd25b6 Better handling of unexpected working counters; introduced Florian Pose <fp@igh-essen.com> parents: 637 diff changeset	316	ec_datagram_print_wc_error(datagram);
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	317	return;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	318	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	319
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	320	fsm->cycles_start = datagram->cycles_sent;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	321
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	322	ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	323	fsm->retries = EC_FSM_RETRIES;
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	324	fsm->state = ec_fsm_coe_dict_check;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	325	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	326
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	327	/*****************************************************************************/
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	328
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	329	/**
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	330	CoE state: DICT CHECK.
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	331	*/
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	332
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	333	void ec_fsm_coe_dict_check(ec_fsm_coe_t fsm /< finite state machine /)
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	334	{
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	335	ec_datagram_t *datagram = fsm->datagram;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	336	ec_slave_t *slave = fsm->slave;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	337
637 d5d04c868e0e Removed ecrt_master_run(), removed datagram queueing from state Florian Pose <fp@igh-essen.com> parents: 594 diff changeset	338	if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
d5d04c868e0e Removed ecrt_master_run(), removed datagram queueing from state Florian Pose <fp@igh-essen.com> parents: 594 diff changeset	339	return;
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	340
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	341	if (datagram->state != EC_DATAGRAM_RECEIVED) {
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	342	fsm->state = ec_fsm_coe_error;
895 5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	343	EC_ERR("Failed to receive CoE mailbox check datagram for slave %u"
594 07dd2a7df66e Added datagram state to "Failed to receive..." messages. Florian Pose <fp@igh-essen.com> parents: 505 diff changeset	344	" (datagram state %i).\n",
07dd2a7df66e Added datagram state to "Failed to receive..." messages. Florian Pose <fp@igh-essen.com> parents: 505 diff changeset	345	slave->ring_position, datagram->state);
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	346	return;
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	347	}
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	348
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	349	if (datagram->working_counter != 1) {
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	350	fsm->state = ec_fsm_coe_error;
895 5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	351	EC_ERR("Reception of CoE mailbox check datagram failed on slave %u: ",
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	352	slave->ring_position);
713 ae41cadd25b6 Better handling of unexpected working counters; introduced Florian Pose <fp@igh-essen.com> parents: 637 diff changeset	353	ec_datagram_print_wc_error(datagram);
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	354	return;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	355	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	356
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	357	if (!ec_slave_mbox_check(datagram)) {
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	358	if (datagram->cycles_received
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	359	- fsm->cycles_start >= (cycles_t) 100 * cpu_khz) {
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	360	fsm->state = ec_fsm_coe_error;
895 5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	361	EC_ERR("Timeout while checking Sdo dictionary on slave %u.\n",
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	362	slave->ring_position);
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	363	return;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	364	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	365
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	366	ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	367	fsm->retries = EC_FSM_RETRIES;
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	368	return;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	369	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	370
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	371	// Fetch response
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	372	ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	373	fsm->retries = EC_FSM_RETRIES;
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	374	fsm->state = ec_fsm_coe_dict_response;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	375	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	376
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	377	/*****************************************************************************/
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	378
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	379	/**
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	380	CoE state: DICT RESPONSE.
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	381	\todo Timeout behavior
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	382	*/
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	383
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	384	void ec_fsm_coe_dict_response(ec_fsm_coe_t fsm /< finite state machine /)
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	385	{
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	386	ec_datagram_t *datagram = fsm->datagram;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	387	ec_slave_t *slave = fsm->slave;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	388	uint8_t *data, mbox_prot;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	389	size_t rec_size;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	390	unsigned int sdo_count, i;
769 2195c2ea37b4 Fixed normal/expedited bug at CoE upload; more robust behaviour, when Florian Pose <fp@igh-essen.com> parents: 739 diff changeset	391	uint16_t sdo_index, fragments_left;
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	392	ec_sdo_t *sdo;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	393
637 d5d04c868e0e Removed ecrt_master_run(), removed datagram queueing from state Florian Pose <fp@igh-essen.com> parents: 594 diff changeset	394	if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
d5d04c868e0e Removed ecrt_master_run(), removed datagram queueing from state Florian Pose <fp@igh-essen.com> parents: 594 diff changeset	395	return; // FIXME: request again?
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	396
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	397	if (datagram->state != EC_DATAGRAM_RECEIVED) {
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	398	fsm->state = ec_fsm_coe_error;
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	399	EC_ERR("Failed to receive CoE dictionary response datagram for"
895 5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	400	" slave %u (datagram state %i).\n",
594 07dd2a7df66e Added datagram state to "Failed to receive..." messages. Florian Pose <fp@igh-essen.com> parents: 505 diff changeset	401	slave->ring_position, datagram->state);
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	402	return;
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	403	}
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	404
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	405	if (datagram->working_counter != 1) {
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	406	fsm->state = ec_fsm_coe_error;
895 5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	407	EC_ERR("Reception of CoE dictionary response failed on slave %u: ",
713 ae41cadd25b6 Better handling of unexpected working counters; introduced Florian Pose <fp@igh-essen.com> parents: 637 diff changeset	408	slave->ring_position);
ae41cadd25b6 Better handling of unexpected working counters; introduced Florian Pose <fp@igh-essen.com> parents: 637 diff changeset	409	ec_datagram_print_wc_error(datagram);
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	410	return;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	411	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	412
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	413	if (!(data = ec_slave_mbox_fetch(slave, datagram,
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	414	&mbox_prot, &rec_size))) {
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	415	fsm->state = ec_fsm_coe_error;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	416	return;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	417	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	418
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	419	if (mbox_prot != 0x03) { // CoE
446 8fede404c01f Persistent slave lists. Florian Pose <fp@igh-essen.com> parents: 436 diff changeset	420	EC_ERR("Received mailbox protocol 0x%02X as response.\n", mbox_prot);
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	421	fsm->state = ec_fsm_coe_error;
769 2195c2ea37b4 Fixed normal/expedited bug at CoE upload; more robust behaviour, when Florian Pose <fp@igh-essen.com> parents: 739 diff changeset	422	return;
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	423	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	424
895 5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	425	if (ec_fsm_coe_check_emergency(fsm, data, rec_size)) {
5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	426	// check for CoE response again
5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	427	ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	428	fsm->retries = EC_FSM_RETRIES;
5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	429	fsm->state = ec_fsm_coe_dict_check;
5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	430	return;
5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	431	}
5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	432
814 a51f857b1b2d Renamed [PS]DO to [PS]do. Florian Pose <fp@igh-essen.com> parents: 769 diff changeset	433	if (EC_READ_U16(data) >> 12 == 0x8 && // Sdo information
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	434	(EC_READ_U8(data + 2) & 0x7F) == 0x07) { // error response
895 5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	435	EC_ERR("Sdo information error response at slave %u!\n",
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	436	slave->ring_position);
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	437	ec_canopen_abort_msg(EC_READ_U32(data + 6));
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	438	fsm->state = ec_fsm_coe_error;
769 2195c2ea37b4 Fixed normal/expedited bug at CoE upload; more robust behaviour, when Florian Pose <fp@igh-essen.com> parents: 739 diff changeset	439	return;
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	440	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	441
814 a51f857b1b2d Renamed [PS]DO to [PS]do. Florian Pose <fp@igh-essen.com> parents: 769 diff changeset	442	if (EC_READ_U16(data) >> 12 != 0x8 \|\| // Sdo information
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	443	(EC_READ_U8 (data + 2) & 0x7F) != 0x02) { // Get OD List response
895 5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	444	EC_ERR("Invalid Sdo list response at slave %u!\n",
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	445	slave->ring_position);
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	446	ec_print_data(data, rec_size);
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	447	fsm->state = ec_fsm_coe_error;
769 2195c2ea37b4 Fixed normal/expedited bug at CoE upload; more robust behaviour, when Florian Pose <fp@igh-essen.com> parents: 739 diff changeset	448	return;
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	449	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	450
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	451	if (rec_size < 8) {
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	452	EC_ERR("Invalid data size!\n");
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	453	ec_print_data(data, rec_size);
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	454	fsm->state = ec_fsm_coe_error;
769 2195c2ea37b4 Fixed normal/expedited bug at CoE upload; more robust behaviour, when Florian Pose <fp@igh-essen.com> parents: 739 diff changeset	455	return;
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	456	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	457
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	458	sdo_count = (rec_size - 8) / 2;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	459
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	460	for (i = 0; i < sdo_count; i++) {
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	461	sdo_index = EC_READ_U16(data + 8 + i * 2);
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	462	if (!sdo_index) {
461 b9eda9235173 Print certain warnings only at debug_level. Florian Pose <fp@igh-essen.com> parents: 459 diff changeset	463	if (slave->master->debug_level)
895 5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	464	EC_WARN("Sdo dictionary of slave %u contains index 0x0000.\n",
461 b9eda9235173 Print certain warnings only at debug_level. Florian Pose <fp@igh-essen.com> parents: 459 diff changeset	465	slave->ring_position);
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	466	continue;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	467	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	468
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	469	if (!(sdo = (ec_sdo_t *) kmalloc(sizeof(ec_sdo_t), GFP_ATOMIC))) {
814 a51f857b1b2d Renamed [PS]DO to [PS]do. Florian Pose <fp@igh-essen.com> parents: 769 diff changeset	470	EC_ERR("Failed to allocate memory for Sdo!\n");
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	471	fsm->state = ec_fsm_coe_error;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	472	return;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	473	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	474
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	475	if (ec_sdo_init(sdo, sdo_index, slave)) {
814 a51f857b1b2d Renamed [PS]DO to [PS]do. Florian Pose <fp@igh-essen.com> parents: 769 diff changeset	476	EC_ERR("Failed to init Sdo!\n");
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	477	fsm->state = ec_fsm_coe_error;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	478	return;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	479	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	480
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	481	list_add_tail(&sdo->list, &slave->sdo_dictionary);
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	482	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	483
769 2195c2ea37b4 Fixed normal/expedited bug at CoE upload; more robust behaviour, when Florian Pose <fp@igh-essen.com> parents: 739 diff changeset	484	fragments_left = EC_READ_U16(data + 4);
2195c2ea37b4 Fixed normal/expedited bug at CoE upload; more robust behaviour, when Florian Pose <fp@igh-essen.com> parents: 739 diff changeset	485	if (slave->master->debug_level && fragments_left) {
814 a51f857b1b2d Renamed [PS]DO to [PS]do. Florian Pose <fp@igh-essen.com> parents: 769 diff changeset	486	EC_DBG("Sdo list fragments left: %u\n", fragments_left);
769 2195c2ea37b4 Fixed normal/expedited bug at CoE upload; more robust behaviour, when Florian Pose <fp@igh-essen.com> parents: 739 diff changeset	487	}
2195c2ea37b4 Fixed normal/expedited bug at CoE upload; more robust behaviour, when Florian Pose <fp@igh-essen.com> parents: 739 diff changeset	488
2195c2ea37b4 Fixed normal/expedited bug at CoE upload; more robust behaviour, when Florian Pose <fp@igh-essen.com> parents: 739 diff changeset	489	if (EC_READ_U8(data + 2) & 0x80 \|\| fragments_left) { // more messages waiting. check again.
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	490	fsm->cycles_start = datagram->cycles_sent;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	491	ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	492	fsm->retries = EC_FSM_RETRIES;
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	493	fsm->state = ec_fsm_coe_dict_check;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	494	return;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	495	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	496
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	497	if (list_empty(&slave->sdo_dictionary)) {
814 a51f857b1b2d Renamed [PS]DO to [PS]do. Florian Pose <fp@igh-essen.com> parents: 769 diff changeset	498	// no Sdos in dictionary. finished.
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	499	fsm->state = ec_fsm_coe_end; // success
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	500	return;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	501	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	502
814 a51f857b1b2d Renamed [PS]DO to [PS]do. Florian Pose <fp@igh-essen.com> parents: 769 diff changeset	503	// fetch Sdo descriptions
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	504	fsm->sdo = list_entry(slave->sdo_dictionary.next, ec_sdo_t, list);
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	505
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	506	if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 8))) {
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	507	fsm->state = ec_fsm_coe_error;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	508	return;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	509	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	510
814 a51f857b1b2d Renamed [PS]DO to [PS]do. Florian Pose <fp@igh-essen.com> parents: 769 diff changeset	511	EC_WRITE_U16(data, 0x8 << 12); // Sdo information
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	512	EC_WRITE_U8 (data + 2, 0x03); // Get object description request
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	513	EC_WRITE_U8 (data + 3, 0x00);
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	514	EC_WRITE_U16(data + 4, 0x0000);
814 a51f857b1b2d Renamed [PS]DO to [PS]do. Florian Pose <fp@igh-essen.com> parents: 769 diff changeset	515	EC_WRITE_U16(data + 6, fsm->sdo->index); // Sdo index
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	516
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	517	fsm->retries = EC_FSM_RETRIES;
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	518	fsm->state = ec_fsm_coe_dict_desc_request;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	519	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	520
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	521	/*****************************************************************************/
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	522
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	523	/**
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	524	CoE state: DICT DESC REQUEST.
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	525	\todo Timeout behavior
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	526	*/
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	527
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	528	void ec_fsm_coe_dict_desc_request(ec_fsm_coe_t fsm /< finite state machine /)
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	529	{
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	530	ec_datagram_t *datagram = fsm->datagram;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	531	ec_slave_t *slave = fsm->slave;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	532
637 d5d04c868e0e Removed ecrt_master_run(), removed datagram queueing from state Florian Pose <fp@igh-essen.com> parents: 594 diff changeset	533	if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
d5d04c868e0e Removed ecrt_master_run(), removed datagram queueing from state Florian Pose <fp@igh-essen.com> parents: 594 diff changeset	534	return; // FIXME: check for response first?
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	535
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	536	if (datagram->state != EC_DATAGRAM_RECEIVED) {
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	537	fsm->state = ec_fsm_coe_error;
814 a51f857b1b2d Renamed [PS]DO to [PS]do. Florian Pose <fp@igh-essen.com> parents: 769 diff changeset	538	EC_ERR("Failed to receive CoE Sdo description request datagram for"
895 5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	539	" slave %u (datagram state %i).\n",
594 07dd2a7df66e Added datagram state to "Failed to receive..." messages. Florian Pose <fp@igh-essen.com> parents: 505 diff changeset	540	slave->ring_position, datagram->state);
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	541	return;
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	542	}
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	543
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	544	if (datagram->working_counter != 1) {
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	545	fsm->state = ec_fsm_coe_error;
814 a51f857b1b2d Renamed [PS]DO to [PS]do. Florian Pose <fp@igh-essen.com> parents: 769 diff changeset	546	EC_ERR("Reception of CoE Sdo description"
895 5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	547	" request failed on slave %u: ", slave->ring_position);
713 ae41cadd25b6 Better handling of unexpected working counters; introduced Florian Pose <fp@igh-essen.com> parents: 637 diff changeset	548	ec_datagram_print_wc_error(datagram);
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	549	return;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	550	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	551
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	552	fsm->cycles_start = datagram->cycles_sent;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	553
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	554	ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	555	fsm->retries = EC_FSM_RETRIES;
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	556	fsm->state = ec_fsm_coe_dict_desc_check;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	557	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	558
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	559	/*****************************************************************************/
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	560
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	561	/**
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	562	CoE state: DICT DESC CHECK.
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	563	*/
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	564
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	565	void ec_fsm_coe_dict_desc_check(ec_fsm_coe_t fsm /< finite state machine /)
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	566	{
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	567	ec_datagram_t *datagram = fsm->datagram;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	568	ec_slave_t *slave = fsm->slave;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	569
637 d5d04c868e0e Removed ecrt_master_run(), removed datagram queueing from state Florian Pose <fp@igh-essen.com> parents: 594 diff changeset	570	if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
d5d04c868e0e Removed ecrt_master_run(), removed datagram queueing from state Florian Pose <fp@igh-essen.com> parents: 594 diff changeset	571	return;
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	572
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	573	if (datagram->state != EC_DATAGRAM_RECEIVED) {
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	574	fsm->state = ec_fsm_coe_error;
895 5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	575	EC_ERR("Failed to receive CoE mailbox check datagram from slave %u"
594 07dd2a7df66e Added datagram state to "Failed to receive..." messages. Florian Pose <fp@igh-essen.com> parents: 505 diff changeset	576	" (datagram state %i).\n",
07dd2a7df66e Added datagram state to "Failed to receive..." messages. Florian Pose <fp@igh-essen.com> parents: 505 diff changeset	577	slave->ring_position, datagram->state);
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	578	return;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	579	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	580
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	581	if (datagram->working_counter != 1) {
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	582	fsm->state = ec_fsm_coe_error;
713 ae41cadd25b6 Better handling of unexpected working counters; introduced Florian Pose <fp@igh-essen.com> parents: 637 diff changeset	583	EC_ERR("Reception of CoE mailbox check"
895 5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	584	" datagram failed on slave %u: ", slave->ring_position);
713 ae41cadd25b6 Better handling of unexpected working counters; introduced Florian Pose <fp@igh-essen.com> parents: 637 diff changeset	585	ec_datagram_print_wc_error(datagram);
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	586	return;
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	587	}
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	588
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	589	if (!ec_slave_mbox_check(datagram)) {
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	590	if (datagram->cycles_received
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	591	- fsm->cycles_start >= (cycles_t) 100 * cpu_khz) {
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	592	fsm->state = ec_fsm_coe_error;
895 5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	593	EC_ERR("Timeout while checking Sdo description on slave %u.\n",
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	594	slave->ring_position);
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	595	return;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	596	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	597
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	598	ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	599	fsm->retries = EC_FSM_RETRIES;
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	600	return;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	601	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	602
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	603	// Fetch response
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	604	ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	605	fsm->retries = EC_FSM_RETRIES;
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	606	fsm->state = ec_fsm_coe_dict_desc_response;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	607	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	608
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	609	/*****************************************************************************/
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	610
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	611	/**
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	612	CoE state: DICT DESC RESPONSE.
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	613	\todo Timeout behavior
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	614	*/
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	615
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	616	void ec_fsm_coe_dict_desc_response(ec_fsm_coe_t *fsm
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	617	/*< finite state machine /)
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	618	{
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	619	ec_datagram_t *datagram = fsm->datagram;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	620	ec_slave_t *slave = fsm->slave;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	621	ec_sdo_t *sdo = fsm->sdo;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	622	uint8_t *data, mbox_prot;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	623	size_t rec_size, name_size;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	624
637 d5d04c868e0e Removed ecrt_master_run(), removed datagram queueing from state Florian Pose <fp@igh-essen.com> parents: 594 diff changeset	625	if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
d5d04c868e0e Removed ecrt_master_run(), removed datagram queueing from state Florian Pose <fp@igh-essen.com> parents: 594 diff changeset	626	return; // FIXME: request again?
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	627
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	628	if (datagram->state != EC_DATAGRAM_RECEIVED) {
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	629	fsm->state = ec_fsm_coe_error;
814 a51f857b1b2d Renamed [PS]DO to [PS]do. Florian Pose <fp@igh-essen.com> parents: 769 diff changeset	630	EC_ERR("Failed to receive CoE Sdo description response datagram from"
895 5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	631	" slave %u (datagram state %i).\n",
594 07dd2a7df66e Added datagram state to "Failed to receive..." messages. Florian Pose <fp@igh-essen.com> parents: 505 diff changeset	632	slave->ring_position, datagram->state);
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	633	return;
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	634	}
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	635
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	636	if (datagram->working_counter != 1) {
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	637	fsm->state = ec_fsm_coe_error;
814 a51f857b1b2d Renamed [PS]DO to [PS]do. Florian Pose <fp@igh-essen.com> parents: 769 diff changeset	638	EC_ERR("Reception of CoE Sdo description"
895 5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	639	" response failed on slave %u: ", slave->ring_position);
713 ae41cadd25b6 Better handling of unexpected working counters; introduced Florian Pose <fp@igh-essen.com> parents: 637 diff changeset	640	ec_datagram_print_wc_error(datagram);
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	641	return;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	642	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	643
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	644	if (!(data = ec_slave_mbox_fetch(slave, datagram,
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	645	&mbox_prot, &rec_size))) {
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	646	fsm->state = ec_fsm_coe_error;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	647	return;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	648	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	649
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	650	if (mbox_prot != 0x03) { // CoE
446 8fede404c01f Persistent slave lists. Florian Pose <fp@igh-essen.com> parents: 436 diff changeset	651	EC_ERR("Received mailbox protocol 0x%02X as response.\n", mbox_prot);
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	652	fsm->state = ec_fsm_coe_error;
895 5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	653	return;
5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	654	}
5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	655
5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	656	if (ec_fsm_coe_check_emergency(fsm, data, rec_size)) {
5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	657	// check for CoE response again
5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	658	ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	659	fsm->retries = EC_FSM_RETRIES;
5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	660	fsm->state = ec_fsm_coe_dict_desc_check;
5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	661	return;
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	662	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	663
814 a51f857b1b2d Renamed [PS]DO to [PS]do. Florian Pose <fp@igh-essen.com> parents: 769 diff changeset	664	if (EC_READ_U16(data) >> 12 == 0x8 && // Sdo information
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	665	(EC_READ_U8 (data + 2) & 0x7F) == 0x07) { // error response
895 5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	666	EC_ERR("Sdo information error response at slave %u while"
814 a51f857b1b2d Renamed [PS]DO to [PS]do. Florian Pose <fp@igh-essen.com> parents: 769 diff changeset	667	" fetching Sdo 0x%04X!\n", slave->ring_position,
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	668	sdo->index);
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	669	ec_canopen_abort_msg(EC_READ_U32(data + 6));
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	670	fsm->state = ec_fsm_coe_error;
895 5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	671	return;
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	672	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	673
814 a51f857b1b2d Renamed [PS]DO to [PS]do. Florian Pose <fp@igh-essen.com> parents: 769 diff changeset	674	if (EC_READ_U16(data) >> 12 != 0x8 \|\| // Sdo information
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	675	(EC_READ_U8 (data + 2) & 0x7F) != 0x04 \|\| // Object desc. response
814 a51f857b1b2d Renamed [PS]DO to [PS]do. Florian Pose <fp@igh-essen.com> parents: 769 diff changeset	676	EC_READ_U16(data + 6) != sdo->index) { // Sdo index
895 5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	677	EC_ERR("Invalid object description response at slave %u while"
814 a51f857b1b2d Renamed [PS]DO to [PS]do. Florian Pose <fp@igh-essen.com> parents: 769 diff changeset	678	" fetching Sdo 0x%04X!\n", slave->ring_position,
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	679	sdo->index);
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	680	ec_print_data(data, rec_size);
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	681	fsm->state = ec_fsm_coe_error;
895 5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	682	return;
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	683	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	684
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	685	if (rec_size < 12) {
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	686	EC_ERR("Invalid data size!\n");
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	687	ec_print_data(data, rec_size);
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	688	fsm->state = ec_fsm_coe_error;
895 5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	689	return;
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	690	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	691
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	692	sdo->subindices = EC_READ_U8(data + 10);
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	693	sdo->object_code = EC_READ_U8(data + 11);
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	694
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	695	name_size = rec_size - 12;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	696	if (name_size) {
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	697	if (!(sdo->name = kmalloc(name_size + 1, GFP_ATOMIC))) {
814 a51f857b1b2d Renamed [PS]DO to [PS]do. Florian Pose <fp@igh-essen.com> parents: 769 diff changeset	698	EC_ERR("Failed to allocate Sdo name!\n");
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	699	fsm->state = ec_fsm_coe_error;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	700	return;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	701	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	702
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	703	memcpy(sdo->name, data + 12, name_size);
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	704	sdo->name[name_size] = 0;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	705	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	706
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	707	if (EC_READ_U8(data + 2) & 0x80) {
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	708	EC_ERR("Fragment follows (not implemented)!\n");
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	709	fsm->state = ec_fsm_coe_error;
895 5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	710	return;
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	711	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	712
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	713	// start fetching entries
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	714
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	715	fsm->subindex = 0;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	716
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	717	if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 10))) {
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	718	fsm->state = ec_fsm_coe_error;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	719	return;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	720	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	721
814 a51f857b1b2d Renamed [PS]DO to [PS]do. Florian Pose <fp@igh-essen.com> parents: 769 diff changeset	722	EC_WRITE_U16(data, 0x8 << 12); // Sdo information
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	723	EC_WRITE_U8 (data + 2, 0x05); // Get entry description request
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	724	EC_WRITE_U8 (data + 3, 0x00);
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	725	EC_WRITE_U16(data + 4, 0x0000);
814 a51f857b1b2d Renamed [PS]DO to [PS]do. Florian Pose <fp@igh-essen.com> parents: 769 diff changeset	726	EC_WRITE_U16(data + 6, sdo->index); // Sdo index
a51f857b1b2d Renamed [PS]DO to [PS]do. Florian Pose <fp@igh-essen.com> parents: 769 diff changeset	727	EC_WRITE_U8 (data + 8, fsm->subindex); // Sdo subindex
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	728	EC_WRITE_U8 (data + 9, 0x00); // value info (no values)
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	729
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	730	fsm->retries = EC_FSM_RETRIES;
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	731	fsm->state = ec_fsm_coe_dict_entry_request;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	732	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	733
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	734	/*****************************************************************************/
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	735
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	736	/**
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	737	CoE state: DICT ENTRY REQUEST.
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	738	\todo Timeout behavior
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	739	*/
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	740
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	741	void ec_fsm_coe_dict_entry_request(ec_fsm_coe_t *fsm
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	742	/*< finite state machine /)
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	743	{
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	744	ec_datagram_t *datagram = fsm->datagram;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	745	ec_slave_t *slave = fsm->slave;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	746
637 d5d04c868e0e Removed ecrt_master_run(), removed datagram queueing from state Florian Pose <fp@igh-essen.com> parents: 594 diff changeset	747	if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
d5d04c868e0e Removed ecrt_master_run(), removed datagram queueing from state Florian Pose <fp@igh-essen.com> parents: 594 diff changeset	748	return; // FIXME: check for response first?
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	749
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	750	if (datagram->state != EC_DATAGRAM_RECEIVED) {
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	751	fsm->state = ec_fsm_coe_error;
814 a51f857b1b2d Renamed [PS]DO to [PS]do. Florian Pose <fp@igh-essen.com> parents: 769 diff changeset	752	EC_ERR("Failed to receive CoE Sdo entry request datagram for"
895 5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	753	" slave %u (datagram state %i).\n",
594 07dd2a7df66e Added datagram state to "Failed to receive..." messages. Florian Pose <fp@igh-essen.com> parents: 505 diff changeset	754	slave->ring_position, datagram->state);
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	755	return;
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	756	}
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	757
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	758	if (datagram->working_counter != 1) {
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	759	fsm->state = ec_fsm_coe_error;
895 5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	760	EC_ERR("Reception of CoE Sdo entry request failed on slave %u: ",
713 ae41cadd25b6 Better handling of unexpected working counters; introduced Florian Pose <fp@igh-essen.com> parents: 637 diff changeset	761	slave->ring_position);
ae41cadd25b6 Better handling of unexpected working counters; introduced Florian Pose <fp@igh-essen.com> parents: 637 diff changeset	762	ec_datagram_print_wc_error(datagram);
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	763	return;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	764	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	765
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	766	fsm->cycles_start = datagram->cycles_sent;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	767
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	768	ec_slave_mbox_prepare_check(slave, datagram); // can not fail
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	769	fsm->retries = EC_FSM_RETRIES;
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	770	fsm->state = ec_fsm_coe_dict_entry_check;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	771	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	772
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	773	/*****************************************************************************/
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	774
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	775	/**
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	776	CoE state: DICT ENTRY CHECK.
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	777	*/
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	778
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	779	void ec_fsm_coe_dict_entry_check(ec_fsm_coe_t *fsm
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	780	/*< finite state machine /)
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	781	{
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	782	ec_datagram_t *datagram = fsm->datagram;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	783	ec_slave_t *slave = fsm->slave;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	784
637 d5d04c868e0e Removed ecrt_master_run(), removed datagram queueing from state Florian Pose <fp@igh-essen.com> parents: 594 diff changeset	785	if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
d5d04c868e0e Removed ecrt_master_run(), removed datagram queueing from state Florian Pose <fp@igh-essen.com> parents: 594 diff changeset	786	return;
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	787
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	788	if (datagram->state != EC_DATAGRAM_RECEIVED) {
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	789	fsm->state = ec_fsm_coe_error;
895 5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	790	EC_ERR("Failed to receive CoE mailbox check datagram from slave %u"
594 07dd2a7df66e Added datagram state to "Failed to receive..." messages. Florian Pose <fp@igh-essen.com> parents: 505 diff changeset	791	" (datagram state %i).\n",
07dd2a7df66e Added datagram state to "Failed to receive..." messages. Florian Pose <fp@igh-essen.com> parents: 505 diff changeset	792	slave->ring_position, datagram->state);
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	793	return;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	794	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	795
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	796	if (datagram->working_counter != 1) {
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	797	fsm->state = ec_fsm_coe_error;
713 ae41cadd25b6 Better handling of unexpected working counters; introduced Florian Pose <fp@igh-essen.com> parents: 637 diff changeset	798	EC_ERR("Reception of CoE mailbox check"
895 5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	799	" datagram failed on slave %u: ", slave->ring_position);
713 ae41cadd25b6 Better handling of unexpected working counters; introduced Florian Pose <fp@igh-essen.com> parents: 637 diff changeset	800	ec_datagram_print_wc_error(datagram);
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	801	return;
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	802	}
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	803
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	804	if (!ec_slave_mbox_check(datagram)) {
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	805	if (datagram->cycles_received
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	806	- fsm->cycles_start >= (cycles_t) 100 * cpu_khz) {
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	807	fsm->state = ec_fsm_coe_error;
895 5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	808	EC_ERR("Timeout while checking Sdo entry on slave %u.\n",
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	809	slave->ring_position);
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	810	return;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	811	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	812
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	813	ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	814	fsm->retries = EC_FSM_RETRIES;
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	815	return;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	816	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	817
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	818	// Fetch response
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	819	ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	820	fsm->retries = EC_FSM_RETRIES;
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	821	fsm->state = ec_fsm_coe_dict_entry_response;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	822	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	823
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	824	/*****************************************************************************/
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	825
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	826	/**
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	827	CoE state: DICT ENTRY RESPONSE.
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	828	\todo Timeout behavior
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	829	*/
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	830
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	831	void ec_fsm_coe_dict_entry_response(ec_fsm_coe_t *fsm
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	832	/*< finite state machine /)
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	833	{
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	834	ec_datagram_t *datagram = fsm->datagram;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	835	ec_slave_t *slave = fsm->slave;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	836	ec_sdo_t *sdo = fsm->sdo;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	837	uint8_t *data, mbox_prot;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	838	size_t rec_size, data_size;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	839	ec_sdo_entry_t *entry;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	840
637 d5d04c868e0e Removed ecrt_master_run(), removed datagram queueing from state Florian Pose <fp@igh-essen.com> parents: 594 diff changeset	841	if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
d5d04c868e0e Removed ecrt_master_run(), removed datagram queueing from state Florian Pose <fp@igh-essen.com> parents: 594 diff changeset	842	return; // FIXME: request again?
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	843
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	844	if (datagram->state != EC_DATAGRAM_RECEIVED) {
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	845	fsm->state = ec_fsm_coe_error;
814 a51f857b1b2d Renamed [PS]DO to [PS]do. Florian Pose <fp@igh-essen.com> parents: 769 diff changeset	846	EC_ERR("Failed to receive CoE Sdo description response datagram from"
895 5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	847	" slave %u (datagram state %i).\n",
594 07dd2a7df66e Added datagram state to "Failed to receive..." messages. Florian Pose <fp@igh-essen.com> parents: 505 diff changeset	848	slave->ring_position, datagram->state);
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	849	return;
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	850	}
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	851
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	852	if (datagram->working_counter != 1) {
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	853	fsm->state = ec_fsm_coe_error;
814 a51f857b1b2d Renamed [PS]DO to [PS]do. Florian Pose <fp@igh-essen.com> parents: 769 diff changeset	854	EC_ERR("Reception of CoE Sdo description"
895 5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	855	" response failed on slave %u: ", slave->ring_position);
713 ae41cadd25b6 Better handling of unexpected working counters; introduced Florian Pose <fp@igh-essen.com> parents: 637 diff changeset	856	ec_datagram_print_wc_error(datagram);
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	857	return;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	858	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	859
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	860	if (!(data = ec_slave_mbox_fetch(slave, datagram,
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	861	&mbox_prot, &rec_size))) {
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	862	fsm->state = ec_fsm_coe_error;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	863	return;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	864	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	865
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	866	if (mbox_prot != 0x03) { // CoE
446 8fede404c01f Persistent slave lists. Florian Pose <fp@igh-essen.com> parents: 436 diff changeset	867	EC_ERR("Received mailbox protocol 0x%02X as response.\n", mbox_prot);
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	868	fsm->state = ec_fsm_coe_error;
895 5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	869	return;
5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	870	}
5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	871
5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	872	if (ec_fsm_coe_check_emergency(fsm, data, rec_size)) {
5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	873	// check for CoE response again
5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	874	ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	875	fsm->retries = EC_FSM_RETRIES;
5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	876	fsm->state = ec_fsm_coe_dict_entry_check;
5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	877	return;
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	878	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	879
814 a51f857b1b2d Renamed [PS]DO to [PS]do. Florian Pose <fp@igh-essen.com> parents: 769 diff changeset	880	if (EC_READ_U16(data) >> 12 == 0x8 && // Sdo information
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	881	(EC_READ_U8 (data + 2) & 0x7F) == 0x07) { // error response
895 5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	882	EC_ERR("Sdo information error response at slave %u while"
926 6bb33c6ec770 Output subindices with %02X. Florian Pose <fp@igh-essen.com> parents: 895 diff changeset	883	" fetching Sdo entry 0x%04X:%02X!\n", slave->ring_position,
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	884	sdo->index, fsm->subindex);
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	885	ec_canopen_abort_msg(EC_READ_U32(data + 6));
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	886	fsm->state = ec_fsm_coe_error;
895 5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	887	return;
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	888	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	889
814 a51f857b1b2d Renamed [PS]DO to [PS]do. Florian Pose <fp@igh-essen.com> parents: 769 diff changeset	890	if (EC_READ_U16(data) >> 12 != 0x8 \|\| // Sdo information
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	891	(EC_READ_U8(data + 2) & 0x7F) != 0x06 \|\| // Entry desc. response
814 a51f857b1b2d Renamed [PS]DO to [PS]do. Florian Pose <fp@igh-essen.com> parents: 769 diff changeset	892	EC_READ_U16(data + 6) != sdo->index \|\| // Sdo index
a51f857b1b2d Renamed [PS]DO to [PS]do. Florian Pose <fp@igh-essen.com> parents: 769 diff changeset	893	EC_READ_U8(data + 8) != fsm->subindex) { // Sdo subindex
895 5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	894	EC_ERR("Invalid entry description response at slave %u while"
926 6bb33c6ec770 Output subindices with %02X. Florian Pose <fp@igh-essen.com> parents: 895 diff changeset	895	" fetching Sdo entry 0x%04X:%02X!\n", slave->ring_position,
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	896	sdo->index, fsm->subindex);
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	897	ec_print_data(data, rec_size);
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	898	fsm->state = ec_fsm_coe_error;
895 5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	899	return;
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	900	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	901
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	902	if (rec_size < 16) {
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	903	EC_ERR("Invalid data size!\n");
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	904	ec_print_data(data, rec_size);
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	905	fsm->state = ec_fsm_coe_error;
895 5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	906	return;
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	907	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	908
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	909	data_size = rec_size - 16;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	910
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	911	if (!(entry = (ec_sdo_entry_t *)
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	912	kmalloc(sizeof(ec_sdo_entry_t), GFP_ATOMIC))) {
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	913	EC_ERR("Failed to allocate entry!\n");
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	914	fsm->state = ec_fsm_coe_error;
895 5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	915	return;
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	916	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	917
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	918	if (ec_sdo_entry_init(entry, fsm->subindex, sdo)) {
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	919	EC_ERR("Failed to init entry!\n");
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	920	fsm->state = ec_fsm_coe_error;
895 5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	921	return;
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	922	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	923
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	924	entry->data_type = EC_READ_U16(data + 10);
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	925	entry->bit_length = EC_READ_U16(data + 12);
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	926
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	927	if (data_size) {
484 9fde4a17b820 kobject_add() in constructors. Florian Pose <fp@igh-essen.com> parents: 476 diff changeset	928	uint8_t *desc;
9fde4a17b820 kobject_add() in constructors. Florian Pose <fp@igh-essen.com> parents: 476 diff changeset	929	if (!(desc = kmalloc(data_size + 1, GFP_ATOMIC))) {
814 a51f857b1b2d Renamed [PS]DO to [PS]do. Florian Pose <fp@igh-essen.com> parents: 769 diff changeset	930	EC_ERR("Failed to allocate Sdo entry name!\n");
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	931	fsm->state = ec_fsm_coe_error;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	932	return;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	933	}
484 9fde4a17b820 kobject_add() in constructors. Florian Pose <fp@igh-essen.com> parents: 476 diff changeset	934	memcpy(desc, data + 16, data_size);
9fde4a17b820 kobject_add() in constructors. Florian Pose <fp@igh-essen.com> parents: 476 diff changeset	935	desc[data_size] = 0;
9fde4a17b820 kobject_add() in constructors. Florian Pose <fp@igh-essen.com> parents: 476 diff changeset	936	entry->description = desc;
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	937	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	938
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	939	list_add_tail(&entry->list, &sdo->entries);
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	940
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	941	if (fsm->subindex < sdo->subindices) {
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	942	fsm->subindex++;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	943
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	944	if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 10))) {
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	945	fsm->state = ec_fsm_coe_error;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	946	return;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	947	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	948
814 a51f857b1b2d Renamed [PS]DO to [PS]do. Florian Pose <fp@igh-essen.com> parents: 769 diff changeset	949	EC_WRITE_U16(data, 0x8 << 12); // Sdo information
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	950	EC_WRITE_U8 (data + 2, 0x05); // Get entry description request
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	951	EC_WRITE_U8 (data + 3, 0x00);
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	952	EC_WRITE_U16(data + 4, 0x0000);
814 a51f857b1b2d Renamed [PS]DO to [PS]do. Florian Pose <fp@igh-essen.com> parents: 769 diff changeset	953	EC_WRITE_U16(data + 6, sdo->index); // Sdo index
a51f857b1b2d Renamed [PS]DO to [PS]do. Florian Pose <fp@igh-essen.com> parents: 769 diff changeset	954	EC_WRITE_U8 (data + 8, fsm->subindex); // Sdo subindex
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	955	EC_WRITE_U8 (data + 9, 0x00); // value info (no values)
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	956
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	957	fsm->retries = EC_FSM_RETRIES;
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	958	fsm->state = ec_fsm_coe_dict_entry_request;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	959	return;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	960	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	961
814 a51f857b1b2d Renamed [PS]DO to [PS]do. Florian Pose <fp@igh-essen.com> parents: 769 diff changeset	962	// another Sdo description to fetch?
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	963	if (fsm->sdo->list.next != &slave->sdo_dictionary) {
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	964	fsm->sdo = list_entry(fsm->sdo->list.next, ec_sdo_t, list);
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	965
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	966	if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 8))) {
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	967	fsm->state = ec_fsm_coe_error;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	968	return;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	969	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	970
814 a51f857b1b2d Renamed [PS]DO to [PS]do. Florian Pose <fp@igh-essen.com> parents: 769 diff changeset	971	EC_WRITE_U16(data, 0x8 << 12); // Sdo information
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	972	EC_WRITE_U8 (data + 2, 0x03); // Get object description request
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	973	EC_WRITE_U8 (data + 3, 0x00);
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	974	EC_WRITE_U16(data + 4, 0x0000);
814 a51f857b1b2d Renamed [PS]DO to [PS]do. Florian Pose <fp@igh-essen.com> parents: 769 diff changeset	975	EC_WRITE_U16(data + 6, fsm->sdo->index); // Sdo index
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	976
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	977	fsm->retries = EC_FSM_RETRIES;
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	978	fsm->state = ec_fsm_coe_dict_desc_request;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	979	return;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	980	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	981
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	982	fsm->state = ec_fsm_coe_end;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	983	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	984
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	985	/******************************************************************************
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	986	* CoE state machine
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	987	*****************************************************************************/
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	988
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	989	/**
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	990	CoE state: DOWN START.
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	991	*/
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	992
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	993	void ec_fsm_coe_down_start(ec_fsm_coe_t fsm /< finite state machine /)
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	994	{
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	995	ec_datagram_t *datagram = fsm->datagram;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	996	ec_slave_t *slave = fsm->slave;
854 f4f53be425ac Removed ec_sdodata_t; CoE state machines work on ec_sdo_request_t. Florian Pose <fp@igh-essen.com> parents: 834 diff changeset	997	ec_sdo_request_t *request = fsm->request;
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	998	uint8_t *data;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	999
874 022f869ea40f Added CoE debugging. Florian Pose <fp@igh-essen.com> parents: 860 diff changeset	1000	if (fsm->slave->master->debug_level) {
926 6bb33c6ec770 Output subindices with %02X. Florian Pose <fp@igh-essen.com> parents: 895 diff changeset	1001	EC_DBG("Downloading Sdo 0x%04X:%02X to slave %u.\n",
854 f4f53be425ac Removed ec_sdodata_t; CoE state machines work on ec_sdo_request_t. Florian Pose <fp@igh-essen.com> parents: 834 diff changeset	1002	request->index, request->subindex, slave->ring_position);
874 022f869ea40f Added CoE debugging. Florian Pose <fp@igh-essen.com> parents: 860 diff changeset	1003	ec_print_data(request->data, request->data_size);
022f869ea40f Added CoE debugging. Florian Pose <fp@igh-essen.com> parents: 860 diff changeset	1004	}
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1005
834 0791aac03180 Separated SII data from slave. Florian Pose <fp@igh-essen.com> parents: 831 diff changeset	1006	if (!(slave->sii.mailbox_protocols & EC_MBOX_COE)) {
825 3771f5a0ed90 CoE state machines raising error, if the slave does not support CoE. Florian Pose <fp@igh-essen.com> parents: 814 diff changeset	1007	EC_ERR("Slave %u does not support CoE!\n", slave->ring_position);
3771f5a0ed90 CoE state machines raising error, if the slave does not support CoE. Florian Pose <fp@igh-essen.com> parents: 814 diff changeset	1008	fsm->state = ec_fsm_coe_error;
3771f5a0ed90 CoE state machines raising error, if the slave does not support CoE. Florian Pose <fp@igh-essen.com> parents: 814 diff changeset	1009	return;
3771f5a0ed90 CoE state machines raising error, if the slave does not support CoE. Florian Pose <fp@igh-essen.com> parents: 814 diff changeset	1010	}
3771f5a0ed90 CoE state machines raising error, if the slave does not support CoE. Florian Pose <fp@igh-essen.com> parents: 814 diff changeset	1011
854 f4f53be425ac Removed ec_sdodata_t; CoE state machines work on ec_sdo_request_t. Florian Pose <fp@igh-essen.com> parents: 834 diff changeset	1012	if (slave->sii.rx_mailbox_size < 6 + 10 + request->data_size) {
814 a51f857b1b2d Renamed [PS]DO to [PS]do. Florian Pose <fp@igh-essen.com> parents: 769 diff changeset	1013	EC_ERR("Sdo fragmenting not supported yet!\n");
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1014	fsm->state = ec_fsm_coe_error;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1015	return;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1016	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1017
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1018	if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03,
854 f4f53be425ac Removed ec_sdodata_t; CoE state machines work on ec_sdo_request_t. Florian Pose <fp@igh-essen.com> parents: 834 diff changeset	1019	request->data_size + 10))) {
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1020	fsm->state = ec_fsm_coe_error;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1021	return;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1022	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1023
814 a51f857b1b2d Renamed [PS]DO to [PS]do. Florian Pose <fp@igh-essen.com> parents: 769 diff changeset	1024	EC_WRITE_U16(data, 0x2 << 12); // Sdo request
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1025	EC_WRITE_U8 (data + 2, (0x1 // size specified
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1026	\| 0x1 << 5)); // Download request
854 f4f53be425ac Removed ec_sdodata_t; CoE state machines work on ec_sdo_request_t. Florian Pose <fp@igh-essen.com> parents: 834 diff changeset	1027	EC_WRITE_U16(data + 3, request->index);
f4f53be425ac Removed ec_sdodata_t; CoE state machines work on ec_sdo_request_t. Florian Pose <fp@igh-essen.com> parents: 834 diff changeset	1028	EC_WRITE_U8 (data + 5, request->subindex);
f4f53be425ac Removed ec_sdodata_t; CoE state machines work on ec_sdo_request_t. Florian Pose <fp@igh-essen.com> parents: 834 diff changeset	1029	EC_WRITE_U32(data + 6, request->data_size);
f4f53be425ac Removed ec_sdodata_t; CoE state machines work on ec_sdo_request_t. Florian Pose <fp@igh-essen.com> parents: 834 diff changeset	1030	memcpy(data + 10, request->data, request->data_size);
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1031
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1032	fsm->retries = EC_FSM_RETRIES;
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1033	fsm->state = ec_fsm_coe_down_request;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1034	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1035
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1036	/*****************************************************************************/
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1037
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1038	/**
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1039	CoE state: DOWN REQUEST.
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1040	\todo Timeout behavior
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1041	*/
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1042
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1043	void ec_fsm_coe_down_request(ec_fsm_coe_t fsm /< finite state machine /)
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1044	{
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1045	ec_datagram_t *datagram = fsm->datagram;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1046	ec_slave_t *slave = fsm->slave;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1047
637 d5d04c868e0e Removed ecrt_master_run(), removed datagram queueing from state Florian Pose <fp@igh-essen.com> parents: 594 diff changeset	1048	if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
d5d04c868e0e Removed ecrt_master_run(), removed datagram queueing from state Florian Pose <fp@igh-essen.com> parents: 594 diff changeset	1049	return; // FIXME: check for response first?
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1050
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1051	if (datagram->state != EC_DATAGRAM_RECEIVED) {
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1052	fsm->state = ec_fsm_coe_error;
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1053	EC_ERR("Failed to receive CoE download request datagram for"
895 5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	1054	" slave %u (datagram state %i).\n",
594 07dd2a7df66e Added datagram state to "Failed to receive..." messages. Florian Pose <fp@igh-essen.com> parents: 505 diff changeset	1055	slave->ring_position, datagram->state);
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1056	return;
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1057	}
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1058
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1059	if (datagram->working_counter != 1) {
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1060	fsm->state = ec_fsm_coe_error;
895 5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	1061	EC_ERR("Reception of CoE download request failed on slave %u: ",
713 ae41cadd25b6 Better handling of unexpected working counters; introduced Florian Pose <fp@igh-essen.com> parents: 637 diff changeset	1062	slave->ring_position);
ae41cadd25b6 Better handling of unexpected working counters; introduced Florian Pose <fp@igh-essen.com> parents: 637 diff changeset	1063	ec_datagram_print_wc_error(datagram);
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1064	return;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1065	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1066
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1067	fsm->cycles_start = datagram->cycles_sent;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1068
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1069	ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1070	fsm->retries = EC_FSM_RETRIES;
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1071	fsm->state = ec_fsm_coe_down_check;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1072	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1073
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1074	/*****************************************************************************/
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1075
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1076	/**
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1077	CoE state: DOWN CHECK.
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1078	*/
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1079
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1080	void ec_fsm_coe_down_check(ec_fsm_coe_t fsm /< finite state machine /)
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1081	{
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1082	ec_datagram_t *datagram = fsm->datagram;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1083	ec_slave_t *slave = fsm->slave;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1084
637 d5d04c868e0e Removed ecrt_master_run(), removed datagram queueing from state Florian Pose <fp@igh-essen.com> parents: 594 diff changeset	1085	if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
d5d04c868e0e Removed ecrt_master_run(), removed datagram queueing from state Florian Pose <fp@igh-essen.com> parents: 594 diff changeset	1086	return;
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1087
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1088	if (datagram->state != EC_DATAGRAM_RECEIVED) {
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1089	fsm->state = ec_fsm_coe_error;
895 5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	1090	EC_ERR("Failed to receive CoE mailbox check datagram for slave %u"
594 07dd2a7df66e Added datagram state to "Failed to receive..." messages. Florian Pose <fp@igh-essen.com> parents: 505 diff changeset	1091	" (datagram state %i).\n",
07dd2a7df66e Added datagram state to "Failed to receive..." messages. Florian Pose <fp@igh-essen.com> parents: 505 diff changeset	1092	slave->ring_position, datagram->state);
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1093	return;
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1094	}
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1095
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1096	if (datagram->working_counter != 1) {
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1097	fsm->state = ec_fsm_coe_error;
713 ae41cadd25b6 Better handling of unexpected working counters; introduced Florian Pose <fp@igh-essen.com> parents: 637 diff changeset	1098	EC_ERR("Reception of CoE mailbox check"
895 5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	1099	" datagram failed on slave %u: ", slave->ring_position);
713 ae41cadd25b6 Better handling of unexpected working counters; introduced Florian Pose <fp@igh-essen.com> parents: 637 diff changeset	1100	ec_datagram_print_wc_error(datagram);
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1101	return;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1102	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1103
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1104	if (!ec_slave_mbox_check(datagram)) {
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1105	if (datagram->cycles_received
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1106	- fsm->cycles_start >= (cycles_t) 100 * cpu_khz) {
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1107	fsm->state = ec_fsm_coe_error;
895 5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	1108	EC_ERR("Timeout while checking Sdo configuration on slave %u.\n",
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1109	slave->ring_position);
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1110	return;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1111	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1112
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1113	ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1114	fsm->retries = EC_FSM_RETRIES;
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1115	return;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1116	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1117
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1118	// Fetch response
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1119	ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1120	fsm->retries = EC_FSM_RETRIES;
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1121	fsm->state = ec_fsm_coe_down_response;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1122	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1123
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1124	/*****************************************************************************/
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1125
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1126	/**
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1127	CoE state: DOWN RESPONSE.
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1128	\todo Timeout behavior
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1129	*/
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1130
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1131	void ec_fsm_coe_down_response(ec_fsm_coe_t fsm /< finite state machine /)
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1132	{
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1133	ec_datagram_t *datagram = fsm->datagram;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1134	ec_slave_t *slave = fsm->slave;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1135	uint8_t *data, mbox_prot;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1136	size_t rec_size;
854 f4f53be425ac Removed ec_sdodata_t; CoE state machines work on ec_sdo_request_t. Florian Pose <fp@igh-essen.com> parents: 834 diff changeset	1137	ec_sdo_request_t *request = fsm->request;
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1138
637 d5d04c868e0e Removed ecrt_master_run(), removed datagram queueing from state Florian Pose <fp@igh-essen.com> parents: 594 diff changeset	1139	if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
d5d04c868e0e Removed ecrt_master_run(), removed datagram queueing from state Florian Pose <fp@igh-essen.com> parents: 594 diff changeset	1140	return; // FIXME: request again?
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1141
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1142	if (datagram->state != EC_DATAGRAM_RECEIVED) {
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1143	fsm->state = ec_fsm_coe_error;
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1144	EC_ERR("Failed to receive CoE download response datagram from"
895 5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	1145	" slave %u (datagram state %i).\n",
594 07dd2a7df66e Added datagram state to "Failed to receive..." messages. Florian Pose <fp@igh-essen.com> parents: 505 diff changeset	1146	slave->ring_position, datagram->state);
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1147	return;
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1148	}
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1149
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1150	if (datagram->working_counter != 1) {
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1151	fsm->state = ec_fsm_coe_error;
895 5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	1152	EC_ERR("Reception of CoE download response failed on slave %u: ",
713 ae41cadd25b6 Better handling of unexpected working counters; introduced Florian Pose <fp@igh-essen.com> parents: 637 diff changeset	1153	slave->ring_position);
ae41cadd25b6 Better handling of unexpected working counters; introduced Florian Pose <fp@igh-essen.com> parents: 637 diff changeset	1154	ec_datagram_print_wc_error(datagram);
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1155	return;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1156	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1157
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1158	if (!(data = ec_slave_mbox_fetch(slave, datagram,
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1159	&mbox_prot, &rec_size))) {
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1160	fsm->state = ec_fsm_coe_error;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1161	return;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1162	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1163
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1164	if (mbox_prot != 0x03) { // CoE
860 ba5c38f0bb30 Minor change: Moved state assign to beginning. Florian Pose <fp@igh-essen.com> parents: 859 diff changeset	1165	fsm->state = ec_fsm_coe_error;
446 8fede404c01f Persistent slave lists. Florian Pose <fp@igh-essen.com> parents: 436 diff changeset	1166	EC_ERR("Received mailbox protocol 0x%02X as response.\n", mbox_prot);
860 ba5c38f0bb30 Minor change: Moved state assign to beginning. Florian Pose <fp@igh-essen.com> parents: 859 diff changeset	1167	return;
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1168	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1169
895 5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	1170	if (ec_fsm_coe_check_emergency(fsm, data, rec_size)) {
5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	1171	// check for CoE response again
5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	1172	ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	1173	fsm->retries = EC_FSM_RETRIES;
5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	1174	fsm->state = ec_fsm_coe_down_check;
5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	1175	return;
5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	1176	}
5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	1177
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1178	if (rec_size < 6) {
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1179	fsm->state = ec_fsm_coe_error;
860 ba5c38f0bb30 Minor change: Moved state assign to beginning. Florian Pose <fp@igh-essen.com> parents: 859 diff changeset	1180	EC_ERR("Received data is too small (%u bytes):\n", rec_size);
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1181	ec_print_data(data, rec_size);
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1182	return;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1183	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1184
814 a51f857b1b2d Renamed [PS]DO to [PS]do. Florian Pose <fp@igh-essen.com> parents: 769 diff changeset	1185	if (EC_READ_U16(data) >> 12 == 0x2 && // Sdo request
a51f857b1b2d Renamed [PS]DO to [PS]do. Florian Pose <fp@igh-essen.com> parents: 769 diff changeset	1186	EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort Sdo transfer request
a51f857b1b2d Renamed [PS]DO to [PS]do. Florian Pose <fp@igh-essen.com> parents: 769 diff changeset	1187	fsm->state = ec_fsm_coe_error;
926 6bb33c6ec770 Output subindices with %02X. Florian Pose <fp@igh-essen.com> parents: 895 diff changeset	1188	EC_ERR("Sdo download 0x%04X:%02X (%u bytes) aborted on slave %u.\n",
854 f4f53be425ac Removed ec_sdodata_t; CoE state machines work on ec_sdo_request_t. Florian Pose <fp@igh-essen.com> parents: 834 diff changeset	1189	request->index, request->subindex, request->data_size,
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1190	slave->ring_position);
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1191	if (rec_size < 10) {
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1192	EC_ERR("Incomplete Abort command:\n");
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1193	ec_print_data(data, rec_size);
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1194	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1195	else
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1196	ec_canopen_abort_msg(EC_READ_U32(data + 6));
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1197	return;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1198	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1199
814 a51f857b1b2d Renamed [PS]DO to [PS]do. Florian Pose <fp@igh-essen.com> parents: 769 diff changeset	1200	if (EC_READ_U16(data) >> 12 != 0x3 \|\| // Sdo response
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1201	EC_READ_U8 (data + 2) >> 5 != 0x3 \|\| // Download response
854 f4f53be425ac Removed ec_sdodata_t; CoE state machines work on ec_sdo_request_t. Florian Pose <fp@igh-essen.com> parents: 834 diff changeset	1202	EC_READ_U16(data + 3) != request->index \|\| // index
f4f53be425ac Removed ec_sdodata_t; CoE state machines work on ec_sdo_request_t. Florian Pose <fp@igh-essen.com> parents: 834 diff changeset	1203	EC_READ_U8 (data + 5) != request->subindex) { // subindex
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1204	fsm->state = ec_fsm_coe_error;
926 6bb33c6ec770 Output subindices with %02X. Florian Pose <fp@igh-essen.com> parents: 895 diff changeset	1205	EC_ERR("Sdo download 0x%04X:%02X (%u bytes) failed:\n",
854 f4f53be425ac Removed ec_sdodata_t; CoE state machines work on ec_sdo_request_t. Florian Pose <fp@igh-essen.com> parents: 834 diff changeset	1206	request->index, request->subindex, request->data_size);
895 5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	1207	EC_ERR("Invalid Sdo download response at slave %u!\n",
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1208	slave->ring_position);
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1209	ec_print_data(data, rec_size);
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1210	return;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1211	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1212
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1213	fsm->state = ec_fsm_coe_end; // success
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1214	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1215
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1216	/*****************************************************************************/
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1217
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1218	/**
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1219	CoE state: UP START.
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1220	*/
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1221
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1222	void ec_fsm_coe_up_start(ec_fsm_coe_t fsm /< finite state machine /)
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1223	{
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1224	ec_datagram_t *datagram = fsm->datagram;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1225	ec_slave_t *slave = fsm->slave;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1226	ec_master_t *master = slave->master;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1227	ec_sdo_request_t *request = fsm->request;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1228	uint8_t *data;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1229
459 f0da52cf6b5b Print certain logs only at debug_level. Florian Pose <fp@igh-essen.com> parents: 446 diff changeset	1230	if (master->debug_level)
926 6bb33c6ec770 Output subindices with %02X. Florian Pose <fp@igh-essen.com> parents: 895 diff changeset	1231	EC_DBG("Uploading Sdo 0x%04X:%02X from slave %u.\n",
831 ded9519c8d6e Reading of Pdo mapping via CoE during scan. Florian Pose <fp@igh-essen.com> parents: 825 diff changeset	1232	request->index, request->subindex, slave->ring_position);
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1233
834 0791aac03180 Separated SII data from slave. Florian Pose <fp@igh-essen.com> parents: 831 diff changeset	1234	if (!(slave->sii.mailbox_protocols & EC_MBOX_COE)) {
825 3771f5a0ed90 CoE state machines raising error, if the slave does not support CoE. Florian Pose <fp@igh-essen.com> parents: 814 diff changeset	1235	EC_ERR("Slave %u does not support CoE!\n", slave->ring_position);
3771f5a0ed90 CoE state machines raising error, if the slave does not support CoE. Florian Pose <fp@igh-essen.com> parents: 814 diff changeset	1236	fsm->state = ec_fsm_coe_error;
3771f5a0ed90 CoE state machines raising error, if the slave does not support CoE. Florian Pose <fp@igh-essen.com> parents: 814 diff changeset	1237	return;
3771f5a0ed90 CoE state machines raising error, if the slave does not support CoE. Florian Pose <fp@igh-essen.com> parents: 814 diff changeset	1238	}
3771f5a0ed90 CoE state machines raising error, if the slave does not support CoE. Florian Pose <fp@igh-essen.com> parents: 814 diff changeset	1239
476 fd7c0ee645f8 Fixed CoE Upload Request, now 10 bytes of Mailbox Service Data. Florian Pose <fp@igh-essen.com> parents: 461 diff changeset	1240	if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 10))) {
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1241	fsm->state = ec_fsm_coe_error;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1242	return;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1243	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1244
814 a51f857b1b2d Renamed [PS]DO to [PS]do. Florian Pose <fp@igh-essen.com> parents: 769 diff changeset	1245	EC_WRITE_U16(data, 0x2 << 12); // Sdo request
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1246	EC_WRITE_U8 (data + 2, 0x2 << 5); // initiate upload request
831 ded9519c8d6e Reading of Pdo mapping via CoE during scan. Florian Pose <fp@igh-essen.com> parents: 825 diff changeset	1247	EC_WRITE_U16(data + 3, request->index);
ded9519c8d6e Reading of Pdo mapping via CoE during scan. Florian Pose <fp@igh-essen.com> parents: 825 diff changeset	1248	EC_WRITE_U8 (data + 5, request->subindex);
476 fd7c0ee645f8 Fixed CoE Upload Request, now 10 bytes of Mailbox Service Data. Florian Pose <fp@igh-essen.com> parents: 461 diff changeset	1249	memset(data + 6, 0x00, 4);
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1250
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1251	if (master->debug_level) {
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1252	EC_DBG("Upload request:\n");
476 fd7c0ee645f8 Fixed CoE Upload Request, now 10 bytes of Mailbox Service Data. Florian Pose <fp@igh-essen.com> parents: 461 diff changeset	1253	ec_print_data(data, 10);
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1254	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1255
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1256	fsm->retries = EC_FSM_RETRIES;
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1257	fsm->state = ec_fsm_coe_up_request;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1258	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1259
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1260	/*****************************************************************************/
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1261
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1262	/**
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1263	CoE state: UP REQUEST.
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1264	\todo Timeout behavior
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1265	*/
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1266
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1267	void ec_fsm_coe_up_request(ec_fsm_coe_t fsm /< finite state machine /)
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1268	{
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1269	ec_datagram_t *datagram = fsm->datagram;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1270	ec_slave_t *slave = fsm->slave;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1271
637 d5d04c868e0e Removed ecrt_master_run(), removed datagram queueing from state Florian Pose <fp@igh-essen.com> parents: 594 diff changeset	1272	if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
d5d04c868e0e Removed ecrt_master_run(), removed datagram queueing from state Florian Pose <fp@igh-essen.com> parents: 594 diff changeset	1273	return; // FIXME: check for response first?
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1274
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1275	if (datagram->state != EC_DATAGRAM_RECEIVED) {
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1276	fsm->state = ec_fsm_coe_error;
895 5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	1277	EC_ERR("Failed to receive CoE upload request for slave %u"
594 07dd2a7df66e Added datagram state to "Failed to receive..." messages. Florian Pose <fp@igh-essen.com> parents: 505 diff changeset	1278	" (datagram state %i).\n",
07dd2a7df66e Added datagram state to "Failed to receive..." messages. Florian Pose <fp@igh-essen.com> parents: 505 diff changeset	1279	slave->ring_position, datagram->state);
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1280	return;
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1281	}
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1282
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1283	if (datagram->working_counter != 1) {
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1284	fsm->state = ec_fsm_coe_error;
895 5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	1285	EC_ERR("Reception of CoE upload request failed on slave %u: ",
713 ae41cadd25b6 Better handling of unexpected working counters; introduced Florian Pose <fp@igh-essen.com> parents: 637 diff changeset	1286	slave->ring_position);
ae41cadd25b6 Better handling of unexpected working counters; introduced Florian Pose <fp@igh-essen.com> parents: 637 diff changeset	1287	ec_datagram_print_wc_error(datagram);
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1288	return;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1289	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1290
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1291	fsm->cycles_start = datagram->cycles_sent;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1292
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1293	ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1294	fsm->retries = EC_FSM_RETRIES;
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1295	fsm->state = ec_fsm_coe_up_check;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1296	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1297
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1298	/*****************************************************************************/
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1299
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1300	/**
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1301	CoE state: UP CHECK.
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1302	*/
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1303
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1304	void ec_fsm_coe_up_check(ec_fsm_coe_t fsm /< finite state machine /)
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1305	{
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1306	ec_datagram_t *datagram = fsm->datagram;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1307	ec_slave_t *slave = fsm->slave;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1308
637 d5d04c868e0e Removed ecrt_master_run(), removed datagram queueing from state Florian Pose <fp@igh-essen.com> parents: 594 diff changeset	1309	if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
d5d04c868e0e Removed ecrt_master_run(), removed datagram queueing from state Florian Pose <fp@igh-essen.com> parents: 594 diff changeset	1310	return;
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1311
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1312	if (datagram->state != EC_DATAGRAM_RECEIVED) {
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1313	fsm->state = ec_fsm_coe_error;
895 5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	1314	EC_ERR("Failed to receive CoE mailbox check datagram from slave %u"
594 07dd2a7df66e Added datagram state to "Failed to receive..." messages. Florian Pose <fp@igh-essen.com> parents: 505 diff changeset	1315	" (datagram state %i).\n",
07dd2a7df66e Added datagram state to "Failed to receive..." messages. Florian Pose <fp@igh-essen.com> parents: 505 diff changeset	1316	slave->ring_position, datagram->state);
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1317	return;
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1318	}
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1319
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1320	if (datagram->working_counter != 1) {
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1321	fsm->state = ec_fsm_coe_error;
713 ae41cadd25b6 Better handling of unexpected working counters; introduced Florian Pose <fp@igh-essen.com> parents: 637 diff changeset	1322	EC_ERR("Reception of CoE mailbox check"
895 5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	1323	" datagram failed on slave %u: ", slave->ring_position);
713 ae41cadd25b6 Better handling of unexpected working counters; introduced Florian Pose <fp@igh-essen.com> parents: 637 diff changeset	1324	ec_datagram_print_wc_error(datagram);
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1325	return;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1326	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1327
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1328	if (!ec_slave_mbox_check(datagram)) {
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1329	if (datagram->cycles_received
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1330	- fsm->cycles_start >= (cycles_t) 100 * cpu_khz) {
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1331	fsm->state = ec_fsm_coe_error;
895 5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	1332	EC_ERR("Timeout while checking Sdo upload on slave %u.\n",
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1333	slave->ring_position);
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1334	return;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1335	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1336
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1337	ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1338	fsm->retries = EC_FSM_RETRIES;
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1339	return;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1340	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1341
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1342	// Fetch response
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1343	ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1344	fsm->retries = EC_FSM_RETRIES;
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1345	fsm->state = ec_fsm_coe_up_response;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1346	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1347
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1348	/*****************************************************************************/
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1349
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1350	/**
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1351	CoE state: UP RESPONSE.
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1352	\todo Timeout behavior
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1353	*/
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1354
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1355	void ec_fsm_coe_up_response(ec_fsm_coe_t fsm /< finite state machine /)
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1356	{
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1357	ec_datagram_t *datagram = fsm->datagram;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1358	ec_slave_t *slave = fsm->slave;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1359	ec_master_t *master = slave->master;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1360	uint8_t *data, mbox_prot;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1361	size_t rec_size, data_size;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1362	ec_sdo_request_t *request = fsm->request;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1363	uint32_t complete_size;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1364	unsigned int expedited, size_specified;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1365
637 d5d04c868e0e Removed ecrt_master_run(), removed datagram queueing from state Florian Pose <fp@igh-essen.com> parents: 594 diff changeset	1366	if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
d5d04c868e0e Removed ecrt_master_run(), removed datagram queueing from state Florian Pose <fp@igh-essen.com> parents: 594 diff changeset	1367	return; // FIXME: request again?
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1368
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1369	if (datagram->state != EC_DATAGRAM_RECEIVED) {
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1370	fsm->state = ec_fsm_coe_error;
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1371	EC_ERR("Failed to receive CoE upload response datagram for"
895 5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	1372	" slave %u (datagram state %i).\n",
594 07dd2a7df66e Added datagram state to "Failed to receive..." messages. Florian Pose <fp@igh-essen.com> parents: 505 diff changeset	1373	slave->ring_position, datagram->state);
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1374	return;
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1375	}
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1376
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1377	if (datagram->working_counter != 1) {
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1378	fsm->state = ec_fsm_coe_error;
895 5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	1379	EC_ERR("Reception of CoE upload response failed on slave %u: ",
713 ae41cadd25b6 Better handling of unexpected working counters; introduced Florian Pose <fp@igh-essen.com> parents: 637 diff changeset	1380	slave->ring_position);
ae41cadd25b6 Better handling of unexpected working counters; introduced Florian Pose <fp@igh-essen.com> parents: 637 diff changeset	1381	ec_datagram_print_wc_error(datagram);
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1382	return;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1383	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1384
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1385	if (!(data = ec_slave_mbox_fetch(slave, datagram,
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1386	&mbox_prot, &rec_size))) {
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1387	fsm->state = ec_fsm_coe_error;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1388	return;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1389	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1390
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1391	if (master->debug_level) {
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1392	EC_DBG("Upload response:\n");
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1393	ec_print_data(data, rec_size);
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1394	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1395
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1396	if (mbox_prot != 0x03) { // CoE
860 ba5c38f0bb30 Minor change: Moved state assign to beginning. Florian Pose <fp@igh-essen.com> parents: 859 diff changeset	1397	fsm->state = ec_fsm_coe_error;
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1398	EC_WARN("Received mailbox protocol 0x%02X as response.\n", mbox_prot);
769 2195c2ea37b4 Fixed normal/expedited bug at CoE upload; more robust behaviour, when Florian Pose <fp@igh-essen.com> parents: 739 diff changeset	1399	return;
2195c2ea37b4 Fixed normal/expedited bug at CoE upload; more robust behaviour, when Florian Pose <fp@igh-essen.com> parents: 739 diff changeset	1400	}
2195c2ea37b4 Fixed normal/expedited bug at CoE upload; more robust behaviour, when Florian Pose <fp@igh-essen.com> parents: 739 diff changeset	1401
895 5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	1402	if (ec_fsm_coe_check_emergency(fsm, data, rec_size)) {
5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	1403	// check for CoE response again
5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	1404	ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	1405	fsm->retries = EC_FSM_RETRIES;
5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	1406	fsm->state = ec_fsm_coe_up_check;
5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	1407	return;
5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	1408	}
5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	1409
769 2195c2ea37b4 Fixed normal/expedited bug at CoE upload; more robust behaviour, when Florian Pose <fp@igh-essen.com> parents: 739 diff changeset	1410	if (rec_size < 3) {
860 ba5c38f0bb30 Minor change: Moved state assign to beginning. Florian Pose <fp@igh-essen.com> parents: 859 diff changeset	1411	fsm->state = ec_fsm_coe_error;
814 a51f857b1b2d Renamed [PS]DO to [PS]do. Florian Pose <fp@igh-essen.com> parents: 769 diff changeset	1412	EC_ERR("Received currupted Sdo upload response (%u bytes)!\n", rec_size);
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1413	ec_print_data(data, rec_size);
769 2195c2ea37b4 Fixed normal/expedited bug at CoE upload; more robust behaviour, when Florian Pose <fp@igh-essen.com> parents: 739 diff changeset	1414	return;
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1415	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1416
814 a51f857b1b2d Renamed [PS]DO to [PS]do. Florian Pose <fp@igh-essen.com> parents: 769 diff changeset	1417	if (EC_READ_U16(data) >> 12 == 0x2 && // Sdo request
a51f857b1b2d Renamed [PS]DO to [PS]do. Florian Pose <fp@igh-essen.com> parents: 769 diff changeset	1418	EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort Sdo transfer request
926 6bb33c6ec770 Output subindices with %02X. Florian Pose <fp@igh-essen.com> parents: 895 diff changeset	1419	EC_ERR("Sdo upload 0x%04X:%02X aborted on slave %u.\n",
831 ded9519c8d6e Reading of Pdo mapping via CoE during scan. Florian Pose <fp@igh-essen.com> parents: 825 diff changeset	1420	request->index, request->subindex, slave->ring_position);
769 2195c2ea37b4 Fixed normal/expedited bug at CoE upload; more robust behaviour, when Florian Pose <fp@igh-essen.com> parents: 739 diff changeset	1421	if (rec_size >= 10)
2195c2ea37b4 Fixed normal/expedited bug at CoE upload; more robust behaviour, when Florian Pose <fp@igh-essen.com> parents: 739 diff changeset	1422	ec_canopen_abort_msg(EC_READ_U32(data + 6));
2195c2ea37b4 Fixed normal/expedited bug at CoE upload; more robust behaviour, when Florian Pose <fp@igh-essen.com> parents: 739 diff changeset	1423	else
2195c2ea37b4 Fixed normal/expedited bug at CoE upload; more robust behaviour, when Florian Pose <fp@igh-essen.com> parents: 739 diff changeset	1424	EC_ERR("No abort message.\n");
2195c2ea37b4 Fixed normal/expedited bug at CoE upload; more robust behaviour, when Florian Pose <fp@igh-essen.com> parents: 739 diff changeset	1425	fsm->state = ec_fsm_coe_error;
2195c2ea37b4 Fixed normal/expedited bug at CoE upload; more robust behaviour, when Florian Pose <fp@igh-essen.com> parents: 739 diff changeset	1426	return;
2195c2ea37b4 Fixed normal/expedited bug at CoE upload; more robust behaviour, when Florian Pose <fp@igh-essen.com> parents: 739 diff changeset	1427	}
2195c2ea37b4 Fixed normal/expedited bug at CoE upload; more robust behaviour, when Florian Pose <fp@igh-essen.com> parents: 739 diff changeset	1428
2195c2ea37b4 Fixed normal/expedited bug at CoE upload; more robust behaviour, when Florian Pose <fp@igh-essen.com> parents: 739 diff changeset	1429	// normal or expedited?
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1430	expedited = EC_READ_U8(data + 2) & 0x02;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1431
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1432	if (expedited) {
769 2195c2ea37b4 Fixed normal/expedited bug at CoE upload; more robust behaviour, when Florian Pose <fp@igh-essen.com> parents: 739 diff changeset	1433	if (rec_size < 7) {
860 ba5c38f0bb30 Minor change: Moved state assign to beginning. Florian Pose <fp@igh-essen.com> parents: 859 diff changeset	1434	fsm->state = ec_fsm_coe_error;
814 a51f857b1b2d Renamed [PS]DO to [PS]do. Florian Pose <fp@igh-essen.com> parents: 769 diff changeset	1435	EC_ERR("Received currupted Sdo expedited upload"
769 2195c2ea37b4 Fixed normal/expedited bug at CoE upload; more robust behaviour, when Florian Pose <fp@igh-essen.com> parents: 739 diff changeset	1436	" response (only %u bytes)!\n", rec_size);
2195c2ea37b4 Fixed normal/expedited bug at CoE upload; more robust behaviour, when Florian Pose <fp@igh-essen.com> parents: 739 diff changeset	1437	ec_print_data(data, rec_size);
2195c2ea37b4 Fixed normal/expedited bug at CoE upload; more robust behaviour, when Florian Pose <fp@igh-essen.com> parents: 739 diff changeset	1438	return;
2195c2ea37b4 Fixed normal/expedited bug at CoE upload; more robust behaviour, when Florian Pose <fp@igh-essen.com> parents: 739 diff changeset	1439	}
2195c2ea37b4 Fixed normal/expedited bug at CoE upload; more robust behaviour, when Florian Pose <fp@igh-essen.com> parents: 739 diff changeset	1440
814 a51f857b1b2d Renamed [PS]DO to [PS]do. Florian Pose <fp@igh-essen.com> parents: 769 diff changeset	1441	if (EC_READ_U16(data) >> 12 != 0x3 \|\| // Sdo response
769 2195c2ea37b4 Fixed normal/expedited bug at CoE upload; more robust behaviour, when Florian Pose <fp@igh-essen.com> parents: 739 diff changeset	1442	EC_READ_U8 (data + 2) >> 5 != 0x2 \|\| // upload response
831 ded9519c8d6e Reading of Pdo mapping via CoE during scan. Florian Pose <fp@igh-essen.com> parents: 825 diff changeset	1443	EC_READ_U16(data + 3) != request->index \|\| // index
ded9519c8d6e Reading of Pdo mapping via CoE during scan. Florian Pose <fp@igh-essen.com> parents: 825 diff changeset	1444	EC_READ_U8 (data + 5) != request->subindex) { // subindex
860 ba5c38f0bb30 Minor change: Moved state assign to beginning. Florian Pose <fp@igh-essen.com> parents: 859 diff changeset	1445	fsm->state = ec_fsm_coe_error;
926 6bb33c6ec770 Output subindices with %02X. Florian Pose <fp@igh-essen.com> parents: 895 diff changeset	1446	EC_ERR("Sdo upload 0x%04X:%02X failed:\n",
831 ded9519c8d6e Reading of Pdo mapping via CoE during scan. Florian Pose <fp@igh-essen.com> parents: 825 diff changeset	1447	request->index, request->subindex);
895 5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	1448	EC_ERR("Invalid Sdo upload expedited response at slave %u!\n",
769 2195c2ea37b4 Fixed normal/expedited bug at CoE upload; more robust behaviour, when Florian Pose <fp@igh-essen.com> parents: 739 diff changeset	1449	slave->ring_position);
2195c2ea37b4 Fixed normal/expedited bug at CoE upload; more robust behaviour, when Florian Pose <fp@igh-essen.com> parents: 739 diff changeset	1450	ec_print_data(data, rec_size);
2195c2ea37b4 Fixed normal/expedited bug at CoE upload; more robust behaviour, when Florian Pose <fp@igh-essen.com> parents: 739 diff changeset	1451	return;
2195c2ea37b4 Fixed normal/expedited bug at CoE upload; more robust behaviour, when Florian Pose <fp@igh-essen.com> parents: 739 diff changeset	1452	}
2195c2ea37b4 Fixed normal/expedited bug at CoE upload; more robust behaviour, when Florian Pose <fp@igh-essen.com> parents: 739 diff changeset	1453
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1454	size_specified = EC_READ_U8(data + 2) & 0x01;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1455	if (size_specified) {
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1456	complete_size = 4 - ((EC_READ_U8(data + 2) & 0x0C) >> 2);
769 2195c2ea37b4 Fixed normal/expedited bug at CoE upload; more robust behaviour, when Florian Pose <fp@igh-essen.com> parents: 739 diff changeset	1457	} else {
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1458	complete_size = 4;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1459	}
769 2195c2ea37b4 Fixed normal/expedited bug at CoE upload; more robust behaviour, when Florian Pose <fp@igh-essen.com> parents: 739 diff changeset	1460
2195c2ea37b4 Fixed normal/expedited bug at CoE upload; more robust behaviour, when Florian Pose <fp@igh-essen.com> parents: 739 diff changeset	1461	if (rec_size < 6 + complete_size) {
860 ba5c38f0bb30 Minor change: Moved state assign to beginning. Florian Pose <fp@igh-essen.com> parents: 859 diff changeset	1462	fsm->state = ec_fsm_coe_error;
814 a51f857b1b2d Renamed [PS]DO to [PS]do. Florian Pose <fp@igh-essen.com> parents: 769 diff changeset	1463	EC_ERR("Received currupted Sdo expedited upload"
769 2195c2ea37b4 Fixed normal/expedited bug at CoE upload; more robust behaviour, when Florian Pose <fp@igh-essen.com> parents: 739 diff changeset	1464	" response (only %u bytes)!\n", rec_size);
2195c2ea37b4 Fixed normal/expedited bug at CoE upload; more robust behaviour, when Florian Pose <fp@igh-essen.com> parents: 739 diff changeset	1465	ec_print_data(data, rec_size);
2195c2ea37b4 Fixed normal/expedited bug at CoE upload; more robust behaviour, when Florian Pose <fp@igh-essen.com> parents: 739 diff changeset	1466	return;
2195c2ea37b4 Fixed normal/expedited bug at CoE upload; more robust behaviour, when Florian Pose <fp@igh-essen.com> parents: 739 diff changeset	1467	}
2195c2ea37b4 Fixed normal/expedited bug at CoE upload; more robust behaviour, when Florian Pose <fp@igh-essen.com> parents: 739 diff changeset	1468
854 f4f53be425ac Removed ec_sdodata_t; CoE state machines work on ec_sdo_request_t. Florian Pose <fp@igh-essen.com> parents: 834 diff changeset	1469	if (ec_sdo_request_copy_data(request, data + 6, complete_size)) {
f4f53be425ac Removed ec_sdodata_t; CoE state machines work on ec_sdo_request_t. Florian Pose <fp@igh-essen.com> parents: 834 diff changeset	1470	fsm->state = ec_fsm_coe_error;
f4f53be425ac Removed ec_sdodata_t; CoE state machines work on ec_sdo_request_t. Florian Pose <fp@igh-essen.com> parents: 834 diff changeset	1471	return;
f4f53be425ac Removed ec_sdodata_t; CoE state machines work on ec_sdo_request_t. Florian Pose <fp@igh-essen.com> parents: 834 diff changeset	1472	}
769 2195c2ea37b4 Fixed normal/expedited bug at CoE upload; more robust behaviour, when Florian Pose <fp@igh-essen.com> parents: 739 diff changeset	1473	} else { // normal
2195c2ea37b4 Fixed normal/expedited bug at CoE upload; more robust behaviour, when Florian Pose <fp@igh-essen.com> parents: 739 diff changeset	1474	if (rec_size < 10) {
860 ba5c38f0bb30 Minor change: Moved state assign to beginning. Florian Pose <fp@igh-essen.com> parents: 859 diff changeset	1475	fsm->state = ec_fsm_coe_error;
814 a51f857b1b2d Renamed [PS]DO to [PS]do. Florian Pose <fp@igh-essen.com> parents: 769 diff changeset	1476	EC_ERR("Received currupted Sdo normal upload"
769 2195c2ea37b4 Fixed normal/expedited bug at CoE upload; more robust behaviour, when Florian Pose <fp@igh-essen.com> parents: 739 diff changeset	1477	" response (only %u bytes)!\n", rec_size);
2195c2ea37b4 Fixed normal/expedited bug at CoE upload; more robust behaviour, when Florian Pose <fp@igh-essen.com> parents: 739 diff changeset	1478	ec_print_data(data, rec_size);
2195c2ea37b4 Fixed normal/expedited bug at CoE upload; more robust behaviour, when Florian Pose <fp@igh-essen.com> parents: 739 diff changeset	1479	return;
2195c2ea37b4 Fixed normal/expedited bug at CoE upload; more robust behaviour, when Florian Pose <fp@igh-essen.com> parents: 739 diff changeset	1480	}
2195c2ea37b4 Fixed normal/expedited bug at CoE upload; more robust behaviour, when Florian Pose <fp@igh-essen.com> parents: 739 diff changeset	1481
814 a51f857b1b2d Renamed [PS]DO to [PS]do. Florian Pose <fp@igh-essen.com> parents: 769 diff changeset	1482	if (EC_READ_U16(data) >> 12 != 0x3 \|\| // Sdo response
769 2195c2ea37b4 Fixed normal/expedited bug at CoE upload; more robust behaviour, when Florian Pose <fp@igh-essen.com> parents: 739 diff changeset	1483	EC_READ_U8 (data + 2) >> 5 != 0x2 \|\| // upload response
831 ded9519c8d6e Reading of Pdo mapping via CoE during scan. Florian Pose <fp@igh-essen.com> parents: 825 diff changeset	1484	EC_READ_U16(data + 3) != request->index \|\| // index
ded9519c8d6e Reading of Pdo mapping via CoE during scan. Florian Pose <fp@igh-essen.com> parents: 825 diff changeset	1485	EC_READ_U8 (data + 5) != request->subindex) { // subindex
860 ba5c38f0bb30 Minor change: Moved state assign to beginning. Florian Pose <fp@igh-essen.com> parents: 859 diff changeset	1486	fsm->state = ec_fsm_coe_error;
926 6bb33c6ec770 Output subindices with %02X. Florian Pose <fp@igh-essen.com> parents: 895 diff changeset	1487	EC_ERR("Sdo upload 0x%04X:%02X failed:\n",
831 ded9519c8d6e Reading of Pdo mapping via CoE during scan. Florian Pose <fp@igh-essen.com> parents: 825 diff changeset	1488	request->index, request->subindex);
895 5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	1489	EC_ERR("Invalid Sdo normal upload response at slave %u!\n",
769 2195c2ea37b4 Fixed normal/expedited bug at CoE upload; more robust behaviour, when Florian Pose <fp@igh-essen.com> parents: 739 diff changeset	1490	slave->ring_position);
2195c2ea37b4 Fixed normal/expedited bug at CoE upload; more robust behaviour, when Florian Pose <fp@igh-essen.com> parents: 739 diff changeset	1491	ec_print_data(data, rec_size);
2195c2ea37b4 Fixed normal/expedited bug at CoE upload; more robust behaviour, when Florian Pose <fp@igh-essen.com> parents: 739 diff changeset	1492	return;
2195c2ea37b4 Fixed normal/expedited bug at CoE upload; more robust behaviour, when Florian Pose <fp@igh-essen.com> parents: 739 diff changeset	1493	}
2195c2ea37b4 Fixed normal/expedited bug at CoE upload; more robust behaviour, when Florian Pose <fp@igh-essen.com> parents: 739 diff changeset	1494
2195c2ea37b4 Fixed normal/expedited bug at CoE upload; more robust behaviour, when Florian Pose <fp@igh-essen.com> parents: 739 diff changeset	1495	data_size = rec_size - 10;
2195c2ea37b4 Fixed normal/expedited bug at CoE upload; more robust behaviour, when Florian Pose <fp@igh-essen.com> parents: 739 diff changeset	1496	complete_size = EC_READ_U32(data + 6);
2195c2ea37b4 Fixed normal/expedited bug at CoE upload; more robust behaviour, when Florian Pose <fp@igh-essen.com> parents: 739 diff changeset	1497
2195c2ea37b4 Fixed normal/expedited bug at CoE upload; more robust behaviour, when Florian Pose <fp@igh-essen.com> parents: 739 diff changeset	1498	if (!complete_size) {
860 ba5c38f0bb30 Minor change: Moved state assign to beginning. Florian Pose <fp@igh-essen.com> parents: 859 diff changeset	1499	fsm->state = ec_fsm_coe_error;
769 2195c2ea37b4 Fixed normal/expedited bug at CoE upload; more robust behaviour, when Florian Pose <fp@igh-essen.com> parents: 739 diff changeset	1500	EC_ERR("No complete size supplied!\n");
2195c2ea37b4 Fixed normal/expedited bug at CoE upload; more robust behaviour, when Florian Pose <fp@igh-essen.com> parents: 739 diff changeset	1501	ec_print_data(data, rec_size);
2195c2ea37b4 Fixed normal/expedited bug at CoE upload; more robust behaviour, when Florian Pose <fp@igh-essen.com> parents: 739 diff changeset	1502	return;
2195c2ea37b4 Fixed normal/expedited bug at CoE upload; more robust behaviour, when Florian Pose <fp@igh-essen.com> parents: 739 diff changeset	1503	}
2195c2ea37b4 Fixed normal/expedited bug at CoE upload; more robust behaviour, when Florian Pose <fp@igh-essen.com> parents: 739 diff changeset	1504
854 f4f53be425ac Removed ec_sdodata_t; CoE state machines work on ec_sdo_request_t. Florian Pose <fp@igh-essen.com> parents: 834 diff changeset	1505	if (ec_sdo_request_alloc(request, complete_size)) {
f4f53be425ac Removed ec_sdodata_t; CoE state machines work on ec_sdo_request_t. Florian Pose <fp@igh-essen.com> parents: 834 diff changeset	1506	fsm->state = ec_fsm_coe_error;
f4f53be425ac Removed ec_sdodata_t; CoE state machines work on ec_sdo_request_t. Florian Pose <fp@igh-essen.com> parents: 834 diff changeset	1507	return;
f4f53be425ac Removed ec_sdodata_t; CoE state machines work on ec_sdo_request_t. Florian Pose <fp@igh-essen.com> parents: 834 diff changeset	1508	}
f4f53be425ac Removed ec_sdodata_t; CoE state machines work on ec_sdo_request_t. Florian Pose <fp@igh-essen.com> parents: 834 diff changeset	1509
f4f53be425ac Removed ec_sdodata_t; CoE state machines work on ec_sdo_request_t. Florian Pose <fp@igh-essen.com> parents: 834 diff changeset	1510	if (ec_sdo_request_copy_data(request, data + 10, data_size)) {
f4f53be425ac Removed ec_sdodata_t; CoE state machines work on ec_sdo_request_t. Florian Pose <fp@igh-essen.com> parents: 834 diff changeset	1511	fsm->state = ec_fsm_coe_error;
f4f53be425ac Removed ec_sdodata_t; CoE state machines work on ec_sdo_request_t. Florian Pose <fp@igh-essen.com> parents: 834 diff changeset	1512	return;
f4f53be425ac Removed ec_sdodata_t; CoE state machines work on ec_sdo_request_t. Florian Pose <fp@igh-essen.com> parents: 834 diff changeset	1513	}
f4f53be425ac Removed ec_sdodata_t; CoE state machines work on ec_sdo_request_t. Florian Pose <fp@igh-essen.com> parents: 834 diff changeset	1514
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1515	fsm->toggle = 0;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1516
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1517	if (data_size < complete_size) {
814 a51f857b1b2d Renamed [PS]DO to [PS]do. Florian Pose <fp@igh-essen.com> parents: 769 diff changeset	1518	EC_WARN("Sdo data incomplete (%i / %i).\n",
446 8fede404c01f Persistent slave lists. Florian Pose <fp@igh-essen.com> parents: 436 diff changeset	1519	data_size, complete_size);
8fede404c01f Persistent slave lists. Florian Pose <fp@igh-essen.com> parents: 436 diff changeset	1520
8fede404c01f Persistent slave lists. Florian Pose <fp@igh-essen.com> parents: 436 diff changeset	1521	if (!(data = ec_slave_mbox_prepare_send(slave, datagram,
8fede404c01f Persistent slave lists. Florian Pose <fp@igh-essen.com> parents: 436 diff changeset	1522	0x03, 3))) {
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1523	fsm->state = ec_fsm_coe_error;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1524	return;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1525	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1526
814 a51f857b1b2d Renamed [PS]DO to [PS]do. Florian Pose <fp@igh-essen.com> parents: 769 diff changeset	1527	EC_WRITE_U16(data, 0x2 << 12); // Sdo request
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1528	EC_WRITE_U8 (data + 2, (fsm->toggle << 4 // toggle
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1529	\| 0x3 << 5)); // upload segment request
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1530
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1531	if (master->debug_level) {
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1532	EC_DBG("Upload segment request:\n");
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1533	ec_print_data(data, 3);
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1534	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1535
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1536	fsm->retries = EC_FSM_RETRIES;
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1537	fsm->state = ec_fsm_coe_up_seg_request;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1538	return;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1539	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1540	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1541
874 022f869ea40f Added CoE debugging. Florian Pose <fp@igh-essen.com> parents: 860 diff changeset	1542	if (master->debug_level) {
022f869ea40f Added CoE debugging. Florian Pose <fp@igh-essen.com> parents: 860 diff changeset	1543	EC_DBG("Uploaded data:\n");
022f869ea40f Added CoE debugging. Florian Pose <fp@igh-essen.com> parents: 860 diff changeset	1544	ec_print_data(request->data, request->data_size);
022f869ea40f Added CoE debugging. Florian Pose <fp@igh-essen.com> parents: 860 diff changeset	1545	}
022f869ea40f Added CoE debugging. Florian Pose <fp@igh-essen.com> parents: 860 diff changeset	1546
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1547	fsm->state = ec_fsm_coe_end; // success
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1548	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1549
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1550	/*****************************************************************************/
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1551
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1552	/**
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1553	CoE state: UP REQUEST.
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1554	\todo Timeout behavior
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1555	*/
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1556
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1557	void ec_fsm_coe_up_seg_request(ec_fsm_coe_t fsm /< finite state machine /)
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1558	{
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1559	ec_datagram_t *datagram = fsm->datagram;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1560	ec_slave_t *slave = fsm->slave;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1561
637 d5d04c868e0e Removed ecrt_master_run(), removed datagram queueing from state Florian Pose <fp@igh-essen.com> parents: 594 diff changeset	1562	if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
d5d04c868e0e Removed ecrt_master_run(), removed datagram queueing from state Florian Pose <fp@igh-essen.com> parents: 594 diff changeset	1563	return; // FIXME: check for response first?
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1564
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1565	if (datagram->state != EC_DATAGRAM_RECEIVED) {
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1566	fsm->state = ec_fsm_coe_error;
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1567	EC_ERR("Failed to receive CoE upload segment request datagram for"
895 5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	1568	" slave %u (datagram state %i).\n",
594 07dd2a7df66e Added datagram state to "Failed to receive..." messages. Florian Pose <fp@igh-essen.com> parents: 505 diff changeset	1569	slave->ring_position, datagram->state);
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1570	return;
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1571	}
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1572
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1573	if (datagram->working_counter != 1) {
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1574	fsm->state = ec_fsm_coe_error;
713 ae41cadd25b6 Better handling of unexpected working counters; introduced Florian Pose <fp@igh-essen.com> parents: 637 diff changeset	1575	EC_ERR("Reception of CoE upload segment"
895 5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	1576	" request failed on slave %u: ", slave->ring_position);
713 ae41cadd25b6 Better handling of unexpected working counters; introduced Florian Pose <fp@igh-essen.com> parents: 637 diff changeset	1577	ec_datagram_print_wc_error(datagram);
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1578	return;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1579	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1580
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1581	fsm->cycles_start = datagram->cycles_sent;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1582
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1583	ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1584	fsm->retries = EC_FSM_RETRIES;
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1585	fsm->state = ec_fsm_coe_up_seg_check;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1586	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1587
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1588	/*****************************************************************************/
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1589
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1590	/**
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1591	CoE state: UP CHECK.
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1592	*/
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1593
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1594	void ec_fsm_coe_up_seg_check(ec_fsm_coe_t fsm /< finite state machine /)
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1595	{
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1596	ec_datagram_t *datagram = fsm->datagram;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1597	ec_slave_t *slave = fsm->slave;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1598
637 d5d04c868e0e Removed ecrt_master_run(), removed datagram queueing from state Florian Pose <fp@igh-essen.com> parents: 594 diff changeset	1599	if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
d5d04c868e0e Removed ecrt_master_run(), removed datagram queueing from state Florian Pose <fp@igh-essen.com> parents: 594 diff changeset	1600	return;
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1601
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1602	if (datagram->state != EC_DATAGRAM_RECEIVED) {
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1603	fsm->state = ec_fsm_coe_error;
895 5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	1604	EC_ERR("Failed to receive CoE mailbox check datagram for slave %u"
594 07dd2a7df66e Added datagram state to "Failed to receive..." messages. Florian Pose <fp@igh-essen.com> parents: 505 diff changeset	1605	" (datagram state %i).\n",
07dd2a7df66e Added datagram state to "Failed to receive..." messages. Florian Pose <fp@igh-essen.com> parents: 505 diff changeset	1606	slave->ring_position, datagram->state);
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1607	return;
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1608	}
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1609
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1610	if (datagram->working_counter != 1) {
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1611	fsm->state = ec_fsm_coe_error;
713 ae41cadd25b6 Better handling of unexpected working counters; introduced Florian Pose <fp@igh-essen.com> parents: 637 diff changeset	1612	EC_ERR("Reception of CoE mailbox check"
895 5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	1613	" datagram failed on slave %u: ", slave->ring_position);
713 ae41cadd25b6 Better handling of unexpected working counters; introduced Florian Pose <fp@igh-essen.com> parents: 637 diff changeset	1614	ec_datagram_print_wc_error(datagram);
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1615	return;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1616	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1617
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1618	if (!ec_slave_mbox_check(datagram)) {
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1619	if (datagram->cycles_received
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1620	- fsm->cycles_start >= (cycles_t) 100 * cpu_khz) {
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1621	fsm->state = ec_fsm_coe_error;
895 5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	1622	EC_ERR("Timeout while checking Sdo upload segment on slave %u.\n",
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1623	slave->ring_position);
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1624	return;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1625	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1626
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1627	ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1628	fsm->retries = EC_FSM_RETRIES;
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1629	return;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1630	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1631
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1632	// Fetch response
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1633	ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1634	fsm->retries = EC_FSM_RETRIES;
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1635	fsm->state = ec_fsm_coe_up_seg_response;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1636	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1637
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1638	/*****************************************************************************/
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1639
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1640	/**
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1641	CoE state: UP RESPONSE.
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1642	\todo Timeout behavior
854 f4f53be425ac Removed ec_sdodata_t; CoE state machines work on ec_sdo_request_t. Florian Pose <fp@igh-essen.com> parents: 834 diff changeset	1643	\todo Check for \a data_size exceeding \a complete_size.
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1644	*/
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1645
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1646	void ec_fsm_coe_up_seg_response(ec_fsm_coe_t fsm /< finite state machine /)
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1647	{
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1648	ec_datagram_t *datagram = fsm->datagram;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1649	ec_slave_t *slave = fsm->slave;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1650	ec_master_t *master = slave->master;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1651	uint8_t *data, mbox_prot;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1652	size_t rec_size, data_size;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1653	ec_sdo_request_t *request = fsm->request;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1654	uint32_t seg_size;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1655	unsigned int last_segment;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1656
637 d5d04c868e0e Removed ecrt_master_run(), removed datagram queueing from state Florian Pose <fp@igh-essen.com> parents: 594 diff changeset	1657	if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
d5d04c868e0e Removed ecrt_master_run(), removed datagram queueing from state Florian Pose <fp@igh-essen.com> parents: 594 diff changeset	1658	return; // FIXME: request again?
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1659
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1660	if (datagram->state != EC_DATAGRAM_RECEIVED) {
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1661	fsm->state = ec_fsm_coe_error;
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1662	EC_ERR("Failed to receive CoE upload segment response datagram for"
895 5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	1663	" slave %u (datagram state %i).\n",
594 07dd2a7df66e Added datagram state to "Failed to receive..." messages. Florian Pose <fp@igh-essen.com> parents: 505 diff changeset	1664	slave->ring_position, datagram->state);
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1665	return;
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1666	}
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1667
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1668	if (datagram->working_counter != 1) {
bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1669	fsm->state = ec_fsm_coe_error;
713 ae41cadd25b6 Better handling of unexpected working counters; introduced Florian Pose <fp@igh-essen.com> parents: 637 diff changeset	1670	EC_ERR("Reception of CoE upload segment"
895 5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	1671	" response failed on slave %u: ", slave->ring_position);
713 ae41cadd25b6 Better handling of unexpected working counters; introduced Florian Pose <fp@igh-essen.com> parents: 637 diff changeset	1672	ec_datagram_print_wc_error(datagram);
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1673	return;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1674	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1675
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1676	if (!(data = ec_slave_mbox_fetch(slave, datagram,
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1677	&mbox_prot, &rec_size))) {
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1678	fsm->state = ec_fsm_coe_error;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1679	return;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1680	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1681
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1682	if (master->debug_level) {
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1683	EC_DBG("Upload segment response:\n");
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1684	ec_print_data(data, rec_size);
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1685	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1686
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1687	if (mbox_prot != 0x03) { // CoE
446 8fede404c01f Persistent slave lists. Florian Pose <fp@igh-essen.com> parents: 436 diff changeset	1688	EC_ERR("Received mailbox protocol 0x%02X as response.\n", mbox_prot);
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1689	fsm->state = ec_fsm_coe_error;
895 5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	1690	return;
5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	1691	}
5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	1692
5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	1693	if (ec_fsm_coe_check_emergency(fsm, data, rec_size)) {
5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	1694	// check for CoE response again
5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	1695	ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	1696	fsm->retries = EC_FSM_RETRIES;
5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	1697	fsm->state = ec_fsm_coe_up_seg_check;
5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	1698	return;
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1699	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1700
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1701	if (rec_size < 10) {
814 a51f857b1b2d Renamed [PS]DO to [PS]do. Florian Pose <fp@igh-essen.com> parents: 769 diff changeset	1702	EC_ERR("Received currupted Sdo upload segment response!\n");
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1703	ec_print_data(data, rec_size);
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1704	fsm->state = ec_fsm_coe_error;
895 5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	1705	return;
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1706	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1707
814 a51f857b1b2d Renamed [PS]DO to [PS]do. Florian Pose <fp@igh-essen.com> parents: 769 diff changeset	1708	if (EC_READ_U16(data) >> 12 == 0x2 && // Sdo request
a51f857b1b2d Renamed [PS]DO to [PS]do. Florian Pose <fp@igh-essen.com> parents: 769 diff changeset	1709	EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort Sdo transfer request
926 6bb33c6ec770 Output subindices with %02X. Florian Pose <fp@igh-essen.com> parents: 895 diff changeset	1710	EC_ERR("Sdo upload 0x%04X:%02X aborted on slave %u.\n",
831 ded9519c8d6e Reading of Pdo mapping via CoE during scan. Florian Pose <fp@igh-essen.com> parents: 825 diff changeset	1711	request->index, request->subindex, slave->ring_position);
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1712	ec_canopen_abort_msg(EC_READ_U32(data + 6));
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1713	fsm->state = ec_fsm_coe_error;
895 5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	1714	return;
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1715	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1716
814 a51f857b1b2d Renamed [PS]DO to [PS]do. Florian Pose <fp@igh-essen.com> parents: 769 diff changeset	1717	if (EC_READ_U16(data) >> 12 != 0x3 \|\| // Sdo response
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1718	EC_READ_U8 (data + 2) >> 5 != 0x0) { // upload segment response
926 6bb33c6ec770 Output subindices with %02X. Florian Pose <fp@igh-essen.com> parents: 895 diff changeset	1719	EC_ERR("Sdo upload 0x%04X:%02X failed:\n", request->index, request->subindex);
895 5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	1720	EC_ERR("Invalid Sdo upload segment response at slave %u!\n",
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1721	slave->ring_position);
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1722	ec_print_data(data, rec_size);
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1723	fsm->state = ec_fsm_coe_error;
895 5c2125247dde Check for emergency requests during CoE transfers. Florian Pose <fp@igh-essen.com> parents: 874 diff changeset	1724	return;
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1725	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1726
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1727	last_segment = EC_READ_U8(data + 2) & 0x01;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1728	seg_size = (EC_READ_U8(data + 2) & 0xE) >> 1;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1729	data_size = rec_size - 10;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1730
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1731	if (data_size != seg_size) {
814 a51f857b1b2d Renamed [PS]DO to [PS]do. Florian Pose <fp@igh-essen.com> parents: 769 diff changeset	1732	EC_WARN("Sdo segment data invalid (%i / %i)"
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1733	" - Fragmenting not implemented.\n",
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1734	data_size, seg_size);
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1735	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1736
854 f4f53be425ac Removed ec_sdodata_t; CoE state machines work on ec_sdo_request_t. Florian Pose <fp@igh-essen.com> parents: 834 diff changeset	1737	memcpy(request->data + request->data_size, data + 10, data_size);
f4f53be425ac Removed ec_sdodata_t; CoE state machines work on ec_sdo_request_t. Florian Pose <fp@igh-essen.com> parents: 834 diff changeset	1738	request->data_size += data_size;
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1739
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1740	if (!last_segment) {
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1741	fsm->toggle = !fsm->toggle;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1742
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1743	if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 3))) {
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1744	fsm->state = ec_fsm_coe_error;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1745	return;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1746	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1747
814 a51f857b1b2d Renamed [PS]DO to [PS]do. Florian Pose <fp@igh-essen.com> parents: 769 diff changeset	1748	EC_WRITE_U16(data, 0x2 << 12); // Sdo request
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1749	EC_WRITE_U8 (data + 2, (fsm->toggle << 4 // toggle
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1750	\| 0x3 << 5)); // upload segment request
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1751
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1752	if (master->debug_level) {
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1753	EC_DBG("Upload segment request:\n");
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1754	ec_print_data(data, 3);
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1755	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1756
505 bc443ca0077f State machines re-send datagrams on timeout. Florian Pose <fp@igh-essen.com> parents: 501 diff changeset	1757	fsm->retries = EC_FSM_RETRIES;
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1758	fsm->state = ec_fsm_coe_up_seg_request;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1759	return;
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1760	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1761
874 022f869ea40f Added CoE debugging. Florian Pose <fp@igh-essen.com> parents: 860 diff changeset	1762	if (master->debug_level) {
022f869ea40f Added CoE debugging. Florian Pose <fp@igh-essen.com> parents: 860 diff changeset	1763	EC_DBG("Uploaded data:\n");
022f869ea40f Added CoE debugging. Florian Pose <fp@igh-essen.com> parents: 860 diff changeset	1764	ec_print_data(request->data, request->data_size);
022f869ea40f Added CoE debugging. Florian Pose <fp@igh-essen.com> parents: 860 diff changeset	1765	}
022f869ea40f Added CoE debugging. Florian Pose <fp@igh-essen.com> parents: 860 diff changeset	1766
436 63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1767	fsm->state = ec_fsm_coe_end; // success
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1768	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1769
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1770	/*****************************************************************************/
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1771
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1772	/**
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1773	State: ERROR.
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1774	*/
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1775
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1776	void ec_fsm_coe_error(ec_fsm_coe_t fsm /< finite state machine /)
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1777	{
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1778	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1779
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1780	/*****************************************************************************/
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1781
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1782	/**
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1783	State: END.
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1784	*/
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1785
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1786	void ec_fsm_coe_end(ec_fsm_coe_t fsm /< finite state machine /)
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1787	{
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1788	}
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1789
63214beb641d Layed out CoE state machine. Florian Pose <fp@igh-essen.com> parents: diff changeset	1790	/*****************************************************************************/

author	Florian Pose <fp@igh-essen.com>
	Wed, 04 Jun 2008 13:17:15 +0000
changeset 950	8b00e63fff90
parent 926	6bb33c6ec770
child 962	ea83a7aab57a
permissions	-rw-r--r--