35 /*****************************************************************************/ |
35 /*****************************************************************************/ |
36 |
36 |
37 #include <linux/slab.h> |
37 #include <linux/slab.h> |
38 #include <linux/delay.h> |
38 #include <linux/delay.h> |
39 |
39 |
|
40 #include "slave.h" |
40 #include "mailbox.h" |
41 #include "mailbox.h" |
41 #include "datagram.h" |
42 #include "datagram.h" |
42 #include "master.h" |
43 #include "master.h" |
43 |
44 |
|
45 |
|
46 /*****************************************************************************/ |
|
47 |
|
48 /** |
|
49 Mailbox constructor. |
|
50 */ |
|
51 |
|
52 void ec_mbox_init(ec_mailbox_t* mbox, /** mailbox */ |
|
53 ec_datagram_t* datagram /**< Datagram used for the mailbox content. */ |
|
54 ) |
|
55 { |
|
56 mbox->datagram = datagram; |
|
57 #ifdef EC_REDUCE_MBOXFRAMESIZE |
|
58 ec_datagram_init(&mbox->end_datagram); |
|
59 #endif |
|
60 } |
|
61 |
|
62 |
|
63 /*****************************************************************************/ |
|
64 |
|
65 /** |
|
66 Clears mailbox datagrams. |
|
67 */ |
|
68 |
|
69 void ec_mbox_clear(ec_mailbox_t* mbox /** mailbox */ |
|
70 ) |
|
71 { |
|
72 #ifdef EC_REDUCE_MBOXFRAMESIZE |
|
73 ec_datagram_clear(&mbox->end_datagram); |
|
74 #endif |
|
75 } |
|
76 |
|
77 |
|
78 /*****************************************************************************/ |
|
79 |
|
80 /** |
|
81 Queues the slave datagrams. |
|
82 */ |
|
83 |
|
84 void ec_slave_mbox_queue_datagrams(const ec_slave_t* slave, /** slave */ |
|
85 ec_mailbox_t* mbox /** mailbox */ |
|
86 ) |
|
87 { |
|
88 ec_master_queue_request_fsm_datagram(slave->master, mbox->datagram); |
|
89 #ifdef EC_REDUCE_MBOXFRAMESIZE |
|
90 if (mbox->end_datagram.type != EC_DATAGRAM_NONE) |
|
91 { |
|
92 ec_master_queue_request_fsm_datagram(slave->master, &mbox->end_datagram); |
|
93 } |
|
94 #endif |
|
95 } |
|
96 |
|
97 |
|
98 /*****************************************************************************/ |
|
99 |
|
100 /** |
|
101 Queues the datagrams. |
|
102 */ |
|
103 |
|
104 void ec_master_mbox_queue_datagrams(ec_master_t* master, /** master */ |
|
105 ec_mailbox_t* mbox /** mailbox */ |
|
106 ) |
|
107 { |
|
108 ec_master_queue_fsm_datagram(master, mbox->datagram); |
|
109 #ifdef EC_REDUCE_MBOXFRAMESIZE |
|
110 if (mbox->end_datagram.type != EC_DATAGRAM_NONE) |
|
111 { |
|
112 ec_master_queue_fsm_datagram(master, &mbox->end_datagram); |
|
113 } |
|
114 #endif |
|
115 } |
|
116 |
|
117 |
44 /*****************************************************************************/ |
118 /*****************************************************************************/ |
45 |
119 |
46 /** |
120 /** |
47 Prepares a mailbox-send datagram. |
121 Prepares a mailbox-send datagram. |
48 \return Pointer to mailbox datagram data, or ERR_PTR() code. |
122 \return Pointer to mailbox datagram data, or ERR_PTR() code. |
49 */ |
123 */ |
50 |
124 |
51 uint8_t *ec_slave_mbox_prepare_send(const ec_slave_t *slave, /**< slave */ |
125 uint8_t *ec_slave_mbox_prepare_send(const ec_slave_t* slave, /** slave */ |
52 ec_datagram_t *datagram, /**< datagram */ |
126 ec_mailbox_t* mbox, /** mailbox */ |
53 uint8_t type, /**< mailbox protocol */ |
127 uint8_t type, /**< mailbox protocol */ |
54 size_t size /**< size of the data */ |
128 size_t size /**< size of the data */ |
55 ) |
129 ) |
56 { |
130 { |
|
131 ec_datagram_t* datagram = mbox->datagram; |
57 size_t total_size; |
132 size_t total_size; |
58 int ret; |
133 int ret; |
59 |
134 |
60 if (unlikely(!slave->sii.mailbox_protocols)) { |
135 if (unlikely(!slave->sii.mailbox_protocols)) { |
61 EC_SLAVE_ERR(slave, "Slave does not support mailbox" |
136 EC_SLAVE_ERR(slave, "Slave does not support mailbox" |
70 total_size, slave->configured_rx_mailbox_size); |
145 total_size, slave->configured_rx_mailbox_size); |
71 return ERR_PTR(-EOVERFLOW); |
146 return ERR_PTR(-EOVERFLOW); |
72 } |
147 } |
73 |
148 |
74 ret = ec_datagram_fpwr(datagram, slave->station_address, |
149 ret = ec_datagram_fpwr(datagram, slave->station_address, |
75 slave->configured_rx_mailbox_offset, |
150 slave->configured_rx_mailbox_offset, |
76 slave->configured_rx_mailbox_size); |
151 #ifdef EC_REDUCE_MBOXFRAMESIZE |
|
152 total_size |
|
153 #else |
|
154 slave->configured_rx_mailbox_size |
|
155 #endif |
|
156 ); |
77 if (ret) |
157 if (ret) |
78 return ERR_PTR(ret); |
158 return ERR_PTR(ret); |
79 |
159 |
80 EC_WRITE_U16(datagram->data, size); // mailbox service data length |
160 EC_WRITE_U16(datagram->data, size); // mailbox service data length |
81 EC_WRITE_U16(datagram->data + 2, slave->station_address); // station addr. |
161 EC_WRITE_U16(datagram->data + 2, slave->station_address); // station addr. |
82 EC_WRITE_U8 (datagram->data + 4, 0x00); // channel & priority |
162 EC_WRITE_U8 (datagram->data + 4, 0x00); // channel & priority |
83 EC_WRITE_U8 (datagram->data + 5, type); // underlying protocol type |
163 EC_WRITE_U8 (datagram->data + 5, type); // underlying protocol type |
84 |
164 |
|
165 #ifdef EC_REDUCE_MBOXFRAMESIZE |
|
166 /* in order to fulfil the ESC's mailbox protocol, |
|
167 at least the last byte of the mailbox must be written */ |
|
168 if (total_size < slave->configured_rx_mailbox_size) { |
|
169 ret = ec_datagram_fpwr(&mbox->end_datagram, slave->station_address, |
|
170 slave->configured_rx_mailbox_offset+slave->configured_rx_mailbox_size-1, |
|
171 1); |
|
172 if (ret) |
|
173 return ERR_PTR(ret); |
|
174 } |
|
175 #endif |
85 return datagram->data + EC_MBOX_HEADER_SIZE; |
176 return datagram->data + EC_MBOX_HEADER_SIZE; |
86 } |
177 } |
87 |
178 |
88 /*****************************************************************************/ |
179 /*****************************************************************************/ |
89 |
180 |
91 Prepares a datagram for checking the mailbox state. |
182 Prepares a datagram for checking the mailbox state. |
92 \todo Determine sync manager used for receive mailbox |
183 \todo Determine sync manager used for receive mailbox |
93 \return 0 in case of success, else < 0 |
184 \return 0 in case of success, else < 0 |
94 */ |
185 */ |
95 |
186 |
96 int ec_slave_mbox_prepare_check(const ec_slave_t *slave, /**< slave */ |
187 int ec_slave_mbox_prepare_check(const ec_slave_t* slave, /** slave */ |
97 ec_datagram_t *datagram /**< datagram */ |
188 ec_mailbox_t* mbox /** mailbox */ |
98 ) |
189 ) |
99 { |
190 { |
|
191 ec_datagram_t* datagram = mbox->datagram; |
100 int ret = ec_datagram_fprd(datagram, slave->station_address, 0x808, 8); |
192 int ret = ec_datagram_fprd(datagram, slave->station_address, 0x808, 8); |
101 if (ret) |
193 if (ret) |
102 return ret; |
194 return ret; |
103 |
195 |
104 ec_datagram_zero(datagram); |
196 ec_datagram_zero(datagram); |
|
197 #ifdef EC_REDUCE_MBOXFRAMESIZE |
|
198 mbox->end_datagram.type = EC_DATAGRAM_NONE; |
|
199 #endif |
105 return 0; |
200 return 0; |
106 } |
201 } |
107 |
202 |
108 /*****************************************************************************/ |
203 /*****************************************************************************/ |
109 |
204 |
110 /** |
205 /** |
111 Processes a mailbox state checking datagram. |
206 Processes a mailbox state checking datagram. |
112 \return 0 in case of success, else < 0 |
207 \return 0 in case of success, else < 0 |
113 */ |
208 */ |
114 |
209 |
115 int ec_slave_mbox_check(const ec_datagram_t *datagram /**< datagram */) |
210 int ec_slave_mbox_check(ec_mailbox_t* mbox /** mailbox */) |
116 { |
211 { |
117 return EC_READ_U8(datagram->data + 5) & 8 ? 1 : 0; |
212 return EC_READ_U8(mbox->datagram->data + 5) & 8 ? 1 : 0; |
118 } |
213 } |
119 |
214 |
120 /*****************************************************************************/ |
215 /*****************************************************************************/ |
121 |
216 |
122 /** |
217 /** |
123 Prepares a datagram to fetch mailbox data. |
218 Prepares a datagram to fetch mailbox data. |
124 \return 0 in case of success, else < 0 |
219 \return 0 in case of success, else < 0 |
125 */ |
220 */ |
126 |
221 |
127 int ec_slave_mbox_prepare_fetch(const ec_slave_t *slave, /**< slave */ |
222 int ec_slave_mbox_prepare_fetch(const ec_slave_t* slave, /** slave */ |
128 ec_datagram_t *datagram /**< datagram */ |
223 ec_mailbox_t* mbox /** mailbox */ |
129 ) |
224 ) |
130 { |
225 { |
|
226 ec_datagram_t* datagram = mbox->datagram; |
131 int ret = ec_datagram_fprd(datagram, slave->station_address, |
227 int ret = ec_datagram_fprd(datagram, slave->station_address, |
132 slave->configured_tx_mailbox_offset, |
228 slave->configured_tx_mailbox_offset, |
133 slave->configured_tx_mailbox_size); |
229 slave->configured_tx_mailbox_size); |
134 if (ret) |
230 if (ret) |
135 return ret; |
231 return ret; |
136 |
232 |
137 ec_datagram_zero(datagram); |
233 ec_datagram_zero(datagram); |
|
234 #ifdef EC_REDUCE_MBOXFRAMESIZE |
|
235 mbox->end_datagram.type = EC_DATAGRAM_NONE; |
|
236 #endif |
138 return 0; |
237 return 0; |
139 } |
238 } |
140 |
239 |
141 /*****************************************************************************/ |
240 /*****************************************************************************/ |
142 |
241 |
160 |
259 |
161 /** Processes received mailbox data. |
260 /** Processes received mailbox data. |
162 * |
261 * |
163 * \return Pointer to the received data, or ERR_PTR() code. |
262 * \return Pointer to the received data, or ERR_PTR() code. |
164 */ |
263 */ |
165 uint8_t *ec_slave_mbox_fetch(const ec_slave_t *slave, /**< slave */ |
264 uint8_t *ec_slave_mbox_fetch(const ec_slave_t* slave, /** slave */ |
166 ec_datagram_t *datagram, /**< datagram */ |
265 ec_mailbox_t* mbox, /** mailbox */ |
167 uint8_t *type, /**< expected mailbox protocol */ |
266 uint8_t *type, /**< expected mailbox protocol */ |
168 size_t *size /**< size of the received data */ |
267 size_t *size /**< size of the received data */ |
169 ) |
268 ) |
170 { |
269 { |
|
270 ec_datagram_t* datagram = mbox->datagram; |
171 size_t data_size; |
271 size_t data_size; |
172 |
272 |
173 data_size = EC_READ_U16(datagram->data); |
273 data_size = EC_READ_U16(datagram->data); |
174 |
274 |
175 if (data_size + EC_MBOX_HEADER_SIZE > slave->configured_tx_mailbox_size) { |
275 if (data_size + EC_MBOX_HEADER_SIZE > slave->configured_tx_mailbox_size) { |