40 |
40 |
41 #include <linux/slab.h> |
41 #include <linux/slab.h> |
42 #include <linux/delay.h> |
42 #include <linux/delay.h> |
43 |
43 |
44 #include "mailbox.h" |
44 #include "mailbox.h" |
45 #include "command.h" |
45 #include "datagram.h" |
46 #include "master.h" |
46 #include "master.h" |
47 |
47 |
48 /*****************************************************************************/ |
48 /*****************************************************************************/ |
49 |
49 |
50 /** |
50 /** |
51 Prepares a mailbox-send command. |
51 Prepares a mailbox-send datagram. |
52 \return pointer to mailbox command data |
52 \return pointer to mailbox datagram data |
53 */ |
53 */ |
54 |
54 |
55 uint8_t *ec_slave_mbox_prepare_send(const ec_slave_t *slave, /**< slave */ |
55 uint8_t *ec_slave_mbox_prepare_send(const ec_slave_t *slave, /**< slave */ |
56 ec_command_t *command, /**< command */ |
56 ec_datagram_t *datagram, /**< datagram */ |
57 uint8_t type, /**< mailbox protocol */ |
57 uint8_t type, /**< mailbox protocol */ |
58 size_t size /**< size of the data */ |
58 size_t size /**< size of the data */ |
59 ) |
59 ) |
60 { |
60 { |
61 size_t total_size; |
61 size_t total_size; |
70 if (unlikely(total_size > slave->sii_rx_mailbox_size)) { |
70 if (unlikely(total_size > slave->sii_rx_mailbox_size)) { |
71 EC_ERR("Data size does not fit in mailbox!\n"); |
71 EC_ERR("Data size does not fit in mailbox!\n"); |
72 return NULL; |
72 return NULL; |
73 } |
73 } |
74 |
74 |
75 if (ec_command_npwr(command, slave->station_address, |
75 if (ec_datagram_npwr(datagram, slave->station_address, |
76 slave->sii_rx_mailbox_offset, |
76 slave->sii_rx_mailbox_offset, |
77 slave->sii_rx_mailbox_size)) |
77 slave->sii_rx_mailbox_size)) |
78 return NULL; |
78 return NULL; |
79 |
79 |
80 EC_WRITE_U16(command->data, size); // mailbox service data length |
80 EC_WRITE_U16(datagram->data, size); // mailbox service data length |
81 EC_WRITE_U16(command->data + 2, slave->station_address); // station address |
81 EC_WRITE_U16(datagram->data + 2, slave->station_address); // station addr. |
82 EC_WRITE_U8 (command->data + 4, 0x00); // channel & priority |
82 EC_WRITE_U8 (datagram->data + 4, 0x00); // channel & priority |
83 EC_WRITE_U8 (command->data + 5, type); // underlying protocol type |
83 EC_WRITE_U8 (datagram->data + 5, type); // underlying protocol type |
84 |
84 |
85 return command->data + 6; |
85 return datagram->data + 6; |
86 } |
86 } |
87 |
87 |
88 /*****************************************************************************/ |
88 /*****************************************************************************/ |
89 |
89 |
90 /** |
90 /** |
91 Prepares a command for checking the mailbox state. |
91 Prepares a datagram for checking the mailbox state. |
92 \return 0 in case of success, else < 0 |
92 \return 0 in case of success, else < 0 |
93 */ |
93 */ |
94 |
94 |
95 int ec_slave_mbox_prepare_check(const ec_slave_t *slave, /**< slave */ |
95 int ec_slave_mbox_prepare_check(const ec_slave_t *slave, /**< slave */ |
96 ec_command_t *command /**< command */ |
96 ec_datagram_t *datagram /**< datagram */ |
97 ) |
97 ) |
98 { |
98 { |
99 // FIXME: second sync manager? |
99 // FIXME: second sync manager? |
100 if (ec_command_nprd(command, slave->station_address, 0x808, 8)) |
100 if (ec_datagram_nprd(datagram, slave->station_address, 0x808, 8)) |
101 return -1; |
101 return -1; |
102 |
102 |
103 return 0; |
103 return 0; |
104 } |
104 } |
105 |
105 |
106 /*****************************************************************************/ |
106 /*****************************************************************************/ |
107 |
107 |
108 /** |
108 /** |
109 Processes a mailbox state checking command. |
109 Processes a mailbox state checking datagram. |
110 \return 0 in case of success, else < 0 |
110 \return 0 in case of success, else < 0 |
111 */ |
111 */ |
112 |
112 |
113 int ec_slave_mbox_check(const ec_command_t *command /**< command */) |
113 int ec_slave_mbox_check(const ec_datagram_t *datagram /**< datagram */) |
114 { |
114 { |
115 return EC_READ_U8(command->data + 5) & 8 ? 1 : 0; |
115 return EC_READ_U8(datagram->data + 5) & 8 ? 1 : 0; |
116 } |
116 } |
117 |
117 |
118 /*****************************************************************************/ |
118 /*****************************************************************************/ |
119 |
119 |
120 /** |
120 /** |
121 Prepares a command to fetch mailbox data. |
121 Prepares a datagram to fetch mailbox data. |
122 \return 0 in case of success, else < 0 |
122 \return 0 in case of success, else < 0 |
123 */ |
123 */ |
124 |
124 |
125 int ec_slave_mbox_prepare_fetch(const ec_slave_t *slave, /**< slave */ |
125 int ec_slave_mbox_prepare_fetch(const ec_slave_t *slave, /**< slave */ |
126 ec_command_t *command /**< command */ |
126 ec_datagram_t *datagram /**< datagram */ |
127 ) |
127 ) |
128 { |
128 { |
129 if (ec_command_nprd(command, slave->station_address, |
129 if (ec_datagram_nprd(datagram, slave->station_address, |
130 slave->sii_tx_mailbox_offset, |
130 slave->sii_tx_mailbox_offset, |
131 slave->sii_tx_mailbox_size)) return -1; |
131 slave->sii_tx_mailbox_size)) return -1; |
132 return 0; |
132 return 0; |
133 } |
133 } |
134 |
134 |
135 /*****************************************************************************/ |
135 /*****************************************************************************/ |
136 |
136 |
138 Processes received mailbox data. |
138 Processes received mailbox data. |
139 \return pointer to the received data |
139 \return pointer to the received data |
140 */ |
140 */ |
141 |
141 |
142 uint8_t *ec_slave_mbox_fetch(const ec_slave_t *slave, /**< slave */ |
142 uint8_t *ec_slave_mbox_fetch(const ec_slave_t *slave, /**< slave */ |
143 ec_command_t *command, /**< command */ |
143 ec_datagram_t *datagram, /**< datagram */ |
144 uint8_t type, /**< expected mailbox protocol */ |
144 uint8_t type, /**< expected mailbox protocol */ |
145 size_t *size /**< size of the received data */ |
145 size_t *size /**< size of the received data */ |
146 ) |
146 ) |
147 { |
147 { |
148 size_t data_size; |
148 size_t data_size; |
149 |
149 |
150 if ((EC_READ_U8(command->data + 5) & 0x0F) != type) { |
150 if ((EC_READ_U8(datagram->data + 5) & 0x0F) != type) { |
151 EC_ERR("Unexpected mailbox protocol 0x%02X (exp.: 0x%02X) at" |
151 EC_ERR("Unexpected mailbox protocol 0x%02X (exp.: 0x%02X) at" |
152 " slave %i!\n", EC_READ_U8(command->data + 5), type, |
152 " slave %i!\n", EC_READ_U8(datagram->data + 5), type, |
153 slave->ring_position); |
153 slave->ring_position); |
154 return NULL; |
154 return NULL; |
155 } |
155 } |
156 |
156 |
157 if ((data_size = EC_READ_U16(command->data)) > |
157 if ((data_size = EC_READ_U16(datagram->data)) > |
158 slave->sii_tx_mailbox_size - 6) { |
158 slave->sii_tx_mailbox_size - 6) { |
159 EC_ERR("Currupt mailbox response detected!\n"); |
159 EC_ERR("Currupt mailbox response detected!\n"); |
160 return NULL; |
160 return NULL; |
161 } |
161 } |
162 |
162 |
163 *size = data_size; |
163 *size = data_size; |
164 return command->data + 6; |
164 return datagram->data + 6; |
165 } |
165 } |
166 |
166 |
167 /*****************************************************************************/ |
167 /*****************************************************************************/ |
168 |
168 |
169 /** |
169 /** |
170 Sends a mailbox command and waits for its reception. |
170 Sends a mailbox datagram and waits for its reception. |
171 \return pointer to the received data |
171 \return pointer to the received data |
172 */ |
172 */ |
173 |
173 |
174 uint8_t *ec_slave_mbox_simple_io(const ec_slave_t *slave, /**< slave */ |
174 uint8_t *ec_slave_mbox_simple_io(const ec_slave_t *slave, /**< slave */ |
175 ec_command_t *command, /**< command */ |
175 ec_datagram_t *datagram, /**< datagram */ |
176 size_t *size /**< size of the received data */ |
176 size_t *size /**< size of the received data */ |
177 ) |
177 ) |
178 { |
178 { |
179 uint8_t type; |
179 uint8_t type; |
180 |
180 |
181 type = EC_READ_U8(command->data + 5); |
181 type = EC_READ_U8(datagram->data + 5); |
182 |
182 |
183 if (unlikely(ec_master_simple_io(slave->master, command))) { |
183 if (unlikely(ec_master_simple_io(slave->master, datagram))) { |
184 EC_ERR("Mailbox checking failed on slave %i!\n", |
184 EC_ERR("Mailbox checking failed on slave %i!\n", |
185 slave->ring_position); |
185 slave->ring_position); |
186 return NULL; |
186 return NULL; |
187 } |
187 } |
188 |
188 |
189 return ec_slave_mbox_simple_receive(slave, command, type, size); |
189 return ec_slave_mbox_simple_receive(slave, datagram, type, size); |
190 } |
190 } |
191 |
191 |
192 /*****************************************************************************/ |
192 /*****************************************************************************/ |
193 |
193 |
194 /** |
194 /** |
195 Waits for the reception of a mailbox command. |
195 Waits for the reception of a mailbox datagram. |
196 \return pointer to the received data |
196 \return pointer to the received data |
197 */ |
197 */ |
198 |
198 |
199 uint8_t *ec_slave_mbox_simple_receive(const ec_slave_t *slave, /**< slave */ |
199 uint8_t *ec_slave_mbox_simple_receive(const ec_slave_t *slave, /**< slave */ |
200 ec_command_t *command, /**< command */ |
200 ec_datagram_t *datagram, /**< datagram */ |
201 uint8_t type, /**< expected protocol */ |
201 uint8_t type, /**< expected protocol */ |
202 size_t *size /**< received data size */ |
202 size_t *size /**< received data size */ |
203 ) |
203 ) |
204 { |
204 { |
205 cycles_t start, end, timeout; |
205 cycles_t start, end, timeout; |
207 start = get_cycles(); |
207 start = get_cycles(); |
208 timeout = (cycles_t) 100 * cpu_khz; // 100ms |
208 timeout = (cycles_t) 100 * cpu_khz; // 100ms |
209 |
209 |
210 while (1) |
210 while (1) |
211 { |
211 { |
212 if (ec_slave_mbox_prepare_check(slave, command)) return NULL; |
212 if (ec_slave_mbox_prepare_check(slave, datagram)) return NULL; |
213 if (unlikely(ec_master_simple_io(slave->master, command))) { |
213 if (unlikely(ec_master_simple_io(slave->master, datagram))) { |
214 EC_ERR("Mailbox checking failed on slave %i!\n", |
214 EC_ERR("Mailbox checking failed on slave %i!\n", |
215 slave->ring_position); |
215 slave->ring_position); |
216 return NULL; |
216 return NULL; |
217 } |
217 } |
218 |
218 |
219 end = get_cycles(); |
219 end = get_cycles(); |
220 |
220 |
221 if (ec_slave_mbox_check(command)) |
221 if (ec_slave_mbox_check(datagram)) |
222 break; // proceed with receiving data |
222 break; // proceed with receiving data |
223 |
223 |
224 if ((end - start) >= timeout) { |
224 if ((end - start) >= timeout) { |
225 EC_ERR("Mailbox check - Slave %i timed out.\n", |
225 EC_ERR("Mailbox check - Slave %i timed out.\n", |
226 slave->ring_position); |
226 slave->ring_position); |
228 } |
228 } |
229 |
229 |
230 udelay(100); |
230 udelay(100); |
231 } |
231 } |
232 |
232 |
233 if (ec_slave_mbox_prepare_fetch(slave, command)) return NULL; |
233 if (ec_slave_mbox_prepare_fetch(slave, datagram)) return NULL; |
234 if (unlikely(ec_master_simple_io(slave->master, command))) { |
234 if (unlikely(ec_master_simple_io(slave->master, datagram))) { |
235 EC_ERR("Mailbox receiving failed on slave %i!\n", |
235 EC_ERR("Mailbox receiving failed on slave %i!\n", |
236 slave->ring_position); |
236 slave->ring_position); |
237 return NULL; |
237 return NULL; |
238 } |
238 } |
239 |
239 |
240 if (unlikely(slave->master->debug_level) > 1) |
240 if (unlikely(slave->master->debug_level) > 1) |
241 EC_DBG("Mailbox receive took %ius.\n", ((u32) (end - start) * 1000 |
241 EC_DBG("Mailbox receive took %ius.\n", |
242 / cpu_khz)); |
242 ((unsigned int) (end - start) * 1000 / cpu_khz)); |
243 |
243 |
244 return ec_slave_mbox_fetch(slave, command, type, size); |
244 return ec_slave_mbox_fetch(slave, datagram, type, size); |
245 } |
245 } |
246 |
246 |
247 /*****************************************************************************/ |
247 /*****************************************************************************/ |