371 master->dc_ref_clock = NULL; |
373 master->dc_ref_clock = NULL; |
372 |
374 |
373 // external requests are obsolete, so we wake pending waiters and remove |
375 // external requests are obsolete, so we wake pending waiters and remove |
374 // them from the list |
376 // them from the list |
375 // |
377 // |
376 // SII requests |
378 // SII requests |
377 while (1) { |
379 while (1) { |
378 ec_sii_write_request_t *request; |
380 ec_sii_write_request_t *request; |
379 if (list_empty(&master->sii_requests)) |
381 if (list_empty(&master->sii_requests)) |
380 break; |
382 break; |
381 // get first request |
383 // get first request |
382 request = list_entry(master->sii_requests.next, |
384 request = list_entry(master->sii_requests.next, |
383 ec_sii_write_request_t, list); |
385 ec_sii_write_request_t, list); |
384 list_del_init(&request->list); // dequeue |
386 list_del_init(&request->list); // dequeue |
385 EC_INFO("Discarding SII request, slave %u does not exist anymore.\n", |
387 EC_INFO("Discarding SII request, slave %u does not exist anymore.\n", |
386 request->slave->ring_position); |
388 request->slave->ring_position); |
387 request->state = EC_INT_REQUEST_FAILURE; |
389 request->state = EC_INT_REQUEST_FAILURE; |
388 wake_up(&master->sii_queue); |
390 wake_up(&master->sii_queue); |
389 } |
391 } |
390 |
392 |
391 // Register requests |
393 // Register requests |
392 while (1) { |
394 while (1) { |
393 ec_reg_request_t *request; |
395 ec_reg_request_t *request; |
394 if (list_empty(&master->reg_requests)) |
396 if (list_empty(&master->reg_requests)) |
395 break; |
397 break; |
396 // get first request |
398 // get first request |
397 request = list_entry(master->reg_requests.next, |
399 request = list_entry(master->reg_requests.next, |
398 ec_reg_request_t, list); |
400 ec_reg_request_t, list); |
399 list_del_init(&request->list); // dequeue |
401 list_del_init(&request->list); // dequeue |
400 EC_INFO("Discarding Reg request, slave %u does not exist anymore.\n", |
402 EC_INFO("Discarding Reg request, slave %u does not exist anymore.\n", |
401 request->slave->ring_position); |
403 request->slave->ring_position); |
402 request->state = EC_INT_REQUEST_FAILURE; |
404 request->state = EC_INT_REQUEST_FAILURE; |
403 wake_up(&master->reg_queue); |
405 wake_up(&master->reg_queue); |
404 } |
406 } |
405 |
407 |
406 for (slave = master->slaves; |
408 for (slave = master->slaves; |
407 slave < master->slaves + master->slave_count; |
409 slave < master->slaves + master->slave_count; |
408 slave++) { |
410 slave++) { |
409 // SDO requests |
411 // SDO requests |
687 } |
689 } |
688 |
690 |
689 |
691 |
690 /*****************************************************************************/ |
692 /*****************************************************************************/ |
691 |
693 |
692 /** Injects external datagrams that fit into the datagram queue |
694 /** Injects external datagrams that fit into the datagram queue. |
693 */ |
695 */ |
694 void ec_master_inject_external_datagrams( |
696 void ec_master_inject_external_datagrams( |
695 ec_master_t *master /**< EtherCAT master */ |
697 ec_master_t *master /**< EtherCAT master */ |
696 ) |
698 ) |
697 { |
699 { |
698 ec_datagram_t *datagram, *n; |
700 ec_datagram_t *datagram, *n; |
699 size_t queue_size = 0; |
701 size_t queue_size = 0; |
700 list_for_each_entry(datagram, &master->datagram_queue, queue) { |
702 |
701 queue_size += datagram->data_size; |
703 list_for_each_entry(datagram, &master->datagram_queue, queue) { |
702 } |
704 queue_size += datagram->data_size; |
703 list_for_each_entry_safe(datagram, n, &master->external_datagram_queue, queue) { |
705 } |
704 queue_size += datagram->data_size; |
706 |
705 if (queue_size <= master->max_queue_size) { |
707 list_for_each_entry_safe(datagram, n, &master->external_datagram_queue, |
706 list_del_init(&datagram->queue); |
708 queue) { |
|
709 queue_size += datagram->data_size; |
|
710 if (queue_size <= master->max_queue_size) { |
|
711 list_del_init(&datagram->queue); |
707 #if DEBUG_INJECT |
712 #if DEBUG_INJECT |
708 if (master->debug_level) { |
713 if (master->debug_level) { |
709 EC_DBG("Injecting external datagram %08x size=%u, queue_size=%u\n",(unsigned int)datagram,datagram->data_size,queue_size); |
714 EC_DBG("Injecting external datagram %08x size=%u," |
710 } |
715 " queue_size=%u\n", (unsigned int) datagram, |
|
716 datagram->data_size, queue_size); |
|
717 } |
711 #endif |
718 #endif |
712 #ifdef EC_HAVE_CYCLES |
719 #ifdef EC_HAVE_CYCLES |
713 datagram->cycles_sent = 0; |
720 datagram->cycles_sent = 0; |
714 #endif |
721 #endif |
715 datagram->jiffies_sent = 0; |
722 datagram->jiffies_sent = 0; |
716 ec_master_queue_datagram(master, datagram); |
723 ec_master_queue_datagram(master, datagram); |
717 } |
724 } |
718 else { |
725 else { |
719 if (datagram->data_size > master->max_queue_size) { |
726 if (datagram->data_size > master->max_queue_size) { |
720 list_del_init(&datagram->queue); |
727 list_del_init(&datagram->queue); |
721 datagram->state = EC_DATAGRAM_ERROR; |
728 datagram->state = EC_DATAGRAM_ERROR; |
722 EC_ERR("External datagram %08x is too large, size=%u, max_queue_size=%u\n",(unsigned int)datagram,datagram->data_size,master->max_queue_size); |
729 EC_ERR("External datagram %p is too large," |
723 } |
730 " size=%u, max_queue_size=%u\n", |
724 else { |
731 datagram, datagram->data_size, |
|
732 master->max_queue_size); |
|
733 } else { |
725 #ifdef EC_HAVE_CYCLES |
734 #ifdef EC_HAVE_CYCLES |
726 cycles_t cycles_now = get_cycles(); |
735 cycles_t cycles_now = get_cycles(); |
727 if (cycles_now - datagram->cycles_sent |
736 |
728 > sdo_injection_timeout_cycles) { |
737 if (cycles_now - datagram->cycles_sent |
|
738 > sdo_injection_timeout_cycles) |
729 #else |
739 #else |
730 if (jiffies - datagram->jiffies_sent |
740 if (jiffies - datagram->jiffies_sent |
731 > sdo_injection_timeout_jiffies) { |
741 > sdo_injection_timeout_jiffies) |
732 #endif |
742 #endif |
733 unsigned int time_us; |
743 { |
734 list_del_init(&datagram->queue); |
744 unsigned int time_us; |
735 datagram->state = EC_DATAGRAM_ERROR; |
745 |
|
746 list_del_init(&datagram->queue); |
|
747 datagram->state = EC_DATAGRAM_ERROR; |
736 #ifdef EC_HAVE_CYCLES |
748 #ifdef EC_HAVE_CYCLES |
737 time_us = (unsigned int) ((cycles_now - datagram->cycles_sent) * 1000LL) / cpu_khz; |
749 time_us = (unsigned int) |
|
750 ((cycles_now - datagram->cycles_sent) * 1000LL) |
|
751 / cpu_khz; |
738 #else |
752 #else |
739 time_us = (unsigned int) ((jiffies - datagram->jiffies_sent) * 1000000 / HZ); |
753 time_us = (unsigned int) |
740 #endif |
754 ((jiffies - datagram->jiffies_sent) * 1000000 / HZ); |
741 EC_ERR("Timeout %u us: injecting external datagram %08x size=%u, max_queue_size=%u\n",time_us,(unsigned int)datagram,datagram->data_size,master->max_queue_size); |
755 #endif |
742 } |
756 EC_ERR("Timeout %u us: injecting external datagram %p" |
743 else { |
757 " size=%u, max_queue_size=%u\n", |
|
758 time_us, datagram, |
|
759 datagram->data_size, master->max_queue_size); |
|
760 } |
744 #if DEBUG_INJECT |
761 #if DEBUG_INJECT |
745 if (master->debug_level) { |
762 else if (master->debug_level) { |
746 EC_DBG("Deferred injecting of external datagram %08x size=%u, queue_size=%u\n",(unsigned int)datagram,datagram->data_size,queue_size); |
763 EC_DBG("Deferred injecting of external datagram %p" |
747 } |
764 " size=%u, queue_size=%u\n", |
748 #endif |
765 datagram, datagram->data_size, queue_size); |
749 } |
766 } |
750 } |
767 #endif |
751 } |
768 } |
752 } |
769 } |
753 } |
770 } |
754 |
771 } |
755 /*****************************************************************************/ |
772 |
756 |
773 /*****************************************************************************/ |
757 /** sets the expected interval between calls to ecrt_master_send |
774 |
758 and calculates the maximum amount of data to queue |
775 /** Sets the expected interval between calls to ecrt_master_send |
|
776 * and calculates the maximum amount of data to queue. |
759 */ |
777 */ |
760 void ec_master_set_send_interval( |
778 void ec_master_set_send_interval( |
761 ec_master_t *master, /**< EtherCAT master */ |
779 ec_master_t *master, /**< EtherCAT master */ |
762 size_t send_interval /**< send interval */ |
780 size_t send_interval /**< Send interval */ |
763 ) |
781 ) |
764 { |
782 { |
765 master->send_interval = send_interval; |
783 master->send_interval = send_interval; |
766 master->max_queue_size = (send_interval * 1000) / EC_BYTE_TRANSMITION_TIME; |
784 master->max_queue_size = |
767 master->max_queue_size -= master->max_queue_size / 10; |
785 (send_interval * 1000) / EC_BYTE_TRANSMISSION_TIME_NS; |
768 } |
786 master->max_queue_size -= master->max_queue_size / 10; |
769 |
787 } |
770 |
788 |
771 /*****************************************************************************/ |
789 /*****************************************************************************/ |
772 |
790 |
773 /** Places an external datagram in the sdo datagram queue. |
791 /** Places an external datagram in the sdo datagram queue. |
774 */ |
792 */ |
778 ) |
796 ) |
779 { |
797 { |
780 ec_datagram_t *queued_datagram; |
798 ec_datagram_t *queued_datagram; |
781 |
799 |
782 down(&master->io_sem); |
800 down(&master->io_sem); |
|
801 |
783 // check, if the datagram is already queued |
802 // check, if the datagram is already queued |
784 list_for_each_entry(queued_datagram, &master->external_datagram_queue, queue) { |
803 list_for_each_entry(queued_datagram, &master->external_datagram_queue, |
|
804 queue) { |
785 if (queued_datagram == datagram) { |
805 if (queued_datagram == datagram) { |
786 datagram->state = EC_DATAGRAM_QUEUED; |
806 datagram->state = EC_DATAGRAM_QUEUED; |
787 return; |
807 return; |
788 } |
808 } |
789 } |
809 } |
|
810 |
790 #if DEBUG_INJECT |
811 #if DEBUG_INJECT |
791 if (master->debug_level) { |
812 if (master->debug_level) { |
792 EC_DBG("Requesting external datagram %08x size=%u\n",(unsigned int)datagram,datagram->data_size); |
813 EC_DBG("Requesting external datagram %p size=%u\n", |
|
814 datagram, datagram->data_size); |
793 } |
815 } |
794 #endif |
816 #endif |
|
817 |
795 list_add_tail(&datagram->queue, &master->external_datagram_queue); |
818 list_add_tail(&datagram->queue, &master->external_datagram_queue); |
796 datagram->state = EC_DATAGRAM_QUEUED; |
819 datagram->state = EC_DATAGRAM_QUEUED; |
797 #ifdef EC_HAVE_CYCLES |
820 #ifdef EC_HAVE_CYCLES |
798 datagram->cycles_sent = get_cycles(); |
821 datagram->cycles_sent = get_cycles(); |
799 #endif |
822 #endif |
1114 } |
1137 } |
1115 } |
1138 } |
1116 |
1139 |
1117 |
1140 |
1118 /*****************************************************************************/ |
1141 /*****************************************************************************/ |
|
1142 |
|
1143 #ifdef EC_USE_HRTIMER |
|
1144 |
1119 /* |
1145 /* |
1120 * Sleep related functions: |
1146 * Sleep related functions: |
1121 */ |
1147 */ |
1122 static enum hrtimer_restart ec_master_nanosleep_wakeup(struct hrtimer *timer) |
1148 static enum hrtimer_restart ec_master_nanosleep_wakeup(struct hrtimer *timer) |
1123 { |
1149 { |
1124 struct hrtimer_sleeper *t = |
1150 struct hrtimer_sleeper *t = |
1125 container_of(timer, struct hrtimer_sleeper, timer); |
1151 container_of(timer, struct hrtimer_sleeper, timer); |
1126 struct task_struct *task = t->task; |
1152 struct task_struct *task = t->task; |
1127 |
1153 |
1128 t->task = NULL; |
1154 t->task = NULL; |
1129 if (task) |
1155 if (task) |
1130 wake_up_process(task); |
1156 wake_up_process(task); |
1131 |
1157 |
1132 return HRTIMER_NORESTART; |
1158 return HRTIMER_NORESTART; |
1133 } |
1159 } |
|
1160 |
|
1161 /*****************************************************************************/ |
1134 |
1162 |
1135 #if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,28) |
1163 #if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,28) |
|
1164 |
1136 /* compatibility with new hrtimer interface */ |
1165 /* compatibility with new hrtimer interface */ |
1137 static inline ktime_t hrtimer_get_expires(const struct hrtimer *timer) |
1166 static inline ktime_t hrtimer_get_expires(const struct hrtimer *timer) |
1138 { |
1167 { |
1139 return timer->expires; |
1168 return timer->expires; |
1140 } |
1169 } |
|
1170 |
|
1171 /*****************************************************************************/ |
1141 |
1172 |
1142 static inline void hrtimer_set_expires(struct hrtimer *timer, ktime_t time) |
1173 static inline void hrtimer_set_expires(struct hrtimer *timer, ktime_t time) |
1143 { |
1174 { |
1144 timer->expires = time; |
1175 timer->expires = time; |
1145 } |
1176 } |
1146 #endif |
1177 |
1147 |
1178 #endif |
|
1179 |
|
1180 /*****************************************************************************/ |
1148 |
1181 |
1149 void ec_master_nanosleep(const unsigned long nsecs) |
1182 void ec_master_nanosleep(const unsigned long nsecs) |
1150 { |
1183 { |
1151 struct hrtimer_sleeper t; |
1184 struct hrtimer_sleeper t; |
1152 enum hrtimer_mode mode = HRTIMER_MODE_REL; |
1185 enum hrtimer_mode mode = HRTIMER_MODE_REL; |
1153 hrtimer_init(&t.timer, CLOCK_MONOTONIC,mode); |
1186 |
1154 t.timer.function = ec_master_nanosleep_wakeup; |
1187 hrtimer_init(&t.timer, CLOCK_MONOTONIC, mode); |
1155 t.task = current; |
1188 t.timer.function = ec_master_nanosleep_wakeup; |
|
1189 t.task = current; |
1156 #ifdef CONFIG_HIGH_RES_TIMERS |
1190 #ifdef CONFIG_HIGH_RES_TIMERS |
1157 #if LINUX_VERSION_CODE <= KERNEL_VERSION(2, 6, 24) |
1191 #if LINUX_VERSION_CODE <= KERNEL_VERSION(2, 6, 24) |
1158 t.timer.cb_mode = HRTIMER_CB_IRQSAFE_NO_RESTART; |
1192 t.timer.cb_mode = HRTIMER_CB_IRQSAFE_NO_RESTART; |
1159 #elif LINUX_VERSION_CODE <= KERNEL_VERSION(2, 6, 26) |
1193 #elif LINUX_VERSION_CODE <= KERNEL_VERSION(2, 6, 26) |
1160 t.timer.cb_mode = HRTIMER_CB_IRQSAFE_NO_SOFTIRQ; |
1194 t.timer.cb_mode = HRTIMER_CB_IRQSAFE_NO_SOFTIRQ; |
1161 #elif LINUX_VERSION_CODE <= KERNEL_VERSION(2, 6, 28) |
1195 #elif LINUX_VERSION_CODE <= KERNEL_VERSION(2, 6, 28) |
1162 t.timer.cb_mode = HRTIMER_CB_IRQSAFE_UNLOCKED; |
1196 t.timer.cb_mode = HRTIMER_CB_IRQSAFE_UNLOCKED; |
1163 #endif |
1197 #endif |
1164 #endif |
1198 #endif |
1165 hrtimer_set_expires(&t.timer, ktime_set(0,nsecs)); |
1199 hrtimer_set_expires(&t.timer, ktime_set(0, nsecs)); |
1166 do { |
1200 |
1167 set_current_state(TASK_INTERRUPTIBLE); |
1201 do { |
1168 hrtimer_start(&t.timer, hrtimer_get_expires(&t.timer), mode); |
1202 set_current_state(TASK_INTERRUPTIBLE); |
1169 |
1203 hrtimer_start(&t.timer, hrtimer_get_expires(&t.timer), mode); |
1170 if (likely(t.task)) |
1204 |
1171 schedule(); |
1205 if (likely(t.task)) |
1172 |
1206 schedule(); |
1173 hrtimer_cancel(&t.timer); |
1207 |
1174 mode = HRTIMER_MODE_ABS; |
1208 hrtimer_cancel(&t.timer); |
1175 |
1209 mode = HRTIMER_MODE_ABS; |
1176 } while (t.task && !signal_pending(current)); |
1210 |
1177 } |
1211 } while (t.task && !signal_pending(current)); |
1178 |
1212 } |
|
1213 |
|
1214 #endif // EC_USE_HRTIMER |
1179 |
1215 |
1180 /*****************************************************************************/ |
1216 /*****************************************************************************/ |
1181 |
1217 |
1182 /** Master kernel thread function for IDLE phase. |
1218 /** Master kernel thread function for IDLE phase. |
1183 */ |
1219 */ |
1216 if (fsm_exec) { |
1257 if (fsm_exec) { |
1217 ec_master_queue_datagram(master, &master->fsm_datagram); |
1258 ec_master_queue_datagram(master, &master->fsm_datagram); |
1218 } |
1259 } |
1219 ec_master_inject_external_datagrams(master); |
1260 ec_master_inject_external_datagrams(master); |
1220 ecrt_master_send(master); |
1261 ecrt_master_send(master); |
1221 sent_bytes = master->main_device.tx_skb[master->main_device.tx_ring_index]->len; |
1262 sent_bytes = master->main_device.tx_skb[ |
|
1263 master->main_device.tx_ring_index]->len; |
1222 up(&master->io_sem); |
1264 up(&master->io_sem); |
1223 |
1265 |
1224 if (ec_fsm_master_idle(&master->fsm)) |
1266 if (ec_fsm_master_idle(&master->fsm)) { |
1225 ec_master_nanosleep(master->send_interval*1000); |
1267 #ifdef EC_USE_HRTIMER |
1226 else |
1268 ec_master_nanosleep(master->send_interval * 1000); |
1227 ec_master_nanosleep(sent_bytes*EC_BYTE_TRANSMITION_TIME); |
1269 #else |
|
1270 set_current_state(TASK_INTERRUPTIBLE); |
|
1271 schedule_timeout(1); |
|
1272 #endif |
|
1273 } else { |
|
1274 #ifdef EC_USE_HRTIMER |
|
1275 ec_master_nanosleep(sent_bytes * EC_BYTE_TRANSMISSION_TIME_NS); |
|
1276 #else |
|
1277 schedule(); |
|
1278 #endif |
|
1279 } |
1228 } |
1280 } |
1229 |
1281 |
1230 if (master->debug_level) |
1282 if (master->debug_level) |
1231 EC_DBG("Master IDLE thread exiting...\n"); |
1283 EC_DBG("Master IDLE thread exiting...\n"); |
|
1284 |
1232 return 0; |
1285 return 0; |
1233 } |
1286 } |
1234 |
1287 |
1235 /*****************************************************************************/ |
1288 /*****************************************************************************/ |
1236 |
1289 |
2136 return IS_ERR(sc) ? NULL : sc; |
2205 return IS_ERR(sc) ? NULL : sc; |
2137 } |
2206 } |
2138 |
2207 |
2139 /*****************************************************************************/ |
2208 /*****************************************************************************/ |
2140 |
2209 |
|
2210 int ecrt_master(ec_master_t *master, ec_master_info_t *master_info) |
|
2211 { |
|
2212 if (master->debug_level) |
|
2213 EC_DBG("ecrt_master(master = 0x%p, master_info = 0x%p)\n", |
|
2214 master, master_info); |
|
2215 |
|
2216 master_info->slave_count = master->slave_count; |
|
2217 master_info->link_up = master->main_device.link_state; |
|
2218 master_info->scan_busy = master->scan_busy; |
|
2219 master_info->app_time = master->app_time; |
|
2220 return 0; |
|
2221 } |
|
2222 |
|
2223 /*****************************************************************************/ |
|
2224 |
|
2225 int ecrt_master_get_slave(ec_master_t *master, uint16_t slave_position, |
|
2226 ec_slave_info_t *slave_info) |
|
2227 { |
|
2228 const ec_slave_t *slave; |
|
2229 |
|
2230 if (down_interruptible(&master->master_sem)) { |
|
2231 return -EINTR; |
|
2232 } |
|
2233 |
|
2234 slave = ec_master_find_slave_const(master, 0, slave_position); |
|
2235 |
|
2236 slave_info->position = slave->ring_position; |
|
2237 slave_info->vendor_id = slave->sii.vendor_id; |
|
2238 slave_info->product_code = slave->sii.product_code; |
|
2239 slave_info->revision_number = slave->sii.revision_number; |
|
2240 slave_info->serial_number = slave->sii.serial_number; |
|
2241 slave_info->alias = slave->sii.alias; |
|
2242 slave_info->current_on_ebus = slave->sii.current_on_ebus; |
|
2243 slave_info->al_state = slave->current_state; |
|
2244 slave_info->error_flag = slave->error_flag; |
|
2245 slave_info->sync_count = slave->sii.sync_count; |
|
2246 slave_info->sdo_count = ec_slave_sdo_count(slave); |
|
2247 if (slave->sii.name) { |
|
2248 strncpy(slave_info->name, slave->sii.name, EC_MAX_STRING_LENGTH); |
|
2249 } else { |
|
2250 slave_info->name[0] = 0; |
|
2251 } |
|
2252 |
|
2253 up(&master->master_sem); |
|
2254 |
|
2255 return 0; |
|
2256 } |
|
2257 |
|
2258 /*****************************************************************************/ |
|
2259 |
2141 void ecrt_master_callbacks(ec_master_t *master, |
2260 void ecrt_master_callbacks(ec_master_t *master, |
2142 void (*send_cb)(void *), void (*receive_cb)(void *), void *cb_data) |
2261 void (*send_cb)(void *), void (*receive_cb)(void *), void *cb_data) |
2143 { |
2262 { |
2144 if (master->debug_level) |
2263 if (master->debug_level) |
2145 EC_DBG("ecrt_master_callbacks(master = 0x%p, send_cb = 0x%p, " |
2264 EC_DBG("ecrt_master_callbacks(master = 0x%p, send_cb = 0x%p, " |