author | Florian Pose <fp@igh-essen.com> |
Wed, 04 Jan 2012 18:05:38 +0100 | |
branch | stable-1.5 |
changeset 2193 | 334f4530c1df |
parent 1952 | 7d9fb723fc4b |
child 2045 | ff2a13a4603c |
child 2498 | 9cdd7669dc0b |
permissions | -rw-r--r-- |
1831
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
1 |
/****************************************************************************** |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
2 |
* |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
3 |
* $Id$ |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
4 |
* |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
5 |
* Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
6 |
* |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
7 |
* This file is part of the IgH EtherCAT Master. |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
8 |
* |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
9 |
* The IgH EtherCAT Master is free software; you can redistribute it and/or |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
10 |
* modify it under the terms of the GNU General Public License version 2, as |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
11 |
* published by the Free Software Foundation. |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
12 |
* |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
13 |
* The IgH EtherCAT Master is distributed in the hope that it will be useful, |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
14 |
* but WITHOUT ANY WARRANTY; without even the implied warranty of |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
15 |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
16 |
* Public License for more details. |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
17 |
* |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
18 |
* You should have received a copy of the GNU General Public License along |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
19 |
* with the IgH EtherCAT Master; if not, write to the Free Software |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
20 |
* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
21 |
* |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
22 |
* --- |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
23 |
* |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
24 |
* The license mentioned above concerns the source code only. Using the |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
25 |
* EtherCAT technology and brand is only permitted in compliance with the |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
26 |
* industrial property and similar rights of Beckhoff Automation GmbH. |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
27 |
* |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
28 |
*****************************************************************************/ |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
29 |
|
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
30 |
/** |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
31 |
\file |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
32 |
EtherCAT SoE state machines. |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
33 |
*/ |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
34 |
|
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
35 |
/*****************************************************************************/ |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
36 |
|
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
37 |
#include "globals.h" |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
38 |
#include "master.h" |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
39 |
#include "mailbox.h" |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
40 |
#include "fsm_soe.h" |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
41 |
|
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
42 |
/*****************************************************************************/ |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
43 |
|
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
44 |
/** Mailbox type for SoE. |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
45 |
*/ |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
46 |
#define EC_MBOX_TYPE_SOE 0x05 |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
47 |
|
1905
5abe24872083
Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents:
1882
diff
changeset
|
48 |
/** SoE operations |
5abe24872083
Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents:
1882
diff
changeset
|
49 |
*/ |
5abe24872083
Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents:
1882
diff
changeset
|
50 |
enum ec_soe_opcodes { |
5abe24872083
Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents:
1882
diff
changeset
|
51 |
OPCODE_READ_REQUEST = 0x01, /**< Read request. */ |
5abe24872083
Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents:
1882
diff
changeset
|
52 |
OPCODE_READ_RESPONSE = 0x02, /**< Read response. */ |
5abe24872083
Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents:
1882
diff
changeset
|
53 |
OPCODE_WRITE_REQUEST = 0x03, /**< Write request. */ |
5abe24872083
Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents:
1882
diff
changeset
|
54 |
OPCODE_WRITE_RESPONSE = 0x04 /**< Write response. */ |
5abe24872083
Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents:
1882
diff
changeset
|
55 |
}; |
5abe24872083
Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents:
1882
diff
changeset
|
56 |
|
5abe24872083
Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents:
1882
diff
changeset
|
57 |
/** Size of all SoE headers. |
5abe24872083
Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents:
1882
diff
changeset
|
58 |
*/ |
5abe24872083
Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents:
1882
diff
changeset
|
59 |
#define EC_SOE_SIZE 0x04 |
5abe24872083
Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents:
1882
diff
changeset
|
60 |
|
5abe24872083
Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents:
1882
diff
changeset
|
61 |
/** SoE response timeout [ms]. |
5abe24872083
Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents:
1882
diff
changeset
|
62 |
*/ |
1831
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
63 |
#define EC_SOE_RESPONSE_TIMEOUT 1000 |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
64 |
|
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
65 |
/*****************************************************************************/ |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
66 |
|
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
67 |
void ec_fsm_soe_read_start(ec_fsm_soe_t *); |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
68 |
void ec_fsm_soe_read_request(ec_fsm_soe_t *); |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
69 |
void ec_fsm_soe_read_check(ec_fsm_soe_t *); |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
70 |
void ec_fsm_soe_read_response(ec_fsm_soe_t *); |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
71 |
|
1837
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
72 |
void ec_fsm_soe_write_start(ec_fsm_soe_t *); |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
73 |
void ec_fsm_soe_write_request(ec_fsm_soe_t *); |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
74 |
void ec_fsm_soe_write_check(ec_fsm_soe_t *); |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
75 |
void ec_fsm_soe_write_response(ec_fsm_soe_t *); |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
76 |
|
1831
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
77 |
void ec_fsm_soe_end(ec_fsm_soe_t *); |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
78 |
void ec_fsm_soe_error(ec_fsm_soe_t *); |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
79 |
|
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
80 |
/*****************************************************************************/ |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
81 |
|
1877
7b77000f9764
Included list of common SoE error messages.
Florian Pose <fp@igh-essen.com>
parents:
1873
diff
changeset
|
82 |
extern const ec_code_msg_t soe_error_codes[]; |
7b77000f9764
Included list of common SoE error messages.
Florian Pose <fp@igh-essen.com>
parents:
1873
diff
changeset
|
83 |
|
7b77000f9764
Included list of common SoE error messages.
Florian Pose <fp@igh-essen.com>
parents:
1873
diff
changeset
|
84 |
/*****************************************************************************/ |
7b77000f9764
Included list of common SoE error messages.
Florian Pose <fp@igh-essen.com>
parents:
1873
diff
changeset
|
85 |
|
7b77000f9764
Included list of common SoE error messages.
Florian Pose <fp@igh-essen.com>
parents:
1873
diff
changeset
|
86 |
/** Outputs an SoE error code. |
7b77000f9764
Included list of common SoE error messages.
Florian Pose <fp@igh-essen.com>
parents:
1873
diff
changeset
|
87 |
*/ |
1921 | 88 |
void ec_print_soe_error(const ec_slave_t *slave, uint16_t error_code) |
1877
7b77000f9764
Included list of common SoE error messages.
Florian Pose <fp@igh-essen.com>
parents:
1873
diff
changeset
|
89 |
{ |
7b77000f9764
Included list of common SoE error messages.
Florian Pose <fp@igh-essen.com>
parents:
1873
diff
changeset
|
90 |
const ec_code_msg_t *error_msg; |
7b77000f9764
Included list of common SoE error messages.
Florian Pose <fp@igh-essen.com>
parents:
1873
diff
changeset
|
91 |
|
7b77000f9764
Included list of common SoE error messages.
Florian Pose <fp@igh-essen.com>
parents:
1873
diff
changeset
|
92 |
for (error_msg = soe_error_codes; error_msg->code; error_msg++) { |
7b77000f9764
Included list of common SoE error messages.
Florian Pose <fp@igh-essen.com>
parents:
1873
diff
changeset
|
93 |
if (error_msg->code == error_code) { |
1921 | 94 |
EC_SLAVE_ERR(slave, "SoE error 0x%04X: \"%s\".\n", |
1877
7b77000f9764
Included list of common SoE error messages.
Florian Pose <fp@igh-essen.com>
parents:
1873
diff
changeset
|
95 |
error_msg->code, error_msg->message); |
7b77000f9764
Included list of common SoE error messages.
Florian Pose <fp@igh-essen.com>
parents:
1873
diff
changeset
|
96 |
return; |
7b77000f9764
Included list of common SoE error messages.
Florian Pose <fp@igh-essen.com>
parents:
1873
diff
changeset
|
97 |
} |
7b77000f9764
Included list of common SoE error messages.
Florian Pose <fp@igh-essen.com>
parents:
1873
diff
changeset
|
98 |
} |
7b77000f9764
Included list of common SoE error messages.
Florian Pose <fp@igh-essen.com>
parents:
1873
diff
changeset
|
99 |
|
1921 | 100 |
EC_SLAVE_ERR(slave, "Unknown SoE error 0x%04X.\n", error_code); |
1877
7b77000f9764
Included list of common SoE error messages.
Florian Pose <fp@igh-essen.com>
parents:
1873
diff
changeset
|
101 |
} |
7b77000f9764
Included list of common SoE error messages.
Florian Pose <fp@igh-essen.com>
parents:
1873
diff
changeset
|
102 |
|
7b77000f9764
Included list of common SoE error messages.
Florian Pose <fp@igh-essen.com>
parents:
1873
diff
changeset
|
103 |
/*****************************************************************************/ |
7b77000f9764
Included list of common SoE error messages.
Florian Pose <fp@igh-essen.com>
parents:
1873
diff
changeset
|
104 |
|
1831
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
105 |
/** Constructor. |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
106 |
*/ |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
107 |
void ec_fsm_soe_init( |
1873
219be3742274
Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents:
1866
diff
changeset
|
108 |
ec_fsm_soe_t *fsm, /**< finite state machine */ |
219be3742274
Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents:
1866
diff
changeset
|
109 |
ec_datagram_t *datagram /**< datagram */ |
219be3742274
Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents:
1866
diff
changeset
|
110 |
) |
1831
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
111 |
{ |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
112 |
fsm->state = NULL; |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
113 |
fsm->datagram = datagram; |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
114 |
} |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
115 |
|
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
116 |
/*****************************************************************************/ |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
117 |
|
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
118 |
/** Destructor. |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
119 |
*/ |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
120 |
void ec_fsm_soe_clear( |
1873
219be3742274
Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents:
1866
diff
changeset
|
121 |
ec_fsm_soe_t *fsm /**< finite state machine */ |
219be3742274
Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents:
1866
diff
changeset
|
122 |
) |
1831
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
123 |
{ |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
124 |
} |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
125 |
|
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
126 |
/*****************************************************************************/ |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
127 |
|
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
128 |
/** Starts to transfer an IDN to/from a slave. |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
129 |
*/ |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
130 |
void ec_fsm_soe_transfer( |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
131 |
ec_fsm_soe_t *fsm, /**< State machine. */ |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
132 |
ec_slave_t *slave, /**< EtherCAT slave. */ |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
133 |
ec_soe_request_t *request /**< SoE request. */ |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
134 |
) |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
135 |
{ |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
136 |
fsm->slave = slave; |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
137 |
fsm->request = request; |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
138 |
if (request->dir == EC_DIR_OUTPUT) { |
1837
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
139 |
fsm->state = ec_fsm_soe_write_start; |
1873
219be3742274
Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents:
1866
diff
changeset
|
140 |
} else { |
1831
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
141 |
fsm->state = ec_fsm_soe_read_start; |
1873
219be3742274
Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents:
1866
diff
changeset
|
142 |
} |
1831
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
143 |
} |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
144 |
|
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
145 |
/*****************************************************************************/ |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
146 |
|
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
147 |
/** |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
148 |
Executes the current state of the state machine. |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
149 |
\return false, if state machine has terminated |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
150 |
*/ |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
151 |
|
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
152 |
int ec_fsm_soe_exec(ec_fsm_soe_t *fsm /**< finite state machine */) |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
153 |
{ |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
154 |
fsm->state(fsm); |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
155 |
|
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
156 |
return fsm->state != ec_fsm_soe_end && fsm->state != ec_fsm_soe_error; |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
157 |
} |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
158 |
|
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
159 |
/*****************************************************************************/ |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
160 |
|
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
161 |
/** |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
162 |
Returns, if the state machine terminated with success. |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
163 |
\return non-zero if successful. |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
164 |
*/ |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
165 |
|
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
166 |
int ec_fsm_soe_success(ec_fsm_soe_t *fsm /**< Finite state machine */) |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
167 |
{ |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
168 |
return fsm->state == ec_fsm_soe_end; |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
169 |
} |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
170 |
|
1881
f0047c6226c0
Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents:
1877
diff
changeset
|
171 |
/*****************************************************************************/ |
f0047c6226c0
Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents:
1877
diff
changeset
|
172 |
|
f0047c6226c0
Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents:
1877
diff
changeset
|
173 |
/** Output information about a failed SoE transfer. |
f0047c6226c0
Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents:
1877
diff
changeset
|
174 |
*/ |
f0047c6226c0
Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents:
1877
diff
changeset
|
175 |
void ec_fsm_soe_print_error(ec_fsm_soe_t *fsm /**< Finite state machine */) |
f0047c6226c0
Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents:
1877
diff
changeset
|
176 |
{ |
f0047c6226c0
Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents:
1877
diff
changeset
|
177 |
ec_soe_request_t *request = fsm->request; |
f0047c6226c0
Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents:
1877
diff
changeset
|
178 |
|
1921 | 179 |
EC_SLAVE_ERR(fsm->slave, ""); |
1881
f0047c6226c0
Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents:
1877
diff
changeset
|
180 |
|
f0047c6226c0
Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents:
1877
diff
changeset
|
181 |
if (request->dir == EC_DIR_OUTPUT) { |
f0047c6226c0
Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents:
1877
diff
changeset
|
182 |
printk("Writing"); |
f0047c6226c0
Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents:
1877
diff
changeset
|
183 |
} else { |
f0047c6226c0
Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents:
1877
diff
changeset
|
184 |
printk("Reading"); |
f0047c6226c0
Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents:
1877
diff
changeset
|
185 |
} |
f0047c6226c0
Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents:
1877
diff
changeset
|
186 |
|
1921 | 187 |
printk(" IDN 0x%04X failed.\n", request->idn); |
1881
f0047c6226c0
Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents:
1877
diff
changeset
|
188 |
} |
f0047c6226c0
Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents:
1877
diff
changeset
|
189 |
|
1831
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
190 |
/****************************************************************************** |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
191 |
* SoE read state machine |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
192 |
*****************************************************************************/ |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
193 |
|
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
194 |
/** SoE state: READ START. |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
195 |
*/ |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
196 |
void ec_fsm_soe_read_start(ec_fsm_soe_t *fsm /**< finite state machine */) |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
197 |
{ |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
198 |
ec_datagram_t *datagram = fsm->datagram; |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
199 |
ec_slave_t *slave = fsm->slave; |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
200 |
ec_master_t *master = slave->master; |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
201 |
ec_soe_request_t *request = fsm->request; |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
202 |
uint8_t *data; |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
203 |
|
1952
7d9fb723fc4b
Added drive_no parameter to SoE layer.
Florian Pose <fp@igh-essen.com>
parents:
1931
diff
changeset
|
204 |
EC_SLAVE_DBG(slave, 1, "Reading IDN 0x%04X of drive %u.\n", request->idn, |
7d9fb723fc4b
Added drive_no parameter to SoE layer.
Florian Pose <fp@igh-essen.com>
parents:
1931
diff
changeset
|
205 |
request->drive_no); |
1831
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
206 |
|
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
207 |
if (!(slave->sii.mailbox_protocols & EC_MBOX_SOE)) { |
1921 | 208 |
EC_SLAVE_ERR(slave, "Slave does not support SoE!\n"); |
1881
f0047c6226c0
Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents:
1877
diff
changeset
|
209 |
fsm->state = ec_fsm_soe_error; |
f0047c6226c0
Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents:
1877
diff
changeset
|
210 |
ec_fsm_soe_print_error(fsm); |
1831
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
211 |
return; |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
212 |
} |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
213 |
|
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
214 |
data = ec_slave_mbox_prepare_send(slave, datagram, EC_MBOX_TYPE_SOE, |
1905
5abe24872083
Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents:
1882
diff
changeset
|
215 |
EC_SOE_SIZE); |
1831
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
216 |
if (IS_ERR(data)) { |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
217 |
fsm->state = ec_fsm_soe_error; |
1881
f0047c6226c0
Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents:
1877
diff
changeset
|
218 |
ec_fsm_soe_print_error(fsm); |
1831
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
219 |
return; |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
220 |
} |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
221 |
|
1952
7d9fb723fc4b
Added drive_no parameter to SoE layer.
Florian Pose <fp@igh-essen.com>
parents:
1931
diff
changeset
|
222 |
EC_WRITE_U8(data, OPCODE_READ_REQUEST | (request->drive_no & 0x07) << 5); |
1831
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
223 |
EC_WRITE_U8(data + 1, 1 << 6); // request value |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
224 |
EC_WRITE_U16(data + 2, request->idn); |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
225 |
|
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
226 |
if (master->debug_level) { |
1921 | 227 |
EC_SLAVE_DBG(slave, 0, "SCC read request:\n"); |
1905
5abe24872083
Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents:
1882
diff
changeset
|
228 |
ec_print_data(data, EC_SOE_SIZE); |
1831
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
229 |
} |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
230 |
|
1865
c6c8b457bb40
Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents:
1864
diff
changeset
|
231 |
fsm->request->data_size = 0; |
1831
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
232 |
fsm->request->jiffies_sent = jiffies; |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
233 |
fsm->retries = EC_FSM_RETRIES; |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
234 |
fsm->state = ec_fsm_soe_read_request; |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
235 |
} |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
236 |
|
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
237 |
/*****************************************************************************/ |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
238 |
|
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
239 |
/** SoE state: READ REQUEST. |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
240 |
*/ |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
241 |
void ec_fsm_soe_read_request(ec_fsm_soe_t *fsm /**< finite state machine */) |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
242 |
{ |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
243 |
ec_datagram_t *datagram = fsm->datagram; |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
244 |
ec_slave_t *slave = fsm->slave; |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
245 |
unsigned long diff_ms; |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
246 |
|
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
247 |
if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
248 |
return; // FIXME: check for response first? |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
249 |
|
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
250 |
if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
251 |
fsm->state = ec_fsm_soe_error; |
1921 | 252 |
EC_SLAVE_ERR(slave, "Failed to receive SoE read request: "); |
1831
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
253 |
ec_datagram_print_state(datagram); |
1881
f0047c6226c0
Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents:
1877
diff
changeset
|
254 |
ec_fsm_soe_print_error(fsm); |
1831
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
255 |
return; |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
256 |
} |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
257 |
|
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
258 |
diff_ms = (jiffies - fsm->request->jiffies_sent) * 1000 / HZ; |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
259 |
|
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
260 |
if (datagram->working_counter != 1) { |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
261 |
if (!datagram->working_counter) { |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
262 |
if (diff_ms < EC_SOE_RESPONSE_TIMEOUT) { |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
263 |
// no response; send request datagram again |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
264 |
return; |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
265 |
} |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
266 |
} |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
267 |
fsm->state = ec_fsm_soe_error; |
1921 | 268 |
EC_SLAVE_ERR(slave, "Reception of SoE read request" |
1931 | 269 |
" failed after %lu ms: ", diff_ms); |
1831
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
270 |
ec_datagram_print_wc_error(datagram); |
1881
f0047c6226c0
Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents:
1877
diff
changeset
|
271 |
ec_fsm_soe_print_error(fsm); |
1831
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
272 |
return; |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
273 |
} |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
274 |
|
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
275 |
fsm->jiffies_start = datagram->jiffies_sent; |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
276 |
ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
277 |
fsm->retries = EC_FSM_RETRIES; |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
278 |
fsm->state = ec_fsm_soe_read_check; |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
279 |
} |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
280 |
|
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
281 |
/*****************************************************************************/ |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
282 |
|
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
283 |
/** CoE state: READ CHECK. |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
284 |
*/ |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
285 |
void ec_fsm_soe_read_check(ec_fsm_soe_t *fsm /**< finite state machine */) |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
286 |
{ |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
287 |
ec_datagram_t *datagram = fsm->datagram; |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
288 |
ec_slave_t *slave = fsm->slave; |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
289 |
|
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
290 |
if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
291 |
return; |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
292 |
|
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
293 |
if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
294 |
fsm->state = ec_fsm_soe_error; |
1921 | 295 |
EC_SLAVE_ERR(slave, "Failed to receive SoE mailbox check datagram: "); |
1831
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
296 |
ec_datagram_print_state(datagram); |
1881
f0047c6226c0
Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents:
1877
diff
changeset
|
297 |
ec_fsm_soe_print_error(fsm); |
1831
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
298 |
return; |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
299 |
} |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
300 |
|
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
301 |
if (datagram->working_counter != 1) { |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
302 |
fsm->state = ec_fsm_soe_error; |
1921 | 303 |
EC_SLAVE_ERR(slave, "Reception of SoE mailbox check" |
304 |
" datagram failed: "); |
|
1831
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
305 |
ec_datagram_print_wc_error(datagram); |
1881
f0047c6226c0
Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents:
1877
diff
changeset
|
306 |
ec_fsm_soe_print_error(fsm); |
1831
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
307 |
return; |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
308 |
} |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
309 |
|
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
310 |
if (!ec_slave_mbox_check(datagram)) { |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
311 |
unsigned long diff_ms = |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
312 |
(datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
313 |
if (diff_ms >= EC_SOE_RESPONSE_TIMEOUT) { |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
314 |
fsm->state = ec_fsm_soe_error; |
1931 | 315 |
EC_SLAVE_ERR(slave, "Timeout after %lu ms while waiting for" |
316 |
" read response.\n", diff_ms); |
|
1881
f0047c6226c0
Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents:
1877
diff
changeset
|
317 |
ec_fsm_soe_print_error(fsm); |
1831
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
318 |
return; |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
319 |
} |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
320 |
|
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
321 |
ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
322 |
fsm->retries = EC_FSM_RETRIES; |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
323 |
return; |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
324 |
} |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
325 |
|
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
326 |
// Fetch response |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
327 |
ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
328 |
fsm->retries = EC_FSM_RETRIES; |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
329 |
fsm->state = ec_fsm_soe_read_response; |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
330 |
} |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
331 |
|
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
332 |
/*****************************************************************************/ |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
333 |
|
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
334 |
/** SoE state: READ RESPONSE. |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
335 |
*/ |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
336 |
void ec_fsm_soe_read_response(ec_fsm_soe_t *fsm /**< finite state machine */) |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
337 |
{ |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
338 |
ec_datagram_t *datagram = fsm->datagram; |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
339 |
ec_slave_t *slave = fsm->slave; |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
340 |
ec_master_t *master = slave->master; |
1864
0a6b3aacc847
Implemented SoE fragmented reading.
Florian Pose <fp@igh-essen.com>
parents:
1840
diff
changeset
|
341 |
uint8_t *data, mbox_prot, header, opcode, incomplete, error_flag, |
0a6b3aacc847
Implemented SoE fragmented reading.
Florian Pose <fp@igh-essen.com>
parents:
1840
diff
changeset
|
342 |
value_included; |
1831
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
343 |
size_t rec_size, data_size; |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
344 |
ec_soe_request_t *req = fsm->request; |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
345 |
|
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
346 |
if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
347 |
return; // FIXME: request again? |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
348 |
|
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
349 |
if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
350 |
fsm->state = ec_fsm_soe_error; |
1921 | 351 |
EC_SLAVE_ERR(slave, "Failed to receive SoE read response datagram: "); |
1831
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
352 |
ec_datagram_print_state(datagram); |
1881
f0047c6226c0
Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents:
1877
diff
changeset
|
353 |
ec_fsm_soe_print_error(fsm); |
1831
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
354 |
return; |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
355 |
} |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
356 |
|
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
357 |
if (datagram->working_counter != 1) { |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
358 |
fsm->state = ec_fsm_soe_error; |
1921 | 359 |
EC_SLAVE_ERR(slave, "Reception of SoE read response failed: "); |
1831
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
360 |
ec_datagram_print_wc_error(datagram); |
1881
f0047c6226c0
Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents:
1877
diff
changeset
|
361 |
ec_fsm_soe_print_error(fsm); |
1831
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
362 |
return; |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
363 |
} |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
364 |
|
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
365 |
data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot, &rec_size); |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
366 |
if (IS_ERR(data)) { |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
367 |
fsm->state = ec_fsm_soe_error; |
1881
f0047c6226c0
Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents:
1877
diff
changeset
|
368 |
ec_fsm_soe_print_error(fsm); |
1831
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
369 |
return; |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
370 |
} |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
371 |
|
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
372 |
if (master->debug_level) { |
1921 | 373 |
EC_SLAVE_DBG(slave, 0, "SCC read response:\n"); |
1831
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
374 |
ec_print_data(data, rec_size); |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
375 |
} |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
376 |
|
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
377 |
if (mbox_prot != EC_MBOX_TYPE_SOE) { |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
378 |
fsm->state = ec_fsm_soe_error; |
1921 | 379 |
EC_SLAVE_ERR(slave, "Received mailbox protocol 0x%02X as response.\n", |
380 |
mbox_prot); |
|
1881
f0047c6226c0
Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents:
1877
diff
changeset
|
381 |
ec_fsm_soe_print_error(fsm); |
1831
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
382 |
return; |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
383 |
} |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
384 |
|
1905
5abe24872083
Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents:
1882
diff
changeset
|
385 |
if (rec_size < EC_SOE_SIZE) { |
1831
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
386 |
fsm->state = ec_fsm_soe_error; |
1921 | 387 |
EC_SLAVE_ERR(slave, "Received currupted SoE read response" |
1881
f0047c6226c0
Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents:
1877
diff
changeset
|
388 |
" (%zu bytes)!\n", rec_size); |
1831
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
389 |
ec_print_data(data, rec_size); |
1881
f0047c6226c0
Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents:
1877
diff
changeset
|
390 |
ec_fsm_soe_print_error(fsm); |
1831
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
391 |
return; |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
392 |
} |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
393 |
|
1864
0a6b3aacc847
Implemented SoE fragmented reading.
Florian Pose <fp@igh-essen.com>
parents:
1840
diff
changeset
|
394 |
header = EC_READ_U8(data); |
1873
219be3742274
Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents:
1866
diff
changeset
|
395 |
opcode = header & 0x7; |
1864
0a6b3aacc847
Implemented SoE fragmented reading.
Florian Pose <fp@igh-essen.com>
parents:
1840
diff
changeset
|
396 |
incomplete = (header >> 3) & 1; |
1873
219be3742274
Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents:
1866
diff
changeset
|
397 |
error_flag = (header >> 4) & 1; |
1864
0a6b3aacc847
Implemented SoE fragmented reading.
Florian Pose <fp@igh-essen.com>
parents:
1840
diff
changeset
|
398 |
|
1905
5abe24872083
Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents:
1882
diff
changeset
|
399 |
if (opcode != OPCODE_READ_RESPONSE) { |
1921 | 400 |
EC_SLAVE_ERR(slave, "Received no read response (opcode %x).\n", |
401 |
opcode); |
|
1831
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
402 |
ec_print_data(data, rec_size); |
1881
f0047c6226c0
Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents:
1877
diff
changeset
|
403 |
ec_fsm_soe_print_error(fsm); |
1831
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
404 |
fsm->state = ec_fsm_soe_error; |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
405 |
return; |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
406 |
} |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
407 |
|
1873
219be3742274
Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents:
1866
diff
changeset
|
408 |
if (error_flag) { |
219be3742274
Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents:
1866
diff
changeset
|
409 |
req->error_code = EC_READ_U16(data + rec_size - 2); |
1921 | 410 |
EC_SLAVE_ERR(slave, "Received error response:\n"); |
411 |
ec_print_soe_error(slave, req->error_code); |
|
1881
f0047c6226c0
Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents:
1877
diff
changeset
|
412 |
ec_fsm_soe_print_error(fsm); |
1873
219be3742274
Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents:
1866
diff
changeset
|
413 |
fsm->state = ec_fsm_soe_error; |
219be3742274
Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents:
1866
diff
changeset
|
414 |
return; |
219be3742274
Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents:
1866
diff
changeset
|
415 |
} else { |
219be3742274
Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents:
1866
diff
changeset
|
416 |
req->error_code = 0x0000; |
219be3742274
Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents:
1866
diff
changeset
|
417 |
} |
219be3742274
Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents:
1866
diff
changeset
|
418 |
|
219be3742274
Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents:
1866
diff
changeset
|
419 |
value_included = (EC_READ_U8(data + 1) >> 6) & 1; |
219be3742274
Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents:
1866
diff
changeset
|
420 |
if (!value_included) { |
1921 | 421 |
EC_SLAVE_ERR(slave, "No value included!\n"); |
1881
f0047c6226c0
Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents:
1877
diff
changeset
|
422 |
ec_fsm_soe_print_error(fsm); |
1873
219be3742274
Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents:
1866
diff
changeset
|
423 |
fsm->state = ec_fsm_soe_error; |
219be3742274
Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents:
1866
diff
changeset
|
424 |
return; |
219be3742274
Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents:
1866
diff
changeset
|
425 |
} |
219be3742274
Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents:
1866
diff
changeset
|
426 |
|
1905
5abe24872083
Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents:
1882
diff
changeset
|
427 |
data_size = rec_size - EC_SOE_SIZE; |
1873
219be3742274
Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents:
1866
diff
changeset
|
428 |
if (ec_soe_request_append_data(req, |
1905
5abe24872083
Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents:
1882
diff
changeset
|
429 |
data + EC_SOE_SIZE, data_size)) { |
1873
219be3742274
Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents:
1866
diff
changeset
|
430 |
fsm->state = ec_fsm_soe_error; |
1881
f0047c6226c0
Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents:
1877
diff
changeset
|
431 |
ec_fsm_soe_print_error(fsm); |
1873
219be3742274
Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents:
1866
diff
changeset
|
432 |
return; |
219be3742274
Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents:
1866
diff
changeset
|
433 |
} |
1831
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
434 |
|
1864
0a6b3aacc847
Implemented SoE fragmented reading.
Florian Pose <fp@igh-essen.com>
parents:
1840
diff
changeset
|
435 |
if (incomplete) { |
1921 | 436 |
EC_SLAVE_DBG(slave, 1, "SoE data incomplete. Waiting for fragment" |
437 |
" at offset %zu.\n", req->data_size); |
|
1864
0a6b3aacc847
Implemented SoE fragmented reading.
Florian Pose <fp@igh-essen.com>
parents:
1840
diff
changeset
|
438 |
fsm->jiffies_start = datagram->jiffies_sent; |
0a6b3aacc847
Implemented SoE fragmented reading.
Florian Pose <fp@igh-essen.com>
parents:
1840
diff
changeset
|
439 |
ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
0a6b3aacc847
Implemented SoE fragmented reading.
Florian Pose <fp@igh-essen.com>
parents:
1840
diff
changeset
|
440 |
fsm->retries = EC_FSM_RETRIES; |
0a6b3aacc847
Implemented SoE fragmented reading.
Florian Pose <fp@igh-essen.com>
parents:
1840
diff
changeset
|
441 |
fsm->state = ec_fsm_soe_read_check; |
0a6b3aacc847
Implemented SoE fragmented reading.
Florian Pose <fp@igh-essen.com>
parents:
1840
diff
changeset
|
442 |
} else { |
0a6b3aacc847
Implemented SoE fragmented reading.
Florian Pose <fp@igh-essen.com>
parents:
1840
diff
changeset
|
443 |
if (master->debug_level) { |
1921 | 444 |
EC_SLAVE_DBG(slave, 0, "IDN data:\n"); |
1864
0a6b3aacc847
Implemented SoE fragmented reading.
Florian Pose <fp@igh-essen.com>
parents:
1840
diff
changeset
|
445 |
ec_print_data(req->data, req->data_size); |
0a6b3aacc847
Implemented SoE fragmented reading.
Florian Pose <fp@igh-essen.com>
parents:
1840
diff
changeset
|
446 |
} |
0a6b3aacc847
Implemented SoE fragmented reading.
Florian Pose <fp@igh-essen.com>
parents:
1840
diff
changeset
|
447 |
|
0a6b3aacc847
Implemented SoE fragmented reading.
Florian Pose <fp@igh-essen.com>
parents:
1840
diff
changeset
|
448 |
fsm->state = ec_fsm_soe_end; // success |
0a6b3aacc847
Implemented SoE fragmented reading.
Florian Pose <fp@igh-essen.com>
parents:
1840
diff
changeset
|
449 |
} |
1831
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
450 |
} |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
451 |
|
1837
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
452 |
/****************************************************************************** |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
453 |
* SoE write state machine |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
454 |
*****************************************************************************/ |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
455 |
|
1865
c6c8b457bb40
Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents:
1864
diff
changeset
|
456 |
/** Write next fragment. |
c6c8b457bb40
Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents:
1864
diff
changeset
|
457 |
*/ |
c6c8b457bb40
Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents:
1864
diff
changeset
|
458 |
void ec_fsm_soe_write_next_fragment( |
c6c8b457bb40
Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents:
1864
diff
changeset
|
459 |
ec_fsm_soe_t *fsm /**< finite state machine */ |
c6c8b457bb40
Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents:
1864
diff
changeset
|
460 |
) |
c6c8b457bb40
Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents:
1864
diff
changeset
|
461 |
{ |
c6c8b457bb40
Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents:
1864
diff
changeset
|
462 |
ec_datagram_t *datagram = fsm->datagram; |
c6c8b457bb40
Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents:
1864
diff
changeset
|
463 |
ec_slave_t *slave = fsm->slave; |
c6c8b457bb40
Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents:
1864
diff
changeset
|
464 |
ec_master_t *master = slave->master; |
c6c8b457bb40
Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents:
1864
diff
changeset
|
465 |
ec_soe_request_t *req = fsm->request; |
c6c8b457bb40
Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents:
1864
diff
changeset
|
466 |
uint8_t incomplete, *data; |
c6c8b457bb40
Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents:
1864
diff
changeset
|
467 |
size_t header_size, max_fragment_size, remaining_size, fragment_size; |
c6c8b457bb40
Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents:
1864
diff
changeset
|
468 |
uint16_t fragments_left; |
c6c8b457bb40
Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents:
1864
diff
changeset
|
469 |
|
1905
5abe24872083
Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents:
1882
diff
changeset
|
470 |
header_size = EC_MBOX_HEADER_SIZE + EC_SOE_SIZE; |
1865
c6c8b457bb40
Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents:
1864
diff
changeset
|
471 |
if (slave->configured_rx_mailbox_size <= header_size) { |
1921 | 472 |
EC_SLAVE_ERR(slave, "Mailbox size (%u) too small for SoE write.\n", |
1865
c6c8b457bb40
Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents:
1864
diff
changeset
|
473 |
slave->configured_rx_mailbox_size); |
c6c8b457bb40
Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents:
1864
diff
changeset
|
474 |
fsm->state = ec_fsm_soe_error; |
1881
f0047c6226c0
Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents:
1877
diff
changeset
|
475 |
ec_fsm_soe_print_error(fsm); |
1865
c6c8b457bb40
Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents:
1864
diff
changeset
|
476 |
return; |
c6c8b457bb40
Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents:
1864
diff
changeset
|
477 |
} |
c6c8b457bb40
Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents:
1864
diff
changeset
|
478 |
|
c6c8b457bb40
Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents:
1864
diff
changeset
|
479 |
remaining_size = req->data_size - fsm->offset; |
c6c8b457bb40
Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents:
1864
diff
changeset
|
480 |
max_fragment_size = slave->configured_rx_mailbox_size - header_size; |
c6c8b457bb40
Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents:
1864
diff
changeset
|
481 |
incomplete = remaining_size > max_fragment_size; |
c6c8b457bb40
Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents:
1864
diff
changeset
|
482 |
fragment_size = incomplete ? max_fragment_size : remaining_size; |
1866
02323e72efc9
Fixed fragmented SoE write.
Florian Pose <fp@igh-essen.com>
parents:
1865
diff
changeset
|
483 |
fragments_left = remaining_size / fragment_size - 1; |
1865
c6c8b457bb40
Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents:
1864
diff
changeset
|
484 |
if (remaining_size % fragment_size) { |
c6c8b457bb40
Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents:
1864
diff
changeset
|
485 |
fragments_left++; |
c6c8b457bb40
Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents:
1864
diff
changeset
|
486 |
} |
c6c8b457bb40
Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents:
1864
diff
changeset
|
487 |
|
c6c8b457bb40
Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents:
1864
diff
changeset
|
488 |
data = ec_slave_mbox_prepare_send(slave, datagram, EC_MBOX_TYPE_SOE, |
1905
5abe24872083
Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents:
1882
diff
changeset
|
489 |
EC_SOE_SIZE + fragment_size); |
1865
c6c8b457bb40
Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents:
1864
diff
changeset
|
490 |
if (IS_ERR(data)) { |
c6c8b457bb40
Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents:
1864
diff
changeset
|
491 |
fsm->state = ec_fsm_soe_error; |
1881
f0047c6226c0
Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents:
1877
diff
changeset
|
492 |
ec_fsm_soe_print_error(fsm); |
1865
c6c8b457bb40
Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents:
1864
diff
changeset
|
493 |
return; |
c6c8b457bb40
Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents:
1864
diff
changeset
|
494 |
} |
c6c8b457bb40
Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents:
1864
diff
changeset
|
495 |
|
1952
7d9fb723fc4b
Added drive_no parameter to SoE layer.
Florian Pose <fp@igh-essen.com>
parents:
1931
diff
changeset
|
496 |
EC_WRITE_U8(data, OPCODE_WRITE_REQUEST | incomplete << 3 | |
7d9fb723fc4b
Added drive_no parameter to SoE layer.
Florian Pose <fp@igh-essen.com>
parents:
1931
diff
changeset
|
497 |
(req->drive_no & 0x07) << 5); |
1865
c6c8b457bb40
Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents:
1864
diff
changeset
|
498 |
EC_WRITE_U8(data + 1, 1 << 6); // only value included |
1866
02323e72efc9
Fixed fragmented SoE write.
Florian Pose <fp@igh-essen.com>
parents:
1865
diff
changeset
|
499 |
EC_WRITE_U16(data + 2, incomplete ? fragments_left : req->idn); |
1873
219be3742274
Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents:
1866
diff
changeset
|
500 |
memcpy(data + 4, req->data + fsm->offset, fragment_size); |
1865
c6c8b457bb40
Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents:
1864
diff
changeset
|
501 |
fsm->offset += fragment_size; |
c6c8b457bb40
Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents:
1864
diff
changeset
|
502 |
|
c6c8b457bb40
Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents:
1864
diff
changeset
|
503 |
if (master->debug_level) { |
1921 | 504 |
EC_SLAVE_DBG(slave, 0, "SCC write request:\n"); |
1905
5abe24872083
Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents:
1882
diff
changeset
|
505 |
ec_print_data(data, EC_SOE_SIZE + fragment_size); |
1865
c6c8b457bb40
Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents:
1864
diff
changeset
|
506 |
} |
c6c8b457bb40
Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents:
1864
diff
changeset
|
507 |
|
c6c8b457bb40
Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents:
1864
diff
changeset
|
508 |
req->jiffies_sent = jiffies; |
c6c8b457bb40
Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents:
1864
diff
changeset
|
509 |
fsm->retries = EC_FSM_RETRIES; |
c6c8b457bb40
Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents:
1864
diff
changeset
|
510 |
fsm->state = ec_fsm_soe_write_request; |
c6c8b457bb40
Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents:
1864
diff
changeset
|
511 |
} |
c6c8b457bb40
Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents:
1864
diff
changeset
|
512 |
|
c6c8b457bb40
Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents:
1864
diff
changeset
|
513 |
/*****************************************************************************/ |
c6c8b457bb40
Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents:
1864
diff
changeset
|
514 |
|
1837
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
515 |
/** SoE state: WRITE START. |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
516 |
*/ |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
517 |
void ec_fsm_soe_write_start(ec_fsm_soe_t *fsm /**< finite state machine */) |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
518 |
{ |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
519 |
ec_slave_t *slave = fsm->slave; |
1865
c6c8b457bb40
Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents:
1864
diff
changeset
|
520 |
ec_soe_request_t *req = fsm->request; |
1837
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
521 |
|
1952
7d9fb723fc4b
Added drive_no parameter to SoE layer.
Florian Pose <fp@igh-essen.com>
parents:
1931
diff
changeset
|
522 |
EC_SLAVE_DBG(slave, 1, "Writing IDN 0x%04X of drive %u (%zu byte).\n", |
7d9fb723fc4b
Added drive_no parameter to SoE layer.
Florian Pose <fp@igh-essen.com>
parents:
1931
diff
changeset
|
523 |
req->idn, req->drive_no, req->data_size); |
1837
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
524 |
|
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
525 |
if (!(slave->sii.mailbox_protocols & EC_MBOX_SOE)) { |
1921 | 526 |
EC_SLAVE_ERR(slave, "Slave does not support SoE!\n"); |
1881
f0047c6226c0
Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents:
1877
diff
changeset
|
527 |
fsm->state = ec_fsm_soe_error; |
f0047c6226c0
Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents:
1877
diff
changeset
|
528 |
ec_fsm_soe_print_error(fsm); |
1837
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
529 |
return; |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
530 |
} |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
531 |
|
1865
c6c8b457bb40
Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents:
1864
diff
changeset
|
532 |
fsm->offset = 0; |
c6c8b457bb40
Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents:
1864
diff
changeset
|
533 |
ec_fsm_soe_write_next_fragment(fsm); |
1837
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
534 |
} |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
535 |
|
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
536 |
/*****************************************************************************/ |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
537 |
|
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
538 |
/** SoE state: WRITE REQUEST. |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
539 |
*/ |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
540 |
void ec_fsm_soe_write_request(ec_fsm_soe_t *fsm /**< finite state machine */) |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
541 |
{ |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
542 |
ec_datagram_t *datagram = fsm->datagram; |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
543 |
ec_slave_t *slave = fsm->slave; |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
544 |
unsigned long diff_ms; |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
545 |
|
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
546 |
if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
547 |
return; // FIXME: check for response first? |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
548 |
|
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
549 |
if (datagram->state != EC_DATAGRAM_RECEIVED) { |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
550 |
fsm->state = ec_fsm_soe_error; |
1921 | 551 |
EC_SLAVE_ERR(slave, "Failed to receive SoE write request: "); |
1837
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
552 |
ec_datagram_print_state(datagram); |
1881
f0047c6226c0
Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents:
1877
diff
changeset
|
553 |
ec_fsm_soe_print_error(fsm); |
1837
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
554 |
return; |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
555 |
} |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
556 |
|
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
557 |
diff_ms = (jiffies - fsm->request->jiffies_sent) * 1000 / HZ; |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
558 |
|
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
559 |
if (datagram->working_counter != 1) { |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
560 |
if (!datagram->working_counter) { |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
561 |
if (diff_ms < EC_SOE_RESPONSE_TIMEOUT) { |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
562 |
// no response; send request datagram again |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
563 |
return; |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
564 |
} |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
565 |
} |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
566 |
fsm->state = ec_fsm_soe_error; |
1921 | 567 |
EC_SLAVE_ERR(slave, "Reception of SoE write request" |
1931 | 568 |
" failed after %lu ms: ", diff_ms); |
1837
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
569 |
ec_datagram_print_wc_error(datagram); |
1881
f0047c6226c0
Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents:
1877
diff
changeset
|
570 |
ec_fsm_soe_print_error(fsm); |
1837
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
571 |
return; |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
572 |
} |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
573 |
|
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
574 |
fsm->jiffies_start = datagram->jiffies_sent; |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
575 |
|
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
576 |
ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
577 |
fsm->retries = EC_FSM_RETRIES; |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
578 |
fsm->state = ec_fsm_soe_write_check; |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
579 |
} |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
580 |
|
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
581 |
/*****************************************************************************/ |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
582 |
|
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
583 |
/** CoE state: WRITE CHECK. |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
584 |
*/ |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
585 |
void ec_fsm_soe_write_check(ec_fsm_soe_t *fsm /**< finite state machine */) |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
586 |
{ |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
587 |
ec_datagram_t *datagram = fsm->datagram; |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
588 |
ec_slave_t *slave = fsm->slave; |
1866
02323e72efc9
Fixed fragmented SoE write.
Florian Pose <fp@igh-essen.com>
parents:
1865
diff
changeset
|
589 |
ec_soe_request_t *req = fsm->request; |
1837
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
590 |
|
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
591 |
if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
592 |
return; |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
593 |
|
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
594 |
if (datagram->state != EC_DATAGRAM_RECEIVED) { |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
595 |
fsm->state = ec_fsm_soe_error; |
1921 | 596 |
EC_SLAVE_ERR(slave, "Failed to receive SoE write request datagram: "); |
1837
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
597 |
ec_datagram_print_state(datagram); |
1881
f0047c6226c0
Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents:
1877
diff
changeset
|
598 |
ec_fsm_soe_print_error(fsm); |
1837
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
599 |
return; |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
600 |
} |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
601 |
|
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
602 |
if (datagram->working_counter != 1) { |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
603 |
fsm->state = ec_fsm_soe_error; |
1921 | 604 |
EC_SLAVE_ERR(slave, "Reception of SoE write request datagram: "); |
1837
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
605 |
ec_datagram_print_wc_error(datagram); |
1881
f0047c6226c0
Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents:
1877
diff
changeset
|
606 |
ec_fsm_soe_print_error(fsm); |
1837
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
607 |
return; |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
608 |
} |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
609 |
|
1866
02323e72efc9
Fixed fragmented SoE write.
Florian Pose <fp@igh-essen.com>
parents:
1865
diff
changeset
|
610 |
if (fsm->offset < req->data_size) { |
02323e72efc9
Fixed fragmented SoE write.
Florian Pose <fp@igh-essen.com>
parents:
1865
diff
changeset
|
611 |
ec_fsm_soe_write_next_fragment(fsm); |
02323e72efc9
Fixed fragmented SoE write.
Florian Pose <fp@igh-essen.com>
parents:
1865
diff
changeset
|
612 |
} else { |
02323e72efc9
Fixed fragmented SoE write.
Florian Pose <fp@igh-essen.com>
parents:
1865
diff
changeset
|
613 |
if (!ec_slave_mbox_check(datagram)) { |
02323e72efc9
Fixed fragmented SoE write.
Florian Pose <fp@igh-essen.com>
parents:
1865
diff
changeset
|
614 |
unsigned long diff_ms = |
02323e72efc9
Fixed fragmented SoE write.
Florian Pose <fp@igh-essen.com>
parents:
1865
diff
changeset
|
615 |
(datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; |
02323e72efc9
Fixed fragmented SoE write.
Florian Pose <fp@igh-essen.com>
parents:
1865
diff
changeset
|
616 |
if (diff_ms >= EC_SOE_RESPONSE_TIMEOUT) { |
02323e72efc9
Fixed fragmented SoE write.
Florian Pose <fp@igh-essen.com>
parents:
1865
diff
changeset
|
617 |
fsm->state = ec_fsm_soe_error; |
1931 | 618 |
EC_SLAVE_ERR(slave, "Timeout after %lu ms while waiting" |
619 |
" for write response.\n", diff_ms); |
|
1881
f0047c6226c0
Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents:
1877
diff
changeset
|
620 |
ec_fsm_soe_print_error(fsm); |
1866
02323e72efc9
Fixed fragmented SoE write.
Florian Pose <fp@igh-essen.com>
parents:
1865
diff
changeset
|
621 |
return; |
02323e72efc9
Fixed fragmented SoE write.
Florian Pose <fp@igh-essen.com>
parents:
1865
diff
changeset
|
622 |
} |
02323e72efc9
Fixed fragmented SoE write.
Florian Pose <fp@igh-essen.com>
parents:
1865
diff
changeset
|
623 |
|
02323e72efc9
Fixed fragmented SoE write.
Florian Pose <fp@igh-essen.com>
parents:
1865
diff
changeset
|
624 |
ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
02323e72efc9
Fixed fragmented SoE write.
Florian Pose <fp@igh-essen.com>
parents:
1865
diff
changeset
|
625 |
fsm->retries = EC_FSM_RETRIES; |
1837
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
626 |
return; |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
627 |
} |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
628 |
|
1866
02323e72efc9
Fixed fragmented SoE write.
Florian Pose <fp@igh-essen.com>
parents:
1865
diff
changeset
|
629 |
// Fetch response |
02323e72efc9
Fixed fragmented SoE write.
Florian Pose <fp@igh-essen.com>
parents:
1865
diff
changeset
|
630 |
ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. |
1837
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
631 |
fsm->retries = EC_FSM_RETRIES; |
1866
02323e72efc9
Fixed fragmented SoE write.
Florian Pose <fp@igh-essen.com>
parents:
1865
diff
changeset
|
632 |
fsm->state = ec_fsm_soe_write_response; |
02323e72efc9
Fixed fragmented SoE write.
Florian Pose <fp@igh-essen.com>
parents:
1865
diff
changeset
|
633 |
} |
1837
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
634 |
} |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
635 |
|
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
636 |
/*****************************************************************************/ |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
637 |
|
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
638 |
/** SoE state: WRITE RESPONSE. |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
639 |
*/ |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
640 |
void ec_fsm_soe_write_response(ec_fsm_soe_t *fsm /**< finite state machine */) |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
641 |
{ |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
642 |
ec_datagram_t *datagram = fsm->datagram; |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
643 |
ec_slave_t *slave = fsm->slave; |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
644 |
ec_master_t *master = slave->master; |
1866
02323e72efc9
Fixed fragmented SoE write.
Florian Pose <fp@igh-essen.com>
parents:
1865
diff
changeset
|
645 |
ec_soe_request_t *req = fsm->request; |
1837
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
646 |
uint8_t *data, mbox_prot, opcode, error_flag; |
1873
219be3742274
Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents:
1866
diff
changeset
|
647 |
uint16_t idn; |
1837
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
648 |
size_t rec_size; |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
649 |
|
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
650 |
if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
651 |
return; // FIXME: request again? |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
652 |
|
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
653 |
if (datagram->state != EC_DATAGRAM_RECEIVED) { |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
654 |
fsm->state = ec_fsm_soe_error; |
1921 | 655 |
EC_SLAVE_ERR(slave, "Failed to receive SoE write" |
656 |
" response datagram: "); |
|
1837
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
657 |
ec_datagram_print_state(datagram); |
1881
f0047c6226c0
Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents:
1877
diff
changeset
|
658 |
ec_fsm_soe_print_error(fsm); |
1837
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
659 |
return; |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
660 |
} |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
661 |
|
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
662 |
if (datagram->working_counter != 1) { |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
663 |
fsm->state = ec_fsm_soe_error; |
1921 | 664 |
EC_SLAVE_ERR(slave, "Reception of SoE write response failed: "); |
1837
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
665 |
ec_datagram_print_wc_error(datagram); |
1881
f0047c6226c0
Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents:
1877
diff
changeset
|
666 |
ec_fsm_soe_print_error(fsm); |
1837
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
667 |
return; |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
668 |
} |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
669 |
|
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
670 |
data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot, &rec_size); |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
671 |
if (IS_ERR(data)) { |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
672 |
fsm->state = ec_fsm_soe_error; |
1881
f0047c6226c0
Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents:
1877
diff
changeset
|
673 |
ec_fsm_soe_print_error(fsm); |
1837
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
674 |
return; |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
675 |
} |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
676 |
|
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
677 |
if (master->debug_level) { |
1921 | 678 |
EC_SLAVE_DBG(slave, 0, "SCC write response:\n"); |
1837
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
679 |
ec_print_data(data, rec_size); |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
680 |
} |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
681 |
|
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
682 |
if (mbox_prot != EC_MBOX_TYPE_SOE) { |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
683 |
fsm->state = ec_fsm_soe_error; |
1921 | 684 |
EC_SLAVE_ERR(slave, "Received mailbox protocol 0x%02X as response.\n", |
685 |
mbox_prot); |
|
1881
f0047c6226c0
Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents:
1877
diff
changeset
|
686 |
ec_fsm_soe_print_error(fsm); |
1837
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
687 |
return; |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
688 |
} |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
689 |
|
1905
5abe24872083
Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents:
1882
diff
changeset
|
690 |
if (rec_size < EC_SOE_SIZE) { |
1837
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
691 |
fsm->state = ec_fsm_soe_error; |
1921 | 692 |
EC_SLAVE_ERR(slave, "Received currupted SoE write response" |
693 |
" (%zu bytes)!\n", rec_size); |
|
1873
219be3742274
Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents:
1866
diff
changeset
|
694 |
ec_print_data(data, rec_size); |
1881
f0047c6226c0
Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents:
1877
diff
changeset
|
695 |
ec_fsm_soe_print_error(fsm); |
1873
219be3742274
Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents:
1866
diff
changeset
|
696 |
return; |
219be3742274
Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents:
1866
diff
changeset
|
697 |
} |
219be3742274
Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents:
1866
diff
changeset
|
698 |
|
219be3742274
Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents:
1866
diff
changeset
|
699 |
opcode = EC_READ_U8(data) & 0x7; |
1905
5abe24872083
Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents:
1882
diff
changeset
|
700 |
if (opcode != OPCODE_WRITE_RESPONSE) { |
1921 | 701 |
EC_SLAVE_ERR(slave, "Received no write response" |
702 |
" (opcode %x).\n", opcode); |
|
1837
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
703 |
ec_print_data(data, rec_size); |
1881
f0047c6226c0
Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents:
1877
diff
changeset
|
704 |
ec_fsm_soe_print_error(fsm); |
1837
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
705 |
fsm->state = ec_fsm_soe_error; |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
706 |
return; |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
707 |
} |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
708 |
|
1873
219be3742274
Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents:
1866
diff
changeset
|
709 |
idn = EC_READ_U16(data + 2); |
219be3742274
Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents:
1866
diff
changeset
|
710 |
if (idn != req->idn) { |
1921 | 711 |
EC_SLAVE_ERR(slave, "Received response for" |
712 |
" wrong IDN 0x%04x.\n", idn); |
|
1873
219be3742274
Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents:
1866
diff
changeset
|
713 |
ec_print_data(data, rec_size); |
1881
f0047c6226c0
Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents:
1877
diff
changeset
|
714 |
ec_fsm_soe_print_error(fsm); |
1873
219be3742274
Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents:
1866
diff
changeset
|
715 |
fsm->state = ec_fsm_soe_error; |
219be3742274
Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents:
1866
diff
changeset
|
716 |
return; |
219be3742274
Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents:
1866
diff
changeset
|
717 |
} |
219be3742274
Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents:
1866
diff
changeset
|
718 |
|
219be3742274
Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents:
1866
diff
changeset
|
719 |
error_flag = (EC_READ_U8(data) >> 4) & 1; |
219be3742274
Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents:
1866
diff
changeset
|
720 |
if (error_flag) { |
1905
5abe24872083
Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents:
1882
diff
changeset
|
721 |
if (rec_size < EC_SOE_SIZE + 2) { |
1921 | 722 |
EC_SLAVE_ERR(slave, "Received corrupted error response" |
723 |
" - error flag set, but received size is %zu.\n", |
|
724 |
rec_size); |
|
1873
219be3742274
Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents:
1866
diff
changeset
|
725 |
} else { |
1905
5abe24872083
Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents:
1882
diff
changeset
|
726 |
req->error_code = EC_READ_U16(data + EC_SOE_SIZE); |
1921 | 727 |
EC_SLAVE_ERR(slave, "Received error response:\n"); |
728 |
ec_print_soe_error(slave, req->error_code); |
|
1873
219be3742274
Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents:
1866
diff
changeset
|
729 |
} |
219be3742274
Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents:
1866
diff
changeset
|
730 |
ec_print_data(data, rec_size); |
1881
f0047c6226c0
Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents:
1877
diff
changeset
|
731 |
ec_fsm_soe_print_error(fsm); |
1873
219be3742274
Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents:
1866
diff
changeset
|
732 |
fsm->state = ec_fsm_soe_error; |
219be3742274
Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents:
1866
diff
changeset
|
733 |
return; |
219be3742274
Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents:
1866
diff
changeset
|
734 |
} else { |
219be3742274
Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents:
1866
diff
changeset
|
735 |
req->error_code = 0x0000; |
219be3742274
Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents:
1866
diff
changeset
|
736 |
} |
1837
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
737 |
|
1866
02323e72efc9
Fixed fragmented SoE write.
Florian Pose <fp@igh-essen.com>
parents:
1865
diff
changeset
|
738 |
fsm->state = ec_fsm_soe_end; // success |
1837
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
739 |
} |
32136215c1fa
Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents:
1831
diff
changeset
|
740 |
|
1831
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
741 |
/*****************************************************************************/ |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
742 |
|
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
743 |
/** State: ERROR. |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
744 |
*/ |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
745 |
void ec_fsm_soe_error(ec_fsm_soe_t *fsm /**< finite state machine */) |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
746 |
{ |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
747 |
} |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
748 |
|
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
749 |
/*****************************************************************************/ |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
750 |
|
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
751 |
/** State: END. |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
752 |
*/ |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
753 |
void ec_fsm_soe_end(ec_fsm_soe_t *fsm /**< finite state machine */) |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
754 |
{ |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
755 |
} |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
756 |
|
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff
changeset
|
757 |
/*****************************************************************************/ |