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