32 EtherCAT datagram pair methods. |
32 EtherCAT datagram pair methods. |
33 */ |
33 */ |
34 |
34 |
35 /*****************************************************************************/ |
35 /*****************************************************************************/ |
36 |
36 |
|
37 #include <linux/slab.h> |
|
38 |
|
39 #include "master.h" |
37 #include "datagram_pair.h" |
40 #include "datagram_pair.h" |
38 |
41 |
39 /*****************************************************************************/ |
42 /*****************************************************************************/ |
40 |
43 |
41 /** Datagram pair constructor. |
44 /** Datagram pair constructor. |
42 */ |
45 */ |
43 void ec_datagram_pair_init( |
46 int ec_datagram_pair_init( |
44 ec_datagram_pair_t *pair /**< Datagram pair. */ |
47 ec_datagram_pair_t *pair, /**< Datagram pair. */ |
|
48 ec_domain_t *domain, /**< Parent domain. */ |
|
49 uint32_t logical_offset, |
|
50 uint8_t *data, |
|
51 size_t data_size, /**< Data size. */ |
|
52 const unsigned int used[] /**< input/output use count. */ |
45 ) |
53 ) |
46 { |
54 { |
47 unsigned int i; |
55 unsigned int dev_idx; |
|
56 int ret; |
48 |
57 |
49 INIT_LIST_HEAD(&pair->list); |
58 INIT_LIST_HEAD(&pair->list); |
|
59 pair->domain = domain; |
50 |
60 |
51 for (i = 0; i < EC_NUM_DEVICES; i++) { |
61 for (dev_idx = 0; dev_idx < EC_NUM_DEVICES; dev_idx++) { |
52 ec_datagram_init(&pair->datagrams[i]); |
62 ec_datagram_init(&pair->datagrams[dev_idx]); |
|
63 snprintf(pair->datagrams[dev_idx].name, |
|
64 EC_DATAGRAM_NAME_SIZE, "domain%u-%u-%s", domain->index, |
|
65 logical_offset, dev_idx ? "backup" : "main"); |
53 } |
66 } |
|
67 |
|
68 pair->expected_working_counter = 0U; |
|
69 |
|
70 /* backup datagram has its own memory */ |
|
71 ret = ec_datagram_prealloc(&pair->datagrams[EC_DEVICE_BACKUP], |
|
72 data_size); |
|
73 if (ret) { |
|
74 goto out_datagrams; |
|
75 } |
|
76 |
|
77 if (!(pair->send_buffer = kmalloc(data_size, GFP_KERNEL))) { |
|
78 EC_MASTER_ERR(domain->master, |
|
79 "Failed to allocate domain send buffer!\n"); |
|
80 ret = -ENOMEM; |
|
81 goto out_datagrams; |
|
82 } |
|
83 |
|
84 /* The ec_datagram_lxx() calls below can not fail, because either the |
|
85 * datagram has external memory or it is preallocated. */ |
|
86 |
|
87 if (used[EC_DIR_OUTPUT] && used[EC_DIR_INPUT]) { // inputs and outputs |
|
88 ec_datagram_lrw_ext(&pair->datagrams[EC_DEVICE_MAIN], |
|
89 logical_offset, data_size, data); |
|
90 ec_datagram_lrw(&pair->datagrams[EC_DEVICE_BACKUP], |
|
91 logical_offset, data_size); |
|
92 |
|
93 // If LRW is used, output FMMUs increment the working counter by 2, |
|
94 // while input FMMUs increment it by 1. |
|
95 pair->expected_working_counter = |
|
96 used[EC_DIR_OUTPUT] * 2 + used[EC_DIR_INPUT]; |
|
97 } else if (used[EC_DIR_OUTPUT]) { // outputs only |
|
98 ec_datagram_lwr_ext(&pair->datagrams[EC_DEVICE_MAIN], |
|
99 logical_offset, data_size, data); |
|
100 ec_datagram_lwr(&pair->datagrams[EC_DEVICE_BACKUP], |
|
101 logical_offset, data_size); |
|
102 |
|
103 pair->expected_working_counter = used[EC_DIR_OUTPUT]; |
|
104 } else { // inputs only (or nothing) |
|
105 ec_datagram_lrd_ext(&pair->datagrams[EC_DEVICE_MAIN], |
|
106 logical_offset, data_size, data); |
|
107 ec_datagram_lrd(&pair->datagrams[EC_DEVICE_BACKUP], |
|
108 logical_offset, data_size); |
|
109 |
|
110 pair->expected_working_counter = used[EC_DIR_INPUT]; |
|
111 } |
|
112 |
|
113 for (dev_idx = 0; dev_idx < EC_NUM_DEVICES; dev_idx++) { |
|
114 ec_datagram_zero(&pair->datagrams[dev_idx]); |
|
115 } |
|
116 |
|
117 return 0; |
|
118 |
|
119 out_datagrams: |
|
120 for (dev_idx = 0; dev_idx < EC_NUM_DEVICES; dev_idx++) { |
|
121 ec_datagram_clear(&pair->datagrams[dev_idx]); |
|
122 } |
|
123 |
|
124 return ret; |
54 } |
125 } |
55 |
126 |
56 /*****************************************************************************/ |
127 /*****************************************************************************/ |
57 |
128 |
58 /** Datagram pair destructor. |
129 /** Datagram pair destructor. |
59 */ |
130 */ |
60 void ec_datagram_pair_clear( |
131 void ec_datagram_pair_clear( |
61 ec_datagram_pair_t *pair /**< Datagram pair. */ |
132 ec_datagram_pair_t *pair /**< Datagram pair. */ |
62 ) |
133 ) |
63 { |
134 { |
64 unsigned int i; |
135 unsigned int dev_idx; |
65 |
136 |
66 for (i = 0; i < EC_NUM_DEVICES; i++) { |
137 for (dev_idx = 0; dev_idx < EC_NUM_DEVICES; dev_idx++) { |
67 ec_datagram_clear(&pair->datagrams[i]); |
138 ec_datagram_clear(&pair->datagrams[dev_idx]); |
|
139 } |
|
140 |
|
141 if (pair->send_buffer) { |
|
142 kfree(pair->send_buffer); |
68 } |
143 } |
69 } |
144 } |
70 |
145 |
71 /*****************************************************************************/ |
146 /*****************************************************************************/ |
|
147 |
|
148 /** Process received data. |
|
149 */ |
|
150 unsigned int ec_datagram_pair_process( |
|
151 ec_datagram_pair_t *pair /**< Datagram pair. */ |
|
152 ) |
|
153 { |
|
154 unsigned int dev_idx, wc_sum = 0; |
|
155 |
|
156 for (dev_idx = 0; dev_idx < EC_NUM_DEVICES; dev_idx++) { |
|
157 ec_datagram_t *datagram = &pair->datagrams[dev_idx]; |
|
158 |
|
159 ec_datagram_output_stats(datagram); |
|
160 |
|
161 if (datagram->state == EC_DATAGRAM_RECEIVED) { |
|
162 wc_sum += datagram->working_counter; |
|
163 } |
|
164 } |
|
165 |
|
166 return wc_sum; |
|
167 } |
|
168 |
|
169 /*****************************************************************************/ |
|
170 |
|
171 /** Process received data. |
|
172 */ |
|
173 int ec_datagram_pair_data_changed( |
|
174 const ec_datagram_pair_t *pair, |
|
175 size_t offset, |
|
176 size_t size, |
|
177 ec_device_index_t dev_idx |
|
178 ) |
|
179 { |
|
180 uint8_t *sent = pair->send_buffer + offset; |
|
181 uint8_t *recv = pair->datagrams[dev_idx].data + offset; |
|
182 size_t i; |
|
183 |
|
184 for (i = 0; i < size; i++) { |
|
185 if (recv[i] != sent[i]) { |
|
186 return 1; |
|
187 } |
|
188 } |
|
189 |
|
190 return 0; |
|
191 } |
|
192 |
|
193 /*****************************************************************************/ |