64 EC_ERR("Slave %u does not support mailbox communication!\n", |
64 EC_ERR("Slave %u does not support mailbox communication!\n", |
65 slave->ring_position); |
65 slave->ring_position); |
66 return NULL; |
66 return NULL; |
67 } |
67 } |
68 |
68 |
69 total_size = size + 6; |
69 total_size = EC_MBOX_HEADER_SIZE + 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 |
80 EC_WRITE_U16(datagram->data, size); // mailbox service data length |
80 EC_WRITE_U16(datagram->data, size); // mailbox service data length |
81 EC_WRITE_U16(datagram->data + 2, slave->station_address); // station addr. |
81 EC_WRITE_U16(datagram->data + 2, slave->station_address); // station addr. |
82 EC_WRITE_U8 (datagram->data + 4, 0x00); // channel & priority |
82 EC_WRITE_U8 (datagram->data + 4, 0x00); // channel & priority |
83 EC_WRITE_U8 (datagram->data + 5, type); // underlying protocol type |
83 EC_WRITE_U8 (datagram->data + 5, type); // underlying protocol type |
84 |
84 |
85 return datagram->data + 6; |
85 return datagram->data + EC_MBOX_HEADER_SIZE; |
86 } |
86 } |
87 |
87 |
88 /*****************************************************************************/ |
88 /*****************************************************************************/ |
89 |
89 |
90 /** |
90 /** |
165 { |
165 { |
166 size_t data_size; |
166 size_t data_size; |
167 |
167 |
168 data_size = EC_READ_U16(datagram->data); |
168 data_size = EC_READ_U16(datagram->data); |
169 |
169 |
170 if (data_size > slave->sii.tx_mailbox_size - 6) { |
170 if (data_size + EC_MBOX_HEADER_SIZE > slave->sii.tx_mailbox_size) { |
171 EC_ERR("Corrupt mailbox response received from slave %u!\n", |
171 EC_ERR("Corrupt mailbox response received from slave %u!\n", |
172 slave->ring_position); |
172 slave->ring_position); |
173 ec_print_data(datagram->data, slave->sii.tx_mailbox_size); |
173 ec_print_data(datagram->data, slave->sii.tx_mailbox_size); |
174 return NULL; |
174 return NULL; |
175 } |
175 } |
177 *type = EC_READ_U8(datagram->data + 5) & 0x0F; |
177 *type = EC_READ_U8(datagram->data + 5) & 0x0F; |
178 *size = data_size; |
178 *size = data_size; |
179 |
179 |
180 if (*type == 0x00) { |
180 if (*type == 0x00) { |
181 const ec_code_msg_t *mbox_msg; |
181 const ec_code_msg_t *mbox_msg; |
182 uint16_t code = EC_READ_U16(datagram->data + 8); |
182 uint16_t code = EC_READ_U16(datagram->data + 8); |
183 |
183 |
184 EC_ERR("Mailbox error response received from slave %u - ", |
184 EC_ERR("Mailbox error response received from slave %u - ", |
185 slave->ring_position); |
185 slave->ring_position); |
186 |
186 |
187 for (mbox_msg = mbox_error_messages; mbox_msg->code; mbox_msg++) { |
187 for (mbox_msg = mbox_error_messages; mbox_msg->code; mbox_msg++) { |
188 if (mbox_msg->code != code) continue; |
188 if (mbox_msg->code != code) continue; |
189 printk("Code 0x%04X: \"%s\".\n", |
189 printk("Code 0x%04X: \"%s\".\n", |
190 mbox_msg->code, mbox_msg->message); |
190 mbox_msg->code, mbox_msg->message); |
191 break; |
191 break; |
192 } |
192 } |
193 |
193 |
194 if (!mbox_msg->code) |
194 if (!mbox_msg->code) |
195 printk("Unknown error reply code 0x%04X.\n", code); |
195 printk("Unknown error reply code 0x%04X.\n", code); |
196 |
196 |
197 if (slave->master->debug_level) |
197 if (slave->master->debug_level) |
198 ec_print_data(datagram->data + 6, data_size); |
198 ec_print_data(datagram->data + EC_MBOX_HEADER_SIZE, data_size); |
199 |
199 |
200 return NULL; |
200 return NULL; |
201 } |
201 } |
202 |
202 |
203 return datagram->data + 6; |
203 return datagram->data + EC_MBOX_HEADER_SIZE; |
204 } |
204 } |
205 |
205 |
206 /*****************************************************************************/ |
206 /*****************************************************************************/ |