43 |
43 |
44 /** Mailbox type for SoE. |
44 /** Mailbox type for SoE. |
45 */ |
45 */ |
46 #define EC_MBOX_TYPE_SOE 0x05 |
46 #define EC_MBOX_TYPE_SOE 0x05 |
47 |
47 |
48 #define EC_SOE_OPCODE_READ_REQUEST 0x01 |
48 /** SoE operations |
49 #define EC_SOE_OPCODE_READ_RESPONSE 0x02 |
49 */ |
50 #define EC_SOE_OPCODE_WRITE_REQUEST 0x03 |
50 enum ec_soe_opcodes { |
51 #define EC_SOE_OPCODE_WRITE_RESPONSE 0x04 |
51 OPCODE_READ_REQUEST = 0x01, /**< Read request. */ |
52 |
52 OPCODE_READ_RESPONSE = 0x02, /**< Read response. */ |
53 #define EC_SOE_READ_REQUEST_SIZE 0x04 |
53 OPCODE_WRITE_REQUEST = 0x03, /**< Write request. */ |
54 #define EC_SOE_READ_RESPONSE_SIZE 0x04 |
54 OPCODE_WRITE_RESPONSE = 0x04 /**< Write response. */ |
55 #define EC_SOE_WRITE_REQUEST_SIZE 0x04 |
55 }; |
56 #define EC_SOE_WRITE_RESPONSE_SIZE 0x04 |
56 |
57 |
57 /** Size of all SoE headers. |
|
58 */ |
|
59 #define EC_SOE_SIZE 0x04 |
|
60 |
|
61 /** SoE response timeout [ms]. |
|
62 */ |
58 #define EC_SOE_RESPONSE_TIMEOUT 1000 |
63 #define EC_SOE_RESPONSE_TIMEOUT 1000 |
59 |
64 |
60 /*****************************************************************************/ |
65 /*****************************************************************************/ |
61 |
66 |
62 void ec_fsm_soe_read_start(ec_fsm_soe_t *); |
67 void ec_fsm_soe_read_start(ec_fsm_soe_t *); |
72 void ec_fsm_soe_end(ec_fsm_soe_t *); |
77 void ec_fsm_soe_end(ec_fsm_soe_t *); |
73 void ec_fsm_soe_error(ec_fsm_soe_t *); |
78 void ec_fsm_soe_error(ec_fsm_soe_t *); |
74 |
79 |
75 /*****************************************************************************/ |
80 /*****************************************************************************/ |
76 |
81 |
|
82 extern const ec_code_msg_t soe_error_codes[]; |
|
83 |
|
84 /*****************************************************************************/ |
|
85 |
|
86 /** Outputs an SoE error code. |
|
87 */ |
|
88 void ec_print_soe_error(uint16_t error_code) |
|
89 { |
|
90 const ec_code_msg_t *error_msg; |
|
91 |
|
92 for (error_msg = soe_error_codes; error_msg->code; error_msg++) { |
|
93 if (error_msg->code == error_code) { |
|
94 EC_ERR("SoE error 0x%04X: \"%s\".\n", |
|
95 error_msg->code, error_msg->message); |
|
96 return; |
|
97 } |
|
98 } |
|
99 |
|
100 EC_ERR("Unknown SoE error 0x%04X.\n", error_code); |
|
101 } |
|
102 |
|
103 /*****************************************************************************/ |
|
104 |
77 /** Constructor. |
105 /** Constructor. |
78 */ |
106 */ |
79 void ec_fsm_soe_init( |
107 void ec_fsm_soe_init( |
80 ec_fsm_soe_t *fsm, /**< finite state machine */ |
108 ec_fsm_soe_t *fsm, /**< finite state machine */ |
81 ec_datagram_t *datagram /**< datagram */ |
109 ec_datagram_t *datagram /**< datagram */ |
82 ) |
110 ) |
83 { |
111 { |
84 fsm->state = NULL; |
112 fsm->state = NULL; |
85 fsm->datagram = datagram; |
113 fsm->datagram = datagram; |
86 } |
114 } |
87 |
115 |
88 /*****************************************************************************/ |
116 /*****************************************************************************/ |
89 |
117 |
90 /** Destructor. |
118 /** Destructor. |
91 */ |
119 */ |
92 void ec_fsm_soe_clear( |
120 void ec_fsm_soe_clear( |
93 ec_fsm_soe_t *fsm /**< finite state machine */ |
121 ec_fsm_soe_t *fsm /**< finite state machine */ |
94 ) |
122 ) |
95 { |
123 { |
96 } |
124 } |
97 |
125 |
98 /*****************************************************************************/ |
126 /*****************************************************************************/ |
99 |
127 |
157 if (master->debug_level) |
205 if (master->debug_level) |
158 EC_DBG("Reading IDN 0x%04X from slave %u.\n", |
206 EC_DBG("Reading IDN 0x%04X from slave %u.\n", |
159 request->idn, slave->ring_position); |
207 request->idn, slave->ring_position); |
160 |
208 |
161 if (!(slave->sii.mailbox_protocols & EC_MBOX_SOE)) { |
209 if (!(slave->sii.mailbox_protocols & EC_MBOX_SOE)) { |
162 EC_ERR("Slave %u does not support SoE!\n", slave->ring_position); |
210 EC_ERR("Slave does not support SoE!\n"); |
163 fsm->state = ec_fsm_soe_error; |
211 fsm->state = ec_fsm_soe_error; |
|
212 ec_fsm_soe_print_error(fsm); |
164 return; |
213 return; |
165 } |
214 } |
166 |
215 |
167 data = ec_slave_mbox_prepare_send(slave, datagram, EC_MBOX_TYPE_SOE, |
216 data = ec_slave_mbox_prepare_send(slave, datagram, EC_MBOX_TYPE_SOE, |
168 EC_SOE_READ_REQUEST_SIZE); |
217 EC_SOE_SIZE); |
169 if (IS_ERR(data)) { |
218 if (IS_ERR(data)) { |
170 fsm->state = ec_fsm_soe_error; |
219 fsm->state = ec_fsm_soe_error; |
171 return; |
220 ec_fsm_soe_print_error(fsm); |
172 } |
221 return; |
173 |
222 } |
174 EC_WRITE_U8(data, EC_SOE_OPCODE_READ_REQUEST); |
223 |
|
224 EC_WRITE_U8(data, OPCODE_READ_REQUEST); |
175 EC_WRITE_U8(data + 1, 1 << 6); // request value |
225 EC_WRITE_U8(data + 1, 1 << 6); // request value |
176 EC_WRITE_U16(data + 2, request->idn); |
226 EC_WRITE_U16(data + 2, request->idn); |
177 |
227 |
178 if (master->debug_level) { |
228 if (master->debug_level) { |
179 EC_DBG("SCC read request:\n"); |
229 EC_DBG("SCC read request:\n"); |
180 ec_print_data(data, EC_SOE_READ_REQUEST_SIZE); |
230 ec_print_data(data, EC_SOE_SIZE); |
181 } |
231 } |
182 |
232 |
183 fsm->request->data_size = 0; |
233 fsm->request->data_size = 0; |
184 fsm->request->jiffies_sent = jiffies; |
234 fsm->request->jiffies_sent = jiffies; |
185 fsm->retries = EC_FSM_RETRIES; |
235 fsm->retries = EC_FSM_RETRIES; |
243 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
292 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
244 return; |
293 return; |
245 |
294 |
246 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
295 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
247 fsm->state = ec_fsm_soe_error; |
296 fsm->state = ec_fsm_soe_error; |
248 EC_ERR("Failed to receive SoE mailbox check datagram from slave %u: ", |
297 EC_ERR("Failed to receive SoE mailbox check datagram: "); |
249 slave->ring_position); |
|
250 ec_datagram_print_state(datagram); |
298 ec_datagram_print_state(datagram); |
|
299 ec_fsm_soe_print_error(fsm); |
251 return; |
300 return; |
252 } |
301 } |
253 |
302 |
254 if (datagram->working_counter != 1) { |
303 if (datagram->working_counter != 1) { |
255 fsm->state = ec_fsm_soe_error; |
304 fsm->state = ec_fsm_soe_error; |
256 EC_ERR("Reception of SoE mailbox check datagram failed on slave %u: ", |
305 EC_ERR("Reception of SoE mailbox check datagram failed: "); |
257 slave->ring_position); |
|
258 ec_datagram_print_wc_error(datagram); |
306 ec_datagram_print_wc_error(datagram); |
|
307 ec_fsm_soe_print_error(fsm); |
259 return; |
308 return; |
260 } |
309 } |
261 |
310 |
262 if (!ec_slave_mbox_check(datagram)) { |
311 if (!ec_slave_mbox_check(datagram)) { |
263 unsigned long diff_ms = |
312 unsigned long diff_ms = |
264 (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; |
313 (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; |
265 if (diff_ms >= EC_SOE_RESPONSE_TIMEOUT) { |
314 if (diff_ms >= EC_SOE_RESPONSE_TIMEOUT) { |
266 fsm->state = ec_fsm_soe_error; |
315 fsm->state = ec_fsm_soe_error; |
267 EC_ERR("Timeout after %u ms while waiting for IDN 0x%04x" |
316 EC_ERR("Timeout after %u ms while waiting for read response.\n", |
268 " read response on slave %u.\n", (u32) diff_ms, |
317 (u32) diff_ms); |
269 fsm->request->idn, slave->ring_position); |
318 ec_fsm_soe_print_error(fsm); |
270 return; |
319 return; |
271 } |
320 } |
272 |
321 |
273 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
322 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
274 fsm->retries = EC_FSM_RETRIES; |
323 fsm->retries = EC_FSM_RETRIES; |
298 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
347 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
299 return; // FIXME: request again? |
348 return; // FIXME: request again? |
300 |
349 |
301 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
350 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
302 fsm->state = ec_fsm_soe_error; |
351 fsm->state = ec_fsm_soe_error; |
303 EC_ERR("Failed to receive SoE read response datagram for" |
352 EC_ERR("Failed to receive SoE read response datagram: "); |
304 " slave %u: ", slave->ring_position); |
|
305 ec_datagram_print_state(datagram); |
353 ec_datagram_print_state(datagram); |
|
354 ec_fsm_soe_print_error(fsm); |
306 return; |
355 return; |
307 } |
356 } |
308 |
357 |
309 if (datagram->working_counter != 1) { |
358 if (datagram->working_counter != 1) { |
310 fsm->state = ec_fsm_soe_error; |
359 fsm->state = ec_fsm_soe_error; |
311 EC_ERR("Reception of SoE read response failed on slave %u: ", |
360 EC_ERR("Reception of SoE read response failed: "); |
312 slave->ring_position); |
|
313 ec_datagram_print_wc_error(datagram); |
361 ec_datagram_print_wc_error(datagram); |
|
362 ec_fsm_soe_print_error(fsm); |
314 return; |
363 return; |
315 } |
364 } |
316 |
365 |
317 data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot, &rec_size); |
366 data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot, &rec_size); |
318 if (IS_ERR(data)) { |
367 if (IS_ERR(data)) { |
319 fsm->state = ec_fsm_soe_error; |
368 fsm->state = ec_fsm_soe_error; |
|
369 ec_fsm_soe_print_error(fsm); |
320 return; |
370 return; |
321 } |
371 } |
322 |
372 |
323 if (master->debug_level) { |
373 if (master->debug_level) { |
324 EC_DBG("SCC read response:\n"); |
374 EC_DBG("SCC read response:\n"); |
325 ec_print_data(data, rec_size); |
375 ec_print_data(data, rec_size); |
326 } |
376 } |
327 |
377 |
328 if (mbox_prot != EC_MBOX_TYPE_SOE) { |
378 if (mbox_prot != EC_MBOX_TYPE_SOE) { |
329 fsm->state = ec_fsm_soe_error; |
379 fsm->state = ec_fsm_soe_error; |
330 EC_WARN("Received mailbox protocol 0x%02X as response.\n", mbox_prot); |
380 EC_ERR("Received mailbox protocol 0x%02X as response.\n", mbox_prot); |
331 return; |
381 ec_fsm_soe_print_error(fsm); |
332 } |
382 return; |
333 |
383 } |
334 if (rec_size < EC_SOE_READ_RESPONSE_SIZE) { |
384 |
335 fsm->state = ec_fsm_soe_error; |
385 if (rec_size < EC_SOE_SIZE) { |
336 EC_ERR("Received currupted SoE read response (%zu bytes)!\n", |
386 fsm->state = ec_fsm_soe_error; |
337 rec_size); |
387 EC_ERR("Received currupted SoE read response" |
|
388 " (%zu bytes)!\n", rec_size); |
338 ec_print_data(data, rec_size); |
389 ec_print_data(data, rec_size); |
|
390 ec_fsm_soe_print_error(fsm); |
339 return; |
391 return; |
340 } |
392 } |
341 |
393 |
342 header = EC_READ_U8(data); |
394 header = EC_READ_U8(data); |
343 opcode = header & 0x7; |
395 opcode = header & 0x7; |
344 incomplete = (header >> 3) & 1; |
396 incomplete = (header >> 3) & 1; |
345 error_flag = (header >> 4) & 1; |
397 error_flag = (header >> 4) & 1; |
346 |
398 |
347 if (opcode != EC_SOE_OPCODE_READ_RESPONSE) { |
399 if (opcode != OPCODE_READ_RESPONSE) { |
348 EC_ERR("Received no read response (opcode %x).\n", opcode); |
400 EC_ERR("Received no read response (opcode %x).\n", opcode); |
349 ec_print_data(data, rec_size); |
401 ec_print_data(data, rec_size); |
350 fsm->state = ec_fsm_soe_error; |
402 ec_fsm_soe_print_error(fsm); |
351 return; |
403 fsm->state = ec_fsm_soe_error; |
352 } |
404 return; |
353 |
405 } |
354 if (error_flag) { |
406 |
355 req->error_code = EC_READ_U16(data + rec_size - 2); |
407 if (error_flag) { |
356 EC_ERR("Received error response: 0x%04x.\n", |
408 req->error_code = EC_READ_U16(data + rec_size - 2); |
357 req->error_code); |
409 EC_ERR("Received error response:\n"); |
358 fsm->state = ec_fsm_soe_error; |
410 ec_print_soe_error(req->error_code); |
359 return; |
411 ec_fsm_soe_print_error(fsm); |
360 } else { |
412 fsm->state = ec_fsm_soe_error; |
361 req->error_code = 0x0000; |
413 return; |
362 } |
414 } else { |
363 |
415 req->error_code = 0x0000; |
364 value_included = (EC_READ_U8(data + 1) >> 6) & 1; |
416 } |
365 if (!value_included) { |
417 |
366 EC_ERR("No value included!\n"); |
418 value_included = (EC_READ_U8(data + 1) >> 6) & 1; |
367 fsm->state = ec_fsm_soe_error; |
419 if (!value_included) { |
368 return; |
420 EC_ERR("No value included!\n"); |
369 } |
421 ec_fsm_soe_print_error(fsm); |
370 |
422 fsm->state = ec_fsm_soe_error; |
371 data_size = rec_size - EC_SOE_READ_RESPONSE_SIZE; |
423 return; |
372 if (ec_soe_request_append_data(req, |
424 } |
373 data + EC_SOE_READ_RESPONSE_SIZE, data_size)) { |
425 |
374 fsm->state = ec_fsm_soe_error; |
426 data_size = rec_size - EC_SOE_SIZE; |
375 return; |
427 if (ec_soe_request_append_data(req, |
376 } |
428 data + EC_SOE_SIZE, data_size)) { |
|
429 fsm->state = ec_fsm_soe_error; |
|
430 ec_fsm_soe_print_error(fsm); |
|
431 return; |
|
432 } |
377 |
433 |
378 if (incomplete) { |
434 if (incomplete) { |
379 if (master->debug_level) { |
435 if (master->debug_level) { |
380 EC_DBG("SoE data incomplete. Waiting for fragment" |
436 EC_DBG("SoE data incomplete. Waiting for fragment" |
381 " at offset %zu.\n", req->data_size); |
437 " at offset %zu.\n", req->data_size); |
410 ec_soe_request_t *req = fsm->request; |
466 ec_soe_request_t *req = fsm->request; |
411 uint8_t incomplete, *data; |
467 uint8_t incomplete, *data; |
412 size_t header_size, max_fragment_size, remaining_size, fragment_size; |
468 size_t header_size, max_fragment_size, remaining_size, fragment_size; |
413 uint16_t fragments_left; |
469 uint16_t fragments_left; |
414 |
470 |
415 header_size = EC_MBOX_HEADER_SIZE + EC_SOE_WRITE_REQUEST_SIZE; |
471 header_size = EC_MBOX_HEADER_SIZE + EC_SOE_SIZE; |
416 if (slave->configured_rx_mailbox_size <= header_size) { |
472 if (slave->configured_rx_mailbox_size <= header_size) { |
417 EC_ERR("Mailbox size (%u) too small for SoE write.\n", |
473 EC_ERR("Mailbox size (%u) too small for SoE write.\n", |
418 slave->configured_rx_mailbox_size); |
474 slave->configured_rx_mailbox_size); |
419 fsm->state = ec_fsm_soe_error; |
475 fsm->state = ec_fsm_soe_error; |
|
476 ec_fsm_soe_print_error(fsm); |
420 return; |
477 return; |
421 } |
478 } |
422 |
479 |
423 remaining_size = req->data_size - fsm->offset; |
480 remaining_size = req->data_size - fsm->offset; |
424 max_fragment_size = slave->configured_rx_mailbox_size - header_size; |
481 max_fragment_size = slave->configured_rx_mailbox_size - header_size; |
428 if (remaining_size % fragment_size) { |
485 if (remaining_size % fragment_size) { |
429 fragments_left++; |
486 fragments_left++; |
430 } |
487 } |
431 |
488 |
432 data = ec_slave_mbox_prepare_send(slave, datagram, EC_MBOX_TYPE_SOE, |
489 data = ec_slave_mbox_prepare_send(slave, datagram, EC_MBOX_TYPE_SOE, |
433 EC_SOE_WRITE_REQUEST_SIZE + fragment_size); |
490 EC_SOE_SIZE + fragment_size); |
434 if (IS_ERR(data)) { |
491 if (IS_ERR(data)) { |
435 fsm->state = ec_fsm_soe_error; |
492 fsm->state = ec_fsm_soe_error; |
436 return; |
493 ec_fsm_soe_print_error(fsm); |
437 } |
494 return; |
438 |
495 } |
439 EC_WRITE_U8(data, EC_SOE_OPCODE_WRITE_REQUEST | incomplete << 3); |
496 |
|
497 EC_WRITE_U8(data, OPCODE_WRITE_REQUEST | incomplete << 3); |
440 EC_WRITE_U8(data + 1, 1 << 6); // only value included |
498 EC_WRITE_U8(data + 1, 1 << 6); // only value included |
441 EC_WRITE_U16(data + 2, incomplete ? fragments_left : req->idn); |
499 EC_WRITE_U16(data + 2, incomplete ? fragments_left : req->idn); |
442 memcpy(data + 4, req->data + fsm->offset, fragment_size); |
500 memcpy(data + 4, req->data + fsm->offset, fragment_size); |
443 fsm->offset += fragment_size; |
501 fsm->offset += fragment_size; |
444 |
502 |
445 if (master->debug_level) { |
503 if (master->debug_level) { |
446 EC_DBG("SCC write request:\n"); |
504 EC_DBG("SCC write request:\n"); |
447 ec_print_data(data, EC_SOE_WRITE_REQUEST_SIZE + fragment_size); |
505 ec_print_data(data, EC_SOE_SIZE + fragment_size); |
448 } |
506 } |
449 |
507 |
450 req->jiffies_sent = jiffies; |
508 req->jiffies_sent = jiffies; |
451 fsm->retries = EC_FSM_RETRIES; |
509 fsm->retries = EC_FSM_RETRIES; |
452 fsm->state = ec_fsm_soe_write_request; |
510 fsm->state = ec_fsm_soe_write_request; |
535 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
593 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
536 return; |
594 return; |
537 |
595 |
538 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
596 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
539 fsm->state = ec_fsm_soe_error; |
597 fsm->state = ec_fsm_soe_error; |
540 EC_ERR("Failed to receive SoE write request datagram from slave %u: ", |
598 EC_ERR("Failed to receive SoE write request datagram: "); |
541 slave->ring_position); |
|
542 ec_datagram_print_state(datagram); |
599 ec_datagram_print_state(datagram); |
|
600 ec_fsm_soe_print_error(fsm); |
543 return; |
601 return; |
544 } |
602 } |
545 |
603 |
546 if (datagram->working_counter != 1) { |
604 if (datagram->working_counter != 1) { |
547 fsm->state = ec_fsm_soe_error; |
605 fsm->state = ec_fsm_soe_error; |
548 EC_ERR("Reception of SoE write request datagram failed on slave %u: ", |
606 EC_ERR("Reception of SoE write request datagram: "); |
549 slave->ring_position); |
|
550 ec_datagram_print_wc_error(datagram); |
607 ec_datagram_print_wc_error(datagram); |
|
608 ec_fsm_soe_print_error(fsm); |
551 return; |
609 return; |
552 } |
610 } |
553 |
611 |
554 if (fsm->offset < req->data_size) { |
612 if (fsm->offset < req->data_size) { |
555 ec_fsm_soe_write_next_fragment(fsm); |
613 ec_fsm_soe_write_next_fragment(fsm); |
586 ec_datagram_t *datagram = fsm->datagram; |
644 ec_datagram_t *datagram = fsm->datagram; |
587 ec_slave_t *slave = fsm->slave; |
645 ec_slave_t *slave = fsm->slave; |
588 ec_master_t *master = slave->master; |
646 ec_master_t *master = slave->master; |
589 ec_soe_request_t *req = fsm->request; |
647 ec_soe_request_t *req = fsm->request; |
590 uint8_t *data, mbox_prot, opcode, error_flag; |
648 uint8_t *data, mbox_prot, opcode, error_flag; |
591 uint16_t idn; |
649 uint16_t idn; |
592 size_t rec_size; |
650 size_t rec_size; |
593 |
651 |
594 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
652 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
595 return; // FIXME: request again? |
653 return; // FIXME: request again? |
596 |
654 |
597 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
655 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
598 fsm->state = ec_fsm_soe_error; |
656 fsm->state = ec_fsm_soe_error; |
599 EC_ERR("Failed to receive SoE write response datagram for" |
657 EC_ERR("Failed to receive SoE write response datagram: "); |
600 " slave %u: ", slave->ring_position); |
|
601 ec_datagram_print_state(datagram); |
658 ec_datagram_print_state(datagram); |
|
659 ec_fsm_soe_print_error(fsm); |
602 return; |
660 return; |
603 } |
661 } |
604 |
662 |
605 if (datagram->working_counter != 1) { |
663 if (datagram->working_counter != 1) { |
606 fsm->state = ec_fsm_soe_error; |
664 fsm->state = ec_fsm_soe_error; |
607 EC_ERR("Reception of SoE write response failed on slave %u: ", |
665 EC_ERR("Reception of SoE write response failed: "); |
608 slave->ring_position); |
|
609 ec_datagram_print_wc_error(datagram); |
666 ec_datagram_print_wc_error(datagram); |
|
667 ec_fsm_soe_print_error(fsm); |
610 return; |
668 return; |
611 } |
669 } |
612 |
670 |
613 data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot, &rec_size); |
671 data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot, &rec_size); |
614 if (IS_ERR(data)) { |
672 if (IS_ERR(data)) { |
615 fsm->state = ec_fsm_soe_error; |
673 fsm->state = ec_fsm_soe_error; |
|
674 ec_fsm_soe_print_error(fsm); |
616 return; |
675 return; |
617 } |
676 } |
618 |
677 |
619 if (master->debug_level) { |
678 if (master->debug_level) { |
620 EC_DBG("SCC write response:\n"); |
679 EC_DBG("SCC write response:\n"); |
621 ec_print_data(data, rec_size); |
680 ec_print_data(data, rec_size); |
622 } |
681 } |
623 |
682 |
624 if (mbox_prot != EC_MBOX_TYPE_SOE) { |
683 if (mbox_prot != EC_MBOX_TYPE_SOE) { |
625 fsm->state = ec_fsm_soe_error; |
684 fsm->state = ec_fsm_soe_error; |
626 EC_WARN("Received mailbox protocol 0x%02X as response.\n", mbox_prot); |
685 EC_ERR("Received mailbox protocol 0x%02X as response.\n", mbox_prot); |
627 return; |
686 ec_fsm_soe_print_error(fsm); |
628 } |
687 return; |
629 |
688 } |
630 if (rec_size < EC_SOE_WRITE_RESPONSE_SIZE) { |
689 |
|
690 if (rec_size < EC_SOE_SIZE) { |
631 fsm->state = ec_fsm_soe_error; |
691 fsm->state = ec_fsm_soe_error; |
632 EC_ERR("Received currupted SoE write response (%zu bytes)!\n", |
692 EC_ERR("Received currupted SoE write response (%zu bytes)!\n", |
633 rec_size); |
693 rec_size); |
634 ec_print_data(data, rec_size); |
694 ec_print_data(data, rec_size); |
635 return; |
695 ec_fsm_soe_print_error(fsm); |
636 } |
696 return; |
637 |
697 } |
638 opcode = EC_READ_U8(data) & 0x7; |
698 |
639 if (opcode != EC_SOE_OPCODE_WRITE_RESPONSE) { |
699 opcode = EC_READ_U8(data) & 0x7; |
|
700 if (opcode != OPCODE_WRITE_RESPONSE) { |
640 EC_ERR("Received no write response (opcode %x).\n", opcode); |
701 EC_ERR("Received no write response (opcode %x).\n", opcode); |
641 ec_print_data(data, rec_size); |
702 ec_print_data(data, rec_size); |
642 fsm->state = ec_fsm_soe_error; |
703 ec_fsm_soe_print_error(fsm); |
643 return; |
704 fsm->state = ec_fsm_soe_error; |
644 } |
705 return; |
645 |
706 } |
646 idn = EC_READ_U16(data + 2); |
707 |
647 if (idn != req->idn) { |
708 idn = EC_READ_U16(data + 2); |
648 EC_ERR("Received response for wrong IDN 0x%04x.\n", idn); |
709 if (idn != req->idn) { |
|
710 EC_ERR("Received response for wrong IDN 0x%04x.\n", idn); |
649 ec_print_data(data, rec_size); |
711 ec_print_data(data, rec_size); |
650 fsm->state = ec_fsm_soe_error; |
712 ec_fsm_soe_print_error(fsm); |
651 return; |
713 fsm->state = ec_fsm_soe_error; |
652 } |
714 return; |
653 |
715 } |
654 error_flag = (EC_READ_U8(data) >> 4) & 1; |
716 |
655 if (error_flag) { |
717 error_flag = (EC_READ_U8(data) >> 4) & 1; |
656 if (rec_size < EC_SOE_WRITE_RESPONSE_SIZE + 2) { |
718 if (error_flag) { |
657 EC_ERR("Received corrupted error response - error flag set," |
719 if (rec_size < EC_SOE_SIZE + 2) { |
658 " but received size is %zu.\n", rec_size); |
720 EC_ERR("Received corrupted error response - error flag set," |
659 } else { |
721 " but received size is %zu.\n", rec_size); |
660 req->error_code = EC_READ_U16(data + EC_SOE_WRITE_RESPONSE_SIZE); |
722 } else { |
661 EC_ERR("Received error response: 0x%04x.\n", |
723 req->error_code = EC_READ_U16(data + EC_SOE_SIZE); |
662 req->error_code); |
724 EC_ERR("Received error response:\n"); |
663 } |
725 ec_print_soe_error(req->error_code); |
664 ec_print_data(data, rec_size); |
726 } |
665 fsm->state = ec_fsm_soe_error; |
727 ec_print_data(data, rec_size); |
666 return; |
728 ec_fsm_soe_print_error(fsm); |
667 } else { |
729 fsm->state = ec_fsm_soe_error; |
668 req->error_code = 0x0000; |
730 return; |
669 } |
731 } else { |
|
732 req->error_code = 0x0000; |
|
733 } |
670 |
734 |
671 fsm->state = ec_fsm_soe_end; // success |
735 fsm->state = ec_fsm_soe_end; // success |
672 } |
736 } |
673 |
737 |
674 /*****************************************************************************/ |
738 /*****************************************************************************/ |