From 79633a2b0fc092fdb6db6e72ffed4fae0b964654 Mon Sep 17 00:00:00 2001 From: cbb <2625274797@qq.com> Date: Sat, 5 Sep 2026 01:20:14 +0800 Subject: [PATCH] =?UTF-8?q?feat(motor):=20=E8=87=AA=E9=80=82=E5=BA=94=20PD?= =?UTF-8?q?O=20=E7=AA=97=E5=8F=A3=E4=B8=8E=E9=80=9A=E9=81=93=E6=B4=BB?= =?UTF-8?q?=E5=8A=A8=E6=8C=87=E7=A4=BA=EF=BC=8C=E6=96=B0=E5=A2=9E=E5=8E=9F?= =?UTF-8?q?=E7=94=9F=20SOEM=20=E6=97=B6=E5=BA=8F=E8=AF=8A=E6=96=AD?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 后端 PDO 循环改为按在线槽收缩状态窗口，稀疏总线下单槽每周期都能刷新， 并补上尾部槽的发现扫描游标；新增自旋节拍源（阈值/显式覆盖）、P 核识别 与线程级钉核，自旋期间释放 GIL 以免 HTTP 线程被饿死；启动时先关停旧实例， 避免两个进程争用 Npcap 适配器。 记录每拍完整时间线、GC stop-the-world 事件与网格跳拍数，经 /api/motor/dbg 导出用于归因固定频率的超时；配 --- build_native.bat | 14 + build_native/exports.txt | 148 + build_native/one.bat | 4 + build_native/wpcap.def | 121 + cadence_diag.py | 141 + ecat_native.c | 397 +++ exports2def.py | 28 + rate_probe.py | 156 ++ soem-src/osal/erika/osal.c | 103 + soem-src/osal/erika/osal_defs.h | 41 + soem-src/osal/intime/osal.c | 88 + soem-src/osal/intime/osal_defs.h | 43 + soem-src/osal/linux/osal.c | 144 + soem-src/osal/linux/osal_defs.h | 38 + soem-src/osal/macosx/osal.c | 144 + soem-src/osal/macosx/osal_defs.h | 38 + soem-src/osal/osal.h | 59 + soem-src/osal/rtems/osal.c | 144 + soem-src/osal/rtems/osal_defs.h | 38 + soem-src/osal/rtk/osal.c | 145 + soem-src/osal/rtk/osal_defs.h | 37 + soem-src/osal/vxworks/osal.c | 187 ++ soem-src/osal/vxworks/osal_defs.h | 28 + soem-src/osal/win32/osal.c | 153 ++ soem-src/osal/win32/osal_defs.h | 45 + soem-src/osal/win32/osal_win32.h | 36 + soem-src/oshw/erika/nicdrv.c | 561 ++++ soem-src/oshw/erika/nicdrv.h | 122 + soem-src/oshw/erika/oshw.c | 79 + soem-src/oshw/erika/oshw.h | 31 + soem-src/oshw/intime/nicdrv.c | 744 +++++ soem-src/oshw/intime/nicdrv.h | 116 + soem-src/oshw/intime/oshw.c | 48 + soem-src/oshw/intime/oshw.h | 31 + soem-src/oshw/linux/nicdrv.c | 648 +++++ soem-src/oshw/linux/nicdrv.h | 120 + soem-src/oshw/linux/oshw.c | 117 + soem-src/oshw/linux/oshw.h | 31 + soem-src/oshw/macosx/nicdrv.c | 649 +++++ soem-src/oshw/macosx/nicdrv.h | 120 + soem-src/oshw/macosx/oshw.c | 117 + soem-src/oshw/macosx/oshw.h | 31 + soem-src/oshw/rtems/nicdrv.c | 696 +++++ soem-src/oshw/rtems/nicdrv.h | 120 + soem-src/oshw/rtems/oshw.c | 117 + soem-src/oshw/rtems/oshw.h | 31 + soem-src/oshw/rtk/fec/fec_ecat.c | 802 ++++++ soem-src/oshw/rtk/fec/fec_ecat.h | 56 + soem-src/oshw/rtk/lw_mac/lw_emac.c | 423 +++ soem-src/oshw/rtk/lw_mac/lw_emac.h | 12 + soem-src/oshw/rtk/nicdrv.c | 628 +++++ soem-src/oshw/rtk/nicdrv.h | 109 + soem-src/oshw/rtk/oshw.c | 53 + soem-src/oshw/rtk/oshw.h | 35 + soem-src/oshw/vxworks/nicdrv.c | 980 +++++++ soem-src/oshw/vxworks/nicdrv.h | 130 + soem-src/oshw/vxworks/oshw.c | 60 + soem-src/oshw/vxworks/oshw.h | 23 + soem-src/oshw/win32/nicdrv.c | 648 +++++ soem-src/oshw/win32/nicdrv.h | 123 + soem-src/oshw/win32/oshw.c | 121 + soem-src/oshw/win32/oshw.h | 32 + soem-src/oshw/win32/wpcap/Include/Packet32.h | 359 +++ .../win32/wpcap/Include/Win32-Extensions.h | 113 + soem-src/oshw/win32/wpcap/Include/bittypes.h | 137 + soem-src/oshw/win32/wpcap/Include/ip6_misc.h | 163 ++ soem-src/oshw/win32/wpcap/Include/pcap-bpf.h | 47 + .../oshw/win32/wpcap/Include/pcap-namedb.h | 42 + .../oshw/win32/wpcap/Include/pcap-stdinc.h | 93 + soem-src/oshw/win32/wpcap/Include/pcap.h | 45 + .../oshw/win32/wpcap/Include/pcap/bluetooth.h | 48 + soem-src/oshw/win32/wpcap/Include/pcap/bpf.h | 934 +++++++ .../oshw/win32/wpcap/Include/pcap/namedb.h | 89 + soem-src/oshw/win32/wpcap/Include/pcap/pcap.h | 407 +++ soem-src/oshw/win32/wpcap/Include/pcap/sll.h | 129 + soem-src/oshw/win32/wpcap/Include/pcap/usb.h | 90 + soem-src/oshw/win32/wpcap/Include/pcap/vlan.h | 46 + .../oshw/win32/wpcap/Include/remote-ext.h | 444 +++ soem-src/soem/ethercat.h | 26 + soem-src/soem/ethercatbase.c | 637 +++++ soem-src/soem/ethercatbase.h | 63 + soem-src/soem/ethercatcoe.c | 1505 ++++++++++ soem-src/soem/ethercatcoe.h | 95 + soem-src/soem/ethercatconfig.c | 1807 ++++++++++++ soem-src/soem/ethercatconfig.h | 47 + soem-src/soem/ethercatconfiglist.h | 66 + soem-src/soem/ethercatdc.c | 448 +++ soem-src/soem/ethercatdc.h | 33 + soem-src/soem/ethercateoe.c | 634 +++++ soem-src/soem/ethercateoe.h | 213 ++ soem-src/soem/ethercatfoe.c | 416 +++ soem-src/soem/ethercatfoe.h | 33 + soem-src/soem/ethercatmain.c | 2418 +++++++++++++++++ soem-src/soem/ethercatmain.h | 539 ++++ soem-src/soem/ethercatprint.c | 375 +++ soem-src/soem/ethercatprint.h | 34 + soem-src/soem/ethercatsoe.c | 389 +++ soem-src/soem/ethercatsoe.h | 130 + soem-src/soem/ethercattype.h | 581 ++++ soem-src/test/intime/ec_master/ec_master.c | 362 +++ soem-src/test/linux/aliastool.c | 429 +++ soem-src/test/linux/ebox/ebox.c | 386 +++ soem-src/test/linux/eepromtool/eepromtool.c | 485 ++++ soem-src/test/linux/eoe_test/eoe_test.c | 419 +++ soem-src/test/linux/firm_update/firm_update.c | 162 ++ soem-src/test/linux/red_test/red_test.c | 341 +++ soem-src/test/linux/simple_test/simple_test.c | 260 ++ soem-src/test/linux/slaveinfo/slaveinfo.c | 729 +++++ soem-src/test/rtk/main.c | 518 ++++ soem-src/test/simple_ng/simple_ng.c | 327 +++ soem-src/test/win32/ebox/ebox.c | 388 +++ soem-src/test/win32/eepromtool/eepromtool.c | 390 +++ soem-src/test/win32/firm_update/firm_update.c | 159 ++ soem-src/test/win32/red_test/red_test.c | 257 ++ soem-src/test/win32/simple_test/simple_test.c | 375 +++ soem-src/test/win32/slaveinfo/slaveinfo.c | 648 +++++ src/ecat_server.py | 970 ++++++- src/test_motor_backend.py | 409 ++- src/web/ecat.css | 48 +- src/web/motor.html | 20 +- src/web/motor.js | 122 +- 121 files changed, 33088 insertions(+), 75 deletions(-) create mode 100644 build_native.bat create mode 100644 build_native/exports.txt create mode 100644 build_native/one.bat create mode 100644 build_native/wpcap.def create mode 100644 cadence_diag.py create mode 100644 ecat_native.c create mode 100644 exports2def.py create mode 100644 rate_probe.py create mode 100644 soem-src/osal/erika/osal.c create mode 100644 soem-src/osal/erika/osal_defs.h create mode 100644 soem-src/osal/intime/osal.c create mode 100644 soem-src/osal/intime/osal_defs.h create mode 100644 soem-src/osal/linux/osal.c create mode 100644 soem-src/osal/linux/osal_defs.h create mode 100644 soem-src/osal/macosx/osal.c create mode 100644 soem-src/osal/macosx/osal_defs.h create mode 100644 soem-src/osal/osal.h create mode 100644 soem-src/osal/rtems/osal.c create mode 100644 soem-src/osal/rtems/osal_defs.h create mode 100644 soem-src/osal/rtk/osal.c create mode 100644 soem-src/osal/rtk/osal_defs.h create mode 100644 soem-src/osal/vxworks/osal.c create mode 100644 soem-src/osal/vxworks/osal_defs.h create mode 100644 soem-src/osal/win32/osal.c create mode 100644 soem-src/osal/win32/osal_defs.h create mode 100644 soem-src/osal/win32/osal_win32.h create mode 100644 soem-src/oshw/erika/nicdrv.c create mode 100644 soem-src/oshw/erika/nicdrv.h create mode 100644 soem-src/oshw/erika/oshw.c create mode 100644 soem-src/oshw/erika/oshw.h create mode 100644 soem-src/oshw/intime/nicdrv.c create mode 100644 soem-src/oshw/intime/nicdrv.h create mode 100644 soem-src/oshw/intime/oshw.c create mode 100644 soem-src/oshw/intime/oshw.h create mode 100644 soem-src/oshw/linux/nicdrv.c create mode 100644 soem-src/oshw/linux/nicdrv.h create mode 100644 soem-src/oshw/linux/oshw.c create mode 100644 soem-src/oshw/linux/oshw.h create mode 100644 soem-src/oshw/macosx/nicdrv.c create mode 100644 soem-src/oshw/macosx/nicdrv.h create mode 100644 soem-src/oshw/macosx/oshw.c create mode 100644 soem-src/oshw/macosx/oshw.h create mode 100644 soem-src/oshw/rtems/nicdrv.c create mode 100644 soem-src/oshw/rtems/nicdrv.h create mode 100644 soem-src/oshw/rtems/oshw.c create mode 100644 soem-src/oshw/rtems/oshw.h create mode 100644 soem-src/oshw/rtk/fec/fec_ecat.c create mode 100644 soem-src/oshw/rtk/fec/fec_ecat.h create mode 100644 soem-src/oshw/rtk/lw_mac/lw_emac.c create mode 100644 soem-src/oshw/rtk/lw_mac/lw_emac.h create mode 100644 soem-src/oshw/rtk/nicdrv.c create mode 100644 soem-src/oshw/rtk/nicdrv.h create mode 100644 soem-src/oshw/rtk/oshw.c create mode 100644 soem-src/oshw/rtk/oshw.h create mode 100644 soem-src/oshw/vxworks/nicdrv.c create mode 100644 soem-src/oshw/vxworks/nicdrv.h create mode 100644 soem-src/oshw/vxworks/oshw.c create mode 100644 soem-src/oshw/vxworks/oshw.h create mode 100644 soem-src/oshw/win32/nicdrv.c create mode 100644 soem-src/oshw/win32/nicdrv.h create mode 100644 soem-src/oshw/win32/oshw.c create mode 100644 soem-src/oshw/win32/oshw.h create mode 100644 soem-src/oshw/win32/wpcap/Include/Packet32.h create mode 100644 soem-src/oshw/win32/wpcap/Include/Win32-Extensions.h create mode 100644 soem-src/oshw/win32/wpcap/Include/bittypes.h create mode 100644 soem-src/oshw/win32/wpcap/Include/ip6_misc.h create mode 100644 soem-src/oshw/win32/wpcap/Include/pcap-bpf.h create mode 100644 soem-src/oshw/win32/wpcap/Include/pcap-namedb.h create mode 100644 soem-src/oshw/win32/wpcap/Include/pcap-stdinc.h create mode 100644 soem-src/oshw/win32/wpcap/Include/pcap.h create mode 100644 soem-src/oshw/win32/wpcap/Include/pcap/bluetooth.h create mode 100644 soem-src/oshw/win32/wpcap/Include/pcap/bpf.h create mode 100644 soem-src/oshw/win32/wpcap/Include/pcap/namedb.h create mode 100644 soem-src/oshw/win32/wpcap/Include/pcap/pcap.h create mode 100644 soem-src/oshw/win32/wpcap/Include/pcap/sll.h create mode 100644 soem-src/oshw/win32/wpcap/Include/pcap/usb.h create mode 100644 soem-src/oshw/win32/wpcap/Include/pcap/vlan.h create mode 100644 soem-src/oshw/win32/wpcap/Include/remote-ext.h create mode 100644 soem-src/soem/ethercat.h create mode 100644 soem-src/soem/ethercatbase.c create mode 100644 soem-src/soem/ethercatbase.h create mode 100644 soem-src/soem/ethercatcoe.c create mode 100644 soem-src/soem/ethercatcoe.h create mode 100644 soem-src/soem/ethercatconfig.c create mode 100644 soem-src/soem/ethercatconfig.h create mode 100644 soem-src/soem/ethercatconfiglist.h create mode 100644 soem-src/soem/ethercatdc.c create mode 100644 soem-src/soem/ethercatdc.h create mode 100644 soem-src/soem/ethercateoe.c create mode 100644 soem-src/soem/ethercateoe.h create mode 100644 soem-src/soem/ethercatfoe.c create mode 100644 soem-src/soem/ethercatfoe.h create mode 100644 soem-src/soem/ethercatmain.c create mode 100644 soem-src/soem/ethercatmain.h create mode 100644 soem-src/soem/ethercatprint.c create mode 100644 soem-src/soem/ethercatprint.h create mode 100644 soem-src/soem/ethercatsoe.c create mode 100644 soem-src/soem/ethercatsoe.h create mode 100644 soem-src/soem/ethercattype.h create mode 100644 soem-src/test/intime/ec_master/ec_master.c create mode 100644 soem-src/test/linux/aliastool.c create mode 100644 soem-src/test/linux/ebox/ebox.c create mode 100644 soem-src/test/linux/eepromtool/eepromtool.c create mode 100644 soem-src/test/linux/eoe_test/eoe_test.c create mode 100644 soem-src/test/linux/firm_update/firm_update.c create mode 100644 soem-src/test/linux/red_test/red_test.c create mode 100644 soem-src/test/linux/simple_test/simple_test.c create mode 100644 soem-src/test/linux/slaveinfo/slaveinfo.c create mode 100644 soem-src/test/rtk/main.c create mode 100644 soem-src/test/simple_ng/simple_ng.c create mode 100644 soem-src/test/win32/ebox/ebox.c create mode 100644 soem-src/test/win32/eepromtool/eepromtool.c create mode 100644 soem-src/test/win32/firm_update/firm_update.c create mode 100644 soem-src/test/win32/red_test/red_test.c create mode 100644 soem-src/test/win32/simple_test/simple_test.c create mode 100644 soem-src/test/win32/slaveinfo/slaveinfo.c diff --git a/build_native.bat b/build_native.bat new file mode 100644 index 0000000..526f351 --- /dev/null +++ b/build_native.bat @@ -0,0 +1,14 @@ +@echo off +setlocal +call "C:\Program Files\Microsoft Visual Studio\2022\Community\VC\Auxiliary\Build\vcvars64.bat" >nul +cd /d G:\Robot_v2\DCU\tools\ecat +if not exist build_native mkdir build_native +if not exist build_native\wpcap.lib ( + dumpbin /exports C:\Windows\System32\Npcap\wpcap.dll > build_native\exports.txt + venv\Scripts\python.exe exports2def.py build_native\exports.txt build_native\wpcap.def wpcap + lib /def:build_native\wpcap.def /machine:X64 /out:build_native\wpcap.lib + if errorlevel 1 exit /b 1 +) +cl /nologo /O2 /W3 /utf-8 /D_CRT_SECURE_NO_WARNINGS /DWIN32 /I soem-src\soem /I soem-src\osal /I soem-src\osal\win32 /I soem-src\oshw\win32 /I soem-src\oshw\win32\wpcap\Include /Fo:build_native\ ecat_native.c soem-src\soem\ethercatbase.c soem-src\soem\ethercatcoe.c soem-src\soem\ethercatconfig.c soem-src\soem\ethercatdc.c soem-src\soem\ethercateoe.c soem-src\soem\ethercatfoe.c soem-src\soem\ethercatmain.c soem-src\soem\ethercatprint.c soem-src\soem\ethercatsoe.c soem-src\osal\win32\osal.c soem-src\oshw\win32\nicdrv.c soem-src\oshw\win32\oshw.c /link /OUT:build_native\ecat_native.exe build_native\wpcap.lib ws2_32.lib winmm.lib +if errorlevel 1 exit /b 1 +echo [OK] build_native\ecat_native.exe diff --git a/build_native/exports.txt b/build_native/exports.txt new file mode 100644 index 0000000..51cc287 --- /dev/null +++ b/build_native/exports.txt @@ -0,0 +1,148 @@ +Microsoft (R) COFF/PE Dumper Version 14.43.34809.0 +Copyright (C) Microsoft Corporation. All rights reserved. + + +Dump of file C:\Windows\System32\Npcap\wpcap.dll + +File Type: DLL + + Section contains the following exports for wpcap.dll + + 00000000 characteristics + FFFFFFFF time date stamp + 0.00 version + 1 ordinal base + 119 number of functions + 119 number of names + + ordinal hint RVA name + + 1 0 00001010 bpf_dump + 2 1 00001170 bpf_filter + 3 2 00001C40 bpf_image + 4 3 00001190 bpf_validate + 5 4 00073000 eproto_db + 6 5 00018850 pcap_activate + 7 6 000189C0 pcap_breakloop + 8 7 000189E0 pcap_bufsize + 9 8 00018A00 pcap_can_set_rfmon + 10 9 00018A40 pcap_close + 11 A 00012DD0 pcap_compile + 12 B 000131B0 pcap_compile_nopcap + 13 C 00018A70 pcap_create + 14 D 00018BC0 pcap_createsrcstr + 15 E 00018BF0 pcap_datalink + 16 F 00018C10 pcap_datalink_ext + 17 10 00018C30 pcap_datalink_name_to_val + 18 11 00018D10 pcap_datalink_val_to_description + 19 12 00018D60 pcap_datalink_val_to_description_or_dlt + 20 13 00018E00 pcap_datalink_val_to_name + 21 14 00018E50 pcap_dispatch + 22 15 0001DED0 pcap_dump + 23 16 0001DF60 pcap_dump_close + 24 17 0001DF90 pcap_dump_file + 25 18 0001DFA0 pcap_dump_flush + 26 19 0001DFC0 pcap_dump_ftell + 27 1A 0001DFD0 pcap_dump_ftell64 + 28 1B 0001DFE0 pcap_dump_hopen + 29 1C 0001E0F0 pcap_dump_open + 30 1D 0001E240 pcap_dump_open_append + 31 1E 000136B0 pcap_ether_aton + 32 1F 00013720 pcap_ether_hostton + 33 20 00018E70 pcap_file + 34 21 00018E80 pcap_fileno + 35 22 00018EA0 pcap_findalldevs + 36 23 000220E0 pcap_findalldevs_ex + 37 24 00019070 pcap_free_datalinks + 38 25 00019070 pcap_free_tstamp_types + 39 26 00019080 pcap_freealldevs + 40 27 00013230 pcap_freecode + 41 28 00019140 pcap_get_airpcap_handle + 42 29 000191D0 pcap_get_tstamp_precision + 43 2A 000191E0 pcap_geterr + 44 2B 000191F0 pcap_getevent + 45 2C 00019270 pcap_getnonblock + 46 2D 0001C130 pcap_hopen_offline + 47 2E 0001C140 pcap_hopen_offline_with_tstamp_precision + 48 2F 00019340 pcap_init + 49 30 000193D0 pcap_inject + 50 31 00019490 pcap_is_swapped + 51 32 0001FF90 pcap_lib_version + 52 33 000194B0 pcap_list_datalinks + 53 34 00019580 pcap_list_tstamp_types + 54 35 00019640 pcap_live_dump + 55 36 00019690 pcap_live_dump_ended + 56 37 00020050 pcap_lookupdev + 57 38 000202B0 pcap_lookupnet + 58 39 000196A0 pcap_loop + 59 3A 00019720 pcap_major_version + 60 3B 00019740 pcap_minor_version + 61 3C 00013800 pcap_nametoaddr + 62 3D 00013870 pcap_nametoaddrinfo + 63 3E 000138C0 pcap_nametoeproto + 64 3F 00013920 pcap_nametollc + 65 40 000136A0 pcap_nametonetaddr + 66 41 00013980 pcap_nametoport + 67 42 00013B40 pcap_nametoportrange + 68 43 00013BF0 pcap_nametoproto + 69 44 00019760 pcap_next + 70 45 00002360 pcap_next_etherent + 71 46 000197B0 pcap_next_ex + 72 47 00019810 pcap_offline_filter + 73 48 00019830 pcap_oid_get_request + 74 49 00019880 pcap_oid_set_request + 75 4A 000225D0 pcap_open + 76 4B 000198D0 pcap_open_dead + 77 4C 000198E0 pcap_open_dead_with_tstamp_precision + 78 4D 00019B10 pcap_open_live + 79 4E 0001C200 pcap_open_offline + 80 4F 0001C210 pcap_open_offline_with_tstamp_precision + 81 50 0001A3D0 pcap_parsesrcstr + 82 51 0001A6D0 pcap_perror + 83 52 00023F70 pcap_remoteact_accept + 84 53 00023FA0 pcap_remoteact_accept_ex + 85 54 00024350 pcap_remoteact_cleanup + 86 55 00024380 pcap_remoteact_close + 87 56 00024510 pcap_remoteact_list + 88 57 0001A740 pcap_sendpacket + 89 58 0001A7A0 pcap_sendqueue_alloc + 90 59 0001A800 pcap_sendqueue_destroy + 91 5A 0001A820 pcap_sendqueue_queue + 92 5B 0001A890 pcap_sendqueue_transmit + 93 5C 0001A8D0 pcap_set_buffer_size + 94 5D 0001A910 pcap_set_datalink + 95 5E 0001AA80 pcap_set_immediate_mode + 96 5F 0001AAC0 pcap_set_promisc + 97 60 0001AB00 pcap_set_rfmon + 98 61 0001AB40 pcap_set_snaplen + 99 62 0001AB80 pcap_set_timeout + 100 63 0001ABC0 pcap_set_tstamp_precision + 101 64 0001AC50 pcap_set_tstamp_type + 102 65 0001ACE0 pcap_setbuff + 103 66 0001AD20 pcap_setdirection + 104 67 0001ADD0 pcap_setfilter + 105 68 0001AE10 pcap_setmintocopy + 106 69 0001AE50 pcap_setmode + 107 6A 0001AE90 pcap_setnonblock + 108 6B 00022850 pcap_setsampling + 109 6C 0001AF00 pcap_setuserbuffer + 110 6D 0001AF40 pcap_snapshot + 111 6E 0001AF60 pcap_stats + 112 6F 0001AFA0 pcap_stats_ex + 113 70 0001AFE0 pcap_statustostr + 114 71 0001B150 pcap_strerror + 115 72 0001B1B0 pcap_tstamp_type_name_to_val + 116 73 0001B290 pcap_tstamp_type_val_to_description + 117 74 0001B2E0 pcap_tstamp_type_val_to_name + 118 75 00073120 pcap_version + 119 76 000136A0 pcap_wsockinit + + Summary + + A000 .data + 1000 .fptable + 4000 .pdata + 26000 .rdata + 1000 .reloc + 1000 .rsrc + 4C000 .text diff --git a/build_native/one.bat b/build_native/one.bat new file mode 100644 index 0000000..8513fbd --- /dev/null +++ b/build_native/one.bat @@ -0,0 +1,4 @@ +@echo off +call "C:\Program Files\Microsoft Visual Studio\2022\Community\VC\Auxiliary\Build\vcvars64.bat" >nul +cd /d G:\Robot_v2\DCU\tools\ecat +cl /nologo /c /O2 /W3 /utf-8 /D_CRT_SECURE_NO_WARNINGS /I soem-src\soem /I soem-src\osal /I soem-src\osal\win32 /I soem-src\oshw\win32 /I soem-src\oshw\win32\wpcap\Include /Fobuild_native\ soem-src\oshw\win32\nicdrv.c 2>&1 diff --git a/build_native/wpcap.def b/build_native/wpcap.def new file mode 100644 index 0000000..5d7f317 --- /dev/null +++ b/build_native/wpcap.def @@ -0,0 +1,121 @@ +LIBRARY wpcap +EXPORTS +bpf_dump +bpf_filter +bpf_image +bpf_validate +eproto_db +pcap_activate +pcap_breakloop +pcap_bufsize +pcap_can_set_rfmon +pcap_close +pcap_compile +pcap_compile_nopcap +pcap_create +pcap_createsrcstr +pcap_datalink +pcap_datalink_ext +pcap_datalink_name_to_val +pcap_datalink_val_to_description +pcap_datalink_val_to_description_or_dlt +pcap_datalink_val_to_name +pcap_dispatch +pcap_dump +pcap_dump_close +pcap_dump_file +pcap_dump_flush +pcap_dump_ftell +pcap_dump_ftell64 +pcap_dump_hopen +pcap_dump_open +pcap_dump_open_append +pcap_ether_aton +pcap_ether_hostton +pcap_file +pcap_fileno +pcap_findalldevs +pcap_findalldevs_ex +pcap_free_datalinks +pcap_free_tstamp_types +pcap_freealldevs +pcap_freecode +pcap_get_airpcap_handle +pcap_get_tstamp_precision +pcap_geterr +pcap_getevent +pcap_getnonblock +pcap_hopen_offline +pcap_hopen_offline_with_tstamp_precision +pcap_init +pcap_inject +pcap_is_swapped +pcap_lib_version +pcap_list_datalinks +pcap_list_tstamp_types +pcap_live_dump +pcap_live_dump_ended +pcap_lookupdev +pcap_lookupnet +pcap_loop +pcap_major_version +pcap_minor_version +pcap_nametoaddr +pcap_nametoaddrinfo +pcap_nametoeproto +pcap_nametollc +pcap_nametonetaddr +pcap_nametoport +pcap_nametoportrange +pcap_nametoproto +pcap_next +pcap_next_etherent +pcap_next_ex +pcap_offline_filter +pcap_oid_get_request +pcap_oid_set_request +pcap_open +pcap_open_dead +pcap_open_dead_with_tstamp_precision +pcap_open_live +pcap_open_offline +pcap_open_offline_with_tstamp_precision +pcap_parsesrcstr +pcap_perror +pcap_remoteact_accept +pcap_remoteact_accept_ex +pcap_remoteact_cleanup +pcap_remoteact_close +pcap_remoteact_list +pcap_sendpacket +pcap_sendqueue_alloc +pcap_sendqueue_destroy +pcap_sendqueue_queue +pcap_sendqueue_transmit +pcap_set_buffer_size +pcap_set_datalink +pcap_set_immediate_mode +pcap_set_promisc +pcap_set_rfmon +pcap_set_snaplen +pcap_set_timeout +pcap_set_tstamp_precision +pcap_set_tstamp_type +pcap_setbuff +pcap_setdirection +pcap_setfilter +pcap_setmintocopy +pcap_setmode +pcap_setnonblock +pcap_setsampling +pcap_setuserbuffer +pcap_snapshot +pcap_stats +pcap_stats_ex +pcap_statustostr +pcap_strerror +pcap_tstamp_type_name_to_val +pcap_tstamp_type_val_to_description +pcap_tstamp_type_val_to_name +pcap_version +pcap_wsockinit diff --git a/cadence_diag.py b/cadence_diag.py new file mode 100644 index 0000000..2097f8e --- /dev/null +++ b/cadence_diag.py @@ -0,0 +1,141 @@ +#!/usr/bin/env python3 +"""归因"某一拍为什么超时"：把 PDO 时间线与 GC / HTTP 事件时间线做交集。 + +背景：250us 和 200us 档的达成率都是 96~97%，几乎不随档位变化 —— 丢拍不像 +"总线差几十 us"这种比例性原因，更像固定频率的事件(GC 停止世界、HTTP 线程 +持 GIL 序列化快照、系统杂活)一撞就是一整个时隙。本脚本从 /api/motor/dbg +拉三条原始时间线，对每一个超时拍给出责任归属。 + +用法(目标服务必须已在跑，本脚本自己不会拉服务)： + python cadence_diag.py --period 0.25 --sample 6 + python cadence_diag.py --period 0.2 --gc off # A/B：关掉 GC 看超时拍是否消失 + python cadence_diag.py --period 0.2 --gc on --hammer 2 # 再叠 2 路 /motor/state 轮询 + +判读： + 超时拍 = loop_end-t0(work) 超过设定周期的拍；它们正是网格跳掉的拍。 + 归因窗口 = 上一拍 loop_end → 本拍 loop_end；GC/HTTP 事件落在窗口内即认领。 + "裸超时"占大头才说明是内核/中断层面的抢占，而不是进程内流程问题。 +""" +import argparse +import json +import statistics +import threading +import time +import urllib.request + +BASE_DEFAULT = "http://127.0.0.1:8090" + + +def api(base, path, req): + r = urllib.request.Request(base + "/api/" + path, + data=json.dumps(req).encode(), + headers={"Content-Type": "application/json"}) + with urllib.request.urlopen(r, timeout=30) as resp: + return json.loads(resp.read().decode()) + + +def hammer_worker(base, stop, path="motor/state"): + while not stop.is_set(): + try: + api(base, path, {}) + except Exception: + pass + + +def classify(beats, gc_ev, http_ev, period_us): + """给每个超时拍贴标签。返回 (late列表, 计数统计)。beats: 按 t0 排序的样本。""" + counts = {"GC": 0, "HTTP": 0, "GC+HTTP": 0, "裸超时": 0} + late = [] + for i in range(1, len(beats)): + t0, ts, tm, tr, te, k = beats[i] + work = (te - t0) * 1e6 + if work <= period_us: + continue + w0, w1 = beats[i - 1][4], te # 上一拍结束 → 本拍结束 + g = [e for e in gc_ev if w0 <= e[0] <= w1 and e[1] > 20.0] + h = [x for x in http_ev if x[0] < w1 and x[1] > w0 and (x[1] - x[0]) * 1e6 > 20.0] + if g and h: + tag = "GC+HTTP" + elif g: + tag = "GC(gen%d %.0fus)" % (g[-1][2], g[-1][1]) + counts["GC"] += 1 + elif h: + tag = "HTTP(%s %.0fus)" % (h[-1][2].replace("/api/", ""), (h[-1][1] - h[-1][0]) * 1e6) + counts["HTTP"] += 1 + else: + tag = "裸超时" + counts["裸超时"] += 1 + if g and h: + counts["GC+HTTP"] += 1 + late.append((work, k, tag, (t0 - beats[i - 1][4]) * 1e6)) + return late, counts + + +def main(): + ap = argparse.ArgumentParser() + ap.add_argument("--base", default=BASE_DEFAULT) + ap.add_argument("--period", type=float, required=True, help="周期 ms(0.25=250us)") + ap.add_argument("--sample", type=float, default=6.0, help="设周期后预热秒数，之后才取窗口") + ap.add_argument("--gc", choices=["on", "off", "keep"], default="keep") + ap.add_argument("--hammer", type=int, default=0, help="叠加 N 路 /motor/state 轮询线程") + args = ap.parse_args() + per_us = args.period * 1000.0 + + if args.gc != "keep": + r = api(args.base, "motor/dbg", {"action": "gc", "enable": args.gc == "on"}) + print(f"GC → {r.get('gc_enabled')}") + + r = api(args.base, "motor/period", {"period_ms": args.period}) + print("period:", r.get("log")) + if r.get("spin") is False and args.period < 1: + print("!! 自旋没生效，这组数据没有意义") + return 1 + + stop = threading.Event() + threads = [threading.Thread(target=hammer_worker, args=(args.base, stop), daemon=True) + for _ in range(args.hammer)] + for t in threads: + t.start() + try: + time.sleep(args.sample) + d = api(args.base, "motor/dbg", {}) + finally: + stop.set() + for t in threads: + t.join(timeout=1) + + beats = sorted(d["cyc"])[:-50] # 尾部几十拍被 dump 自身的 GIL 占用污染，丢弃 + if len(beats) < 100: + print("样本不足", len(beats)) + return 1 + iv = [(beats[i][0] - beats[i - 1][0]) * 1e6 for i in range(1, len(beats))] + hz = 1e6 / statistics.fmean(iv) + work = sorted((e - t) * 1e6 for t, _, _, _, e, _ in beats) + print(f"\n窗口 {len(beats)} 拍 ≈ {sum(iv) / 1e6:.2f}s | 周期 {per_us:.0f}us | " + f"实测 {hz:.0f}Hz (达成 {hz / (1e6 / per_us) * 100:.1f}%) | " + f"跳拍合计 {sum(k for *_, k in beats)}") + gc_in_win = [e[2] for e in d["gc_events"] if beats[0][0] <= e[0] <= beats[-1][4]] + print(f"work p50/p90/max = {work[len(work) // 2]:.0f}/{work[int(len(work) * .9)]:.0f}/" + f"{work[-1]:.0f}us | 窗口内 GC g0/g1/g2 = " + f"{gc_in_win.count(0)}/{gc_in_win.count(1)}/{gc_in_win.count(2)}") + + # 事件按窗口时间过滤，再交集 + w0, w1 = beats[0][0], beats[-1][4] + gc_ev = [e for e in d["gc_events"] if w0 <= e[0] <= w1] + http_ev = [x for x in d["http"] if x[1] >= w0 and x[0] <= w1] + late, counts = classify(beats, gc_ev, http_ev, per_us) + n_late = len(late) + print(f"超时拍 {n_late}/{len(beats)} = {n_late / len(beats) * 100:.2f}% | " + + " | ".join(f"{k}: {v}" for k, v in counts.items())) + if late: + lateness = sorted(x[0] for x in late) + print(f"超时拍的 work: 中位 {statistics.median(lateness):.0f}us, " + f"p90 {lateness[int(len(lateness) * .9)]:.0f}us, max {lateness[-1]:.0f}us") + print("最狠的 8 拍:") + for work_us, k, tag, gap in sorted(late, reverse=True)[:8]: + print(f" work={work_us:7.0f}us 跳{k}拍 上游空闲{gap:6.0f}us {tag}") + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/ecat_native.c b/ecat_native.c new file mode 100644 index 0000000..688bff2 --- /dev/null +++ b/ecat_native.c @@ -0,0 +1,397 @@ +/* ecat_native.c — 原型 A：原生 PDO 守护进程（SOEM 直用，无 Python/GIL） + * + * 目的：验证"换语言"能把 PDO 循环的天花板从 Python 的 ~5kHz 抬到多高、 + * 尾抖动能多干净。总线核心收发本来就是 C（pysoem 只是 SOEM 的包装）， + * 这里把热循环从解释器里拿出来，其余一切不动。 + * + * 契约：复刻 ecat_server.py 的 3 条热路由（POST /api/motor/state|period|dbg）， + * 只绑 127.0.0.1、无 CORS —— 与 Python 服务同口径，cadence_diag/rate_probe + * 直接 --base http://127.0.0.1:8091 就能打过来。其余路由仍留在 Python。 + * + * 时间线元组与 Python 完全同构：(t0, ts, tm, tr, te, k) + * t0=循环顶 ts=send 前(原生无锁等待，≈t0) tm=send 后 tr=receive 后 + * te=循环底(跳拍判定时刻) k=本拍网格跳过数 + * 时刻用 QueryPerformanceCounter 秒 —— 与 Python perf_counter 同一时间基 + * （实测差 ≤6µs），cadence_diag 的差分口径不变。 + */ +#include +#include +#include +#include +#include +#include +#include +#include "ethercat.h" + +#define RING 2048 /* 与 Python _cyc_t maxlen 同 */ +#define IO_MAP_SIZE 4096 +#define STATS_N 400 /* state 里 cyc_us 分位数的样本窗口，同 _cyc_stats(n=400) */ + +/* ── 计时原语 ── */ +static double g_qpc2s; +static inline double now_s(void) +{ + LARGE_INTEGER c; + QueryPerformanceCounter(&c); + return (double)c.QuadPart * g_qpc2s; +} +static inline void spin_until(double deadline) +{ + while (now_s() < deadline) + YieldProcessor(); /* PAUSE：SMT 兄弟友好，不烧功耗 */ +} + +/* ── 共享状态 ── */ +typedef struct { double t0, ts, tm, tr, te; int k; } beat_t; + +static beat_t g_ring[RING]; +static int g_ring_i; /* 写指针：仅 PDO 线程动 */ +static long long g_total; /* 累计拍数 */ +static long long g_skip_total; /* 累计网格跳过拍 */ +static CRITICAL_SECTION g_ring_lock; /* 保护 ring/total/skip 的读侧 */ +static CRITICAL_SECTION g_cfg_lock; /* 保护 g_period_ms */ +static double g_period_ms = 0.25; +static volatile LONG g_stop; +static volatile LONG g_wkc; +static long long g_expected_wkc; +static int g_slavecount; +static char g_slave_names[16][64]; +static uint8 g_iomap[IO_MAP_SIZE]; +static ULONGLONG g_pcore_mask; /* --mask，0=不钉 */ + +/* ── PDO 线程：网格 + 自旋 + 分段计时，逻辑与 _pdo_worker 一一对应 ── */ +static DWORD WINAPI pdo_worker(LPVOID arg) +{ + double per, t0, ts, tm, tr, now, te, next_t = 0; + int have_next = 0, k, i, wkc; + + SetThreadPriority(GetCurrentThread(), THREAD_PRIORITY_TIME_CRITICAL); + if (g_pcore_mask) + SetThreadAffinityMask(GetCurrentThread(), (DWORD_PTR)g_pcore_mask); + + while (!InterlockedOr(&g_stop, 0)) { + t0 = now_s(); + EnterCriticalSection(&g_cfg_lock); + per = g_period_ms / 1000.0; + LeaveCriticalSection(&g_cfg_lock); + if (!have_next) { next_t = t0 + per; have_next = 1; } + + ts = now_s(); /* 原生无锁无填充 → pre≈0 */ + ec_send_processdata(); + tm = now_s(); + wkc = ec_receive_processdata(EC_TIMEOUTRET); + tr = now_s(); + InterlockedExchange(&g_wkc, wkc); + + /* 绝对网格截止点 + 超期跳过（不挪锚点），与 Python 同 */ + now = now_s(); + k = 0; + if (next_t <= now) { + while (next_t <= now) { next_t += per; k++; } + } + te = now; + + EnterCriticalSection(&g_ring_lock); + i = g_ring_i; + g_ring_i = (i + 1) % RING; + g_ring[i].t0 = t0; g_ring[i].ts = ts; g_ring[i].tm = tm; + g_ring[i].tr = tr; g_ring[i].te = te; g_ring[i].k = k; + g_total++; + g_skip_total += k; + LeaveCriticalSection(&g_ring_lock); + + if (k == 0) { /* 没超期才等下一拍；超期直接落下去补 */ + spin_until(next_t); + next_t += per; + } + } + return 0; +} + +/* ── 分位数（读侧现算，同 _cyc_fresh 的口径）── */ +static int cmp_d(const void *a, const void *b) +{ + double x = *(const double *)a, y = *(const double *)b; + return (x > y) - (x < y); +} +static void pct3(const double *v, int n, char *out, size_t osz) +{ + double *s = (double *)malloc(n * sizeof(double)); + int i; + if (!s) { snprintf(out, osz, "{}"); return; } + memcpy(s, v, n * sizeof(double)); + qsort(s, n, sizeof(double), cmp_d); + snprintf(out, osz, "{\"p50\":%.1f,\"p90\":%.1f,\"max\":%.1f}", + s[(int)(0.5 * (n - 1))], s[(int)(0.9 * (n - 1))], s[n - 1]); + free(s); +} + +/* 取最近 n 拍（按时间序）。ring 单写者，读侧加锁拷贝。 */ +static int recent_beats(beat_t *dst, int n) +{ + int have, start, i; + EnterCriticalSection(&g_ring_lock); + have = (g_total < RING) ? (int)g_total : RING; + if (n > have) n = have; + start = (g_ring_i - n + RING * 2) % RING; + for (i = 0; i < n; i++) dst[i] = g_ring[(start + i) % RING]; + LeaveCriticalSection(&g_ring_lock); + return n; +} + +static void build_cyc_us(char *out, size_t osz) +{ + static beat_t b[RING]; + static double work[STATS_N], send[STATS_N], rx[STATS_N], tot[STATS_N]; + char sw[96], sr[96], st[96], swork[96]; + int n = recent_beats(b, RING), i, m = n < STATS_N ? n : STATS_N; + if (m < 2) { snprintf(out, osz, "{}"); return; } + for (i = 0; i < m; i++) { + beat_t *p = &b[n - m + i]; + work[i] = (p->te - p->t0) * 1e6; + send[i] = (p->tm - p->ts) * 1e6; + rx[i] = (p->tr - p->tm) * 1e6; + tot[i] = (p->tr - p->t0) * 1e6; + } + pct3(work, m, swork, sizeof swork); + pct3(send, m, sw, sizeof sw); + pct3(rx, m, sr, sizeof sr); + pct3(tot, m, st, sizeof st); + snprintf(out, osz, + "\"cyc_us\":{\"n\":%d,\"work\":%s,\"send\":%s,\"rx\":%s,\"total\":%s," + "\"late\":0,\"lost\":0,\"gc\":{\"n\":[0,0,0],\"max_us\":[0,0,0],\"pdo\":0}}", + m, swork, sw, sr, st); +} + +/* ── 极简 HTTP/1.1（POST-only JSON，thread-per-connection）── */ +static int json_num(const char *body, const char *key, double *val) +{ + const char *p = strstr(body, key); + if (!p) return 0; + p += strlen(key); + while (*p && *p != ':') p++; + if (*p != ':') return 0; + p++; + while (*p == ' ' || *p == '\t') p++; + char *end; + double v = strtod(p, &end); + if (end == p) return 0; + *val = v; + return 1; +} + +static void handle_conn(SOCKET cs) +{ + char *buf = (char *)malloc(65536); + int len = 0, i, cl = 0; + char *hdr_end, *body, path[128] = ""; + char *resp = (char *)malloc(256 * 1024); + int rl = 0; + char extra[4096]; + + if (!buf || !resp) { closesocket(cs); free(buf); free(resp); return; } + /* 读到头 */ + for (;;) { + int n = recv(cs, buf + len, 65535 - len, 0); + if (n <= 0) goto done; + len += n; buf[len] = 0; + hdr_end = strstr(buf, "\r\n\r\n"); + if (hdr_end) break; + } + body = hdr_end + 4; + { /* Content-Length + 收满 body */ + char *p = buf; + for (p = strstr(p, "\n"); p && p < hdr_end; p = strstr(p + 1, "\n")) { + /* p 停在上一行的 \n 上，下一行从 p+1 开始 */ + if (!_strnicmp(p + 1, "content-length:", 15)) { cl = atoi(p + 16); break; } + } + while ((body + cl) > (buf + len)) { + int need = (int)((body + cl) - (buf + len)); + int r = recv(cs, buf + len, need > 65535 - len ? 65535 - len : need, 0); + if (r <= 0) goto done; + len += r; buf[len] = 0; + } + if (cl > 65535 - (int)(body - buf)) cl = (int)(buf + len - body); + body[cl] = 0; + } + if (sscanf(buf, "%*s %127s", path) != 1) goto done; + + if (!strcmp(path, "/api/motor/state")) { + build_cyc_us(extra, sizeof extra); + rl = snprintf(resp, 256 * 1024, + "{\"ok\":true,\"log\":[],\"cycles\":%lld,\"period_ms\":%.4f,\"spin\":true," + "\"wkc\":%d,\"expected_wkc\":%d,\"slot_period_ms\":%.4f,\"win_bases\":0," + "\"queue_depth\":0,\"sent_seq\":0,\"sent_frames\":0,\"ts\":0," + "\"head\":{},\"motors\":[", + g_total, g_period_ms, (int)g_wkc, (int)g_expected_wkc, g_period_ms); + for (i = 0; i < g_slavecount && i < 16; i++) + rl += snprintf(resp + rl, 256 * 1024 - rl, "%s{\"name\":\"%s\",\"valid\":true}", + i ? "," : "", g_slave_names[i]); + rl += snprintf(resp + rl, 256 * 1024 - rl, "],\"bridge\":{},\"xrx\":{},\"rx_batch\":{},%s}", extra); + } else if (!strcmp(path, "/api/motor/period")) { + double v = -1, old = g_period_ms; + if (json_num(body, "period_us", &v) && v > 0) v /= 1000.0; + else if (!json_num(body, "period_ms", &v)) v = -1; + if (v < 0.0625 || v > 1000.0) { + rl = snprintf(resp, 256 * 1024, + "{\"ok\":false,\"log\":[\"周期 %s 超出允许范围 [62.5us, 1000ms]\"]}", body); + } else { + EnterCriticalSection(&g_cfg_lock); + g_period_ms = v; + LeaveCriticalSection(&g_cfg_lock); + rl = snprintf(resp, 256 * 1024, + "{\"ok\":true,\"log\":[\"PDO 周期(native) %gms → %gms\"],\"period_ms\":%.4f}", + old, v, v); + } + } else if (!strcmp(path, "/api/motor/dbg")) { + static beat_t b[RING]; + int n = recent_beats(b, RING); + rl = snprintf(resp, 256 * 1024, + "{\"ok\":true,\"period_ms\":%.4f,\"spin\":true,\"gc_enabled\":false," + "\"cycles\":%lld,\"skip_total\":%lld,\"cyc\":[", + g_period_ms, g_total, g_skip_total); + for (i = 0; i < n; i++) + rl += snprintf(resp + rl, 256 * 1024 - rl, + "%s[%.7f,%.7f,%.7f,%.7f,%.7f,%d]", + i ? "," : "", b[i].t0, b[i].ts, b[i].tm, b[i].tr, b[i].te, b[i].k); + rl += snprintf(resp + rl, 256 * 1024 - rl, "],\"gc_events\":[],\"http\":[]}"); + } else if (!strcmp(path, "/") || !strcmp(path, "/api/native/health")) { + rl = snprintf(resp, 256 * 1024, + "{\"ok\":true,\"name\":\"ecat_native\",\"slaves\":%d,\"wkc\":%d,\"cycles\":%lld}", + g_slavecount, (int)g_wkc, g_total); + } else { + rl = snprintf(resp, 256 * 1024, + "{\"ok\":false,\"log\":[\"接口不存在(native 原型只有 state/period/dbg): %s\"]}", path); + } + { /* 发送 */ + char hdr[256]; + int hl = snprintf(hdr, sizeof hdr, + "HTTP/1.1 200 OK\r\nContent-Type: application/json; charset=utf-8\r\n" + "Cache-Control: no-store\r\nContent-Length: %d\r\nConnection: close\r\n\r\n", rl); + send(cs, hdr, hl, 0); + send(cs, resp, rl, 0); + } +done: + closesocket(cs); + free(buf); + free(resp); +} + +static DWORD WINAPI conn_thread(LPVOID p) +{ + handle_conn((SOCKET)(intptr_t)p); + return 0; +} + +int main(int argc, char **argv) +{ + const char *substr = "I226"; + int port = 8091, i; + LARGE_INTEGER f; + ec_adaptert *ad, *pick = NULL; + WSADATA wsa; + SOCKET ls; + struct sockaddr_in sa; + HANDLE th; + + for (i = 1; i < argc - 0; i++) { + if (!strcmp(argv[i], "--substr") && i + 1 < argc) substr = argv[++i]; + else if (!strcmp(argv[i], "--port") && i + 1 < argc) port = atoi(argv[++i]); + else if (!strcmp(argv[i], "--mask") && i + 1 < argc) + g_pcore_mask = strtoull(argv[++i], NULL, 0); + } + + QueryPerformanceFrequency(&f); + g_qpc2s = 1.0 / (double)f.QuadPart; + InitializeCriticalSection(&g_ring_lock); + InitializeCriticalSection(&g_cfg_lock); + + /* 找网卡（desc/name 含 substr） */ + for (ad = ec_find_adapters(); ad; ad = ad->next) { + if (strstr(ad->desc, substr) || strstr(ad->name, substr)) { pick = ad; break; } + } + if (!pick) { + fprintf(stderr, "[native] 找不到含 \"%s\" 的网卡\n", substr); + return 2; + } + if (!ec_init(pick->name)) { + fprintf(stderr, "[native] ec_init 失败: %s\n", pick->name); + return 3; + } + printf("[native] 打开网卡: %s\n", pick->desc); + + /* 预清理：广播 INIT|ACK 清掉上一主站(可能被强杀)残留的 AL 错误位，再重枚举。 + SSC 的 AL_ControlInd 对"错误位已置 + 请求不含 ACK(bit4)"的迁移直接忽略 + (ecatslv.c)，这是 native 之前卡 PREOP 的根因 —— 与 Python motor/start 的 + 预清理同理。config_init 会重写 SM0/SM1，等效"断开重连"。 */ + ec_slave[0].state = EC_STATE_INIT | EC_STATE_ACK; + ec_writestate(0); + ec_statecheck(0, EC_STATE_INIT, 1000000); + if (ec_config_init(FALSE) <= 0) { + fprintf(stderr, "[native] 枚举从站失败（邮箱楔死需断电重启板子）\n"); + ec_close(); + return 4; + } + ec_config_map(&g_iomap); + ec_readstate(); + /* LRW 的期望 WKC：每从站读输出+1、写输入+1、回读+1 → outputsWKC*2 + inputsWKC + （与 pysoem 同式；本机 1 从站 = 1*2+1 = 3，与实测 wkc 一致） */ + g_expected_wkc = ec_group[0].outputsWKC * 2 + ec_group[0].inputsWKC; + g_slavecount = ec_slavecount; + for (i = 1; i <= ec_slavecount && i <= 16; i++) + snprintf(g_slave_names[i - 1], 64, "%s", ec_slave[i].name); + printf("[native] 枚举 %d 从站, config_map 后 AL=0x%02x, expected_wkc=%lld\n", + ec_slavecount, ec_slave[1].state & 0x0F, g_expected_wkc); + + /* config_map 内部请求 SAFEOP 不带 ACK，残留错误位下会被忽略 → 显式补一次 + SAFEOP|ACK 写，确保到 SAFEOP。 */ + ec_slave[0].state = EC_STATE_SAFE_OP | EC_STATE_ACK; + ec_writestate(0); + ec_statecheck(0, EC_STATE_SAFE_OP, 2000000); + + /* SAFEOP→OP：从站要先收到≥1帧过程数据才 ack OP，且带错误位时只有 OP|ACK + 才清错并迁移 → 边发 PD 边带 ACK 重写 OP，每 200ms 一轮，readstate 拿真实态。 + 进 OP 后再起稳态线程（避免主线程与线程并发操作同一 ecx_port 打架）。 */ + for (i = 0; i < 60; i++) { + ec_send_processdata(); + ec_receive_processdata(EC_TIMEOUTRET); + ec_slave[0].state = EC_STATE_OPERATIONAL | EC_STATE_ACK; + ec_writestate(0); + ec_readstate(); + if ((ec_slave[1].state & 0x0F) == EC_STATE_OPERATIONAL) break; + Sleep(200); + } + if ((ec_slave[1].state & 0x0F) != EC_STATE_OPERATIONAL) { + fprintf(stderr, "[native] 进 OP 失败（AL=0x%02x code=0x%04x）—— 若反复如此，" + "可能是上一主站退出后 SSC 邮箱楔死，需断电重启板子\n", + ec_slave[1].state & 0x0F, ec_slave[1].ALstatuscode); + ec_close(); + return 5; + } + printf("[native] OP 达成: %d 从站\n", ec_slavecount); + + th = CreateThread(NULL, 0, pdo_worker, NULL, 0, NULL); + SetThreadPriority(th, THREAD_PRIORITY_TIME_CRITICAL); + + WSAStartup(MAKEWORD(2, 2), &wsa); + ls = socket(AF_INET, SOCK_STREAM, 0); + { int one = 1; setsockopt(ls, SOL_SOCKET, SO_REUSEADDR, (char *)&one, sizeof one); } + sa.sin_family = AF_INET; + sa.sin_port = htons((u_short)port); + sa.sin_addr.s_addr = htonl(INADDR_LOOPBACK); /* 只绑 127.0.0.1，同 Python 口径 */ + if (bind(ls, (struct sockaddr *)&sa, sizeof sa) == SOCKET_ERROR) { + fprintf(stderr, "[native] bind %d 失败: %d\n", port, WSAGetLastError()); + return 6; + } + listen(ls, 32); + printf("[native] 监听 http://127.0.0.1:%d (state/period/dbg)\n", port); + + for (;;) { + SOCKET cs = accept(ls, NULL, NULL); + HANDLE t; + if (cs == INVALID_SOCKET) continue; + t = CreateThread(NULL, 0, conn_thread, (LPVOID)(intptr_t)cs, 0, NULL); + if (t) CloseHandle(t); else closesocket(cs); + } + return 0; +} diff --git a/exports2def.py b/exports2def.py new file mode 100644 index 0000000..973dd12 --- /dev/null +++ b/exports2def.py @@ -0,0 +1,28 @@ +"""exports2def.py — 把 dumpbin /exports 的输出转成 .def（给 lib.exe 生成导入库用）。 + +用法: python exports2def.py +""" +import re +import sys + +def main(): + src, dst, libname = sys.argv[1], sys.argv[2], sys.argv[3] + names = set() + pat = re.compile(r"^\s+\d+\s+[0-9A-Fa-f]+\s+[0-9A-Fa-f]+\s+(\S+)") + with open(src, encoding="utf-8", errors="ignore") as f: + for line in f: + m = pat.match(line) + if m: + names.add(m.group(1)) + if not names: + print("exports2def: 没解析出任何导出名", file=sys.stderr) + return 1 + with open(dst, "w", encoding="ascii", newline="\r\n") as f: + f.write(f"LIBRARY {libname}\nEXPORTS\n") + for n in sorted(names): + f.write(n + "\n") + print(f"exports2def: {len(names)} 个导出 → {dst}") + return 0 + +if __name__ == "__main__": + sys.exit(main()) diff --git a/rate_probe.py b/rate_probe.py new file mode 100644 index 0000000..3427e7d --- /dev/null +++ b/rate_probe.py @@ -0,0 +1,156 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +"""PDO 周期极限测速探针（真机复用工具）。 + +对**正在运行**的 ecat_server 逐级下发热调周期 /api/motor/period，量每一档的 +实测周期率、分段耗时(cyc_us)、wkc、自旋状态和 HTTP 读快照延迟。热调不停循环、 +喂狗不断流，所以跑这个脚本不会把从站弄掉 OP（区别于反复打 motor/start）。 + +前提：ecat_server 已启动（端口 8090 在听）。**循环没跑时脚本会自动 open+start**， + 所以断电重启板子后，直接敲本脚本一条命令即可，不必先开电机页或手敲 start。 + 若自动 start 失败(典型=SSC 邮箱楔死)，脚本会把原因如实打出来并退出。 + +用法(在本目录的 venv 下跑，系统 python 也行——只用标准库)： + venv\\Scripts\\python.exe rate_probe.py + venv\\Scripts\\python.exe rate_probe.py --periods 0.5,0.25,0.125 --sample 2 + venv\\Scripts\\python.exe rate_probe.py --restore 0.25 # 测完留在 4kHz 档 + venv\\Scripts\\python.exe rate_probe.py --no-autostart # 循环没跑时不自动拉起 +默认从快照读当前周期，测完恢复到那个值。--substr 指定自动 open 的网卡子串(默认 I226)。 + +读数怎么解读： + * 真机一送一回(send+receive)实测 ≈165us，这就是物理墙：比 165us 更快的 + 档位"实测率"必然低于目标，不是脚本或设置的错，cyc_us.total.p50 会贴住 + 周期值甚至跑超。保底 4kHz(250us) 在物理预算内。 + * measured 明显低于 target 且 cyc_us 远小于周期 → 是节拍问题（自旋没生效？ + 看 spin 列），不是总线问题。 + * 自旋占满一个逻辑核是预期行为（脚本进程不占，占的是 ecat_server 的 + pdo-cycle 线程，正常应钉在 P 核）。 +""" +import argparse +import json +import statistics +import sys +import time +import urllib.request + +DEFAULT_URL = "http://127.0.0.1:8090" + + +def post(url, path, body, timeout=5.0): + req = urllib.request.Request( + url + path, data=json.dumps(body).encode("utf-8"), + headers={"Content-Type": "application/json"}, method="POST") + with urllib.request.urlopen(req, timeout=timeout) as r: + return json.loads(r.read().decode("utf-8")) + + +def sample(url, seconds): + """在 seconds 窗口里轮询快照：返回(实测Hz, HTTP延迟p50/p90(ms), 最后快照)。 + + 实测时刻必须和 cycles 读数**同取同舍**：旧版分子取最后一次轮询的 cycles、 + 分母却在循环退出后才算 —— 退出前还有一次 sleep(0.1)，dt 比 cycles 窗口多出 + 0.1s，Hz 被系统性低估 3~5% 且与周期无关(2026-09-04 实踩：把 99.2% 报成 96%， + 一度被误读成"HTTP 负载吃掉 3 个点")。 + """ + lat = [] + first = last = None # (读数时刻, cycles) —— 同一时刻成对取 + t_end = time.perf_counter() + seconds + last_snap = None + while time.perf_counter() < t_end: + t0 = time.perf_counter() + snap = post(url, "/api/motor/state", {}) + t1 = time.perf_counter() + lat.append((t1 - t0) * 1000.0) + if not snap.get("ok"): + raise RuntimeError("motor/state 报错(可能掉出 OP): %s" % snap.get("log")) + c = snap.get("cycles", 0) + if first is None: + first = (t1, c) + last = (t1, c) + last_snap = snap + time.sleep(0.1) + dt = last[0] - first[0] + hz = (last[1] - first[1]) / dt if dt > 0 else 0.0 + lat.sort() + return hz, (lat[len(lat) // 2], lat[int(len(lat) * 0.9)]), last_snap + + +def main(): + ap = argparse.ArgumentParser(description="EtherCAT PDO 周期极限测速") + ap.add_argument("--url", default=DEFAULT_URL) + ap.add_argument("--periods", default="1,0.5,0.25,0.125,0.0625", + help="逗号分隔的周期(ms)档位，允许亚毫秒") + ap.add_argument("--sample", type=float, default=2.0, help="每档采样秒数") + ap.add_argument("--restore", type=float, default=None, + help="测完恢复到的周期(ms)；缺省=脚本启动前的原值") + ap.add_argument("--substr", default="I226", + help="网卡匹配子串(自动 open 时用)，默认 I226") + ap.add_argument("--no-autostart", action="store_true", + help="循环没跑时不要自动 open+start，直接报错退出") + a = ap.parse_args() + + st = post(a.url, "/api/motor/state", {}) + if not st.get("ok"): + # 循环没跑：断电重启后这是常态(服务活着但没人 open/start)。自动补上， + # 让"断电后只敲 rate_probe 一条命令"成立 —— 不必先开电机页或手敲 start。 + if a.no_autostart: + print("后台 PDO 循环没在跑(--no-autostart)：", st.get("log")) + return 1 + print("循环没跑，尝试自动 open + motor/start …") + try: + op = post(a.url, "/api/open", {"substr": a.substr}, timeout=25.0) + if not op.get("ok"): + print(" open 失败:", op.get("log")) + return 1 + ms0 = post(a.url, "/api/motor/start", {"period_ms": 4}, timeout=40.0) + if not ms0.get("ok"): + # 这里最典型的失败就是 SSC 邮箱楔死(config_map 报 MailboxError)， + # 如实把原因打出来，别让人以为脚本坏了。 + print(" motor/start 失败(多半是邮箱楔死，需断电重启板子):") + for line in (ms0.get("log") or []): + print(" ", line[:120]) + return 1 + except OSError as e: + print(" 连不上服务(没起? 端口对吗?):", e) + return 1 + st = post(a.url, "/api/motor/state", {}) + if not st.get("ok"): + print(" start 后仍读不到快照:", st.get("log")) + return 1 + print(" 已自动进 OP 并起循环。") + orig = st.get("period_ms", 4) + + print(f"服务 {a.url} · 当前周期 {orig}ms · 从站 wkc={st.get('wkc')}/{st.get('expected_wkc')}") + print(f"{'周期':>9} {'目标Hz':>7} {'实测Hz':>8} {'达成率':>6} " + f"{'cyc_us p50/p90':>15} {'HTTP p50':>8} spin 电机") + for ms in [float(x) for x in a.periods.split(",")]: + r = post(a.url, "/api/motor/period", {"period_ms": ms}) + if not r.get("ok"): + print(" 档位 %s 被拒绝: %s" % (ms, r.get("log"))) + continue + time.sleep(0.3) # 换周期后先让统计攒几个样本 + try: + hz, (p50, p90), snap = sample(a.url, a.sample) + except (RuntimeError, OSError) as e: + print(f" {ms}ms 档采样失败: {e}") + break + cyc = (snap.get("cyc_us") or {}).get("total") or {} + rate = hz / (1000.0 / ms) * 100.0 + label = f"{ms*1000:g}us" if ms < 1 else f"{ms:g}ms" + print(f"{label:>9} {1000.0/ms:>7.0f} {hz:>8.0f} {rate:>5.0f}% " + f"{str(cyc.get('p50'))+' / '+str(cyc.get('p90')):>15} " + f"{p50:>6.2f}ms {str(snap.get('spin'))[0]} {len(snap.get('motors') or [])}") + if snap.get("wkc") != snap.get("expected_wkc"): + print(" !! wkc 不达预期，从站可能在丢帧，停止继续加压") + break + + restore = a.restore if a.restore is not None else orig + r = post(a.url, "/api/motor/period", {"period_ms": restore}) + print("恢复:", r.get("log")) + print("提示: 真机总线一送一回 ≈165us —— 比它快的档位达成率上不去是物理墙，" + "不是配置错；保底 4kHz(250us) 在预算内。") + return 0 + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/soem-src/osal/erika/osal.c b/soem-src/osal/erika/osal.c new file mode 100644 index 0000000..a921c5f --- /dev/null +++ b/soem-src/osal/erika/osal.c @@ -0,0 +1,103 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +#include +#include +#include +#include +#include +#include + +#include "ee_x86_64_tsc.h" + +#define USECS_PER_SEC 1000000 +#define NSECS_PER_SEC 1000000000 + +uint64_t osEE_x86_64_tsc_read(void); + +void ee_usleep(uint32 usec); + +inline int osal_usleep (uint32 usec) +{ + ee_usleep(usec); + return 0; +} + +int osal_gettimeofday(struct timeval *tv, struct timezone *tz) +{ + uint64_t time = osEE_x86_64_tsc_read(); + tv->tv_sec = time/NSECS_PER_SEC; + tv->tv_sec += 946684800UL; /* EtherCAT uses 2000-01-01 as epoch start */ + tv->tv_usec = (time%NSECS_PER_SEC)/1000; + return 0; +} + +ec_timet osal_current_time(void) +{ + struct timeval current_time; + ec_timet ret; + + osal_gettimeofday(¤t_time, 0); + ret.sec = current_time.tv_sec; + ret.usec = current_time.tv_usec; + return ret; +} + +void osal_time_diff(ec_timet *start, ec_timet *end, ec_timet *diff) +{ + if (end->usec < start->usec) { + diff->sec = end->sec - start->sec - 1; + diff->usec = end->usec + USECS_PER_SEC - start->usec; + } else { + diff->sec = end->sec - start->sec; + diff->usec = end->usec - start->usec; + } +} + +void osal_timer_start(osal_timert *self, uint32 timeout_usec) +{ + struct timeval start_time; + struct timeval timeout; + struct timeval stop_time; + + osal_gettimeofday(&start_time, 0); + timeout.tv_sec = timeout_usec / USECS_PER_SEC; + timeout.tv_usec = timeout_usec % USECS_PER_SEC; + timeradd(&start_time, &timeout, &stop_time); + + self->stop_time.sec = stop_time.tv_sec; + self->stop_time.usec = stop_time.tv_usec; +} + +boolean osal_timer_is_expired (osal_timert *self) +{ + struct timeval current_time; + struct timeval stop_time; + int is_not_yet_expired; + + osal_gettimeofday (¤t_time, 0); + stop_time.tv_sec = self->stop_time.sec; + stop_time.tv_usec = self->stop_time.usec; + is_not_yet_expired = timercmp (¤t_time, &stop_time, <); +/* OSEE_PRINT("current: %d:%d -- expire: %d:%d -- result: %d\n", */ +/* current_time.tv_sec, */ +/* current_time.tv_usec, */ +/* stop_time.tv_sec, */ +/* stop_time.tv_usec, */ +/* is_not_yet_expired); */ + + return is_not_yet_expired == FALSE; +} + +void *osal_malloc(size_t size) +{ + return malloc(size); +} + +void osal_free(void *ptr) +{ + free(ptr); +} + diff --git a/soem-src/osal/erika/osal_defs.h b/soem-src/osal/erika/osal_defs.h new file mode 100644 index 0000000..99d0ac3 --- /dev/null +++ b/soem-src/osal/erika/osal_defs.h @@ -0,0 +1,41 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +#ifndef _osal_defs_ +#define _osal_defs_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include +#include +#include + +// define if debug print is needed +#define EC_DEBUG + +#ifdef EC_DEBUG +#define EC_PRINT OSEE_PRINT +#else +#define EC_PRINT(...) do {} while (0) +#endif + +#ifndef PACKED +#define PACKED_BEGIN +#define PACKED __attribute__((__packed__)) +#define PACKED_END +#endif + +int osal_gettimeofday(struct timeval *tv, struct timezone *tz); +void *osal_malloc(size_t size); +void osal_free(void *ptr); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/soem-src/osal/intime/osal.c b/soem-src/osal/intime/osal.c new file mode 100644 index 0000000..357edc1 --- /dev/null +++ b/soem-src/osal/intime/osal.c @@ -0,0 +1,88 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +#include +#include +#include + +static int64_t sysfrequency; +static double qpc2usec; + +#define USECS_PER_SEC 1000000 + +int osal_gettimeofday (struct timeval *tv, struct timezone *tz) +{ + return gettimeofday (tv, tz); +} + +ec_timet osal_current_time (void) +{ + struct timeval current_time; + ec_timet return_value; + + osal_gettimeofday (¤t_time, 0); + return_value.sec = current_time.tv_sec; + return_value.usec = current_time.tv_usec; + return return_value; +} + +void osal_timer_start (osal_timert * self, uint32 timeout_usec) +{ + struct timeval start_time; + struct timeval timeout; + struct timeval stop_time; + + osal_gettimeofday (&start_time, 0); + timeout.tv_sec = timeout_usec / USECS_PER_SEC; + timeout.tv_usec = timeout_usec % USECS_PER_SEC; + timeradd (&start_time, &timeout, &stop_time); + + self->stop_time.sec = stop_time.tv_sec; + self->stop_time.usec = stop_time.tv_usec; +} + +boolean osal_timer_is_expired (osal_timert * self) +{ + struct timeval current_time; + struct timeval stop_time; + int is_not_yet_expired; + + osal_gettimeofday (¤t_time, 0); + stop_time.tv_sec = self->stop_time.sec; + stop_time.tv_usec = self->stop_time.usec; + is_not_yet_expired = timercmp (¤t_time, &stop_time, <); + + return is_not_yet_expired == FALSE; +} + +int osal_usleep(uint32 usec) +{ + RtSleepEx (usec / 1000); + return 1; +} + +/* Mutex is not needed when running single threaded */ + +void osal_mtx_lock(osal_mutex_t * mtx) +{ + /* RtWaitForSingleObject((HANDLE)mtx, INFINITE); */ +} + +void osal_mtx_unlock(osal_mutex_t * mtx) +{ + /* RtReleaseMutex((HANDLE)mtx); */ +} + +int osal_mtx_lock_timeout(osal_mutex_t * mtx, uint32_t time_ms) +{ + /* return RtWaitForSingleObject((HANDLE)mtx, time_ms); */ + return 0; +} + +osal_mutex_t * osal_mtx_create(void) +{ + /* return (void*)RtCreateMutex(NULL, FALSE, NULL); */ + return (void *)0; +} diff --git a/soem-src/osal/intime/osal_defs.h b/soem-src/osal/intime/osal_defs.h new file mode 100644 index 0000000..34a9e87 --- /dev/null +++ b/soem-src/osal/intime/osal_defs.h @@ -0,0 +1,43 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +#ifndef _osal_defs_ +#define _osal_defs_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +// define if debug printf is needed +//#define EC_DEBUG + +#ifdef EC_DEBUG +#define EC_PRINT printf +#else +#define EC_PRINT(...) do {} while (0) +#endif + +#ifndef PACKED + #ifdef _MSC_VER + #define PACKED_BEGIN __pragma(pack(push, 1)) + #define PACKED + #define PACKED_END __pragma(pack(pop)) + #elif defined(__GNUC__) + #define PACKED_BEGIN + #define PACKED __attribute__((__packed__)) + #define PACKED_END + #endif +#endif + +#define OSAL_THREAD_HANDLE RTHANDLE +#define OSAL_THREAD_FUNC void +#define OSAL_THREAD_FUNC_RT void + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/soem-src/osal/linux/osal.c b/soem-src/osal/linux/osal.c new file mode 100644 index 0000000..d998bb9 --- /dev/null +++ b/soem-src/osal/linux/osal.c @@ -0,0 +1,144 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +#include +#include +#include +#include +#include +#include + +#define USECS_PER_SEC 1000000 + +int osal_usleep (uint32 usec) +{ + struct timespec ts; + ts.tv_sec = usec / USECS_PER_SEC; + ts.tv_nsec = (usec % USECS_PER_SEC) * 1000; + /* usleep is deprecated, use nanosleep instead */ + return nanosleep(&ts, NULL); +} + +ec_timet osal_current_time(void) +{ + struct timespec current_time; + ec_timet return_value; + + clock_gettime(CLOCK_REALTIME, ¤t_time); + return_value.sec = current_time.tv_sec; + return_value.usec = current_time.tv_nsec / 1000; + + return return_value; +} + +void osal_time_diff(ec_timet *start, ec_timet *end, ec_timet *diff) +{ + if (end->usec < start->usec) { + diff->sec = end->sec - start->sec - 1; + diff->usec = end->usec + 1000000 - start->usec; + } + else { + diff->sec = end->sec - start->sec; + diff->usec = end->usec - start->usec; + } +} + +/* Returns time from some unspecified moment in past, + * strictly increasing, used for time intervals measurement. */ +static void osal_getrelativetime(struct timeval *tv) +{ + struct timespec ts; + + /* Use clock_gettime to prevent possible live-lock. + * Gettimeofday uses CLOCK_REALTIME that can get NTP timeadjust. + * If this function preempts timeadjust and it uses vpage it live-locks. + * Also when using XENOMAI, only clock_gettime is RT safe */ + clock_gettime(CLOCK_MONOTONIC, &ts); + tv->tv_sec = ts.tv_sec; + tv->tv_usec = ts.tv_nsec / 1000; +} + +void osal_timer_start(osal_timert * self, uint32 timeout_usec) +{ + struct timeval start_time; + struct timeval timeout; + struct timeval stop_time; + + osal_getrelativetime(&start_time); + timeout.tv_sec = timeout_usec / USECS_PER_SEC; + timeout.tv_usec = timeout_usec % USECS_PER_SEC; + timeradd(&start_time, &timeout, &stop_time); + + self->stop_time.sec = stop_time.tv_sec; + self->stop_time.usec = stop_time.tv_usec; +} + +boolean osal_timer_is_expired (osal_timert * self) +{ + struct timeval current_time; + struct timeval stop_time; + int is_not_yet_expired; + + osal_getrelativetime(¤t_time); + stop_time.tv_sec = self->stop_time.sec; + stop_time.tv_usec = self->stop_time.usec; + is_not_yet_expired = timercmp(¤t_time, &stop_time, <); + + return is_not_yet_expired == FALSE; +} + +void *osal_malloc(size_t size) +{ + return malloc(size); +} + +void osal_free(void *ptr) +{ + free(ptr); +} + +int osal_thread_create(void *thandle, int stacksize, void *func, void *param) +{ + int ret; + pthread_attr_t attr; + pthread_t *threadp; + + threadp = thandle; + pthread_attr_init(&attr); + pthread_attr_setstacksize(&attr, stacksize); + ret = pthread_create(threadp, &attr, func, param); + if(ret < 0) + { + return 0; + } + return 1; +} + +int osal_thread_create_rt(void *thandle, int stacksize, void *func, void *param) +{ + int ret; + pthread_attr_t attr; + struct sched_param schparam; + pthread_t *threadp; + + threadp = thandle; + pthread_attr_init(&attr); + pthread_attr_setstacksize(&attr, stacksize); + ret = pthread_create(threadp, &attr, func, param); + pthread_attr_destroy(&attr); + if(ret < 0) + { + return 0; + } + memset(&schparam, 0, sizeof(schparam)); + schparam.sched_priority = 40; + ret = pthread_setschedparam(*threadp, SCHED_FIFO, &schparam); + if(ret < 0) + { + return 0; + } + + return 1; +} diff --git a/soem-src/osal/linux/osal_defs.h b/soem-src/osal/linux/osal_defs.h new file mode 100644 index 0000000..6ec32ef --- /dev/null +++ b/soem-src/osal/linux/osal_defs.h @@ -0,0 +1,38 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +#ifndef _osal_defs_ +#define _osal_defs_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +// define if debug printf is needed +//#define EC_DEBUG + +#ifdef EC_DEBUG +#define EC_PRINT printf +#else +#define EC_PRINT(...) do {} while (0) +#endif + +#ifndef PACKED +#define PACKED_BEGIN +#define PACKED __attribute__((__packed__)) +#define PACKED_END +#endif + +#include +#define OSAL_THREAD_HANDLE pthread_t * +#define OSAL_THREAD_FUNC void +#define OSAL_THREAD_FUNC_RT void + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/soem-src/osal/macosx/osal.c b/soem-src/osal/macosx/osal.c new file mode 100644 index 0000000..d96e970 --- /dev/null +++ b/soem-src/osal/macosx/osal.c @@ -0,0 +1,144 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +#include +#include +#include +#include +#include +#include + +#define USECS_PER_SEC 1000000 + +int osal_usleep (uint32 usec) +{ + struct timespec ts; + ts.tv_sec = usec / USECS_PER_SEC; + ts.tv_nsec = (usec % USECS_PER_SEC) * 1000; + /* usleep is deprecated, use nanosleep instead */ + return nanosleep(&ts, NULL); +} + +int osal_gettimeofday(struct timeval *tv, struct timezone *tz) +{ + struct timespec ts; + int return_value; + (void)tz; /* Not used */ + + /* Use clock_gettime to prevent possible live-lock. + * Gettimeofday uses CLOCK_REALTIME that can get NTP timeadjust. + * If this function preempts timeadjust and it uses vpage it live-locks. + * Also when using XENOMAI, only clock_gettime is RT safe */ + return_value = clock_gettime(CLOCK_MONOTONIC, &ts); + tv->tv_sec = ts.tv_sec; + tv->tv_usec = ts.tv_nsec / 1000; + return return_value; +} + +ec_timet osal_current_time(void) +{ + struct timeval current_time; + ec_timet return_value; + + osal_gettimeofday(¤t_time, 0); + return_value.sec = current_time.tv_sec; + return_value.usec = current_time.tv_usec; + return return_value; +} + +void osal_time_diff(ec_timet *start, ec_timet *end, ec_timet *diff) +{ + if (end->usec < start->usec) { + diff->sec = end->sec - start->sec - 1; + diff->usec = end->usec + 1000000 - start->usec; + } + else { + diff->sec = end->sec - start->sec; + diff->usec = end->usec - start->usec; + } +} + +void osal_timer_start(osal_timert * self, uint32 timeout_usec) +{ + struct timeval start_time; + struct timeval timeout; + struct timeval stop_time; + + osal_gettimeofday(&start_time, 0); + timeout.tv_sec = timeout_usec / USECS_PER_SEC; + timeout.tv_usec = timeout_usec % USECS_PER_SEC; + timeradd(&start_time, &timeout, &stop_time); + + self->stop_time.sec = stop_time.tv_sec; + self->stop_time.usec = stop_time.tv_usec; +} + +boolean osal_timer_is_expired (osal_timert * self) +{ + struct timeval current_time; + struct timeval stop_time; + int is_not_yet_expired; + + osal_gettimeofday(¤t_time, 0); + stop_time.tv_sec = self->stop_time.sec; + stop_time.tv_usec = self->stop_time.usec; + is_not_yet_expired = timercmp(¤t_time, &stop_time, <); + + return is_not_yet_expired == FALSE; +} + +void *osal_malloc(size_t size) +{ + return malloc(size); +} + +void osal_free(void *ptr) +{ + free(ptr); +} + +int osal_thread_create(void *thandle, int stacksize, void *func, void *param) +{ + int ret; + pthread_attr_t attr; + pthread_t *threadp; + + threadp = thandle; + pthread_attr_init(&attr); + pthread_attr_setstacksize(&attr, stacksize); + ret = pthread_create(threadp, &attr, func, param); + if(ret < 0) + { + return 0; + } + return 1; +} + +int osal_thread_create_rt(void *thandle, int stacksize, void *func, void *param) +{ + int ret; + pthread_attr_t attr; + struct sched_param schparam; + pthread_t *threadp; + + threadp = thandle; + pthread_attr_init(&attr); + pthread_attr_setstacksize(&attr, stacksize); + ret = pthread_create(threadp, &attr, func, param); + pthread_attr_destroy(&attr); + if(ret < 0) + { + return 0; + } + memset(&schparam, 0, sizeof(schparam)); + schparam.sched_priority = 40; + ret = pthread_setschedparam(*threadp, SCHED_FIFO, &schparam); + if(ret < 0) + { + return 0; + } + + return 1; +} diff --git a/soem-src/osal/macosx/osal_defs.h b/soem-src/osal/macosx/osal_defs.h new file mode 100644 index 0000000..6ec32ef --- /dev/null +++ b/soem-src/osal/macosx/osal_defs.h @@ -0,0 +1,38 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +#ifndef _osal_defs_ +#define _osal_defs_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +// define if debug printf is needed +//#define EC_DEBUG + +#ifdef EC_DEBUG +#define EC_PRINT printf +#else +#define EC_PRINT(...) do {} while (0) +#endif + +#ifndef PACKED +#define PACKED_BEGIN +#define PACKED __attribute__((__packed__)) +#define PACKED_END +#endif + +#include +#define OSAL_THREAD_HANDLE pthread_t * +#define OSAL_THREAD_FUNC void +#define OSAL_THREAD_FUNC_RT void + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/soem-src/osal/osal.h b/soem-src/osal/osal.h new file mode 100644 index 0000000..ed07c51 --- /dev/null +++ b/soem-src/osal/osal.h @@ -0,0 +1,59 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +#ifndef _osal_ +#define _osal_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include "osal_defs.h" +#include + +/* General types */ +#ifndef TRUE +#define TRUE 1 +#endif +#ifndef FALSE +#define FALSE 0 +#endif +typedef uint8_t boolean; +typedef int8_t int8; +typedef int16_t int16; +typedef int32_t int32; +typedef uint8_t uint8; +typedef uint16_t uint16; +typedef uint32_t uint32; +typedef int64_t int64; +typedef uint64_t uint64; +typedef float float32; +typedef double float64; + +typedef struct +{ + uint32 sec; /*< Seconds elapsed since the Epoch (Jan 1, 1970) */ + uint32 usec; /*< Microseconds elapsed since last second boundary */ +} ec_timet; + +typedef struct osal_timer +{ + ec_timet stop_time; +} osal_timert; + +void osal_timer_start(osal_timert * self, uint32 timeout_us); +boolean osal_timer_is_expired(osal_timert * self); +int osal_usleep(uint32 usec); +ec_timet osal_current_time(void); +void osal_time_diff(ec_timet *start, ec_timet *end, ec_timet *diff); +int osal_thread_create(void *thandle, int stacksize, void *func, void *param); +int osal_thread_create_rt(void *thandle, int stacksize, void *func, void *param); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/soem-src/osal/rtems/osal.c b/soem-src/osal/rtems/osal.c new file mode 100644 index 0000000..d96e970 --- /dev/null +++ b/soem-src/osal/rtems/osal.c @@ -0,0 +1,144 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +#include +#include +#include +#include +#include +#include + +#define USECS_PER_SEC 1000000 + +int osal_usleep (uint32 usec) +{ + struct timespec ts; + ts.tv_sec = usec / USECS_PER_SEC; + ts.tv_nsec = (usec % USECS_PER_SEC) * 1000; + /* usleep is deprecated, use nanosleep instead */ + return nanosleep(&ts, NULL); +} + +int osal_gettimeofday(struct timeval *tv, struct timezone *tz) +{ + struct timespec ts; + int return_value; + (void)tz; /* Not used */ + + /* Use clock_gettime to prevent possible live-lock. + * Gettimeofday uses CLOCK_REALTIME that can get NTP timeadjust. + * If this function preempts timeadjust and it uses vpage it live-locks. + * Also when using XENOMAI, only clock_gettime is RT safe */ + return_value = clock_gettime(CLOCK_MONOTONIC, &ts); + tv->tv_sec = ts.tv_sec; + tv->tv_usec = ts.tv_nsec / 1000; + return return_value; +} + +ec_timet osal_current_time(void) +{ + struct timeval current_time; + ec_timet return_value; + + osal_gettimeofday(¤t_time, 0); + return_value.sec = current_time.tv_sec; + return_value.usec = current_time.tv_usec; + return return_value; +} + +void osal_time_diff(ec_timet *start, ec_timet *end, ec_timet *diff) +{ + if (end->usec < start->usec) { + diff->sec = end->sec - start->sec - 1; + diff->usec = end->usec + 1000000 - start->usec; + } + else { + diff->sec = end->sec - start->sec; + diff->usec = end->usec - start->usec; + } +} + +void osal_timer_start(osal_timert * self, uint32 timeout_usec) +{ + struct timeval start_time; + struct timeval timeout; + struct timeval stop_time; + + osal_gettimeofday(&start_time, 0); + timeout.tv_sec = timeout_usec / USECS_PER_SEC; + timeout.tv_usec = timeout_usec % USECS_PER_SEC; + timeradd(&start_time, &timeout, &stop_time); + + self->stop_time.sec = stop_time.tv_sec; + self->stop_time.usec = stop_time.tv_usec; +} + +boolean osal_timer_is_expired (osal_timert * self) +{ + struct timeval current_time; + struct timeval stop_time; + int is_not_yet_expired; + + osal_gettimeofday(¤t_time, 0); + stop_time.tv_sec = self->stop_time.sec; + stop_time.tv_usec = self->stop_time.usec; + is_not_yet_expired = timercmp(¤t_time, &stop_time, <); + + return is_not_yet_expired == FALSE; +} + +void *osal_malloc(size_t size) +{ + return malloc(size); +} + +void osal_free(void *ptr) +{ + free(ptr); +} + +int osal_thread_create(void *thandle, int stacksize, void *func, void *param) +{ + int ret; + pthread_attr_t attr; + pthread_t *threadp; + + threadp = thandle; + pthread_attr_init(&attr); + pthread_attr_setstacksize(&attr, stacksize); + ret = pthread_create(threadp, &attr, func, param); + if(ret < 0) + { + return 0; + } + return 1; +} + +int osal_thread_create_rt(void *thandle, int stacksize, void *func, void *param) +{ + int ret; + pthread_attr_t attr; + struct sched_param schparam; + pthread_t *threadp; + + threadp = thandle; + pthread_attr_init(&attr); + pthread_attr_setstacksize(&attr, stacksize); + ret = pthread_create(threadp, &attr, func, param); + pthread_attr_destroy(&attr); + if(ret < 0) + { + return 0; + } + memset(&schparam, 0, sizeof(schparam)); + schparam.sched_priority = 40; + ret = pthread_setschedparam(*threadp, SCHED_FIFO, &schparam); + if(ret < 0) + { + return 0; + } + + return 1; +} diff --git a/soem-src/osal/rtems/osal_defs.h b/soem-src/osal/rtems/osal_defs.h new file mode 100644 index 0000000..6ec32ef --- /dev/null +++ b/soem-src/osal/rtems/osal_defs.h @@ -0,0 +1,38 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +#ifndef _osal_defs_ +#define _osal_defs_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +// define if debug printf is needed +//#define EC_DEBUG + +#ifdef EC_DEBUG +#define EC_PRINT printf +#else +#define EC_PRINT(...) do {} while (0) +#endif + +#ifndef PACKED +#define PACKED_BEGIN +#define PACKED __attribute__((__packed__)) +#define PACKED_END +#endif + +#include +#define OSAL_THREAD_HANDLE pthread_t * +#define OSAL_THREAD_FUNC void +#define OSAL_THREAD_FUNC_RT void + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/soem-src/osal/rtk/osal.c b/soem-src/osal/rtk/osal.c new file mode 100644 index 0000000..a519bb7 --- /dev/null +++ b/soem-src/osal/rtk/osal.c @@ -0,0 +1,145 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +#include +#include +#include +#include +#include + +#define timercmp(a, b, CMP) \ + (((a)->tv_sec == (b)->tv_sec) ? \ + ((a)->tv_usec CMP (b)->tv_usec) : \ + ((a)->tv_sec CMP (b)->tv_sec)) +#define timeradd(a, b, result) \ + do { \ + (result)->tv_sec = (a)->tv_sec + (b)->tv_sec; \ + (result)->tv_usec = (a)->tv_usec + (b)->tv_usec; \ + if ((result)->tv_usec >= 1000000) \ + { \ + ++(result)->tv_sec; \ + (result)->tv_usec -= 1000000; \ + } \ + } while (0) +#define timersub(a, b, result) \ + do { \ + (result)->tv_sec = (a)->tv_sec - (b)->tv_sec; \ + (result)->tv_usec = (a)->tv_usec - (b)->tv_usec; \ + if ((result)->tv_usec < 0) { \ + --(result)->tv_sec; \ + (result)->tv_usec += 1000000; \ + } \ + } while (0) + +#define USECS_PER_SEC 1000000 +#define USECS_PER_TICK (USECS_PER_SEC / CFG_TICKS_PER_SECOND) + + +/* Workaround for rt-labs defect 776. + * Default implementation of udelay() didn't work correctly when tick was + * shorter than one millisecond. + */ +void udelay (uint32_t us) +{ + tick_t ticks = (us / USECS_PER_TICK) + 1; + task_delay (ticks); +} + +int gettimeofday(struct timeval *tp, void *tzp) +{ + tick_t tick = tick_get(); + tick_t ticks_left; + + ASSERT (tp != NULL); + + tp->tv_sec = tick / CFG_TICKS_PER_SECOND; + + ticks_left = tick % CFG_TICKS_PER_SECOND; + tp->tv_usec = ticks_left * USECS_PER_TICK; + ASSERT (tp->tv_usec < USECS_PER_SEC); + + return 0; +} + +int osal_usleep (uint32 usec) +{ + udelay(usec); + return 0; +} + +int osal_gettimeofday(struct timeval *tv, struct timezone *tz) +{ + return gettimeofday(tv, tz); +} + +ec_timet osal_current_time (void) +{ + struct timeval current_time; + ec_timet return_value; + + gettimeofday (¤t_time, 0); + return_value.sec = current_time.tv_sec; + return_value.usec = current_time.tv_usec; + return return_value; +} + +void osal_timer_start (osal_timert * self, uint32 timeout_usec) +{ + struct timeval start_time; + struct timeval timeout; + struct timeval stop_time; + + gettimeofday (&start_time, 0); + timeout.tv_sec = timeout_usec / USECS_PER_SEC; + timeout.tv_usec = timeout_usec % USECS_PER_SEC; + timeradd (&start_time, &timeout, &stop_time); + + self->stop_time.sec = stop_time.tv_sec; + self->stop_time.usec = stop_time.tv_usec; +} + +boolean osal_timer_is_expired (osal_timert * self) +{ + struct timeval current_time; + struct timeval stop_time; + int is_not_yet_expired; + + gettimeofday (¤t_time, 0); + stop_time.tv_sec = self->stop_time.sec; + stop_time.tv_usec = self->stop_time.usec; + is_not_yet_expired = timercmp (¤t_time, &stop_time, <); + + return is_not_yet_expired == false; +} + +void *osal_malloc(size_t size) +{ + return malloc(size); +} + +void osal_free(void *ptr) +{ + free(ptr); +} + +int osal_thread_create(void *thandle, int stacksize, void *func, void *param) +{ + thandle = task_spawn ("worker", func, 6,stacksize, param); + if(!thandle) + { + return 0; + } + return 1; +} + +int osal_thread_create_rt(void *thandle, int stacksize, void *func, void *param) +{ + thandle = task_spawn ("worker_rt", func, 15 ,stacksize, param); + if(!thandle) + { + return 0; + } + return 1; +} diff --git a/soem-src/osal/rtk/osal_defs.h b/soem-src/osal/rtk/osal_defs.h new file mode 100644 index 0000000..162a42e --- /dev/null +++ b/soem-src/osal/rtk/osal_defs.h @@ -0,0 +1,37 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +#ifndef _osal_defs_ +#define _osal_defs_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +// define if debug printf is needed +//#define EC_DEBUG + +#ifdef EC_DEBUG +#define EC_PRINT printf +#else +#define EC_PRINT(...) do {} while (0) +#endif + +#ifndef PACKED +#define PACKED_BEGIN +#define PACKED __attribute__((__packed__)) +#define PACKED_END +#endif + +#define OSAL_THREAD_HANDLE task_t * +#define OSAL_THREAD_FUNC void +#define OSAL_THREAD_FUNC_RT void + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/soem-src/osal/vxworks/osal.c b/soem-src/osal/vxworks/osal.c new file mode 100644 index 0000000..cfb0d02 --- /dev/null +++ b/soem-src/osal/vxworks/osal.c @@ -0,0 +1,187 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +#define timercmp(a, b, CMP) \ + (((a)->tv_sec == (b)->tv_sec) ? \ + ((a)->tv_usec CMP (b)->tv_usec) : \ + ((a)->tv_sec CMP (b)->tv_sec)) +#define timeradd(a, b, result) \ + do { \ + (result)->tv_sec = (a)->tv_sec + (b)->tv_sec; \ + (result)->tv_usec = (a)->tv_usec + (b)->tv_usec; \ + if ((result)->tv_usec >= 1000000) \ + { \ + ++(result)->tv_sec; \ + (result)->tv_usec -= 1000000; \ + } \ + } while (0) +#define timersub(a, b, result) \ + do { \ + (result)->tv_sec = (a)->tv_sec - (b)->tv_sec; \ + (result)->tv_usec = (a)->tv_usec - (b)->tv_usec; \ + if ((result)->tv_usec < 0) { \ + --(result)->tv_sec; \ + (result)->tv_usec += 1000000; \ + } \ + } while (0) + +#define USECS_PER_SEC 1000000 + +/* OBS! config worker threads must have higher prio that task running ec_configuration */ +#define ECAT_TASK_PRIO_HIGH 20 /* Priority for high performance network task */ +#define ECAT_TASK_PRIO_LOW 80 /* Priority for high performance network task */ +#define ECAT_STACK_SIZE 10000 /* Stack size for high performance task */ +static int ecatTaskOptions = VX_SUPERVISOR_MODE | VX_UNBREAKABLE; +static int ecatTaskIndex = 0; + +#ifndef use_task_delay +#define use_task_delay 1 +#endif + +int osal_usleep (uint32 usec) +{ +#if (use_task_delay == 1) + /* Task delay 0 only yields */ + taskDelay(usec / 1000); + return 0; +#else + /* The suspension may be longer than requested due to the rounding up of + * the request to the timer's resolution or to other scheduling activities + * (e.g., a higher priority task intervenes). + */ + struct timespec ts; + ts.tv_sec = usec / USECS_PER_SEC; + ts.tv_nsec = (usec % USECS_PER_SEC) * 1000; + return nanosleep(&ts, NULL); +#endif + +} + +int osal_gettimeofday(struct timeval *tv, struct timezone *tz) +{ + struct timespec ts; + int return_value; + (void)tz; /* Not used */ + + /* Use clock_gettime CLOCK_MONOTONIC to a avoid NTP time adjustments */ + return_value = clock_gettime(CLOCK_MONOTONIC, &ts); + tv->tv_sec = ts.tv_sec; + tv->tv_usec = ts.tv_nsec / 1000; + return return_value; +} + +ec_timet osal_current_time(void) +{ + struct timeval current_time; + ec_timet return_value; + + osal_gettimeofday(¤t_time, 0); + return_value.sec = current_time.tv_sec; + return_value.usec = current_time.tv_usec; + return return_value; +} + +void osal_time_diff(ec_timet *start, ec_timet *end, ec_timet *diff) +{ + if (end->usec < start->usec) { + diff->sec = end->sec - start->sec - 1; + diff->usec = end->usec + 1000000 - start->usec; + } + else { + diff->sec = end->sec - start->sec; + diff->usec = end->usec - start->usec; + } +} + +void osal_timer_start(osal_timert * self, uint32 timeout_usec) +{ + struct timeval start_time; + struct timeval timeout; + struct timeval stop_time; + + osal_gettimeofday(&start_time, 0); + timeout.tv_sec = timeout_usec / USECS_PER_SEC; + timeout.tv_usec = timeout_usec % USECS_PER_SEC; + timeradd(&start_time, &timeout, &stop_time); + + self->stop_time.sec = stop_time.tv_sec; + self->stop_time.usec = stop_time.tv_usec; +} + +boolean osal_timer_is_expired (osal_timert * self) +{ + struct timeval current_time; + struct timeval stop_time; + int is_not_yet_expired; + + osal_gettimeofday(¤t_time, 0); + stop_time.tv_sec = self->stop_time.sec; + stop_time.tv_usec = self->stop_time.usec; + is_not_yet_expired = timercmp(¤t_time, &stop_time, <); + + return is_not_yet_expired == FALSE; +} + +void *osal_malloc(size_t size) +{ + return malloc(size); +} + +void osal_free(void *ptr) +{ + free(ptr); +} + +int osal_thread_create(void *thandle, int stacksize, void *func, void *param) +{ + char task_name[20]; + TASK_ID * tid = (TASK_ID *)thandle; + FUNCPTR func_ptr = func; + _Vx_usr_arg_t arg1 = (_Vx_usr_arg_t)param; + + snprintf(task_name,sizeof(task_name),"worker_%d",ecatTaskIndex++); + + *tid = taskSpawn (task_name, ECAT_TASK_PRIO_LOW, + ecatTaskOptions, ECAT_STACK_SIZE, + func_ptr, arg1, 0, 0, 0, 0, 0, 0, 0, 0, 0); + if(*tid == TASK_ID_ERROR) + { + return 0; + } + + return 1; +} + +int osal_thread_create_rt(void *thandle, int stacksize, void *func, void *param) +{ + char task_name[20]; + TASK_ID * tid = (TASK_ID *)thandle; + FUNCPTR func_ptr = func; + _Vx_usr_arg_t arg1 = (_Vx_usr_arg_t)param; + + snprintf(task_name,sizeof(task_name),"worker_rt_%d",ecatTaskIndex++); + + *tid = taskSpawn (task_name, ECAT_TASK_PRIO_HIGH, + ecatTaskOptions, ECAT_STACK_SIZE, + func_ptr, arg1, 0, 0, 0, 0, 0, 0, 0, 0, 0); + + if(*tid == TASK_ID_ERROR) + { + return 0; + } + return 1; +} + diff --git a/soem-src/osal/vxworks/osal_defs.h b/soem-src/osal/vxworks/osal_defs.h new file mode 100644 index 0000000..3aafb73 --- /dev/null +++ b/soem-src/osal/vxworks/osal_defs.h @@ -0,0 +1,28 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +#ifndef _osal_defs_ +#define _osal_defs_ + +// define if debug printf is needed +//#define EC_DEBUG + +#ifdef EC_DEBUG +#define EC_PRINT printf +#else +#define EC_PRINT(...) do {} while (0) +#endif + +#ifndef PACKED +#define PACKED_BEGIN +#define PACKED __attribute__((__packed__)) +#define PACKED_END +#endif + +#define OSAL_THREAD_HANDLE TASK_ID +#define OSAL_THREAD_FUNC void +#define OSAL_THREAD_FUNC_RT void + +#endif diff --git a/soem-src/osal/win32/osal.c b/soem-src/osal/win32/osal.c new file mode 100644 index 0000000..d9572ca --- /dev/null +++ b/soem-src/osal/win32/osal.c @@ -0,0 +1,153 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +#include +#include +#include "osal_win32.h" + +static int64_t sysfrequency; +static double qpc2usec; + +#define USECS_PER_SEC 1000000 + +static int osal_getrelativetime(struct timeval *tv, struct timezone *tz) +{ + int64_t wintime, usecs; + (void)tz; + if(!sysfrequency) + { + timeBeginPeriod(1); + QueryPerformanceFrequency((LARGE_INTEGER *)&sysfrequency); + qpc2usec = 1000000.0 / sysfrequency; + } + QueryPerformanceCounter((LARGE_INTEGER *)&wintime); + usecs = (int64_t)((double)wintime * qpc2usec); + tv->tv_sec = (long)(usecs / 1000000); + tv->tv_usec = (long)(usecs - (tv->tv_sec * 1000000)); + + return 1; +} + +int osal_gettimeofday(struct timeval *tv, struct timezone *tz) +{ + FILETIME system_time; + int64 system_time64, usecs; + (void)tz; + /* The offset variable is required to switch from Windows epoch (January 1, 1601) to + * Unix epoch (January 1, 1970). Number of days between both epochs: 134.774 + * + * The time returned by GetSystemTimeAsFileTime() changes in 100 ns steps, so the + * following factors are required for the conversion from days to 100 ns steps: + * + * 86.400 seconds per day; 1.000.000 microseconds per second; 10 * 100 ns per microsecond + */ + int64 offset = -134774LL * 86400LL * 1000000LL * 10LL; + + GetSystemTimeAsFileTime(&system_time); + + system_time64 = ((int64)(system_time.dwHighDateTime) << 32) + (int64)system_time.dwLowDateTime; + system_time64 += offset; + usecs = system_time64 / 10; + + tv->tv_sec = (long)(usecs / 1000000); + tv->tv_usec = (long)(usecs - (tv->tv_sec * 1000000)); + + return 1; +} + +ec_timet osal_current_time (void) +{ + struct timeval current_time; + ec_timet return_value; + + osal_gettimeofday (¤t_time, 0); + return_value.sec = current_time.tv_sec; + return_value.usec = current_time.tv_usec; + return return_value; +} + +void osal_time_diff(ec_timet *start, ec_timet *end, ec_timet *diff) +{ + if (end->usec < start->usec) { + diff->sec = end->sec - start->sec - 1; + diff->usec = end->usec + 1000000 - start->usec; + } + else { + diff->sec = end->sec - start->sec; + diff->usec = end->usec - start->usec; + } +} + +void osal_timer_start (osal_timert *self, uint32 timeout_usec) +{ + struct timeval start_time; + struct timeval timeout; + struct timeval stop_time; + + osal_getrelativetime (&start_time, 0); + timeout.tv_sec = timeout_usec / USECS_PER_SEC; + timeout.tv_usec = timeout_usec % USECS_PER_SEC; + timeradd (&start_time, &timeout, &stop_time); + + self->stop_time.sec = stop_time.tv_sec; + self->stop_time.usec = stop_time.tv_usec; +} + +boolean osal_timer_is_expired (osal_timert *self) +{ + struct timeval current_time; + struct timeval stop_time; + int is_not_yet_expired; + + osal_getrelativetime (¤t_time, 0); + stop_time.tv_sec = self->stop_time.sec; + stop_time.tv_usec = self->stop_time.usec; + is_not_yet_expired = timercmp (¤t_time, &stop_time, <); + + return is_not_yet_expired == FALSE; +} + +int osal_usleep(uint32 usec) +{ + osal_timert qtime; + osal_timer_start(&qtime, usec); + if(usec >= 1000) + { + SleepEx(usec / 1000, FALSE); + } + while(!osal_timer_is_expired(&qtime)); + return 1; +} + +void *osal_malloc(size_t size) +{ + return malloc(size); +} + +void osal_free(void *ptr) +{ + free(ptr); +} + +int osal_thread_create(void *thandle, int stacksize, void *func, void *param) +{ + *(OSAL_THREAD_HANDLE*)thandle = CreateThread(NULL, stacksize, func, param, 0, NULL); + if(!thandle) + { + return 0; + } + return 1; +} + +int osal_thread_create_rt(void *thandle, int stacksize, void *func, void *param) +{ + int ret; + ret = osal_thread_create(thandle, stacksize, func, param); + if (ret) + { + ret = SetThreadPriority(thandle, THREAD_PRIORITY_TIME_CRITICAL); + } + return ret; +} diff --git a/soem-src/osal/win32/osal_defs.h b/soem-src/osal/win32/osal_defs.h new file mode 100644 index 0000000..e21aa71 --- /dev/null +++ b/soem-src/osal/win32/osal_defs.h @@ -0,0 +1,45 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +#ifndef _osal_defs_ +#define _osal_defs_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#define WIN32_LEAN_AND_MEAN // Exclude some conflicting definitions in windows header +#include +// define if debug printf is needed +//#define EC_DEBUG + +#ifdef EC_DEBUG +#define EC_PRINT printf +#else +#define EC_PRINT(...) do {} while (0) +#endif + +#ifndef PACKED +#define PACKED +#ifdef __GNUC__ +#define PACKED_BEGIN _Pragma("pack(push,1)") +#define PACKED_END _Pragma("pack(pop)") +#else +#define PACKED_BEGIN __pragma(pack(push, 1)) +#define PACKED_END __pragma(pack(pop)) +#endif + +#endif + +#define OSAL_THREAD_HANDLE HANDLE +#define OSAL_THREAD_FUNC void +#define OSAL_THREAD_FUNC_RT void + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/soem-src/osal/win32/osal_win32.h b/soem-src/osal/win32/osal_win32.h new file mode 100644 index 0000000..37a187a --- /dev/null +++ b/soem-src/osal/win32/osal_win32.h @@ -0,0 +1,36 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +#ifndef _osal_win32_ +#define _osal_win32_ + +/* Convenience macros for operations on timevals. + NOTE: `timercmp' does not work for >= or <=. */ +# define timerisset(tvp) ((tvp)->tv_sec || (tvp)->tv_usec) +# define timeradd(a, b, result) \ + do { \ + (result)->tv_sec = (a)->tv_sec + (b)->tv_sec; \ + (result)->tv_usec = (a)->tv_usec + (b)->tv_usec; \ + if ((result)->tv_usec >= 1000000) \ + { \ + ++(result)->tv_sec; \ + (result)->tv_usec -= 1000000; \ + } \ + } while (0) +# define timersub(a, b, result) \ + do { \ + (result)->tv_sec = (a)->tv_sec - (b)->tv_sec; \ + (result)->tv_usec = (a)->tv_usec - (b)->tv_usec; \ + if ((result)->tv_usec < 0) { \ + --(result)->tv_sec; \ + (result)->tv_usec += 1000000; \ + } \ + } while (0) + +struct timezone; +// currently the tz parameter is ignored in the win32 implmentation. +int osal_gettimeofday (struct timeval *tv, struct timezone *tz); + +#endif diff --git a/soem-src/oshw/erika/nicdrv.c b/soem-src/oshw/erika/nicdrv.c new file mode 100644 index 0000000..3ef4574 --- /dev/null +++ b/soem-src/oshw/erika/nicdrv.c @@ -0,0 +1,561 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +/** \file + * \brief + * EtherCAT RAW socket driver. + * + * Low level interface functions to send and receive EtherCAT packets. + * EtherCAT has the property that packets are only send by the master, + * and the send packets always return in the receive buffer. + * There can be multiple packets "on the wire" before they return. + * To combine the received packets with the original send packets a buffer + * system is installed. The identifier is put in the index item of the + * EtherCAT header. The index is stored and compared when a frame is received. + * If there is a match the packet can be combined with the transmit packet + * and returned to the higher level function. + * + * The socket layer can exhibit a reversal in the packet order (rare). + * If the Tx order is A-B-C the return order could be A-C-B. The indexed buffer + * will reorder the packets automatically. + * + * The "redundant" option will configure two sockets and two NIC interfaces. + * Slaves are connected to both interfaces, one on the IN port and one on the + * OUT port. Packets are send via both interfaces. Any one of the connections + * (also an interconnect) can be removed and the slaves are still serviced with + * packets. The software layer will detect the possible failure modes and + * compensate. If needed the packets from interface A are resent through interface B. + * This layer if fully transparent for the higher layers. + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "oshw.h" +#include "osal.h" +#include "nicdrv.h" +#include "ee.h" +#include "intel_i210.h" +#include "ee_x86_64_tsc.h" + +/** Redundancy modes */ +enum +{ + /** No redundancy, single NIC mode */ + ECT_RED_NONE, + /** Double redundant NIC connection */ + ECT_RED_DOUBLE +}; + + +/** Primary source MAC address used for EtherCAT. + * This address is not the MAC address used from the NIC. + * EtherCAT does not care about MAC addressing, but it is used here to + * differentiate the route the packet traverses through the EtherCAT + * segment. This is needed to find out the packet flow in redundant + * configurations. */ +const uint16 priMAC[3] = { 0x0201, 0x0101, 0x0101 }; +/** Secondary source MAC address used for EtherCAT. */ +const uint16 secMAC[3] = { 0x0604, 0x0404, 0x0404 }; + +/** second MAC word is used for identification */ +#define RX_PRIM priMAC[1] +/** second MAC word is used for identification */ +#define RX_SEC secMAC[1] + +void ee_port_lock(void); +void ee_port_unlock(void); + +static inline void ecx_clear_rxbufstat(int *rxbufstat) +{ + int i; + for(i = 0; i < EC_MAXBUF; i++) + rxbufstat[i] = EC_BUF_EMPTY; +} + +/** Basic setup to connect NIC to socket. + * @param[in] port = port context struct + * @param[in] ifname = Name of NIC device, f.e. "eth0" + * @param[in] secondary = if >0 then use secondary stack instead of primary + * @return >0 if succeeded + */ +int ecx_setupnic(ecx_portt *port, const char *ifname, int secondary) +{ + int d; + struct eth_device *dev; + OSEE_PRINT("ecx_setupnic() searching %s...\n", ifname); + + for (d = 0;; ++d) { + dev = eth_get_device(d); + + if (dev == NULL) + break; // ERROR: device not found + + if (!strncmp(dev->name, ifname, MAX_DEVICE_NAME)){ + // Device found + int i; + + eth_setup_device(d, 1, 100); + port->dev_id = d; + + port->sockhandle = -1; + port->lastidx = 0; + port->redstate = ECT_RED_NONE; + port->stack.sock = &(port->sockhandle); + port->stack.txbuf = &(port->txbuf); + port->stack.txbuflength = &(port->txbuflength); + port->stack.tempbuf = &(port->tempinbuf); + port->stack.rxbuf = &(port->rxbuf); + port->stack.rxbufstat = &(port->rxbufstat); + port->stack.rxsa = &(port->rxsa); + ecx_clear_rxbufstat(&(port->rxbufstat[0])); + + /* setup ethernet headers in tx buffers so we don't have to repeat it */ + for (i = 0; i < EC_MAXBUF; i++) + { + ec_setupheader(&(port->txbuf[i])); + port->rxbufstat[i] = EC_BUF_EMPTY; + } + ec_setupheader(&(port->txbuf2)); + + break; // device found + } + } + + return (dev != NULL); +} + +/** Close sockets used + * @param[in] port = port context struct + * @return 0 + */ +inline int ecx_closenic(ecx_portt *port) +{ + // Nothing to do + return 0; +} + +/** Fill buffer with ethernet header structure. + * Destination MAC is always broadcast. + * Ethertype is always ETH_P_ECAT. + * @param[out] p = buffer + */ +void ec_setupheader(void *p) +{ + ec_etherheadert *bp; + bp = p; + bp->da0 = oshw_htons(0xffff); + bp->da1 = oshw_htons(0xffff); + bp->da2 = oshw_htons(0xffff); + bp->sa0 = oshw_htons(priMAC[0]); + bp->sa1 = oshw_htons(priMAC[1]); + bp->sa2 = oshw_htons(priMAC[2]); + bp->etype = oshw_htons(ETH_P_ECAT); +} + +/** Get new frame identifier index and allocate corresponding rx buffer. + * @param[in] port = port context struct + * @return new index. + */ +uint8 ecx_getindex(ecx_portt *port) +{ + uint8 idx; + uint8 cnt = 0; + + ee_port_lock(); + + idx = port->lastidx + 1; + /* index can't be larger than buffer array */ + if (idx >= EC_MAXBUF) + idx = 0; + + /* try to find unused index */ + while ((port->rxbufstat[idx] != EC_BUF_EMPTY) && (cnt < EC_MAXBUF)) { + idx++; + cnt++; + if (idx >= EC_MAXBUF) + idx = 0; + } + port->rxbufstat[idx] = EC_BUF_ALLOC; + if (port->redstate != ECT_RED_NONE) + port->redport->rxbufstat[idx] = EC_BUF_ALLOC; + port->lastidx = idx; + + ee_port_unlock(); + + return idx; +} + +/** Set rx buffer status. + * @param[in] port = port context struct + * @param[in] idx = index in buffer array + * @param[in] bufstat = status to set + */ +void ecx_setbufstat(ecx_portt *port, uint8 idx, int bufstat) +{ + port->rxbufstat[idx] = bufstat; + if (port->redstate != ECT_RED_NONE) + port->redport->rxbufstat[idx] = bufstat; +} + +/** Transmit buffer over socket (non blocking). + * @param[in] port = port context struct + * @param[in] idx = index in tx buffer array + * @param[in] stacknumber = 0=Primary 1=Secondary stack + * @return socket send result + */ +int ecx_outframe(ecx_portt *port, uint8 idx, int stacknumber) +{ + int lp; + ec_stackT *stack; + + if (!stacknumber) + stack = &(port->stack); + else + stack = &(port->redport->stack); + lp = (*stack->txbuflength)[idx]; + (*stack->rxbufstat)[idx] = EC_BUF_TX; + + eth_send_packet(port->dev_id, (*stack->txbuf)[idx], lp, 1); + + return 1; +} + +/** Transmit buffer over socket (non blocking). + * @param[in] port = port context struct + * @param[in] idx = index in tx buffer array + * @return socket send result + */ +int ecx_outframe_red(ecx_portt *port, uint8 idx) +{ + ec_comt *datagramP; + ec_etherheadert *ehp; + int rval; + + ehp = (ec_etherheadert *)&(port->txbuf[idx]); + /* rewrite MAC source address 1 to primary */ + ehp->sa1 = oshw_htons(priMAC[1]); + /* transmit over primary socket*/ + rval = ecx_outframe(port, idx, 0); + if (port->redstate != ECT_RED_NONE) { + ee_port_lock(); + ehp = (ec_etherheadert *)&(port->txbuf2); + /* use dummy frame for secondary socket transmit (BRD) */ + datagramP = (ec_comt*)&(port->txbuf2[ETH_HEADERSIZE]); + /* write index to frame */ + datagramP->index = idx; + /* rewrite MAC source address 1 to secondary */ + ehp->sa1 = oshw_htons(secMAC[1]); + /* transmit over secondary socket */ + port->redport->rxbufstat[idx] = EC_BUF_TX; + eth_send_packet(port->dev_id, &(port->txbuf2), port->txbuflength2, 1); + ee_port_unlock(); + } + + return rval; +} + +/** Non blocking read of socket. Put frame in temporary buffer. + * @param[in] port = port context struct + * @param[in] stacknumber = 0=primary 1=secondary stack + * @return >0 if frame is available and read + */ +static int ecx_recvpkt(ecx_portt *port, int stacknumber) +{ + int lp, bytesrx; + ec_stackT *stack; + + if (!stacknumber) + stack = &(port->stack); + else + stack = &(port->redport->stack); + lp = sizeof(port->tempinbuf); + bytesrx = eth_receive_packet(port->dev_id, (*stack->tempbuf), lp, 1, 0); + port->tempinbufs = bytesrx; + + return (bytesrx > 0); +} + +/** Non blocking receive frame function. Uses RX buffer and index to combine + * read frame with transmitted frame. To compensate for received frames that + * are out-of-order all frames are stored in their respective indexed buffer. + * If a frame was placed in the buffer previously, the function retrieves it + * from that buffer index without calling ec_recvpkt. If the requested index + * is not already in the buffer it calls ec_recvpkt to fetch it. There are + * three options now, 1 no frame read, so exit. 2 frame read but other + * than requested index, store in buffer and exit. 3 frame read with matching + * index, store in buffer, set completed flag in buffer status and exit. + * + * @param[in] port = port context struct + * @param[in] idx = requested index of frame + * @param[in] stacknumber = 0=primary 1=secondary stack + * @return Workcounter if a frame is found with corresponding index, otherwise + * EC_NOFRAME or EC_OTHERFRAME. + */ +int ecx_inframe(ecx_portt *port, uint8 idx, int stacknumber) +{ + uint16 l; + int rval; + uint8 idxf; + ec_etherheadert *ehp; + ec_comt *ecp; + ec_stackT *stack; + ec_bufT *rxbuf; + + if (!stacknumber) + stack = &(port->stack); + else + stack = &(port->redport->stack); + rval = EC_NOFRAME; + rxbuf = &(*stack->rxbuf)[idx]; + /* check if requested index is already in buffer ? */ + if ((idx < EC_MAXBUF) && ((*stack->rxbufstat)[idx] == EC_BUF_RCVD)) { + l = (*rxbuf)[0] + ((uint16)((*rxbuf)[1] & 0x0f) << 8); + /* return WKC */ + rval = ((*rxbuf)[l] + ((uint16)(*rxbuf)[l + 1] << 8)); + /* mark as completed */ + (*stack->rxbufstat)[idx] = EC_BUF_COMPLETE; + } else { + ee_port_lock(); + /* non blocking call to retrieve frame from socket */ + + + while (1) { + if (ecx_recvpkt(port, stacknumber)) { + rval = EC_OTHERFRAME; + ehp =(ec_etherheadert*)(stack->tempbuf); + /* check if it is an EtherCAT frame */ + if (ehp->etype == oshw_htons(ETH_P_ECAT)) { + ecp =(ec_comt*)(&(*stack->tempbuf)[ETH_HEADERSIZE]); + l = etohs(ecp->elength) & 0x0fff; + idxf = ecp->index; + /* found index equals requested index ? */ + if (idxf == idx) { + /* yes, put it in the buffer array (strip ethernet header) */ + memcpy(rxbuf, &(*stack->tempbuf)[ETH_HEADERSIZE], (*stack->txbuflength)[idx] - ETH_HEADERSIZE); + /* return WKC */ + rval = ((*rxbuf)[l] + ((uint16)((*rxbuf)[l + 1]) << 8)); + /* mark as completed */ + (*stack->rxbufstat)[idx] = EC_BUF_COMPLETE; + /* store MAC source word 1 for redundant routing info */ + (*stack->rxsa)[idx] = oshw_ntohs(ehp->sa1); + break; + } else if (idxf < EC_MAXBUF && (*stack->rxbufstat)[idxf] == EC_BUF_TX) { + rxbuf = &(*stack->rxbuf)[idxf]; + /* put it in the buffer array (strip ethernet header) */ + memcpy(rxbuf, &(*stack->tempbuf)[ETH_HEADERSIZE], (*stack->txbuflength)[idxf] - ETH_HEADERSIZE); + /* mark as received */ + (*stack->rxbufstat)[idxf] = EC_BUF_RCVD; + (*stack->rxsa)[idxf] = oshw_ntohs(ehp->sa1); + break; + } else { + OSEE_PRINT("ecx_inframe(): WARNING: strange things happened\n"); + /* strange things happened */ + } + } else { + OSEE_PRINT("ecx_inframe(): WARNING it is NOT an EtherCAT frame!\n"); + } + } else { + // WARNING: no messages received. + break; + } + } + + ee_port_unlock(); + + } + + /* WKC if matching frame found */ + return rval; +} + +/** Blocking redundant receive frame function. If redundant mode is not active then + * it skips the secondary stack and redundancy functions. In redundant mode it waits + * for both (primary and secondary) frames to come in. The result goes in an decision + * tree that decides, depending on the route of the packet and its possible missing arrival, + * how to reroute the original packet to get the data in an other try. + * + * @param[in] port = port context struct + * @param[in] idx = requested index of frame + * @param[in] timer = absolute timeout time + * @return Workcounter if a frame is found with corresponding index, otherwise + * EC_NOFRAME. + */ +static int ecx_waitinframe_red(ecx_portt *port, uint8 idx, osal_timert *timer) +{ + osal_timert timer2; + int wkc = EC_NOFRAME; + int wkc2 = EC_NOFRAME; + int primrx, secrx; + + /* if not in redundant mode then always assume secondary is OK */ + if (port->redstate == ECT_RED_NONE) + wkc2 = 0; + do { + /* only read frame if not already in */ + if (wkc <= EC_NOFRAME) + wkc = ecx_inframe(port, idx, 0); + /* only try secondary if in redundant mode */ + if (port->redstate != ECT_RED_NONE) { + /* only read frame if not already in */ + if (wkc2 <= EC_NOFRAME) + wkc2 = ecx_inframe(port, idx, 1); + } + /* wait for both frames to arrive or timeout */ + } while (((wkc <= EC_NOFRAME) || (wkc2 <= EC_NOFRAME)) && !osal_timer_is_expired(timer)); + /* only do redundant functions when in redundant mode */ + if (port->redstate != ECT_RED_NONE) { + /* primrx if the received MAC source on primary socket */ + primrx = 0; + if (wkc > EC_NOFRAME) primrx = port->rxsa[idx]; + /* secrx if the received MAC source on psecondary socket */ + secrx = 0; + if (wkc2 > EC_NOFRAME) secrx = port->redport->rxsa[idx]; + + /* primary socket got secondary frame and secondary socket got primary frame */ + /* normal situation in redundant mode */ + if ( ((primrx == RX_SEC) && (secrx == RX_PRIM)) ) { + /* copy secondary buffer to primary */ + memcpy(&(port->rxbuf[idx]), &(port->redport->rxbuf[idx]), port->txbuflength[idx] - ETH_HEADERSIZE); + wkc = wkc2; + } + /* primary socket got nothing or primary frame, and secondary socket got secondary frame */ + /* we need to resend TX packet */ + if ( ((primrx == 0) && (secrx == RX_SEC)) || + ((primrx == RX_PRIM) && (secrx == RX_SEC)) ) { + /* If both primary and secondary have partial connection retransmit the primary received + * frame over the secondary socket. The result from the secondary received frame is a combined + * frame that traversed all slaves in standard order. */ + if ( (primrx == RX_PRIM) && (secrx == RX_SEC) ) { + /* copy primary rx to tx buffer */ + memcpy(&(port->txbuf[idx][ETH_HEADERSIZE]), &(port->rxbuf[idx]), port->txbuflength[idx] - ETH_HEADERSIZE); + } + osal_timer_start (&timer2, EC_TIMEOUTRET); + /* resend secondary tx */ + ecx_outframe(port, idx, 1); + do { + /* retrieve frame */ + wkc2 = ecx_inframe(port, idx, 1); + } while ((wkc2 <= EC_NOFRAME) && !osal_timer_is_expired(&timer2)); + if (wkc2 > EC_NOFRAME) { + /* copy secondary result to primary rx buffer */ + memcpy(&(port->rxbuf[idx]), &(port->redport->rxbuf[idx]), port->txbuflength[idx] - ETH_HEADERSIZE); + wkc = wkc2; + } + } + } + + /* return WKC or EC_NOFRAME */ + return wkc; +} + +/** Blocking receive frame function. Calls ec_waitinframe_red(). + * @param[in] port = port context struct + * @param[in] idx = requested index of frame + * @param[in] timeout = timeout in us + * @return Workcounter if a frame is found with corresponding index, otherwise + * EC_NOFRAME. + */ +int ecx_waitinframe(ecx_portt *port, uint8 idx, int timeout) +{ + int wkc; + osal_timert timer; + + osal_timer_start (&timer, timeout); + wkc = ecx_waitinframe_red(port, idx, &timer); + + return wkc; +} + +/** Blocking send and receive frame function. Used for non processdata frames. + * A datagram is build into a frame and transmitted via this function. It waits + * for an answer and returns the workcounter. The function retries if time is + * left and the result is WKC=0 or no frame received. + * + * The function calls ec_outframe_red() and ec_waitinframe_red(). + * + * @param[in] port = port context struct + * @param[in] idx = index of frame + * @param[in] timeout = timeout in us + * @return Workcounter or EC_NOFRAME + */ +int ecx_srconfirm(ecx_portt *port, uint8 idx, int timeout) +{ + int wkc = EC_NOFRAME; + osal_timert timer1, timer2; + + osal_timer_start (&timer1, timeout); + do { + /* tx frame on primary and if in redundant mode a dummy on secondary */ + ecx_outframe_red(port, idx); + if (timeout < EC_TIMEOUTRET) { + osal_timer_start (&timer2, timeout); + } else { + /* normally use partial timeout for rx */ + osal_timer_start (&timer2, EC_TIMEOUTRET); + } + /* get frame from primary or if in redundant mode possibly + from secondary */ + wkc = ecx_waitinframe_red(port, idx, &timer2); + /* wait for answer with WKC>=0 or otherwise retry until timeout */ + } while ((wkc <= EC_NOFRAME) && !osal_timer_is_expired (&timer1)); + + + return wkc; +} + + +#ifdef EC_VER1 +int ec_setupnic(const char *ifname, int secondary) +{ + return ecx_setupnic(&ecx_port, ifname, secondary); +} + +int ec_closenic(void) +{ + return ecx_closenic(&ecx_port); +} + +int ec_getindex(void) +{ + return ecx_getindex(&ecx_port); +} + +void ec_setbufstat(uint8 idx, int bufstat) +{ + ecx_setbufstat(&ecx_port, idx, bufstat); +} + +int ec_outframe(uint8 idx, int stacknumber) +{ + return ecx_outframe(&ecx_port, idx, stacknumber); +} + +int ec_outframe_red(uint8 idx) +{ + return ecx_outframe_red(&ecx_port, idx); +} + +int ec_inframe(uint8 idx, int stacknumber) +{ + return ecx_inframe(&ecx_port, idx, stacknumber); +} + +int ec_waitinframe(uint8 idx, int timeout) +{ + return ecx_waitinframe(&ecx_port, idx, timeout); +} + +int ec_srconfirm(uint8 idx, int timeout) +{ + return ecx_srconfirm(&ecx_port, idx, timeout); +} +#endif diff --git a/soem-src/oshw/erika/nicdrv.h b/soem-src/oshw/erika/nicdrv.h new file mode 100644 index 0000000..20e5d82 --- /dev/null +++ b/soem-src/oshw/erika/nicdrv.h @@ -0,0 +1,122 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +/** \file + * \brief + * Headerfile for nicdrv.c + */ + +#ifndef _nicdrvh_ +#define _nicdrvh_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +typedef struct +{ + /** socket connection used */ + int *sock; + /** tx buffer */ + ec_bufT (*txbuf)[EC_MAXBUF]; + /** tx buffer lengths */ + int (*txbuflength)[EC_MAXBUF]; + /** temporary receive buffer */ + ec_bufT *tempbuf; + /** rx buffers */ + ec_bufT (*rxbuf)[EC_MAXBUF]; + /** rx buffer status fields */ + int (*rxbufstat)[EC_MAXBUF]; + /** received MAC source address (middle word) */ + int (*rxsa)[EC_MAXBUF]; +} ec_stackT; + +/** pointer structure to buffers for redundant port */ +typedef struct +{ + ec_stackT stack; + int sockhandle; + /** rx buffers */ + ec_bufT rxbuf[EC_MAXBUF]; + /** rx buffer status */ + int rxbufstat[EC_MAXBUF]; + /** rx MAC source address */ + int rxsa[EC_MAXBUF]; + /** temporary rx buffer */ + ec_bufT tempinbuf; +} ecx_redportt; + +/** pointer structure to buffers, vars and mutexes for port instantiation */ +typedef struct +{ + ec_stackT stack; + int sockhandle; + /** rx buffers */ + ec_bufT rxbuf[EC_MAXBUF]; + /** rx buffer status */ + int rxbufstat[EC_MAXBUF]; + /** rx MAC source address */ + int rxsa[EC_MAXBUF]; + /** temporary rx buffer */ + ec_bufT tempinbuf; + /** temporary rx buffer status */ + int tempinbufs; + /** transmit buffers */ + ec_bufT txbuf[EC_MAXBUF]; + /** transmit buffer lengths */ + int txbuflength[EC_MAXBUF]; + /** temporary tx buffer */ + ec_bufT txbuf2; + /** temporary tx buffer length */ + int txbuflength2; + /** last used frame index */ + uint8 lastidx; + /** current redundancy state */ + int redstate; + /** pointer to redundancy port and buffers */ + ecx_redportt *redport; + + /** Device id in the device pool */ + int dev_id; + + // TODO: add mutex support +} ecx_portt; + +extern const uint16 priMAC[3]; +extern const uint16 secMAC[3]; + +#ifdef EC_VER1 +extern ecx_portt ecx_port; +extern ecx_redportt ecx_redport; + +int ec_setupnic(const char * ifname, int secondary); +int ec_closenic(void); +void ec_setbufstat(uint8 idx, int bufstat); +uint8 ec_getindex(void); +int ec_outframe(uint8 idx, int sock); +int ec_outframe_red(uint8 idx); +int ec_waitinframe(uint8 idx, int timeout); +int ec_srconfirm(uint8 idx,int timeout); +int ec_inframe(uint8 idx, int stacknumber); +#endif + +void ec_setupheader(void *p); +int ecx_setupnic(ecx_portt *port, const char *ifname, int secondary); +int ecx_closenic(ecx_portt *port); +void ecx_setbufstat(ecx_portt *port, uint8 idx, int bufstat); +uint8 ecx_getindex(ecx_portt *port); +int ecx_outframe(ecx_portt *port, uint8 idx, int sock); +int ecx_outframe_red(ecx_portt *port, uint8 idx); +int ecx_waitinframe(ecx_portt *port, uint8 idx, int timeout); +int ecx_srconfirm(ecx_portt *port, uint8 idx,int timeout); + +int ecx_inframe(ecx_portt *port, uint8 idx, int stacknumber); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/soem-src/oshw/erika/oshw.c b/soem-src/oshw/erika/oshw.c new file mode 100644 index 0000000..256c75c --- /dev/null +++ b/soem-src/oshw/erika/oshw.c @@ -0,0 +1,79 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +#include +#include +#include +#include + +#include "oshw.h" +#include "intel_i210.h" +#include "ethercat.h" + +#if !defined(__gnu_linux__) +#include +#else +#include +#define __htons(x) htobe16(x) +#define __ntohs(x) be16toh(x) +#endif + +ec_adaptert adapters [DEVS_MAX_NB]; + +/** + * Host to Network byte order (i.e. to big endian). + * + * Note that Ethercat uses little endian byte order, except for the Ethernet + * header which is big endian as usual. + */ +inline uint16 oshw_htons(uint16 host) +{ + // __htons() is provided by the bare-metal x86 compiler + return __htons(host); +} + +/** + * Network (i.e. big endian) to Host byte order. + * + * Note that Ethercat uses little endian byte order, except for the Ethernet + * header which is big endian as usual. + */ +inline uint16 oshw_ntohs(uint16 network) +{ + // __ntohs() is provided by the bare-metal x86 compiler + return __ntohs(network); +} + +/** Create list over available network adapters. + * @return First element in linked list of adapters + */ +ec_adaptert* oshw_find_adapters(void) +{ + ec_adaptert *ret = NULL; + if (eth_discover_devices() >= 0) { + for (int i = 0;; ++i) { + struct eth_device *dev = eth_get_device(i); + if (dev == NULL) { + adapters[i-1].next = NULL; + break; + } + strncpy(adapters[i].name, dev->name, MAX_DEVICE_NAME); + adapters[i].next = &adapters[i+1]; + } + ret = &(adapters[0]); + } + return ret; +} + +/** Free memory allocated memory used by adapter collection. + * @param[in] adapter = First element in linked list of adapters + * EC_NOFRAME. + */ +void oshw_free_adapters(ec_adaptert *adapter) +{ +} + +extern int ec_slavecount; + diff --git a/soem-src/oshw/erika/oshw.h b/soem-src/oshw/erika/oshw.h new file mode 100644 index 0000000..03a83c3 --- /dev/null +++ b/soem-src/oshw/erika/oshw.h @@ -0,0 +1,31 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +/** \file + * \brief + * Headerfile for ethercatbase.c + */ + +#ifndef _oshw_ +#define _oshw_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "ethercattype.h" +#include "nicdrv.h" +#include "ethercatmain.h" + +uint16 oshw_htons(uint16 hostshort); +uint16 oshw_ntohs(uint16 networkshort); +ec_adaptert* oshw_find_adapters(void); +void oshw_free_adapters(ec_adaptert * adapter); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/soem-src/oshw/intime/nicdrv.c b/soem-src/oshw/intime/nicdrv.c new file mode 100644 index 0000000..a7751d1 --- /dev/null +++ b/soem-src/oshw/intime/nicdrv.c @@ -0,0 +1,744 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +/** \file + * \brief + * EtherCAT RAW socket driver. + * + * Low level interface functions to send and receive EtherCAT packets. + * EtherCAT has the property that packets are only sent by the master, + * and the sent packets always return in the receive buffer. + * There can be multiple packets "on the wire" before they return. + * To combine the received packets with the original sent packets a buffer + * system is installed. The identifier is put in the index item of the + * EtherCAT header. The index is stored and compared when a frame is received. + * If there is a match the packet can be combined with the transmit packet + * and returned to the higher level function. + * + * The socket layer can exhibit a reversal in the packet order (rare). + * If the Tx order is A-B-C the return order could be A-C-B. The indexed buffer + * will reorder the packets automatically. + * + * The "redundant" option will configure two sockets and two NIC interfaces. + * Slaves are connected to both interfaces, one on the IN port and one on the + * OUT port. Packets are send via both interfaces. Any one of the connections + * (also an interconnect) can be removed and the slaves are still serviced with + * packets. The software layer will detect the possible failure modes and + * compensate. If needed the packets from interface A are resent through interface B. + * This layer if fully transparent for the higher layers. + */ + +#include +#include +#include +#include +#include +#include "hpeif2.h" + +#include "ethercattype.h" +#include "nicdrv.h" +#include "oshw.h" + +/** Redundancy modes */ +enum +{ + /** No redundancy, single NIC mode */ + ECT_RED_NONE, + /** Double redundant NIC connection */ + ECT_RED_DOUBLE +}; + + +/** Primary source MAC address used for EtherCAT. + * This address is not the MAC address used from the NIC. + * EtherCAT does not care about MAC addressing, but it is used here to + * differentiate the route the packet traverses through the EtherCAT + * segment. This is needed to find out the packet flow in redundant + * configurations. */ +const uint16 priMAC[3] = { 0x0101, 0x0101, 0x0101 }; +/** Secondary source MAC address used for EtherCAT. */ +const uint16 secMAC[3] = { 0x0404, 0x0404, 0x0404 }; + +/** second MAC word is used for identification */ +#define RX_PRIM priMAC[1] +/** second MAC word is used for identification */ +#define RX_SEC secMAC[1] + +#ifdef EC_VER1 + +ecx_portt ecx_port; +ecx_redportt ecx_redport; + +#endif + +#define ECAT_PRINT_INFO printf +#define ECAT_PRINT_WARN printf +#define ECAT_PRINT_ERROR printf + +/* HPE port settings */ +const unsigned long interrupt_mode = NO_INTERRUPT; +const unsigned long phy_settings = SPEED_100 | DUPLEX_FULL; + +/** Basic setup to connect NIC to socket. + * @param[in] ifname = Name of NIC device, f.e. "eth0" + * @param[in] secondary = if >0 then use secondary stack instead of primary + * @return >0 if succeeded + */ +int ecx_setupnic(ecx_portt *port, const char *ifname, int secondary) +{ + HPESTATUS status; + HPEMEDIASTATUS mstat; + time_t now, t; + unsigned char mac[6]; + int i; + int dontWaitForLink = 0; + int result = 1; + HPE_CONFIG_OPTIONS conf = { 0, 0, NULL}; + + status = hpeOpen(ifname, phy_settings, interrupt_mode, &(port->handle)); + if (status != E_OK) + { + ECAT_PRINT_ERROR("hpeOpen failed with status %04x ", status); + if(status == E_EXIST) ECAT_PRINT_ERROR("E_EXIST\n"); + else if(status == E_STATE) ECAT_PRINT_ERROR("E_STATE\n"); + else if(status == E_PARAM) ECAT_PRINT_ERROR("E_PARAM\n"); + else if(status == E_INVALID_ADDR) ECAT_PRINT_ERROR("E_INVALID_ADDR\n"); + else if(status == E_IO) ECAT_PRINT_ERROR("E_IO\n"); + else if(status == E_TIME) ECAT_PRINT_ERROR("E_TIME\n"); + else ECAT_PRINT_ERROR("UNKNOWN\n"); + result = 0; + goto end; + } + + conf.option_flags |= OPT_PROMISC; + + status = hpeConfigOptions(port->handle, &conf, sizeof(conf)); + if (status != E_OK) + { + ECAT_PRINT_ERROR("hpeConfigOptions failed with status %04x\n", status); + // NOTE: HPE driver for Intel 10/100 Mbps device currently doesn't support multicast. + result = 0; + goto end; + } + + time(&now); + do + { + status = hpeGetMediaStatus(port->handle, &mstat); + if (status != E_OK) + { + ECAT_PRINT_ERROR("hpeGetMediaStatus failed with status %04x\n", status); + result = 0; + goto end; + } + if (mstat.media_speed == SPEED_NONE) + { + RtSleepEx(1000); + time(&t); + } + } while (mstat.media_speed == SPEED_NONE && t < (now+10)); + + if (((mstat.media_speed & phy_settings) == 0) || ((mstat.media_duplex & phy_settings) == 0)) { + ECAT_PRINT_ERROR("Media not connected as requested: speed=%u, duplex=%u\n", + mstat.media_speed, mstat.media_duplex); + result = 0; + goto end; + } + + status= hpeGetMacAddress(port->handle, mac); + if (status != E_OK) + { + ECAT_PRINT_ERROR("hpeGetMacAddress failed with status %04x\n", status); + result = 0; + goto end; + } + + /* allocate 2 receive buffers and attach them */ + status = hpeAllocateReceiveBufferSet(port->handle, &(port->rx_buffers), EC_MAXBUF, EC_BUFSIZE); + if (status != E_OK) + { + ECAT_PRINT_ERROR("hpeAllocateReceiveBufferSet failed with status %04x\n", status); + result = 0; + goto end; + } + + status = hpeAttachReceiveBufferSet(port->handle, port->rx_buffers); + if (status != E_OK) + { + ECAT_PRINT_ERROR("hpeAttachReceiveBufferSet failed with status %04x\n", status); + result = 0; + goto end; + } + + if (secondary) + { + /* secondary port struct available? */ + if (port->redport) + { + /* when using secondary socket it is automatically a redundant setup */ + port->redstate = ECT_RED_DOUBLE; + } + } + else + { + port->redstate = ECT_RED_NONE; + /* Init regions */ + port->getindex_region = CreateRtRegion (PRIORITY_QUEUING); + port->rx_region = CreateRtRegion (PRIORITY_QUEUING); + port->tx_region = CreateRtRegion (PRIORITY_QUEUING); + port->lastidx = 0; + port->stack.txbuf = &(port->txbuf); + port->stack.txbuflength = &(port->txbuflength); + port->stack.tempbuf = &(port->tempinbuf); + port->stack.rxbuf = &(port->rxbuf); + port->stack.rxbufstat = &(port->rxbufstat); + port->stack.rxsa = &(port->rxsa); + } + + /* setup ethernet headers in tx buffers so we don't have to repeat it */ + for (i = 0; i < EC_MAXBUF; i++) + { + ec_setupheader(&(port->txbuf[i])); + port->rxbufstat[i] = EC_BUF_EMPTY; + // allocate 1 transmit buffer of two fragments and 500 bytes and attach it + port->tx_buffers[i] = (HPETXBUFFERSET *)AllocateRtMemory(sizeof(HPETXBUFFER) + sizeof(HPETXBUFFERSET)); + port->tx_buffers[i]->buffer_count = 1; + port->tx_buffers[i]->buffers[0].fragment_count = 1; + // first fragment is the fixed header, second is dynamic data + port->tx_buffers[i]->buffers[0].fragments[0].ptr = port->txbuf[i]; + port->tx_buffers[i]->buffers[0].fragments[0].size = EC_BUFSIZE; + port->tx_buffers[i]->buffers[0].fragments[0].paddr = 0; + } + ec_setupheader(&(port->txbuf2)); + +end: + return result; +} + +/** Close sockets used + * @return 0 + */ +int ecx_closenic(ecx_portt *port) +{ + HPESTATUS status; + int i; + + if (port->tx_buffers) + { + for (i = 0; i < EC_MAXBUF; i++) + { + FreeRtMemory(port->tx_buffers[i]); + port->tx_buffers[i] = NULL; + } + } + if (port->rx_buffers) + { + hpeFreeReceiveBufferSet(port->handle, &(port->rx_buffers)); + } + + status= hpeClose(port->handle); + if (status != E_OK) + { + ECAT_PRINT_ERROR("hpeClose failed with status %04x\n", status); + } + return 0; +} + +/** Fill buffer with Ethernet header structure. + * Destination MAC is always broadcast. + * Ethertype is always ETH_P_ECAT. + * @param[out] p = buffer + */ +void ec_setupheader(void *p) +{ + ec_etherheadert *bp; + bp = (ec_etherheadert *)p; + bp->da0 = oshw_htons(0xffff); + bp->da1 = oshw_htons(0xffff); + bp->da2 = oshw_htons(0xffff); + bp->sa0 = oshw_htons(priMAC[0]); + bp->sa1 = oshw_htons(priMAC[1]); + bp->sa2 = oshw_htons(priMAC[2]); + bp->etype = oshw_htons(ETH_P_ECAT); +} + +/** Get new frame identifier index and allocate corresponding rx buffer. + * @return new index. + */ +uint8 ecx_getindex(ecx_portt *port) +{ + uint8 idx; + uint8 cnt; + + WaitForRtControl(port->getindex_region); + + idx = port->lastidx + 1; + /* index can't be larger than buffer array */ + if (idx >= EC_MAXBUF) + { + idx = 0; + } + cnt = 0; + /* try to find unused index */ + while ((port->rxbufstat[idx] != EC_BUF_EMPTY) && (cnt < EC_MAXBUF)) + { + idx++; + cnt++; + if (idx >= EC_MAXBUF) + { + idx = 0; + } + } + port->rxbufstat[idx] = EC_BUF_ALLOC; + if ( port->redstate != ECT_RED_NONE) + { + port->redport->rxbufstat[idx] = EC_BUF_ALLOC; + } + + port->lastidx = idx; + ReleaseRtControl(); + + return idx; +} + +/** Set rx buffer status. + * @param[in] idx = index in buffer array + * @param[in] bufstat = status to set + */ +void ecx_setbufstat(ecx_portt *port, uint8 idx, int bufstat) +{ + port->rxbufstat[idx] = bufstat; + if (port->redstate != ECT_RED_NONE) + { + port->redport->rxbufstat[idx] = bufstat; + } +} + +/** Transmit buffer over socket (non blocking). + * @param[in] idx = index in tx buffer array + * @param[in] stacknumber = 0=Primary 1=Secondary stack + * @return socket send result + */ +int ecx_outframe(ecx_portt *port, uint8 idx, int stacknumber) +{ + HPESTATUS status; + DWORD txstate; + int lp; + ec_stackT *stack; + int result = 0; + int retries = 1024; + + if (!stacknumber) + { + stack = &(port->stack); + } + else + { + stack = &(port->redport->stack); + } + lp = (*stack->txbuflength)[idx]; + port->tx_buffers[idx]->buffer_count = 1; + port->tx_buffers[idx]->buffers[0].fragments[0].size = lp; + + // wait for transmit to complete + do + { + result = hpeGetTransmitterState(port->handle, &txstate) == E_OK && txstate == HPE_TXBUSY; + } while (result && retries-- > 0); + + if (result) + { + result = -1; + goto end; + } + + log_RT_event('S',(WORD)2); + status = hpeAttachTransmitBufferSet(port->handle, port->tx_buffers[idx]); + if (status != E_OK) + { + result = -2; + goto end; + } + log_RT_event('S',(WORD)3); + + (*stack->rxbufstat)[idx] = EC_BUF_TX; + status = hpeStartTransmitter(port->handle); + if (status != E_OK) + { + (*stack->rxbufstat)[idx] = EC_BUF_EMPTY; + result = -3; + goto end; + } + + log_RT_event('S',(WORD)4); + result = lp; + +end: + + return result; +} + +/** Transmit buffer over socket (non blocking). + * @param[in] idx = index in tx buffer array + * @return socket send result + */ +int ecx_outframe_red(ecx_portt *port, uint8 idx) +{ + HPESTATUS status; + ec_comt *datagramP; + ec_etherheadert *ehp; + int rval; + + ehp = (ec_etherheadert *)&(port->txbuf[idx]); + /* rewrite MAC source address 1 to primary */ + ehp->sa1 = oshw_htons(priMAC[1]); + /* transmit over primary socket*/ + rval = ecx_outframe(port, idx, 0); + if (port->redstate != ECT_RED_NONE) + { + ehp = (ec_etherheadert *)&(port->txbuf2); + /* use dummy frame for secondary socket transmit (BRD) */ + datagramP = (ec_comt*)&(port->txbuf2)[ETH_HEADERSIZE]; + /* write index to frame */ + datagramP->index = idx; + /* rewrite MAC source address 1 to secondary */ + ehp->sa1 = oshw_htons(secMAC[1]); + /* transmit over secondary socket */ + //send(sockhandle2, &ec_txbuf2, ec_txbuflength2 , 0); + // OBS! redundant not ACTIVE for BFIN, just added to compile + //ASSERT (0); + hpeAttachTransmitBufferSet(port->redport->handle, port->tx_buffers[idx]); + port->redport->rxbufstat[idx] = EC_BUF_TX; + status = hpeStartTransmitter(port->redport->handle); + if (status != E_OK) + { + (*stack->rxbufstat)[idx] = EC_BUF_EMPTY; + } + } + + return rval; +} + +/** Non blocking read of socket. Put frame in temporary buffer. + * @param[in] stacknumber = 0=primary 1=secondary stack + * @return >0 if frame is available and read + */ +static int ecx_recvpkt(ecx_portt *port, int stacknumber) +{ + HPEBUFFER *rxbuffer; + HPESTATUS status; + ec_stackT *stack; + int bytesrx = 0; + + if (!stacknumber) + { + stack = &(port->stack); + } + else + { + stack = &(port->redport->stack); + } + + log_RT_event('R',(WORD)2); + + status = hpeGetReceiveBuffer(port->handle, &rxbuffer); + if (status == E_OK) + { + memcpy(stack->tempbuf,rxbuffer->ptr, rxbuffer->used); + bytesrx = rxbuffer->used; + port->tempinbufs = bytesrx; + // TODO case no interrupt + } + log_RT_event('R',(WORD)3); + + return (bytesrx > 0); +} + +/** Non blocking receive frame function. Uses RX buffer and index to combine + * read frame with transmitted frame. To compensate for received frames that + * are out-of-order all frames are stored in their respective indexed buffer. + * If a frame was placed in the buffer previously, the function retrieves it + * from that buffer index without calling ec_recvpkt. If the requested index + * is not already in the buffer it calls ec_recvpkt to fetch it. There are + * three options now, 1 no frame read, so exit. 2 frame read but other + * than requested index, store in buffer and exit. 3 frame read with matching + * index, store in buffer, set completed flag in buffer status and exit. + * + * @param[in] idx = requested index of frame + * @param[in] stacknumber = 0=primary 1=secondary stack + * @return Workcounter if a frame is found with corresponding index, otherwise + * EC_NOFRAME or EC_OTHERFRAME. + */ +int ecx_inframe(ecx_portt *port, uint8 idx, int stacknumber) +{ + uint16 l; + int rval; + uint8 idxf; + ec_etherheadert *ehp; + ec_comt *ecp; + ec_stackT *stack; + ec_bufT *rxbuf; + + if (!stacknumber) + { + stack = &(port->stack); + } + else + { + stack = &(port->redport->stack); + } + + rval = EC_NOFRAME; + rxbuf = &(*stack->rxbuf)[idx]; + /* check if requested index is already in buffer ? */ + if ((idx < EC_MAXBUF) && ( (*stack->rxbufstat)[idx] == EC_BUF_RCVD)) + { + l = (*rxbuf)[0] + ((uint16)((*rxbuf)[1] & 0x0f) << 8); + /* return WKC */ + rval = ((*rxbuf)[l] + ((uint16)(*rxbuf)[l + 1] << 8)); + /* mark as completed */ + (*stack->rxbufstat)[idx] = EC_BUF_COMPLETE; + } + else + { + WaitForRtControl(port->rx_region); + /* non blocking call to retrieve frame from socket */ + if (ecx_recvpkt(port, stacknumber)) + { + rval = EC_OTHERFRAME; + ehp = (ec_etherheadert*)(stack->tempbuf); + /* check if it is an EtherCAT frame */ + if (ehp->etype == oshw_htons(ETH_P_ECAT)) + { + ecp =(ec_comt*)(&(*stack->tempbuf)[ETH_HEADERSIZE]); + l = etohs(ecp->elength) & 0x0fff; + idxf = ecp->index; + /* found index equals requested index ? */ + if (idxf == idx) + { + /* yes, put it in the buffer array (strip ethernet header) */ + memcpy(rxbuf, &(*stack->tempbuf)[ETH_HEADERSIZE], (*stack->txbuflength)[idx] - ETH_HEADERSIZE); + /* return WKC */ + rval = ((*rxbuf)[l] + ((uint16)((*rxbuf)[l + 1]) << 8)); + /* mark as completed */ + (*stack->rxbufstat)[idx] = EC_BUF_COMPLETE; + /* store MAC source word 1 for redundant routing info */ + (*stack->rxsa)[idx] = oshw_ntohs(ehp->sa1); + } + else + { + /* check if index exist and someone is waiting for it */ + if (idxf < EC_MAXBUF && (*stack->rxbufstat)[idxf] == EC_BUF_TX) + { + rxbuf = &(*stack->rxbuf)[idxf]; + /* put it in the buffer array (strip ethernet header) */ + memcpy(rxbuf, &(*stack->tempbuf)[ETH_HEADERSIZE], (*stack->txbuflength)[idxf] - ETH_HEADERSIZE); + /* mark as received */ + (*stack->rxbufstat)[idxf] = EC_BUF_RCVD; + (*stack->rxsa)[idxf] = oshw_ntohs(ehp->sa1); + } + } + } + } + ReleaseRtControl(); + } + + /* WKC if matching frame found */ + return rval; +} + +/** Blocking redundant receive frame function. If redundant mode is not active then + * it skips the secondary stack and redundancy functions. In redundant mode it waits + * for both (primary and secondary) frames to come in. The result goes in an decision + * tree that decides, depending on the route of the packet and its possible missing arrival, + * how to reroute the original packet to get the data in an other try. + * + * @param[in] idx = requested index of frame + * @param[in] timer = absolute timeout time + * @return Workcounter if a frame is found with corresponding index, otherwise + * EC_NOFRAME. + */ +static int ecx_waitinframe_red(ecx_portt *port, uint8 idx, osal_timert *timer) +{ + osal_timert timer2; + int wkc = EC_NOFRAME; + int wkc2 = EC_NOFRAME; + int primrx, secrx; + + /* if not in redundant mode then always assume secondary is OK */ + if (port->redstate == ECT_RED_NONE) + { + wkc2 = 0; + } + do + { + /* only read frame if not already in */ + if (wkc <= EC_NOFRAME) + wkc = ecx_inframe(port, idx, 0); + /* only try secondary if in redundant mode */ + if (port->redstate != ECT_RED_NONE) + { + /* only read frame if not already in */ + if (wkc2 <= EC_NOFRAME) + { + wkc2 = ecx_inframe(port, idx, 1); + } + } + /* wait for both frames to arrive or timeout */ + } while (((wkc <= EC_NOFRAME) || (wkc2 <= EC_NOFRAME)) && !osal_timer_is_expired(timer)); + /* only do redundant functions when in redundant mode */ + if (port->redstate != ECT_RED_NONE) + { + /* primrx if the received MAC source on primary socket */ + primrx = 0; + if (wkc > EC_NOFRAME) primrx = port->rxsa[idx]; + /* secrx if the received MAC source on psecondary socket */ + secrx = 0; + if (wkc2 > EC_NOFRAME) secrx = port->redport->rxsa[idx]; + + /* primary socket got secondary frame and secondary socket got primary frame */ + /* normal situation in redundant mode */ + if ( ((primrx == RX_SEC) && (secrx == RX_PRIM)) ) + { + /* copy secondary buffer to primary */ + memcpy(&(port->rxbuf[idx]), &(port->redport->rxbuf[idx]), port->txbuflength[idx] - ETH_HEADERSIZE); + wkc = wkc2; + } + /* primary socket got nothing or primary frame, and secondary socket got secondary frame */ + /* we need to resend TX packet */ + if ( ((primrx == 0) && (secrx == RX_SEC)) || + ((primrx == RX_PRIM) && (secrx == RX_SEC)) ) + { + /* If both primary and secondary have partial connection retransmit the primary received + * frame over the secondary socket. The result from the secondary received frame is a combined + * frame that traversed all slaves in standard order. */ + if ( (primrx == RX_PRIM) && (secrx == RX_SEC) ) + { + /* copy primary rx to tx buffer */ + memcpy(&(port->txbuf[idx][ETH_HEADERSIZE]), &(port->rxbuf[idx]), port->txbuflength[idx] - ETH_HEADERSIZE); + } + osal_timer_start (&timer2, EC_TIMEOUTRET); + /* resend secondary tx */ + ecx_outframe(port,idx,1); + do + { + /* retrieve frame */ + wkc2 = ecx_inframe(port, idx, 1); + } while ((wkc2 <= EC_NOFRAME) && !osal_timer_is_expired(&timer2)); + if (wkc2 > EC_NOFRAME) + { + /* copy secondary result to primary rx buffer */ + memcpy(&(port->rxbuf[idx]), &(port->redport->rxbuf[idx]), port->txbuflength[idx] - ETH_HEADERSIZE); + wkc = wkc2; + } + } + } + + /* return WKC or EC_NOFRAME */ + return wkc; +} + +/** Blocking receive frame function. Calls ec_waitinframe_red(). + * @param[in] idx = requested index of frame + * @param[in] timeout = timeout in us + * @return Workcounter if a frame is found with corresponding index, otherwise + * EC_NOFRAME. + */ +int ecx_waitinframe(ecx_portt *port, uint8 idx, int timeout) +{ + int wkc; + osal_timert timer; + + if (timeout == 0 && (port->redstate == ECT_RED_NONE)) + { + int loop_cnt = 0; + /* Allow us to consume MAX buffer number of frames */ + do + { + wkc = ecx_inframe(port, idx, 0); + loop_cnt++; + } while (wkc <= EC_NOFRAME && (loop_cnt <= EC_MAXBUF)); + } + else + { + osal_timer_start (&timer, timeout); + wkc = ecx_waitinframe_red(port, idx, &timer); + } + + return wkc; +} + +/** Blocking send and receive frame function. Used for non processdata frames. + * A datagram is build into a frame and transmitted via this function. It waits + * for an answer and returns the workcounter. The function retries if time is + * left and the result is WKC=0 or no frame received. + * + * The function calls ec_outframe_red() and ec_waitinframe_red(). + * + * @param[in] idx = index of frame + * @param[in] timeout = timeout in us + * @return Workcounter or EC_NOFRAME + */ +int ecx_srconfirm(ecx_portt *port, uint8 idx, int timeout) +{ + int wkc = EC_NOFRAME; + osal_timert timer1; + + osal_timer_start (&timer1, timeout); + /* tx frame on primary and if in redundant mode a dummy on secondary */ + ecx_outframe_red(port, idx); + wkc = ecx_waitinframe_red(port, idx, &timer1); + + return wkc; +} + +#ifdef EC_VER1 + +int ec_setupnic(const char *ifname, int secondary) +{ + return ecx_setupnic(&ecx_port, ifname, secondary); +} + +int ec_closenic(void) +{ + return ecx_closenic(&ecx_port); +} + +uint8 ec_getindex(void) +{ + return ecx_getindex(&ecx_port); +} + +void ec_setbufstat(uint8 idx, int bufstat) +{ + ecx_setbufstat(&ecx_port, idx, bufstat); +} + +int ec_outframe(uint8 idx, int stacknumber) +{ + return ecx_outframe(&ecx_port, idx, stacknumber); +} + +int ec_outframe_red(uint8 idx) +{ + return ecx_outframe_red(&ecx_port, idx); +} + +int ec_inframe(uint8 idx, int stacknumber) +{ + return ecx_inframe(&ecx_port, idx, stacknumber); +} + +int ec_waitinframe(uint8 idx, int timeout) +{ + return ecx_waitinframe(&ecx_port, idx, timeout); +} + +int ec_srconfirm(uint8 idx, int timeout) +{ + return ecx_srconfirm(&ecx_port, idx, timeout); +} + +#endif diff --git a/soem-src/oshw/intime/nicdrv.h b/soem-src/oshw/intime/nicdrv.h new file mode 100644 index 0000000..8802d4e --- /dev/null +++ b/soem-src/oshw/intime/nicdrv.h @@ -0,0 +1,116 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +/** \file + * \brief + * Headerfile for nicdrv.c + */ + +#ifndef _nicdrvh_ +#define _nicdrvh_ + +#define HAVE_REMOTE + +#include +#include "hpeif2.h" + +/** pointer structure to Tx and Rx stacks */ +typedef struct +{ + /** socket connection used */ + int *sock; + /** tx buffer */ + ec_bufT (*txbuf)[EC_MAXBUF]; + /** tx buffer lengths */ + int (*txbuflength)[EC_MAXBUF]; + /** temporary receive buffer */ + ec_bufT *tempbuf; + /** rx buffers */ + ec_bufT (*rxbuf)[EC_MAXBUF]; + /** rx buffer status fields */ + int (*rxbufstat)[EC_MAXBUF]; + /** received MAC source address (middle word) */ + int (*rxsa)[EC_MAXBUF]; +} ec_stackT; + +typedef struct +{ + ec_stackT stack; + /** rx buffers */ + ec_bufT rxbuf[EC_MAXBUF]; + /** rx buffer status */ + int rxbufstat[EC_MAXBUF]; + /** rx MAC source address */ + int rxsa[EC_MAXBUF]; + /** temporary rx buffer */ + ec_bufT tempinbuf; + /* Intime */ + HPEHANDLE handle; + HPERXBUFFERSET *rx_buffers; + HPETXBUFFERSET *tx_buffers[EC_MAXBUF]; +} ecx_redportt; + +typedef struct +{ + ec_stackT stack; + /** rx buffers */ + ec_bufT rxbuf[EC_MAXBUF]; + /** rx buffer status */ + int rxbufstat[EC_MAXBUF]; + /** rx MAC source address */ + int rxsa[EC_MAXBUF]; + /** temporary rx buffer */ + ec_bufT tempinbuf; + /** temporary rx buffer status */ + int tempinbufs; + /** transmit buffers */ + ec_bufT txbuf[EC_MAXBUF]; + /** transmit buffer lengths */ + int txbuflength[EC_MAXBUF]; + /** temporary tx buffer */ + ec_bufT txbuf2; + /** temporary tx buffer length */ + int txbuflength2; + /** last used frame index */ + uint8 lastidx; + /** current redundancy state */ + int redstate; + /** pointer to redundancy port and buffers */ + ecx_redportt *redport; + RTHANDLE getindex_region; + RTHANDLE tx_region; + RTHANDLE rx_region; + /* Intime */ + HPEHANDLE handle; + HPERXBUFFERSET *rx_buffers; + HPETXBUFFERSET *tx_buffers[EC_MAXBUF]; +} ecx_portt; + +extern const uint16 priMAC[3]; +extern const uint16 secMAC[3]; + +//extern ecx_portt ecx_port; +//extern ecx_redportt ecx_redport; + +int ec_setupnic(const char * ifname, int secondary); +int ec_closenic(void); +void ec_setupheader(void *p); +void ec_setbufstat(uint8 idx, int bufstat); +uint8 ec_getindex(void); +int ec_outframe(uint8 idx, int sock); +int ec_outframe_red(uint8 idx); +int ec_waitinframe(uint8 idx, int timeout); +int ec_srconfirm(uint8 idx,int timeout); + +int ecx_setupnic(ecx_portt *port, const char * ifname, int secondary); +int ecx_closenic(ecx_portt *port); +void ecx_setbufstat(ecx_portt *port, uint8 idx, int bufstat); +uint8 ecx_getindex(ecx_portt *port); +int ecx_outframe(ecx_portt *port, uint8 idx, int sock); +int ecx_outframe_red(ecx_portt *port, uint8 idx); +int ecx_waitinframe(ecx_portt *port, uint8 idx, int timeout); +int ecx_srconfirm(ecx_portt *port, uint8 idx,int timeout); + +#endif diff --git a/soem-src/oshw/intime/oshw.c b/soem-src/oshw/intime/oshw.c new file mode 100644 index 0000000..d068050 --- /dev/null +++ b/soem-src/oshw/intime/oshw.c @@ -0,0 +1,48 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +#include +#include "oshw.h" + +/** + * Host to Network byte order (i.e. to big endian). + * + * Note that Ethercat uses little endian byte order, except for the Ethernet + * header which is big endian as usual. + */ +uint16 oshw_htons (uint16 host) +{ + uint16 network = htons (host); + return network; +} + +/** + * Network (i.e. big endian) to Host byte order. + * + * Note that Ethercat uses little endian byte order, except for the Ethernet + * header which is big endian as usual. + */ +uint16 oshw_ntohs (uint16 network) +{ + uint16 host = ntohs (network); + return host; +} + +/* Create list over available network adapters. + * @return First element in linked list of adapters + */ +ec_adaptert * oshw_find_adapters (void) +{ + return NULL; +} + +/** Free memory allocated memory used by adapter collection. + * @param[in] adapter = First element in linked list of adapters + * EC_NOFRAME. + */ +void oshw_free_adapters (ec_adaptert * adapter) +{ + +} diff --git a/soem-src/oshw/intime/oshw.h b/soem-src/oshw/intime/oshw.h new file mode 100644 index 0000000..7939153 --- /dev/null +++ b/soem-src/oshw/intime/oshw.h @@ -0,0 +1,31 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +/** \file + * \brief + * Headerfile for oshw.c + */ + +#ifndef _oshw_ +#define _oshw_ + +#include "ethercattype.h" +#include "ethercatmain.h" +#include "nicdrv.h" + +#ifdef __cplusplus +extern "C" { +#endif + +uint16 oshw_htons (uint16 hostshort); +uint16 oshw_ntohs (uint16 networkshort); +ec_adaptert * oshw_find_adapters (void); +void oshw_free_adapters (ec_adaptert * adapter); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/soem-src/oshw/linux/nicdrv.c b/soem-src/oshw/linux/nicdrv.c new file mode 100644 index 0000000..5498292 --- /dev/null +++ b/soem-src/oshw/linux/nicdrv.c @@ -0,0 +1,648 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +/** \file + * \brief + * EtherCAT RAW socket driver. + * + * Low level interface functions to send and receive EtherCAT packets. + * EtherCAT has the property that packets are only send by the master, + * and the send packets always return in the receive buffer. + * There can be multiple packets "on the wire" before they return. + * To combine the received packets with the original send packets a buffer + * system is installed. The identifier is put in the index item of the + * EtherCAT header. The index is stored and compared when a frame is received. + * If there is a match the packet can be combined with the transmit packet + * and returned to the higher level function. + * + * The socket layer can exhibit a reversal in the packet order (rare). + * If the Tx order is A-B-C the return order could be A-C-B. The indexed buffer + * will reorder the packets automatically. + * + * The "redundant" option will configure two sockets and two NIC interfaces. + * Slaves are connected to both interfaces, one on the IN port and one on the + * OUT port. Packets are send via both interfaces. Any one of the connections + * (also an interconnect) can be removed and the slaves are still serviced with + * packets. The software layer will detect the possible failure modes and + * compensate. If needed the packets from interface A are resent through interface B. + * This layer if fully transparent for the higher layers. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "oshw.h" +#include "osal.h" + +/** Redundancy modes */ +enum +{ + /** No redundancy, single NIC mode */ + ECT_RED_NONE, + /** Double redundant NIC connection */ + ECT_RED_DOUBLE +}; + + +/** Primary source MAC address used for EtherCAT. + * This address is not the MAC address used from the NIC. + * EtherCAT does not care about MAC addressing, but it is used here to + * differentiate the route the packet traverses through the EtherCAT + * segment. This is needed to find out the packet flow in redundant + * configurations. */ +const uint16 priMAC[3] = { 0x0101, 0x0101, 0x0101 }; +/** Secondary source MAC address used for EtherCAT. */ +const uint16 secMAC[3] = { 0x0404, 0x0404, 0x0404 }; + +/** second MAC word is used for identification */ +#define RX_PRIM priMAC[1] +/** second MAC word is used for identification */ +#define RX_SEC secMAC[1] + +static void ecx_clear_rxbufstat(int *rxbufstat) +{ + int i; + for(i = 0; i < EC_MAXBUF; i++) + { + rxbufstat[i] = EC_BUF_EMPTY; + } +} + +/** Basic setup to connect NIC to socket. + * @param[in] port = port context struct + * @param[in] ifname = Name of NIC device, f.e. "eth0" + * @param[in] secondary = if >0 then use secondary stack instead of primary + * @return >0 if succeeded + */ +int ecx_setupnic(ecx_portt *port, const char *ifname, int secondary) +{ + int i; + int r, rval, ifindex; + struct timeval timeout; + struct ifreq ifr; + struct sockaddr_ll sll; + int *psock; + pthread_mutexattr_t mutexattr; + + rval = 0; + if (secondary) + { + /* secondary port struct available? */ + if (port->redport) + { + /* when using secondary socket it is automatically a redundant setup */ + psock = &(port->redport->sockhandle); + *psock = -1; + port->redstate = ECT_RED_DOUBLE; + port->redport->stack.sock = &(port->redport->sockhandle); + port->redport->stack.txbuf = &(port->txbuf); + port->redport->stack.txbuflength = &(port->txbuflength); + port->redport->stack.tempbuf = &(port->redport->tempinbuf); + port->redport->stack.rxbuf = &(port->redport->rxbuf); + port->redport->stack.rxbufstat = &(port->redport->rxbufstat); + port->redport->stack.rxsa = &(port->redport->rxsa); + ecx_clear_rxbufstat(&(port->redport->rxbufstat[0])); + } + else + { + /* fail */ + return 0; + } + } + else + { + pthread_mutexattr_init(&mutexattr); + pthread_mutexattr_setprotocol(&mutexattr , PTHREAD_PRIO_INHERIT); + pthread_mutex_init(&(port->getindex_mutex), &mutexattr); + pthread_mutex_init(&(port->tx_mutex) , &mutexattr); + pthread_mutex_init(&(port->rx_mutex) , &mutexattr); + port->sockhandle = -1; + port->lastidx = 0; + port->redstate = ECT_RED_NONE; + port->stack.sock = &(port->sockhandle); + port->stack.txbuf = &(port->txbuf); + port->stack.txbuflength = &(port->txbuflength); + port->stack.tempbuf = &(port->tempinbuf); + port->stack.rxbuf = &(port->rxbuf); + port->stack.rxbufstat = &(port->rxbufstat); + port->stack.rxsa = &(port->rxsa); + ecx_clear_rxbufstat(&(port->rxbufstat[0])); + psock = &(port->sockhandle); + } + /* we use RAW packet socket, with packet type ETH_P_ECAT */ + *psock = socket(PF_PACKET, SOCK_RAW, htons(ETH_P_ECAT)); + if(*psock < 0) + return 0; + + timeout.tv_sec = 0; + timeout.tv_usec = 1; + r = 0; + r |= setsockopt(*psock, SOL_SOCKET, SO_RCVTIMEO, &timeout, sizeof(timeout)); + r |= setsockopt(*psock, SOL_SOCKET, SO_SNDTIMEO, &timeout, sizeof(timeout)); + i = 1; + r |= setsockopt(*psock, SOL_SOCKET, SO_DONTROUTE, &i, sizeof(i)); + /* connect socket to NIC by name */ + strcpy(ifr.ifr_name, ifname); + r |= ioctl(*psock, SIOCGIFINDEX, &ifr); + ifindex = ifr.ifr_ifindex; + strcpy(ifr.ifr_name, ifname); + ifr.ifr_flags = 0; + /* reset flags of NIC interface */ + r |= ioctl(*psock, SIOCGIFFLAGS, &ifr); + /* set flags of NIC interface, here promiscuous and broadcast */ + ifr.ifr_flags = ifr.ifr_flags | IFF_PROMISC | IFF_BROADCAST; + r |= ioctl(*psock, SIOCSIFFLAGS, &ifr); + /* bind socket to protocol, in this case RAW EtherCAT */ + sll.sll_family = AF_PACKET; + sll.sll_ifindex = ifindex; + sll.sll_protocol = htons(ETH_P_ECAT); + r |= bind(*psock, (struct sockaddr *)&sll, sizeof(sll)); + /* setup ethernet headers in tx buffers so we don't have to repeat it */ + for (i = 0; i < EC_MAXBUF; i++) + { + ec_setupheader(&(port->txbuf[i])); + port->rxbufstat[i] = EC_BUF_EMPTY; + } + ec_setupheader(&(port->txbuf2)); + if (r == 0) rval = 1; + + return rval; +} + +/** Close sockets used + * @param[in] port = port context struct + * @return 0 + */ +int ecx_closenic(ecx_portt *port) +{ + if (port->sockhandle >= 0) + close(port->sockhandle); + if ((port->redport) && (port->redport->sockhandle >= 0)) + close(port->redport->sockhandle); + + return 0; +} + +/** Fill buffer with ethernet header structure. + * Destination MAC is always broadcast. + * Ethertype is always ETH_P_ECAT. + * @param[out] p = buffer + */ +void ec_setupheader(void *p) +{ + ec_etherheadert *bp; + bp = p; + bp->da0 = htons(0xffff); + bp->da1 = htons(0xffff); + bp->da2 = htons(0xffff); + bp->sa0 = htons(priMAC[0]); + bp->sa1 = htons(priMAC[1]); + bp->sa2 = htons(priMAC[2]); + bp->etype = htons(ETH_P_ECAT); +} + +/** Get new frame identifier index and allocate corresponding rx buffer. + * @param[in] port = port context struct + * @return new index. + */ +uint8 ecx_getindex(ecx_portt *port) +{ + uint8 idx; + uint8 cnt; + + pthread_mutex_lock( &(port->getindex_mutex) ); + + idx = port->lastidx + 1; + /* index can't be larger than buffer array */ + if (idx >= EC_MAXBUF) + { + idx = 0; + } + cnt = 0; + /* try to find unused index */ + while ((port->rxbufstat[idx] != EC_BUF_EMPTY) && (cnt < EC_MAXBUF)) + { + idx++; + cnt++; + if (idx >= EC_MAXBUF) + { + idx = 0; + } + } + port->rxbufstat[idx] = EC_BUF_ALLOC; + if (port->redstate != ECT_RED_NONE) + port->redport->rxbufstat[idx] = EC_BUF_ALLOC; + port->lastidx = idx; + + pthread_mutex_unlock( &(port->getindex_mutex) ); + + return idx; +} + +/** Set rx buffer status. + * @param[in] port = port context struct + * @param[in] idx = index in buffer array + * @param[in] bufstat = status to set + */ +void ecx_setbufstat(ecx_portt *port, uint8 idx, int bufstat) +{ + port->rxbufstat[idx] = bufstat; + if (port->redstate != ECT_RED_NONE) + port->redport->rxbufstat[idx] = bufstat; +} + +/** Transmit buffer over socket (non blocking). + * @param[in] port = port context struct + * @param[in] idx = index in tx buffer array + * @param[in] stacknumber = 0=Primary 1=Secondary stack + * @return socket send result + */ +int ecx_outframe(ecx_portt *port, uint8 idx, int stacknumber) +{ + int lp, rval; + ec_stackT *stack; + + if (!stacknumber) + { + stack = &(port->stack); + } + else + { + stack = &(port->redport->stack); + } + lp = (*stack->txbuflength)[idx]; + (*stack->rxbufstat)[idx] = EC_BUF_TX; + rval = send(*stack->sock, (*stack->txbuf)[idx], lp, 0); + if (rval == -1) + { + (*stack->rxbufstat)[idx] = EC_BUF_EMPTY; + } + + return rval; +} + +/** Transmit buffer over socket (non blocking). + * @param[in] port = port context struct + * @param[in] idx = index in tx buffer array + * @return socket send result + */ +int ecx_outframe_red(ecx_portt *port, uint8 idx) +{ + ec_comt *datagramP; + ec_etherheadert *ehp; + int rval; + + ehp = (ec_etherheadert *)&(port->txbuf[idx]); + /* rewrite MAC source address 1 to primary */ + ehp->sa1 = htons(priMAC[1]); + /* transmit over primary socket*/ + rval = ecx_outframe(port, idx, 0); + if (port->redstate != ECT_RED_NONE) + { + pthread_mutex_lock( &(port->tx_mutex) ); + ehp = (ec_etherheadert *)&(port->txbuf2); + /* use dummy frame for secondary socket transmit (BRD) */ + datagramP = (ec_comt*)&(port->txbuf2[ETH_HEADERSIZE]); + /* write index to frame */ + datagramP->index = idx; + /* rewrite MAC source address 1 to secondary */ + ehp->sa1 = htons(secMAC[1]); + /* transmit over secondary socket */ + port->redport->rxbufstat[idx] = EC_BUF_TX; + if (send(port->redport->sockhandle, &(port->txbuf2), port->txbuflength2 , 0) == -1) + { + port->redport->rxbufstat[idx] = EC_BUF_EMPTY; + } + pthread_mutex_unlock( &(port->tx_mutex) ); + } + + return rval; +} + +/** Non blocking read of socket. Put frame in temporary buffer. + * @param[in] port = port context struct + * @param[in] stacknumber = 0=primary 1=secondary stack + * @return >0 if frame is available and read + */ +static int ecx_recvpkt(ecx_portt *port, int stacknumber) +{ + int lp, bytesrx; + ec_stackT *stack; + + if (!stacknumber) + { + stack = &(port->stack); + } + else + { + stack = &(port->redport->stack); + } + lp = sizeof(port->tempinbuf); + bytesrx = recv(*stack->sock, (*stack->tempbuf), lp, 0); + port->tempinbufs = bytesrx; + + return (bytesrx > 0); +} + +/** Non blocking receive frame function. Uses RX buffer and index to combine + * read frame with transmitted frame. To compensate for received frames that + * are out-of-order all frames are stored in their respective indexed buffer. + * If a frame was placed in the buffer previously, the function retrieves it + * from that buffer index without calling ec_recvpkt. If the requested index + * is not already in the buffer it calls ec_recvpkt to fetch it. There are + * three options now, 1 no frame read, so exit. 2 frame read but other + * than requested index, store in buffer and exit. 3 frame read with matching + * index, store in buffer, set completed flag in buffer status and exit. + * + * @param[in] port = port context struct + * @param[in] idx = requested index of frame + * @param[in] stacknumber = 0=primary 1=secondary stack + * @return Workcounter if a frame is found with corresponding index, otherwise + * EC_NOFRAME or EC_OTHERFRAME. + */ +int ecx_inframe(ecx_portt *port, uint8 idx, int stacknumber) +{ + uint16 l; + int rval; + uint8 idxf; + ec_etherheadert *ehp; + ec_comt *ecp; + ec_stackT *stack; + ec_bufT *rxbuf; + + if (!stacknumber) + { + stack = &(port->stack); + } + else + { + stack = &(port->redport->stack); + } + rval = EC_NOFRAME; + rxbuf = &(*stack->rxbuf)[idx]; + /* check if requested index is already in buffer ? */ + if ((idx < EC_MAXBUF) && ((*stack->rxbufstat)[idx] == EC_BUF_RCVD)) + { + l = (*rxbuf)[0] + ((uint16)((*rxbuf)[1] & 0x0f) << 8); + /* return WKC */ + rval = ((*rxbuf)[l] + ((uint16)(*rxbuf)[l + 1] << 8)); + /* mark as completed */ + (*stack->rxbufstat)[idx] = EC_BUF_COMPLETE; + } + else + { + pthread_mutex_lock(&(port->rx_mutex)); + /* non blocking call to retrieve frame from socket */ + if (ecx_recvpkt(port, stacknumber)) + { + rval = EC_OTHERFRAME; + ehp =(ec_etherheadert*)(stack->tempbuf); + /* check if it is an EtherCAT frame */ + if (ehp->etype == htons(ETH_P_ECAT)) + { + ecp =(ec_comt*)(&(*stack->tempbuf)[ETH_HEADERSIZE]); + l = etohs(ecp->elength) & 0x0fff; + idxf = ecp->index; + /* found index equals requested index ? */ + if (idxf == idx) + { + /* yes, put it in the buffer array (strip ethernet header) */ + memcpy(rxbuf, &(*stack->tempbuf)[ETH_HEADERSIZE], (*stack->txbuflength)[idx] - ETH_HEADERSIZE); + /* return WKC */ + rval = ((*rxbuf)[l] + ((uint16)((*rxbuf)[l + 1]) << 8)); + /* mark as completed */ + (*stack->rxbufstat)[idx] = EC_BUF_COMPLETE; + /* store MAC source word 1 for redundant routing info */ + (*stack->rxsa)[idx] = ntohs(ehp->sa1); + } + else + { + /* check if index exist and someone is waiting for it */ + if (idxf < EC_MAXBUF && (*stack->rxbufstat)[idxf] == EC_BUF_TX) + { + rxbuf = &(*stack->rxbuf)[idxf]; + /* put it in the buffer array (strip ethernet header) */ + memcpy(rxbuf, &(*stack->tempbuf)[ETH_HEADERSIZE], (*stack->txbuflength)[idxf] - ETH_HEADERSIZE); + /* mark as received */ + (*stack->rxbufstat)[idxf] = EC_BUF_RCVD; + (*stack->rxsa)[idxf] = ntohs(ehp->sa1); + } + else + { + /* strange things happened */ + } + } + } + } + pthread_mutex_unlock( &(port->rx_mutex) ); + + } + + /* WKC if matching frame found */ + return rval; +} + +/** Blocking redundant receive frame function. If redundant mode is not active then + * it skips the secondary stack and redundancy functions. In redundant mode it waits + * for both (primary and secondary) frames to come in. The result goes in an decision + * tree that decides, depending on the route of the packet and its possible missing arrival, + * how to reroute the original packet to get the data in an other try. + * + * @param[in] port = port context struct + * @param[in] idx = requested index of frame + * @param[in] timer = absolute timeout time + * @return Workcounter if a frame is found with corresponding index, otherwise + * EC_NOFRAME. + */ +static int ecx_waitinframe_red(ecx_portt *port, uint8 idx, osal_timert *timer) +{ + osal_timert timer2; + int wkc = EC_NOFRAME; + int wkc2 = EC_NOFRAME; + int primrx, secrx; + + /* if not in redundant mode then always assume secondary is OK */ + if (port->redstate == ECT_RED_NONE) + wkc2 = 0; + do + { + /* only read frame if not already in */ + if (wkc <= EC_NOFRAME) + wkc = ecx_inframe(port, idx, 0); + /* only try secondary if in redundant mode */ + if (port->redstate != ECT_RED_NONE) + { + /* only read frame if not already in */ + if (wkc2 <= EC_NOFRAME) + wkc2 = ecx_inframe(port, idx, 1); + } + /* wait for both frames to arrive or timeout */ + } while (((wkc <= EC_NOFRAME) || (wkc2 <= EC_NOFRAME)) && !osal_timer_is_expired(timer)); + /* only do redundant functions when in redundant mode */ + if (port->redstate != ECT_RED_NONE) + { + /* primrx if the received MAC source on primary socket */ + primrx = 0; + if (wkc > EC_NOFRAME) primrx = port->rxsa[idx]; + /* secrx if the received MAC source on psecondary socket */ + secrx = 0; + if (wkc2 > EC_NOFRAME) secrx = port->redport->rxsa[idx]; + + /* primary socket got secondary frame and secondary socket got primary frame */ + /* normal situation in redundant mode */ + if ( ((primrx == RX_SEC) && (secrx == RX_PRIM)) ) + { + /* copy secondary buffer to primary */ + memcpy(&(port->rxbuf[idx]), &(port->redport->rxbuf[idx]), port->txbuflength[idx] - ETH_HEADERSIZE); + wkc = wkc2; + } + /* primary socket got nothing or primary frame, and secondary socket got secondary frame */ + /* we need to resend TX packet */ + if ( ((primrx == 0) && (secrx == RX_SEC)) || + ((primrx == RX_PRIM) && (secrx == RX_SEC)) ) + { + /* If both primary and secondary have partial connection retransmit the primary received + * frame over the secondary socket. The result from the secondary received frame is a combined + * frame that traversed all slaves in standard order. */ + if ( (primrx == RX_PRIM) && (secrx == RX_SEC) ) + { + /* copy primary rx to tx buffer */ + memcpy(&(port->txbuf[idx][ETH_HEADERSIZE]), &(port->rxbuf[idx]), port->txbuflength[idx] - ETH_HEADERSIZE); + } + osal_timer_start (&timer2, EC_TIMEOUTRET); + /* resend secondary tx */ + ecx_outframe(port, idx, 1); + do + { + /* retrieve frame */ + wkc2 = ecx_inframe(port, idx, 1); + } while ((wkc2 <= EC_NOFRAME) && !osal_timer_is_expired(&timer2)); + if (wkc2 > EC_NOFRAME) + { + /* copy secondary result to primary rx buffer */ + memcpy(&(port->rxbuf[idx]), &(port->redport->rxbuf[idx]), port->txbuflength[idx] - ETH_HEADERSIZE); + wkc = wkc2; + } + } + } + + /* return WKC or EC_NOFRAME */ + return wkc; +} + +/** Blocking receive frame function. Calls ec_waitinframe_red(). + * @param[in] port = port context struct + * @param[in] idx = requested index of frame + * @param[in] timeout = timeout in us + * @return Workcounter if a frame is found with corresponding index, otherwise + * EC_NOFRAME. + */ +int ecx_waitinframe(ecx_portt *port, uint8 idx, int timeout) +{ + int wkc; + osal_timert timer; + + osal_timer_start (&timer, timeout); + wkc = ecx_waitinframe_red(port, idx, &timer); + + return wkc; +} + +/** Blocking send and receive frame function. Used for non processdata frames. + * A datagram is build into a frame and transmitted via this function. It waits + * for an answer and returns the workcounter. The function retries if time is + * left and the result is WKC=0 or no frame received. + * + * The function calls ec_outframe_red() and ec_waitinframe_red(). + * + * @param[in] port = port context struct + * @param[in] idx = index of frame + * @param[in] timeout = timeout in us + * @return Workcounter or EC_NOFRAME + */ +int ecx_srconfirm(ecx_portt *port, uint8 idx, int timeout) +{ + int wkc = EC_NOFRAME; + osal_timert timer1, timer2; + + osal_timer_start (&timer1, timeout); + do + { + /* tx frame on primary and if in redundant mode a dummy on secondary */ + ecx_outframe_red(port, idx); + if (timeout < EC_TIMEOUTRET) + { + osal_timer_start (&timer2, timeout); + } + else + { + /* normally use partial timeout for rx */ + osal_timer_start (&timer2, EC_TIMEOUTRET); + } + /* get frame from primary or if in redundant mode possibly from secondary */ + wkc = ecx_waitinframe_red(port, idx, &timer2); + /* wait for answer with WKC>=0 or otherwise retry until timeout */ + } while ((wkc <= EC_NOFRAME) && !osal_timer_is_expired (&timer1)); + + return wkc; +} + +#ifdef EC_VER1 +int ec_setupnic(const char *ifname, int secondary) +{ + return ecx_setupnic(&ecx_port, ifname, secondary); +} + +int ec_closenic(void) +{ + return ecx_closenic(&ecx_port); +} + +uint8 ec_getindex(void) +{ + return ecx_getindex(&ecx_port); +} + +void ec_setbufstat(uint8 idx, int bufstat) +{ + ecx_setbufstat(&ecx_port, idx, bufstat); +} + +int ec_outframe(uint8 idx, int stacknumber) +{ + return ecx_outframe(&ecx_port, idx, stacknumber); +} + +int ec_outframe_red(uint8 idx) +{ + return ecx_outframe_red(&ecx_port, idx); +} + +int ec_inframe(uint8 idx, int stacknumber) +{ + return ecx_inframe(&ecx_port, idx, stacknumber); +} + +int ec_waitinframe(uint8 idx, int timeout) +{ + return ecx_waitinframe(&ecx_port, idx, timeout); +} + +int ec_srconfirm(uint8 idx, int timeout) +{ + return ecx_srconfirm(&ecx_port, idx, timeout); +} +#endif diff --git a/soem-src/oshw/linux/nicdrv.h b/soem-src/oshw/linux/nicdrv.h new file mode 100644 index 0000000..c2dd209 --- /dev/null +++ b/soem-src/oshw/linux/nicdrv.h @@ -0,0 +1,120 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +/** \file + * \brief + * Headerfile for nicdrv.c + */ + +#ifndef _nicdrvh_ +#define _nicdrvh_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include + +/** pointer structure to Tx and Rx stacks */ +typedef struct +{ + /** socket connection used */ + int *sock; + /** tx buffer */ + ec_bufT (*txbuf)[EC_MAXBUF]; + /** tx buffer lengths */ + int (*txbuflength)[EC_MAXBUF]; + /** temporary receive buffer */ + ec_bufT *tempbuf; + /** rx buffers */ + ec_bufT (*rxbuf)[EC_MAXBUF]; + /** rx buffer status fields */ + int (*rxbufstat)[EC_MAXBUF]; + /** received MAC source address (middle word) */ + int (*rxsa)[EC_MAXBUF]; +} ec_stackT; + +/** pointer structure to buffers for redundant port */ +typedef struct +{ + ec_stackT stack; + int sockhandle; + /** rx buffers */ + ec_bufT rxbuf[EC_MAXBUF]; + /** rx buffer status */ + int rxbufstat[EC_MAXBUF]; + /** rx MAC source address */ + int rxsa[EC_MAXBUF]; + /** temporary rx buffer */ + ec_bufT tempinbuf; +} ecx_redportt; + +/** pointer structure to buffers, vars and mutexes for port instantiation */ +typedef struct +{ + ec_stackT stack; + int sockhandle; + /** rx buffers */ + ec_bufT rxbuf[EC_MAXBUF]; + /** rx buffer status */ + int rxbufstat[EC_MAXBUF]; + /** rx MAC source address */ + int rxsa[EC_MAXBUF]; + /** temporary rx buffer */ + ec_bufT tempinbuf; + /** temporary rx buffer status */ + int tempinbufs; + /** transmit buffers */ + ec_bufT txbuf[EC_MAXBUF]; + /** transmit buffer lengths */ + int txbuflength[EC_MAXBUF]; + /** temporary tx buffer */ + ec_bufT txbuf2; + /** temporary tx buffer length */ + int txbuflength2; + /** last used frame index */ + uint8 lastidx; + /** current redundancy state */ + int redstate; + /** pointer to redundancy port and buffers */ + ecx_redportt *redport; + pthread_mutex_t getindex_mutex; + pthread_mutex_t tx_mutex; + pthread_mutex_t rx_mutex; +} ecx_portt; + +extern const uint16 priMAC[3]; +extern const uint16 secMAC[3]; + +#ifdef EC_VER1 +extern ecx_portt ecx_port; +extern ecx_redportt ecx_redport; + +int ec_setupnic(const char * ifname, int secondary); +int ec_closenic(void); +void ec_setbufstat(uint8 idx, int bufstat); +uint8 ec_getindex(void); +int ec_outframe(uint8 idx, int sock); +int ec_outframe_red(uint8 idx); +int ec_waitinframe(uint8 idx, int timeout); +int ec_srconfirm(uint8 idx,int timeout); +#endif + +void ec_setupheader(void *p); +int ecx_setupnic(ecx_portt *port, const char * ifname, int secondary); +int ecx_closenic(ecx_portt *port); +void ecx_setbufstat(ecx_portt *port, uint8 idx, int bufstat); +uint8 ecx_getindex(ecx_portt *port); +int ecx_outframe(ecx_portt *port, uint8 idx, int sock); +int ecx_outframe_red(ecx_portt *port, uint8 idx); +int ecx_waitinframe(ecx_portt *port, uint8 idx, int timeout); +int ecx_srconfirm(ecx_portt *port, uint8 idx,int timeout); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/soem-src/oshw/linux/oshw.c b/soem-src/oshw/linux/oshw.c new file mode 100644 index 0000000..d3cb5b8 --- /dev/null +++ b/soem-src/oshw/linux/oshw.c @@ -0,0 +1,117 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +#include +#include +#include +#include +#include +#include +#include +#include "oshw.h" + +/** + * Host to Network byte order (i.e. to big endian). + * + * Note that Ethercat uses little endian byte order, except for the Ethernet + * header which is big endian as usual. + */ +uint16 oshw_htons(uint16 host) +{ + uint16 network = htons (host); + return network; +} + +/** + * Network (i.e. big endian) to Host byte order. + * + * Note that Ethercat uses little endian byte order, except for the Ethernet + * header which is big endian as usual. + */ +uint16 oshw_ntohs(uint16 network) +{ + uint16 host = ntohs (network); + return host; +} + +/** Create list over available network adapters. + * @return First element in linked list of adapters + */ +ec_adaptert * oshw_find_adapters(void) +{ + int i; + struct if_nameindex *ids; + ec_adaptert * adapter; + ec_adaptert * prev_adapter = NULL; + ec_adaptert * ret_adapter = NULL; + + + /* Iterate all devices and create a local copy holding the name and + * description. + */ + + ids = if_nameindex (); + for(i = 0; ids[i].if_index != 0; i++) + { + adapter = (ec_adaptert *)malloc(sizeof(ec_adaptert)); + /* If we got more than one adapter save link list pointer to previous + * adapter. + * Else save as pointer to return. + */ + if (prev_adapter) + { + prev_adapter->next = adapter; + } + else + { + ret_adapter = adapter; + } + + /* fetch description and name, in Linux we use the same on both */ + adapter->next = NULL; + + if (ids[i].if_name) + { + strncpy(adapter->name, ids[i].if_name, EC_MAXLEN_ADAPTERNAME); + adapter->name[EC_MAXLEN_ADAPTERNAME-1] = '\0'; + strncpy(adapter->desc, ids[i].if_name, EC_MAXLEN_ADAPTERNAME); + adapter->desc[EC_MAXLEN_ADAPTERNAME-1] = '\0'; + } + else + { + adapter->name[0] = '\0'; + adapter->desc[0] = '\0'; + } + + prev_adapter = adapter; + } + + if_freenameindex (ids); + + return ret_adapter; +} + +/** Free memory allocated memory used by adapter collection. + * @param[in] adapter = First element in linked list of adapters + * EC_NOFRAME. + */ +void oshw_free_adapters(ec_adaptert * adapter) +{ + ec_adaptert * next_adapter; + /* Iterate the linked list and free all elements holding + * adapter information + */ + if(adapter) + { + next_adapter = adapter->next; + free (adapter); + while (next_adapter) + { + adapter = next_adapter; + next_adapter = adapter->next; + free (adapter); + } + } +} diff --git a/soem-src/oshw/linux/oshw.h b/soem-src/oshw/linux/oshw.h new file mode 100644 index 0000000..78d131b --- /dev/null +++ b/soem-src/oshw/linux/oshw.h @@ -0,0 +1,31 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +/** \file + * \brief + * Headerfile for ethercatbase.c + */ + +#ifndef _oshw_ +#define _oshw_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "ethercattype.h" +#include "nicdrv.h" +#include "ethercatmain.h" + +uint16 oshw_htons(uint16 hostshort); +uint16 oshw_ntohs(uint16 networkshort); +ec_adaptert * oshw_find_adapters(void); +void oshw_free_adapters(ec_adaptert * adapter); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/soem-src/oshw/macosx/nicdrv.c b/soem-src/oshw/macosx/nicdrv.c new file mode 100644 index 0000000..e202bb0 --- /dev/null +++ b/soem-src/oshw/macosx/nicdrv.c @@ -0,0 +1,649 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +/** \file + * \brief + * EtherCAT RAW socket driver. + * + * Low level interface functions to send and receive EtherCAT packets. + * EtherCAT has the property that packets are only send by the master, + * and the send packets always return in the receive buffer. + * There can be multiple packets "on the wire" before they return. + * To combine the received packets with the original send packets a buffer + * system is installed. The identifier is put in the index item of the + * EtherCAT header. The index is stored and compared when a frame is received. + * If there is a match the packet can be combined with the transmit packet + * and returned to the higher level function. + * + * The socket layer can exhibit a reversal in the packet order (rare). + * If the Tx order is A-B-C the return order could be A-C-B. The indexed buffer + * will reorder the packets automatically. + * + * The "redundant" option will configure two sockets and two NIC interfaces. + * Slaves are connected to both interfaces, one on the IN port and one on the + * OUT port. Packets are send via both interfaces. Any one of the connections + * (also an interconnect) can be removed and the slaves are still serviced with + * packets. The software layer will detect the possible failure modes and + * compensate. If needed the packets from interface A are resent through interface B. + * This layer is fully transparent for the higher layers. + */ + + +#include +#include +#include +#include +#include + +#include "ethercattype.h" +#include "nicdrv.h" +#include "osal.h" + +/** Redundancy modes */ +enum +{ + /** No redundancy, single NIC mode */ + ECT_RED_NONE, + /** Double redundant NIC connection */ + ECT_RED_DOUBLE +}; + +/** Primary source MAC address used for EtherCAT. + * This address is not the MAC address used from the NIC. + * EtherCAT does not care about MAC addressing, but it is used here to + * differentiate the route the packet traverses through the EtherCAT + * segment. This is needed to fund out the packet flow in redundant + * configurations. */ +const uint16 priMAC[3] = { 0x0101, 0x0101, 0x0101 }; +/** Secondary source MAC address used for EtherCAT. */ +const uint16 secMAC[3] = { 0x0404, 0x0404, 0x0404 }; + +/** second MAC word is used for identification */ +#define RX_PRIM priMAC[1] +/** second MAC word is used for identification */ +#define RX_SEC secMAC[1] + +static char errbuf[PCAP_ERRBUF_SIZE]; + +static void ecx_clear_rxbufstat(int *rxbufstat) +{ + int i; + for(i = 0; i < EC_MAXBUF; i++) + { + rxbufstat[i] = EC_BUF_EMPTY; + } +} + +/** Basic setup to connect NIC to socket. + * @param[in] port = port context struct + * @param[in] ifname = Name of NIC device, f.e. "eth0" + * @param[in] secondary = if >0 then use secondary stack instead of primary + * @return >0 if succeeded + */ +int ecx_setupnic(ecx_portt *port, const char *ifname, int secondary) +{ + int i, rval; + pcap_t **psock; + + rval = 0; + if (secondary) + { + /* secondary port struct available? */ + if (port->redport) + { + /* when using secondary socket it is automatically a redundant setup */ + psock = &(port->redport->sockhandle); + *psock = NULL; + port->redstate = ECT_RED_DOUBLE; + port->redport->stack.sock = &(port->redport->sockhandle); + port->redport->stack.txbuf = &(port->txbuf); + port->redport->stack.txbuflength = &(port->txbuflength); + port->redport->stack.tempbuf = &(port->redport->tempinbuf); + port->redport->stack.rxbuf = &(port->redport->rxbuf); + port->redport->stack.rxbufstat = &(port->redport->rxbufstat); + port->redport->stack.rxsa = &(port->redport->rxsa); + ecx_clear_rxbufstat(&(port->redport->rxbufstat[0])); + } + else + { + /* fail */ + return 0; + } + } + else + { + pthread_mutex_init(&(port->getindex_mutex), NULL); + pthread_mutex_init(&(port->tx_mutex), NULL); + pthread_mutex_init(&(port->rx_mutex), NULL); + port->sockhandle = NULL; + port->lastidx = 0; + port->redstate = ECT_RED_NONE; + port->stack.sock = &(port->sockhandle); + port->stack.txbuf = &(port->txbuf); + port->stack.txbuflength = &(port->txbuflength); + port->stack.tempbuf = &(port->tempinbuf); + port->stack.rxbuf = &(port->rxbuf); + port->stack.rxbufstat = &(port->rxbufstat); + port->stack.rxsa = &(port->rxsa); + ecx_clear_rxbufstat(&(port->rxbufstat[0])); + psock = &(port->sockhandle); + } + *psock = pcap_open_live(ifname, 65536, 1, 0, errbuf); + if (NULL == *psock) + { + return 0; + } + rval = pcap_setdirection(*psock, PCAP_D_IN); + if (rval != 0) + { + return 0; + } + rval = pcap_setnonblock(*psock, 1, errbuf); + if (rval != 0) + { + return 0; + } + + for (i = 0; i < EC_MAXBUF; i++) + { + ec_setupheader(&(port->txbuf[i])); + port->rxbufstat[i] = EC_BUF_EMPTY; + } + ec_setupheader(&(port->txbuf2)); + + return 1; +} + +/** Close sockets used + * @param[in] port = port context struct + * @return 0 + */ +int ecx_closenic(ecx_portt *port) +{ + if (port->sockhandle != NULL) + { + pthread_mutex_destroy(&(port->getindex_mutex)); + pthread_mutex_destroy(&(port->tx_mutex)); + pthread_mutex_destroy(&(port->rx_mutex)); + pcap_close(port->sockhandle); + port->sockhandle = NULL; + } + if ((port->redport) && (port->redport->sockhandle != NULL)) + { + pcap_close(port->redport->sockhandle); + port->redport->sockhandle = NULL; + } + + return 0; +} + +/** Fill buffer with ethernet header structure. + * Destination MAC is always broadcast. + * Ethertype is always ETH_P_ECAT. + * @param[out] p = buffer + */ +void ec_setupheader(void *p) +{ + ec_etherheadert *bp; + bp = p; + bp->da0 = htons(0xffff); + bp->da1 = htons(0xffff); + bp->da2 = htons(0xffff); + bp->sa0 = htons(priMAC[0]); + bp->sa1 = htons(priMAC[1]); + bp->sa2 = htons(priMAC[2]); + bp->etype = htons(ETH_P_ECAT); +} + +/** Get new frame identifier index and allocate corresponding rx buffer. + * @param[in] port = port context struct + * @return new index. + */ +uint8 ecx_getindex(ecx_portt *port) +{ + uint8 idx; + uint8 cnt; + + pthread_mutex_lock(&(port->getindex_mutex)); + + idx = port->lastidx + 1; + /* index can't be larger than buffer array */ + if (idx >= EC_MAXBUF) + { + idx = 0; + } + cnt = 0; + /* try to find unused index */ + while ((port->rxbufstat[idx] != EC_BUF_EMPTY) && (cnt < EC_MAXBUF)) + { + idx++; + cnt++; + if (idx >= EC_MAXBUF) + { + idx = 0; + } + } + port->rxbufstat[idx] = EC_BUF_ALLOC; + if (port->redstate != ECT_RED_NONE) + port->redport->rxbufstat[idx] = EC_BUF_ALLOC; + port->lastidx = idx; + + pthread_mutex_unlock(&(port->getindex_mutex)); + + return idx; +} + +/** Set rx buffer status. + * @param[in] port = port context struct + * @param[in] idx = index in buffer array + * @param[in] bufstat = status to set + */ +void ecx_setbufstat(ecx_portt *port, uint8 idx, int bufstat) +{ + port->rxbufstat[idx] = bufstat; + if (port->redstate != ECT_RED_NONE) + port->redport->rxbufstat[idx] = bufstat; +} + +/** Transmit buffer over socket (non blocking). + * @param[in] port = port context struct + * @param[in] idx = index in tx buffer array + * @param[in] stacknumber = 0=Primary 1=Secondary stack + * @return socket send result + */ +int ecx_outframe(ecx_portt *port, uint8 idx, int stacknumber) +{ + int lp, rval; + ec_stackT *stack; + + if (!stacknumber) + { + stack = &(port->stack); + } + else + { + stack = &(port->redport->stack); + } + lp = (*stack->txbuflength)[idx]; + (*stack->rxbufstat)[idx] = EC_BUF_TX; + rval = pcap_sendpacket(*stack->sock, (*stack->txbuf)[idx], lp); + if (rval == PCAP_ERROR) + { + (*stack->rxbufstat)[idx] = EC_BUF_EMPTY; + } + + return rval; +} + +/** Transmit buffer over socket (non blocking). + * @param[in] port = port context struct + * @param[in] idx = index in tx buffer array + * @return socket send result + */ +int ecx_outframe_red(ecx_portt *port, uint8 idx) +{ + ec_comt *datagramP; + ec_etherheadert *ehp; + int rval; + + ehp = (ec_etherheadert *)&(port->txbuf[idx]); + /* rewrite MAC source address 1 to primary */ + ehp->sa1 = htons(priMAC[1]); + /* transmit over primary socket*/ + rval = ecx_outframe(port, idx, 0); + if (port->redstate != ECT_RED_NONE) + { + pthread_mutex_lock( &(port->tx_mutex) ); + ehp = (ec_etherheadert *)&(port->txbuf2); + /* use dummy frame for secondary socket transmit (BRD) */ + datagramP = (ec_comt*)&(port->txbuf2[ETH_HEADERSIZE]); + /* write index to frame */ + datagramP->index = idx; + /* rewrite MAC source address 1 to secondary */ + ehp->sa1 = htons(secMAC[1]); + /* transmit over secondary socket */ + port->redport->rxbufstat[idx] = EC_BUF_TX; + if (pcap_sendpacket(port->redport->sockhandle, (u_char const *)&(port->txbuf2), port->txbuflength2) == PCAP_ERROR) + { + port->redport->rxbufstat[idx] = EC_BUF_EMPTY; + } + pthread_mutex_unlock( &(port->tx_mutex) ); + } + + return rval; +} + +/** Non blocking read of socket. Put frame in temporary buffer. + * @param[in] port = port context struct + * @param[in] stacknumber = 0=primary 1=secondary stack + * @return >0 if frame is available and read + */ +static int ecx_recvpkt(ecx_portt *port, int stacknumber) +{ + int lp, bytesrx; + ec_stackT *stack; + struct pcap_pkthdr * header; + unsigned char const * pkt_data; + int res; + + if (!stacknumber) + { + stack = &(port->stack); + } + else + { + stack = &(port->redport->stack); + } + lp = sizeof(port->tempinbuf); + + res = pcap_next_ex(*stack->sock, &header, &pkt_data); + if (res <=0 ) + { + port->tempinbufs = 0; + return 0; + } + + bytesrx = header->len; + if (bytesrx > lp) + { + bytesrx = lp; + } + memcpy(*stack->tempbuf, pkt_data, bytesrx); + port->tempinbufs = bytesrx; + return (bytesrx > 0); +} + +/** Non blocking receive frame function. Uses RX buffer and index to combine + * read frame with transmitted frame. To compensate for received frames that + * are out-of-order all frames are stored in their respective indexed buffer. + * If a frame was placed in the buffer previously, the function retrieves it + * from that buffer index without calling ec_recvpkt. If the requested index + * is not already in the buffer it calls ec_recvpkt to fetch it. There are + * three options now, 1 no frame read, so exit. 2 frame read but other + * than requested index, store in buffer and exit. 3 frame read with matching + * index, store in buffer, set completed flag in buffer status and exit. + * + * @param[in] port = port context struct + * @param[in] idx = requested index of frame + * @param[in] stacknumber = 0=primary 1=secondary stack + * @return Workcounter if a frame is found with corresponding index, otherwise + * EC_NOFRAME or EC_OTHERFRAME. + */ +int ecx_inframe(ecx_portt *port, uint8 idx, int stacknumber) +{ + uint16 l; + int rval; + uint8 idxf; + ec_etherheadert *ehp; + ec_comt *ecp; + ec_stackT *stack; + ec_bufT *rxbuf; + + if (!stacknumber) + { + stack = &(port->stack); + } + else + { + stack = &(port->redport->stack); + } + rval = EC_NOFRAME; + rxbuf = &(*stack->rxbuf)[idx]; + /* check if requested index is already in buffer ? */ + if ((idx < EC_MAXBUF) && ((*stack->rxbufstat)[idx] == EC_BUF_RCVD)) + { + l = (*rxbuf)[0] + ((uint16)((*rxbuf)[1] & 0x0f) << 8); + /* return WKC */ + rval = ((*rxbuf)[l] + ((uint16)(*rxbuf)[l + 1] << 8)); + /* mark as completed */ + (*stack->rxbufstat)[idx] = EC_BUF_COMPLETE; + } + else + { + pthread_mutex_lock(&(port->rx_mutex)); + /* non blocking call to retrieve frame from socket */ + if (ecx_recvpkt(port, stacknumber)) + { + rval = EC_OTHERFRAME; + ehp =(ec_etherheadert*)(stack->tempbuf); + /* check if it is an EtherCAT frame */ + if (ehp->etype == htons(ETH_P_ECAT)) + { + ecp =(ec_comt*)(&(*stack->tempbuf)[ETH_HEADERSIZE]); + l = etohs(ecp->elength) & 0x0fff; + idxf = ecp->index; + /* found index equals requested index ? */ + if (idxf == idx) + { + /* yes, put it in the buffer array (strip ethernet header) */ + memcpy(rxbuf, &(*stack->tempbuf)[ETH_HEADERSIZE], (*stack->txbuflength)[idx] - ETH_HEADERSIZE); + /* return WKC */ + rval = ((*rxbuf)[l] + ((uint16)((*rxbuf)[l + 1]) << 8)); + /* mark as completed */ + (*stack->rxbufstat)[idx] = EC_BUF_COMPLETE; + /* store MAC source word 1 for redundant routing info */ + (*stack->rxsa)[idx] = ntohs(ehp->sa1); + } + else + { + /* check if index exist and someone is waiting for it */ + if (idxf < EC_MAXBUF && (*stack->rxbufstat)[idxf] == EC_BUF_TX) + { + rxbuf = &(*stack->rxbuf)[idxf]; + /* put it in the buffer array (strip ethernet header) */ + memcpy(rxbuf, &(*stack->tempbuf)[ETH_HEADERSIZE], (*stack->txbuflength)[idxf] - ETH_HEADERSIZE); + /* mark as received */ + (*stack->rxbufstat)[idxf] = EC_BUF_RCVD; + (*stack->rxsa)[idxf] = ntohs(ehp->sa1); + } + else + { + /* strange things happened */ + } + } + } + } + pthread_mutex_unlock( &(port->rx_mutex) ); + + } + + /* WKC if matching frame found */ + return rval; +} + +/** Blocking redundant receive frame function. If redundant mode is not active then + * it skips the secondary stack and redundancy functions. In redundant mode it waits + * for both (primary and secondary) frames to come in. The result goes in an decision + * tree that decides, depending on the route of the packet and its possible missing arrival, + * how to reroute the original packet to get the data in an other try. + * + * @param[in] port = port context struct + * @param[in] idx = requested index of frame + * @param[in] tvs = timeout + * @return Workcounter if a frame is found with corresponding index, otherwise + * EC_NOFRAME. + */ +static int ecx_waitinframe_red(ecx_portt *port, uint8 idx, osal_timert *timer) +{ + osal_timert timer2; + int wkc = EC_NOFRAME; + int wkc2 = EC_NOFRAME; + int primrx, secrx; + + /* if not in redundant mode then always assume secondary is OK */ + if (port->redstate == ECT_RED_NONE) + wkc2 = 0; + do + { + /* only read frame if not already in */ + if (wkc <= EC_NOFRAME) + wkc = ecx_inframe(port, idx, 0); + /* only try secondary if in redundant mode */ + if (port->redstate != ECT_RED_NONE) + { + /* only read frame if not already in */ + if (wkc2 <= EC_NOFRAME) + wkc2 = ecx_inframe(port, idx, 1); + } + /* wait for both frames to arrive or timeout */ + } while (((wkc <= EC_NOFRAME) || (wkc2 <= EC_NOFRAME)) && !osal_timer_is_expired(timer)); + /* only do redundant functions when in redundant mode */ + if (port->redstate != ECT_RED_NONE) + { + /* primrx if the received MAC source on primary socket */ + primrx = 0; + if (wkc > EC_NOFRAME) primrx = port->rxsa[idx]; + /* secrx if the received MAC source on psecondary socket */ + secrx = 0; + if (wkc2 > EC_NOFRAME) secrx = port->redport->rxsa[idx]; + + /* primary socket got secondary frame and secondary socket got primary frame */ + /* normal situation in redundant mode */ + if ( ((primrx == RX_SEC) && (secrx == RX_PRIM)) ) + { + /* copy secondary buffer to primary */ + memcpy(&(port->rxbuf[idx]), &(port->redport->rxbuf[idx]), port->txbuflength[idx] - ETH_HEADERSIZE); + wkc = wkc2; + } + /* primary socket got nothing or primary frame, and secondary socket got secondary frame */ + /* we need to resend TX packet */ + if ( ((primrx == 0) && (secrx == RX_SEC)) || + ((primrx == RX_PRIM) && (secrx == RX_SEC)) ) + { + /* If both primary and secondary have partial connection retransmit the primary received + * frame over the secondary socket. The result from the secondary received frame is a combined + * frame that traversed all slaves in standard order. */ + if ( (primrx == RX_PRIM) && (secrx == RX_SEC) ) + { + /* copy primary rx to tx buffer */ + memcpy(&(port->txbuf[idx][ETH_HEADERSIZE]), &(port->rxbuf[idx]), port->txbuflength[idx] - ETH_HEADERSIZE); + } + osal_timer_start (&timer2, EC_TIMEOUTRET); + /* resend secondary tx */ + ecx_outframe(port, idx, 1); + do + { + /* retrieve frame */ + wkc2 = ecx_inframe(port, idx, 1); + } while ((wkc2 <= EC_NOFRAME) && !osal_timer_is_expired(&timer2)); + if (wkc2 > EC_NOFRAME) + { + /* copy secondary result to primary rx buffer */ + memcpy(&(port->rxbuf[idx]), &(port->redport->rxbuf[idx]), port->txbuflength[idx] - ETH_HEADERSIZE); + wkc = wkc2; + } + } + } + + /* return WKC or EC_NOFRAME */ + return wkc; +} + +/** Blocking receive frame function. Calls ec_waitinframe_red(). + * @param[in] port = port context struct + * @param[in] idx = requested index of frame + * @param[in] timeout = timeout in us + * @return Workcounter if a frame is found with corresponding index, otherwise + * EC_NOFRAME. + */ +int ecx_waitinframe(ecx_portt *port, uint8 idx, int timeout) +{ + int wkc; + osal_timert timer; + + osal_timer_start (&timer, timeout); + wkc = ecx_waitinframe_red(port, idx, &timer); + + return wkc; +} + +/** Blocking send and receive frame function. Used for non processdata frames. + * A datagram is build into a frame and transmitted via this function. It waits + * for an answer and returns the workcounter. The function retries if time is + * left and the result is WKC=0 or no frame received. + * + * The function calls ec_outframe_red() and ec_waitinframe_red(). + * + * @param[in] port = port context struct + * @param[in] idx = index of frame + * @param[in] timeout = timeout in us + * @return Workcounter or EC_NOFRAME + */ +int ecx_srconfirm(ecx_portt *port, uint8 idx, int timeout) +{ + int wkc = EC_NOFRAME; + osal_timert timer1, timer2; + + osal_timer_start (&timer1, timeout); + do + { + /* tx frame on primary and if in redundant mode a dummy on secondary */ + ecx_outframe_red(port, idx); + if (timeout < EC_TIMEOUTRET) + { + osal_timer_start (&timer2, timeout); + } + else + { + /* normally use partial timeout for rx */ + osal_timer_start (&timer2, EC_TIMEOUTRET); + } + /* get frame from primary or if in redundant mode possibly from secondary */ + wkc = ecx_waitinframe_red(port, idx, &timer2); + /* wait for answer with WKC>=0 or otherwise retry until timeout */ + } while ((wkc <= EC_NOFRAME) && !osal_timer_is_expired (&timer1)); + + return wkc; +} + + +#ifdef EC_VER1 + +int ec_setupnic(const char *ifname, int secondary) +{ + return ecx_setupnic(&ecx_port, ifname, secondary); +} + +int ec_closenic(void) +{ + return ecx_closenic(&ecx_port); +} + +uint8 ec_getindex(void) +{ + return ecx_getindex(&ecx_port); +} + +void ec_setbufstat(uint8 idx, int bufstat) +{ + ecx_setbufstat(&ecx_port, idx, bufstat); +} + +int ec_outframe(uint8 idx, int stacknumber) +{ + return ecx_outframe(&ecx_port, idx, stacknumber); +} + +int ec_outframe_red(uint8 idx) +{ + return ecx_outframe_red(&ecx_port, idx); +} + +int ec_inframe(uint8 idx, int stacknumber) +{ + return ecx_inframe(&ecx_port, idx, stacknumber); +} + +int ec_waitinframe(uint8 idx, int timeout) +{ + return ecx_waitinframe(&ecx_port, idx, timeout); +} + +int ec_srconfirm(uint8 idx, int timeout) +{ + return ecx_srconfirm(&ecx_port, idx, timeout); +} + +#endif diff --git a/soem-src/oshw/macosx/nicdrv.h b/soem-src/oshw/macosx/nicdrv.h new file mode 100644 index 0000000..45ac04c --- /dev/null +++ b/soem-src/oshw/macosx/nicdrv.h @@ -0,0 +1,120 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +/** \file + * \brief + * Headerfile for nicdrv.c + */ + +#ifndef _nicdrvh_ +#define _nicdrvh_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include + +/** pointer structure to Tx and Rx stacks */ +typedef struct +{ + /** socket connection used */ + pcap_t **sock; + /** tx buffer */ + ec_bufT (*txbuf)[EC_MAXBUF]; + /** tx buffer lengths */ + int (*txbuflength)[EC_MAXBUF]; + /** temporary receive buffer */ + ec_bufT *tempbuf; + /** rx buffers */ + ec_bufT (*rxbuf)[EC_MAXBUF]; + /** rx buffer status fields */ + int (*rxbufstat)[EC_MAXBUF]; + /** received MAC source address (middle word) */ + int (*rxsa)[EC_MAXBUF]; +} ec_stackT; + +/** pointer structure to buffers for redundant port */ +typedef struct +{ + ec_stackT stack; + pcap_t *sockhandle; + /** rx buffers */ + ec_bufT rxbuf[EC_MAXBUF]; + /** rx buffer status */ + int rxbufstat[EC_MAXBUF]; + /** rx MAC source address */ + int rxsa[EC_MAXBUF]; + /** temporary rx buffer */ + ec_bufT tempinbuf; +} ecx_redportt; + +/** pointer structure to buffers, vars and mutexes for port instantiation */ +typedef struct +{ + ec_stackT stack; + pcap_t *sockhandle; + /** rx buffers */ + ec_bufT rxbuf[EC_MAXBUF]; + /** rx buffer status */ + int rxbufstat[EC_MAXBUF]; + /** rx MAC source address */ + int rxsa[EC_MAXBUF]; + /** temporary rx buffer */ + ec_bufT tempinbuf; + /** temporary rx buffer status */ + int tempinbufs; + /** transmit buffers */ + ec_bufT txbuf[EC_MAXBUF]; + /** transmit buffer lenghts */ + int txbuflength[EC_MAXBUF]; + /** temporary tx buffer */ + ec_bufT txbuf2; + /** temporary tx buffer length */ + int txbuflength2; + /** last used frame index */ + uint8 lastidx; + /** current redundancy state */ + int redstate; + /** pointer to redundancy port and buffers */ + ecx_redportt *redport; + pthread_mutex_t getindex_mutex; + pthread_mutex_t tx_mutex; + pthread_mutex_t rx_mutex; +} ecx_portt; + +extern const uint16 priMAC[3]; +extern const uint16 secMAC[3]; + +#ifdef EC_VER1 +extern ecx_portt ecx_port; +extern ecx_redportt ecx_redport; + +int ec_setupnic(const char * ifname, int secondary); +int ec_closenic(void); +void ec_setbufstat(uint8 idx, int bufstat); +uint8 ec_getindex(void); +int ec_outframe(uint8 idx, int sock); +int ec_outframe_red(uint8 idx); +int ec_waitinframe(uint8 idx, int timeout); +int ec_srconfirm(uint8 idx,int timeout); +#endif + +void ec_setupheader(void *p); +int ecx_setupnic(ecx_portt *port, const char * ifname, int secondary); +int ecx_closenic(ecx_portt *port); +void ecx_setbufstat(ecx_portt *port, uint8 idx, int bufstat); +uint8 ecx_getindex(ecx_portt *port); +int ecx_outframe(ecx_portt *port, uint8 idx, int sock); +int ecx_outframe_red(ecx_portt *port, uint8 idx); +int ecx_waitinframe(ecx_portt *port, uint8 idx, int timeout); +int ecx_srconfirm(ecx_portt *port, uint8 idx,int timeout); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/soem-src/oshw/macosx/oshw.c b/soem-src/oshw/macosx/oshw.c new file mode 100644 index 0000000..90406ad --- /dev/null +++ b/soem-src/oshw/macosx/oshw.c @@ -0,0 +1,117 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +#include +#include +#include +#include +#include +#include +#include +#include "oshw.h" + +/** + * Host to Network byte order (i.e. to big endian). + * + * Note that Ethercat uses little endian byte order, except for the Ethernet + * header which is big endian as usual. + */ +uint16 oshw_htons(uint16 host) +{ + uint16 network = htons (host); + return network; +} + +/** + * Network (i.e. big endian) to Host byte order. + * + * Note that Ethercat uses little endian byte order, except for the Ethernet + * header which is big endian as usual. + */ +uint16 oshw_ntohs(uint16 network) +{ + uint16 host = ntohs (network); + return host; +} + +/** Create list over available network adapters. + * @return First element in linked list of adapters + */ +ec_adaptert * oshw_find_adapters(void) +{ + int i; + struct if_nameindex *ids; + ec_adaptert * adapter; + ec_adaptert * prev_adapter; + ec_adaptert * ret_adapter = NULL; + + + /* Iterate all devices and create a local copy holding the name and + * description. + */ + + ids = if_nameindex (); + for(i = 0; ids[i].if_index != 0; i++) + { + adapter = (ec_adaptert *)malloc(sizeof(ec_adaptert)); + /* If we got more than one adapter save link list pointer to previous + * adapter. + * Else save as pointer to return. + */ + if (i) + { + prev_adapter->next = adapter; + } + else + { + ret_adapter = adapter; + } + + /* fetch description and name, in macosx we use the same on both */ + adapter->next = NULL; + + if (ids[i].if_name) + { + strncpy(adapter->name, ids[i].if_name, EC_MAXLEN_ADAPTERNAME); + adapter->name[EC_MAXLEN_ADAPTERNAME-1] = '\0'; + strncpy(adapter->desc, ids[i].if_name, EC_MAXLEN_ADAPTERNAME); + adapter->desc[EC_MAXLEN_ADAPTERNAME-1] = '\0'; + } + else + { + adapter->name[0] = '\0'; + adapter->desc[0] = '\0'; + } + + prev_adapter = adapter; + } + + if_freenameindex (ids); + + return ret_adapter; +} + +/** Free memory allocated memory used by adapter collection. + * @param[in] adapter = First element in linked list of adapters + * EC_NOFRAME. + */ +void oshw_free_adapters(ec_adaptert * adapter) +{ + ec_adaptert * next_adapter; + /* Iterate the linked list and free all elements holding + * adapter information + */ + if(adapter) + { + next_adapter = adapter->next; + free (adapter); + while (next_adapter) + { + adapter = next_adapter; + next_adapter = adapter->next; + free (adapter); + } + } +} diff --git a/soem-src/oshw/macosx/oshw.h b/soem-src/oshw/macosx/oshw.h new file mode 100644 index 0000000..78d131b --- /dev/null +++ b/soem-src/oshw/macosx/oshw.h @@ -0,0 +1,31 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +/** \file + * \brief + * Headerfile for ethercatbase.c + */ + +#ifndef _oshw_ +#define _oshw_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "ethercattype.h" +#include "nicdrv.h" +#include "ethercatmain.h" + +uint16 oshw_htons(uint16 hostshort); +uint16 oshw_ntohs(uint16 networkshort); +ec_adaptert * oshw_find_adapters(void); +void oshw_free_adapters(ec_adaptert * adapter); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/soem-src/oshw/rtems/nicdrv.c b/soem-src/oshw/rtems/nicdrv.c new file mode 100644 index 0000000..b2c9b2f --- /dev/null +++ b/soem-src/oshw/rtems/nicdrv.c @@ -0,0 +1,696 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +/** \file + * \brief + * EtherCAT RAW socket driver. + * + * Low level interface functions to send and receive EtherCAT packets. + * EtherCAT has the property that packets are only send by the master, + * and the send packets always return in the receive buffer. + * There can be multiple packets "on the wire" before they return. + * To combine the received packets with the original send packets a buffer + * system is installed. The identifier is put in the index item of the + * EtherCAT header. The index is stored and compared when a frame is received. + * If there is a match the packet can be combined with the transmit packet + * and returned to the higher level function. + * + * The socket layer can exhibit a reversal in the packet order (rare). + * If the Tx order is A-B-C the return order could be A-C-B. The indexed buffer + * will reorder the packets automatically. + * + * The "redundant" option will configure two sockets and two NIC interfaces. + * Slaves are connected to both interfaces, one on the IN port and one on the + * OUT port. Packets are send via both interfaces. Any one of the connections + * (also an interconnect) can be removed and the slaves are still serviced with + * packets. The software layer will detect the possible failure modes and + * compensate. If needed the packets from interface A are resent through interface B. + * This layer if fully transparent for the higher layers. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "oshw.h" +#include "osal.h" + +/** Redundancy modes */ +enum +{ + /** No redundancy, single NIC mode */ + ECT_RED_NONE, + /** Double redundant NIC connection */ + ECT_RED_DOUBLE +}; + + +/** Primary source MAC address used for EtherCAT. + * This address is not the MAC address used from the NIC. + * EtherCAT does not care about MAC addressing, but it is used here to + * differentiate the route the packet traverses through the EtherCAT + * segment. This is needed to find out the packet flow in redundant + * configurations. */ +const uint16 priMAC[3] = { 0x0201, 0x0101, 0x0101 }; +/** Secondary source MAC address used for EtherCAT. */ +const uint16 secMAC[3] = { 0x0604, 0x0404, 0x0404 }; + +/** second MAC word is used for identification */ +#define RX_PRIM priMAC[1] +/** second MAC word is used for identification */ +#define RX_SEC secMAC[1] + +static void ecx_clear_rxbufstat(int *rxbufstat) +{ + int i; + for(i = 0; i < EC_MAXBUF; i++) + { + rxbufstat[i] = EC_BUF_EMPTY; + } +} + +/** Basic setup to connect NIC to socket. + * @param[in] port = port context struct + * @param[in] ifname = Name of NIC device, f.e. "eth0" + * @param[in] secondary = if >0 then use secondary stack instead of primary + * @return >0 if succeeded + */ +int ecx_setupnic(ecx_portt *port, const char *ifname, int secondary) +{ + int i; + int r, rval; + struct timeval timeout; + struct ifreq ifr; + int *bpf; + // const uint8_t bpffnamelen = 12; + char fname[13] = {0}; + const int maxbpffile = 1000; + uint true_val = 1; + + rval = 0; + if (secondary) + { + /* secondary port struct available? */ + if (port->redport) + { + /* when using secondary socket it is automatically a redundant setup */ + bpf = &(port->redport->sockhandle); + *bpf = -1; + port->redstate = ECT_RED_DOUBLE; + port->redport->stack.sock = &(port->redport->sockhandle); + port->redport->stack.txbuf = &(port->txbuf); + port->redport->stack.txbuflength = &(port->txbuflength); + port->redport->stack.tempbuf = &(port->redport->tempinbuf); + port->redport->stack.rxbuf = &(port->redport->rxbuf); + port->redport->stack.rxbufstat = &(port->redport->rxbufstat); + port->redport->stack.rxsa = &(port->redport->rxsa); + ecx_clear_rxbufstat(&(port->redport->rxbufstat[0])); + } + else + { + /* fail */ + return 0; + } + } + else + { + pthread_mutex_init(&(port->getindex_mutex), NULL); + pthread_mutex_init(&(port->tx_mutex) , NULL); + pthread_mutex_init(&(port->rx_mutex) , NULL); + port->sockhandle = -1; + port->lastidx = 0; + port->redstate = ECT_RED_NONE; + port->stack.sock = &(port->sockhandle); + port->stack.txbuf = &(port->txbuf); + port->stack.txbuflength = &(port->txbuflength); + port->stack.tempbuf = &(port->tempinbuf); + port->stack.rxbuf = &(port->rxbuf); + port->stack.rxbufstat = &(port->rxbufstat); + port->stack.rxsa = &(port->rxsa); + ecx_clear_rxbufstat(&(port->rxbufstat[0])); + bpf = &(port->sockhandle); + } + + /* Open a bpf file descriptor */ + *bpf = -1; + for(i = 0; *bpf == -1 && i < maxbpffile; ++i) + { + sprintf(fname, "/dev/bpf%i", i); + *bpf = open(fname, O_RDWR); + } + + if(*bpf == -1) + { + /* Failed to open bpf */ + perror("Could not open bpf\n"); + return 0; + } + + /* Need to hand the same buffer size as bpf expects, + force bpf to use our buffer size! */ + uint buffer_len = sizeof(ec_bufT); + if (ioctl(*bpf, BIOCSBLEN, &buffer_len) == -1) { + perror("BIOCGBLEN"); + } + + if (buffer_len < 1518) { + printf("buffer_len %d < 1518\n", buffer_len); + return 0; + } + + /* connect bpf to NIC by name */ + strcpy(ifr.ifr_name, ifname); + if(ioctl(*bpf, BIOCSETIF, &ifr) == -1) { + perror("BIOCSETIF"); + return 0; + } + + /* Set required bpf options */ + + /* Activate immediate mode */ + if (ioctl(*bpf, BIOCIMMEDIATE, &true_val) == -1) { + perror("BIOCIMMEDIATE"); + return 0; + } + + /* Set interface in promiscuous mode */ + if (ioctl(*bpf, BIOCPROMISC, &true_val) == -1) { + perror("BIOCPROMISC"); + return 0; + } + + /* Allow to have custom source address */ + if (ioctl(*bpf, BIOCSHDRCMPLT, &true_val) == -1) { + perror("BIOCSHDRCMPLT"); + return 0; + } + + /* Listen only to incomming messages */ + uint direction = BPF_D_IN; + if (ioctl(*bpf, BIOCSDIRECTION, &direction) == -1) { + perror("BIOCSDIRECTION"); + return 0; + } + + /* Set read timeout */ + timeout.tv_sec = 0; + timeout.tv_usec = 11000; + if (ioctl(*bpf, BIOCSRTIMEOUT, &timeout) == -1) { + perror("BIOCSRTIMEOUT"); + return 0; + } + + /* setup ethernet headers in tx buffers so we don't have to repeat it */ + for (i = 0; i < EC_MAXBUF; i++) + { + ec_setupheader(&(port->txbuf[i])); + port->rxbufstat[i] = EC_BUF_EMPTY; + } + ec_setupheader(&(port->txbuf2)); + + return 1; +} + +/** Close sockets used + * @param[in] port = port context struct + * @return 0 + */ +int ecx_closenic(ecx_portt *port) +{ + if (port->sockhandle >= 0) + close(port->sockhandle); + if ((port->redport) && (port->redport->sockhandle >= 0)) + close(port->redport->sockhandle); + + return 0; +} + +/** Fill buffer with ethernet header structure. + * Destination MAC is always broadcast. + * Ethertype is always ETH_P_ECAT. + * @param[out] p = buffer + */ +void ec_setupheader(void *p) +{ + ec_etherheadert *bp; + bp = p; + bp->da0 = htons(0xffff); + bp->da1 = htons(0xffff); + bp->da2 = htons(0xffff); + bp->sa0 = htons(priMAC[0]); + bp->sa1 = htons(priMAC[1]); + bp->sa2 = htons(priMAC[2]); + bp->etype = htons(ETH_P_ECAT); +} + +/** Get new frame identifier index and allocate corresponding rx buffer. + * @param[in] port = port context struct + * @return new index. + */ +uint8 ecx_getindex(ecx_portt *port) +{ + uint8 idx; + uint8 cnt; + + pthread_mutex_lock( &(port->getindex_mutex) ); + + idx = port->lastidx + 1; + /* index can't be larger than buffer array */ + if (idx >= EC_MAXBUF) + { + idx = 0; + } + cnt = 0; + /* try to find unused index */ + while ((port->rxbufstat[idx] != EC_BUF_EMPTY) && (cnt < EC_MAXBUF)) + { + idx++; + cnt++; + if (idx >= EC_MAXBUF) + { + idx = 0; + } + } + port->rxbufstat[idx] = EC_BUF_ALLOC; + if (port->redstate != ECT_RED_NONE) + port->redport->rxbufstat[idx] = EC_BUF_ALLOC; + port->lastidx = idx; + + pthread_mutex_unlock( &(port->getindex_mutex) ); + + return idx; +} + +/** Set rx buffer status. + * @param[in] port = port context struct + * @param[in] idx = index in buffer array + * @param[in] bufstat = status to set + */ +void ecx_setbufstat(ecx_portt *port, uint8 idx, int bufstat) +{ + port->rxbufstat[idx] = bufstat; + if (port->redstate != ECT_RED_NONE) + port->redport->rxbufstat[idx] = bufstat; +} + +/** Transmit buffer over socket (non blocking). + * @param[in] port = port context struct + * @param[in] idx = index in tx buffer array + * @param[in] stacknumber = 0=Primary 1=Secondary stack + * @return socket send result + */ +int ecx_outframe(ecx_portt *port, uint8 idx, int stacknumber) +{ + int lp, rval; + ec_stackT *stack; + + if (!stacknumber) + { + stack = &(port->stack); + } + else + { + stack = &(port->redport->stack); + } + lp = (*stack->txbuflength)[idx]; + //rval = send(*stack->sock, (*stack->txbuf)[idx], lp, 0); + (*stack->rxbufstat)[idx] = EC_BUF_TX; + rval = write (*stack->sock,(*stack->txbuf)[idx], lp); + if (rval == -1) + { + (*stack->rxbufstat)[idx] = EC_BUF_EMPTY; + } + + return rval; +} + +/** Transmit buffer over socket (non blocking). + * @param[in] port = port context struct + * @param[in] idx = index in tx buffer array + * @return socket send result + */ +int ecx_outframe_red(ecx_portt *port, uint8 idx) +{ + ec_comt *datagramP; + ec_etherheadert *ehp; + int rval; + + ehp = (ec_etherheadert *)&(port->txbuf[idx]); + /* rewrite MAC source address 1 to primary */ + ehp->sa1 = htons(priMAC[1]); + /* transmit over primary socket*/ + rval = ecx_outframe(port, idx, 0); + if (port->redstate != ECT_RED_NONE) + { + pthread_mutex_lock( &(port->tx_mutex) ); + ehp = (ec_etherheadert *)&(port->txbuf2); + /* use dummy frame for secondary socket transmit (BRD) */ + datagramP = (ec_comt*)&(port->txbuf2[ETH_HEADERSIZE]); + /* write index to frame */ + datagramP->index = idx; + /* rewrite MAC source address 1 to secondary */ + ehp->sa1 = htons(secMAC[1]); + /* transmit over secondary socket */ + //send(port->redport->sockhandle, &(port->txbuf2), port->txbuflength2 , 0); + port->redport->rxbufstat[idx] = EC_BUF_TX; + if (write(port->redport->sockhandle, &(port->txbuf2), port->txbuflength2) == -1) + { + (*stack->rxbufstat)[idx] = EC_BUF_EMPTY; + } + pthread_mutex_unlock( &(port->tx_mutex) ); + } + + return rval; +} + +/** Non blocking read of socket. Put frame in temporary buffer. + * @param[in] port = port context struct + * @param[in] stacknumber = 0=primary 1=secondary stack + * @return >0 if frame is available and read + */ +static int ecx_recvpkt(ecx_portt *port, int stacknumber) +{ + int lp, bytesrx; + ec_stackT *stack; + + if (!stacknumber) + { + stack = &(port->stack); + } + else + { + stack = &(port->redport->stack); + } + lp = sizeof(port->tempinbuf); + //bytesrx = recv(*stack->sock, (*stack->tempbuf), lp, 0); + bytesrx = read(*stack->sock, (*stack->tempbuf), lp); + port->tempinbufs = bytesrx; + + return (bytesrx > 0); +} + +/** Non blocking receive frame function. Uses RX buffer and index to combine + * read frame with transmitted frame. To compensate for received frames that + * are out-of-order all frames are stored in their respective indexed buffer. + * If a frame was placed in the buffer previously, the function retrieves it + * from that buffer index without calling ec_recvpkt. If the requested index + * is not already in the buffer it calls ec_recvpkt to fetch it. There are + * three options now, 1 no frame read, so exit. 2 frame read but other + * than requested index, store in buffer and exit. 3 frame read with matching + * index, store in buffer, set completed flag in buffer status and exit. + * + * @param[in] port = port context struct + * @param[in] idx = requested index of frame + * @param[in] stacknumber = 0=primary 1=secondary stack + * @return Workcounter if a frame is found with corresponding index, otherwise + * EC_NOFRAME or EC_OTHERFRAME. + */ +int ecx_inframe(ecx_portt *port, uint8 idx, int stacknumber) +{ + uint16 l; + int rval; + uint8 idxf; + ec_etherheadert *ehp; + ec_comt *ecp; + ec_stackT *stack; + ec_bufT *rxbuf; + + if (!stacknumber) + { + stack = &(port->stack); + } + else + { + stack = &(port->redport->stack); + } + rval = EC_NOFRAME; + rxbuf = &(*stack->rxbuf)[idx]; + /* check if requested index is already in buffer ? */ + if ((idx < EC_MAXBUF) && ((*stack->rxbufstat)[idx] == EC_BUF_RCVD)) + { + l = (*rxbuf)[0] + ((uint16)((*rxbuf)[1] & 0x0f) << 8); + /* return WKC */ + rval = ((*rxbuf)[l] + ((uint16)(*rxbuf)[l + 1] << 8)); + /* mark as completed */ + (*stack->rxbufstat)[idx] = EC_BUF_COMPLETE; + } + else + { + pthread_mutex_lock(&(port->rx_mutex)); + /* non blocking call to retrieve frame from socket */ + if (ecx_recvpkt(port, stacknumber)) + { + /* The data read from /dev/bpf includes an extra header, skip it. */ + struct bpf_hdr *bpfh = (struct bpf_hdr *)(stack->tempbuf); + rval = EC_OTHERFRAME; + ehp =(ec_etherheadert*)(*stack->tempbuf + bpfh->bh_hdrlen); + /* check if it is an EtherCAT frame */ + if (ehp->etype == htons(ETH_P_ECAT)) + { + /* The EtherCAT header follows the ethernet frame header. */ + ecp =(ec_comt*)(&ehp[1]); + l = etohs(ecp->elength) & 0x0fff; + idxf = ecp->index; + /* found index equals requested index ? */ + if (idxf == idx) + { + /* yes, put it in the buffer array (strip headers) */ + memcpy(rxbuf, &(ehp[1]), port->tempinbufs - ((uint32_t)ecp - (uint32_t)*stack->tempbuf)); + /* return WKC */ + rval = ((*rxbuf)[l] + ((uint16)((*rxbuf)[l + 1]) << 8)); + /* mark as completed */ + (*stack->rxbufstat)[idx] = EC_BUF_COMPLETE; + /* store MAC source word 1 for redundant routing info */ + (*stack->rxsa)[idx] = ntohs(ehp->sa1); + } + else + { + /* check if index exist and someone is waiting for it */ + if (idxf < EC_MAXBUF && (*stack->rxbufstat)[idxf] == EC_BUF_TX) + { + rxbuf = &(*stack->rxbuf)[idxf]; + /* put it in the buffer array (strip ethernet header) */ + memcpy(rxbuf, &(*stack->tempbuf)[ETH_HEADERSIZE], (*stack->txbuflength)[idxf] - ETH_HEADERSIZE); + /* mark as received */ + (*stack->rxbufstat)[idxf] = EC_BUF_RCVD; + (*stack->rxsa)[idxf] = ntohs(ehp->sa1); + } + else + { + /* strange things happened */ + } + } + } + } + pthread_mutex_unlock( &(port->rx_mutex) ); + + } + + /* WKC if matching frame found */ + return rval; +} + +/** Blocking redundant receive frame function. If redundant mode is not active then + * it skips the secondary stack and redundancy functions. In redundant mode it waits + * for both (primary and secondary) frames to come in. The result goes in an decision + * tree that decides, depending on the route of the packet and its possible missing arrival, + * how to reroute the original packet to get the data in an other try. + * + * @param[in] port = port context struct + * @param[in] idx = requested index of frame + * @param[in] timer = absolute timeout time + * @return Workcounter if a frame is found with corresponding index, otherwise + * EC_NOFRAME. + */ +static int ecx_waitinframe_red(ecx_portt *port, uint8 idx, osal_timert *timer) +{ + osal_timert timer2; + int wkc = EC_NOFRAME; + int wkc2 = EC_NOFRAME; + int primrx, secrx; + + /* if not in redundant mode then always assume secondary is OK */ + if (port->redstate == ECT_RED_NONE) + wkc2 = 0; + do + { + /* only read frame if not already in */ + if (wkc <= EC_NOFRAME) + wkc = ecx_inframe(port, idx, 0); + /* only try secondary if in redundant mode */ + if (port->redstate != ECT_RED_NONE) + { + /* only read frame if not already in */ + if (wkc2 <= EC_NOFRAME) + wkc2 = ecx_inframe(port, idx, 1); + } + /* wait for both frames to arrive or timeout */ + } while (((wkc <= EC_NOFRAME) || (wkc2 <= EC_NOFRAME)) && !osal_timer_is_expired(timer)); + /* only do redundant functions when in redundant mode */ + if (port->redstate != ECT_RED_NONE) + { + /* primrx if the received MAC source on primary socket */ + primrx = 0; + if (wkc > EC_NOFRAME) primrx = port->rxsa[idx]; + /* secrx if the received MAC source on psecondary socket */ + secrx = 0; + if (wkc2 > EC_NOFRAME) secrx = port->redport->rxsa[idx]; + + /* primary socket got secondary frame and secondary socket got primary frame */ + /* normal situation in redundant mode */ + if ( ((primrx == RX_SEC) && (secrx == RX_PRIM)) ) + { + /* copy secondary buffer to primary */ + memcpy(&(port->rxbuf[idx]), &(port->redport->rxbuf[idx]), port->txbuflength[idx] - ETH_HEADERSIZE); + wkc = wkc2; + } + /* primary socket got nothing or primary frame, and secondary socket got secondary frame */ + /* we need to resend TX packet */ + if ( ((primrx == 0) && (secrx == RX_SEC)) || + ((primrx == RX_PRIM) && (secrx == RX_SEC)) ) + { + /* If both primary and secondary have partial connection retransmit the primary received + * frame over the secondary socket. The result from the secondary received frame is a combined + * frame that traversed all slaves in standard order. */ + if ( (primrx == RX_PRIM) && (secrx == RX_SEC) ) + { + /* copy primary rx to tx buffer */ + memcpy(&(port->txbuf[idx][ETH_HEADERSIZE]), &(port->rxbuf[idx]), port->txbuflength[idx] - ETH_HEADERSIZE); + } + osal_timer_start (&timer2, EC_TIMEOUTRET); + /* resend secondary tx */ + ecx_outframe(port, idx, 1); + do + { + /* retrieve frame */ + wkc2 = ecx_inframe(port, idx, 1); + } while ((wkc2 <= EC_NOFRAME) && !osal_timer_is_expired(&timer2)); + if (wkc2 > EC_NOFRAME) + { + /* copy secondary result to primary rx buffer */ + memcpy(&(port->rxbuf[idx]), &(port->redport->rxbuf[idx]), port->txbuflength[idx] - ETH_HEADERSIZE); + wkc = wkc2; + } + } + } + + /* return WKC or EC_NOFRAME */ + return wkc; +} + +/** Blocking receive frame function. Calls ec_waitinframe_red(). + * @param[in] port = port context struct + * @param[in] idx = requested index of frame + * @param[in] timeout = timeout in us + * @return Workcounter if a frame is found with corresponding index, otherwise + * EC_NOFRAME. + */ +int ecx_waitinframe(ecx_portt *port, uint8 idx, int timeout) +{ + int wkc; + osal_timert timer; + + osal_timer_start (&timer, timeout); + wkc = ecx_waitinframe_red(port, idx, &timer); + + return wkc; +} + +/** Blocking send and receive frame function. Used for non processdata frames. + * A datagram is build into a frame and transmitted via this function. It waits + * for an answer and returns the workcounter. The function retries if time is + * left and the result is WKC=0 or no frame received. + * + * The function calls ec_outframe_red() and ec_waitinframe_red(). + * + * @param[in] port = port context struct + * @param[in] idx = index of frame + * @param[in] timeout = timeout in us + * @return Workcounter or EC_NOFRAME + */ +int ecx_srconfirm(ecx_portt *port, uint8 idx, int timeout) +{ + int wkc = EC_NOFRAME; + osal_timert timer1, timer2; + + osal_timer_start (&timer1, timeout); + do + { + /* tx frame on primary and if in redundant mode a dummy on secondary */ + ecx_outframe_red(port, idx); + if (timeout < EC_TIMEOUTRET) + { + osal_timer_start (&timer2, timeout); + } + else + { + /* normally use partial timeout for rx */ + osal_timer_start (&timer2, EC_TIMEOUTRET); + } + /* get frame from primary or if in redundant mode possibly from secondary */ + wkc = ecx_waitinframe_red(port, idx, &timer2); + /* wait for answer with WKC>=0 or otherwise retry until timeout */ + } while ((wkc <= EC_NOFRAME) && !osal_timer_is_expired (&timer1)); + + return wkc; +} + +#ifdef EC_VER1 +int ec_setupnic(const char *ifname, int secondary) +{ + return ecx_setupnic(&ecx_port, ifname, secondary); +} + +int ec_closenic(void) +{ + return ecx_closenic(&ecx_port); +} + +uint8 ec_getindex(void) +{ + return ecx_getindex(&ecx_port); +} + +void ec_setbufstat(uint8 idx, int bufstat) +{ + ecx_setbufstat(&ecx_port, idx, bufstat); +} + +int ec_outframe(uint8 idx, int stacknumber) +{ + return ecx_outframe(&ecx_port, idx, stacknumber); +} + +int ec_outframe_red(uint8 idx) +{ + return ecx_outframe_red(&ecx_port, idx); +} + +int ec_inframe(uint8 idx, int stacknumber) +{ + return ecx_inframe(&ecx_port, idx, stacknumber); +} + +int ec_waitinframe(uint8 idx, int timeout) +{ + return ecx_waitinframe(&ecx_port, idx, timeout); +} + +int ec_srconfirm(uint8 idx, int timeout) +{ + return ecx_srconfirm(&ecx_port, idx, timeout); +} +#endif diff --git a/soem-src/oshw/rtems/nicdrv.h b/soem-src/oshw/rtems/nicdrv.h new file mode 100644 index 0000000..c2dd209 --- /dev/null +++ b/soem-src/oshw/rtems/nicdrv.h @@ -0,0 +1,120 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +/** \file + * \brief + * Headerfile for nicdrv.c + */ + +#ifndef _nicdrvh_ +#define _nicdrvh_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include + +/** pointer structure to Tx and Rx stacks */ +typedef struct +{ + /** socket connection used */ + int *sock; + /** tx buffer */ + ec_bufT (*txbuf)[EC_MAXBUF]; + /** tx buffer lengths */ + int (*txbuflength)[EC_MAXBUF]; + /** temporary receive buffer */ + ec_bufT *tempbuf; + /** rx buffers */ + ec_bufT (*rxbuf)[EC_MAXBUF]; + /** rx buffer status fields */ + int (*rxbufstat)[EC_MAXBUF]; + /** received MAC source address (middle word) */ + int (*rxsa)[EC_MAXBUF]; +} ec_stackT; + +/** pointer structure to buffers for redundant port */ +typedef struct +{ + ec_stackT stack; + int sockhandle; + /** rx buffers */ + ec_bufT rxbuf[EC_MAXBUF]; + /** rx buffer status */ + int rxbufstat[EC_MAXBUF]; + /** rx MAC source address */ + int rxsa[EC_MAXBUF]; + /** temporary rx buffer */ + ec_bufT tempinbuf; +} ecx_redportt; + +/** pointer structure to buffers, vars and mutexes for port instantiation */ +typedef struct +{ + ec_stackT stack; + int sockhandle; + /** rx buffers */ + ec_bufT rxbuf[EC_MAXBUF]; + /** rx buffer status */ + int rxbufstat[EC_MAXBUF]; + /** rx MAC source address */ + int rxsa[EC_MAXBUF]; + /** temporary rx buffer */ + ec_bufT tempinbuf; + /** temporary rx buffer status */ + int tempinbufs; + /** transmit buffers */ + ec_bufT txbuf[EC_MAXBUF]; + /** transmit buffer lengths */ + int txbuflength[EC_MAXBUF]; + /** temporary tx buffer */ + ec_bufT txbuf2; + /** temporary tx buffer length */ + int txbuflength2; + /** last used frame index */ + uint8 lastidx; + /** current redundancy state */ + int redstate; + /** pointer to redundancy port and buffers */ + ecx_redportt *redport; + pthread_mutex_t getindex_mutex; + pthread_mutex_t tx_mutex; + pthread_mutex_t rx_mutex; +} ecx_portt; + +extern const uint16 priMAC[3]; +extern const uint16 secMAC[3]; + +#ifdef EC_VER1 +extern ecx_portt ecx_port; +extern ecx_redportt ecx_redport; + +int ec_setupnic(const char * ifname, int secondary); +int ec_closenic(void); +void ec_setbufstat(uint8 idx, int bufstat); +uint8 ec_getindex(void); +int ec_outframe(uint8 idx, int sock); +int ec_outframe_red(uint8 idx); +int ec_waitinframe(uint8 idx, int timeout); +int ec_srconfirm(uint8 idx,int timeout); +#endif + +void ec_setupheader(void *p); +int ecx_setupnic(ecx_portt *port, const char * ifname, int secondary); +int ecx_closenic(ecx_portt *port); +void ecx_setbufstat(ecx_portt *port, uint8 idx, int bufstat); +uint8 ecx_getindex(ecx_portt *port); +int ecx_outframe(ecx_portt *port, uint8 idx, int sock); +int ecx_outframe_red(ecx_portt *port, uint8 idx); +int ecx_waitinframe(ecx_portt *port, uint8 idx, int timeout); +int ecx_srconfirm(ecx_portt *port, uint8 idx,int timeout); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/soem-src/oshw/rtems/oshw.c b/soem-src/oshw/rtems/oshw.c new file mode 100644 index 0000000..f807570 --- /dev/null +++ b/soem-src/oshw/rtems/oshw.c @@ -0,0 +1,117 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +#include +#include +#include +#include +#include +#include +#include +#include "oshw.h" + +/** + * Host to Network byte order (i.e. to big endian). + * + * Note that Ethercat uses little endian byte order, except for the Ethernet + * header which is big endian as usual. + */ +uint16 oshw_htons(uint16 host) +{ + uint16 network = htons (host); + return network; +} + +/** + * Network (i.e. big endian) to Host byte order. + * + * Note that Ethercat uses little endian byte order, except for the Ethernet + * header which is big endian as usual. + */ +uint16 oshw_ntohs(uint16 network) +{ + uint16 host = ntohs (network); + return host; +} + +/** Create list over available network adapters. + * @return First element in linked list of adapters + */ +ec_adaptert * oshw_find_adapters(void) +{ + int i; + struct if_nameindex *ids; + ec_adaptert * adapter; + ec_adaptert * prev_adapter; + ec_adaptert * ret_adapter = NULL; + + + /* Iterate all devices and create a local copy holding the name and + * description. + */ + + ids = if_nameindex (); + for(i = 0; ids[i].if_index != 0; i++) + { + adapter = (ec_adaptert *)malloc(sizeof(ec_adaptert)); + /* If we got more than one adapter save link list pointer to previous + * adapter. + * Else save as pointer to return. + */ + if (i) + { + prev_adapter->next = adapter; + } + else + { + ret_adapter = adapter; + } + + /* fetch description and name, in rtems we use the same on both */ + adapter->next = NULL; + + if (ids[i].if_name) + { + strncpy(adapter->name, ids[i].if_name, EC_MAXLEN_ADAPTERNAME); + adapter->name[EC_MAXLEN_ADAPTERNAME-1] = '\0'; + strncpy(adapter->desc, ids[i].if_name, EC_MAXLEN_ADAPTERNAME); + adapter->desc[EC_MAXLEN_ADAPTERNAME-1] = '\0'; + } + else + { + adapter->name[0] = '\0'; + adapter->desc[0] = '\0'; + } + + prev_adapter = adapter; + } + + if_freenameindex (ids); + + return ret_adapter; +} + +/** Free memory allocated memory used by adapter collection. + * @param[in] adapter = First element in linked list of adapters + * EC_NOFRAME. + */ +void oshw_free_adapters(ec_adaptert * adapter) +{ + ec_adaptert * next_adapter; + /* Iterate the linked list and free all elements holding + * adapter information + */ + if(adapter) + { + next_adapter = adapter->next; + free (adapter); + while (next_adapter) + { + adapter = next_adapter; + next_adapter = adapter->next; + free (adapter); + } + } +} diff --git a/soem-src/oshw/rtems/oshw.h b/soem-src/oshw/rtems/oshw.h new file mode 100644 index 0000000..78d131b --- /dev/null +++ b/soem-src/oshw/rtems/oshw.h @@ -0,0 +1,31 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +/** \file + * \brief + * Headerfile for ethercatbase.c + */ + +#ifndef _oshw_ +#define _oshw_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "ethercattype.h" +#include "nicdrv.h" +#include "ethercatmain.h" + +uint16 oshw_htons(uint16 hostshort); +uint16 oshw_ntohs(uint16 networkshort); +ec_adaptert * oshw_find_adapters(void); +void oshw_free_adapters(ec_adaptert * adapter); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/soem-src/oshw/rtk/fec/fec_ecat.c b/soem-src/oshw/rtk/fec/fec_ecat.c new file mode 100644 index 0000000..de4b5ac --- /dev/null +++ b/soem-src/oshw/rtk/fec/fec_ecat.c @@ -0,0 +1,802 @@ +/****************************************************************************** +* * *** *** +* *** *** *** +* *** **** ********** *** ***** *** **** ***** +* ********* ********** *** ********* ************ ********* +* **** *** *** *** *** **** *** +* *** *** ****** *** *********** *** **** ***** +* *** *** ****** *** ************* *** **** ***** +* *** **** **** *** *** *** **** *** +* *** ******* ***** ************** ************* ********* +* *** ***** *** ******* ** ** ****** ***** +* t h e r e a l t i m e t a r g e t e x p e r t s +* +* http://www.rt-labs.com +* Copyright (C) 2007. rt-labs AB, Sweden. All rights reserved. +*------------------------------------------------------------------------------ +* $Id: fec_ecat.c 138 2014-04-11 09:11:48Z rtlfrm $ +*------------------------------------------------------------------------------ +*/ + +#include "fec_ecat.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +#undef RTK_DEBUG /* Print debugging info? */ +#undef RTK_DEBUG_DATA /* Print packets? */ + +#ifdef RTK_DEBUG +#define DPRINT(...) rprintp ("FEC: "__VA_ARGS__) +#else +#define DPRINT(...) +#endif /* DEBUG */ + +#ifdef RTK_DEBUG_DATA +#define DUMP_PACKET(p, l) fec_ecat_dump_packet (p, l) +#else +#define DUMP_PACKET(p, l) +#endif /* RTK_DEBUG_DATA */ + +#define ETHERNET_FCS_SIZE_IN_BYTES 4 /* Frame Check Sequence (32-bit CRC) */ + +#define DELAY(ms) task_delay (tick_from_ms (ms) + 1) + +#define ETH_PHY_ADDRESS 0x1 /* Default jumper setting on TWR-K60F */ + +#define NUM_BUFFERS 2 +#define FEC_ALLIGNED __attribute__((section(".dma"),aligned(16))) + +/* + * "The device's system clock is connected to the module clock, + * as named in the Ethernet chapter. The minimum system clock frequency + * for 100 Mbps operation is 25 MHz." + * + * - Kinetis K60 manual ch. 3.9.1.1 "Ethernet Clocking Options" + */ +#define FEC_MIN_MODULE_CLOCK_Hz (25 * 1000 * 1000) + +/* Note that the core clock and the system clock has the same clock frequency */ +#define FEC_MODULE_CLOCK_Hz CFG_IPG_CLOCK + +//----------------------------------------------------------------------------// + +typedef struct fec_mac_t +{ + uint32_t lower; /* octets 0, 1, 2, 3 */ + uint32_t upper; /* octets 4, 5 */ +} fec_mac_t; + +/* Message Information Block (MIB) Counters */ +typedef struct fec_mib +{ + uint32_t rmon_t_drop; /* MBAR_ETH + 0x200 */ + uint32_t rmon_t_packets; /* MBAR_ETH + 0x204 */ + uint32_t rmon_t_bc_pkt; /* MBAR_ETH + 0x208 */ + uint32_t rmon_t_mc_pkt; /* MBAR_ETH + 0x20C */ + uint32_t rmon_t_crc_align; /* MBAR_ETH + 0x210 */ + uint32_t rmon_t_undersize; /* MBAR_ETH + 0x214 */ + uint32_t rmon_t_oversize; /* MBAR_ETH + 0x218 */ + uint32_t rmon_t_frag; /* MBAR_ETH + 0x21C */ + uint32_t rmon_t_jab; /* MBAR_ETH + 0x220 */ + uint32_t rmon_t_col; /* MBAR_ETH + 0x224 */ + uint32_t rmon_t_p64; /* MBAR_ETH + 0x228 */ + uint32_t rmon_t_p65to127; /* MBAR_ETH + 0x22C */ + uint32_t rmon_t_p128to255; /* MBAR_ETH + 0x230 */ + uint32_t rmon_t_p256to511; /* MBAR_ETH + 0x234 */ + uint32_t rmon_t_p512to1023; /* MBAR_ETH + 0x238 */ + uint32_t rmon_t_p1024to2047; /* MBAR_ETH + 0x23C */ + uint32_t rmon_t_p_gte2048; /* MBAR_ETH + 0x240 */ + uint32_t rmon_t_octets; /* MBAR_ETH + 0x244 */ + uint32_t ieee_t_drop; /* MBAR_ETH + 0x248 */ + uint32_t ieee_t_frame_ok; /* MBAR_ETH + 0x24C */ + uint32_t ieee_t_1col; /* MBAR_ETH + 0x250 */ + uint32_t ieee_t_mcol; /* MBAR_ETH + 0x254 */ + uint32_t ieee_t_def; /* MBAR_ETH + 0x258 */ + uint32_t ieee_t_lcol; /* MBAR_ETH + 0x25C */ + uint32_t ieee_t_excol; /* MBAR_ETH + 0x260 */ + uint32_t ieee_t_macerr; /* MBAR_ETH + 0x264 */ + uint32_t ieee_t_cserr; /* MBAR_ETH + 0x268 */ + uint32_t ieee_t_sqe; /* MBAR_ETH + 0x26C */ + uint32_t t_fdxfc; /* MBAR_ETH + 0x270 */ + uint32_t ieee_t_octets_ok; /* MBAR_ETH + 0x274 */ + uint32_t res13[2]; /* MBAR_ETH + 0x278-27C */ + uint32_t rmon_r_drop; /* MBAR_ETH + 0x280 */ + uint32_t rmon_r_packets; /* MBAR_ETH + 0x284 */ + uint32_t rmon_r_bc_pkt; /* MBAR_ETH + 0x288 */ + uint32_t rmon_r_mc_pkt; /* MBAR_ETH + 0x28C */ + uint32_t rmon_r_crc_align; /* MBAR_ETH + 0x290 */ + uint32_t rmon_r_undersize; /* MBAR_ETH + 0x294 */ + uint32_t rmon_r_oversize; /* MBAR_ETH + 0x298 */ + uint32_t rmon_r_frag; /* MBAR_ETH + 0x29C */ + uint32_t rmon_r_jab; /* MBAR_ETH + 0x2A0 */ + uint32_t rmon_r_resvd_0; /* MBAR_ETH + 0x2A4 */ + uint32_t rmon_r_p64; /* MBAR_ETH + 0x2A8 */ + uint32_t rmon_r_p65to127; /* MBAR_ETH + 0x2AC */ + uint32_t rmon_r_p128to255; /* MBAR_ETH + 0x2B0 */ + uint32_t rmon_r_p256to511; /* MBAR_ETH + 0x2B4 */ + uint32_t rmon_r_p512to1023; /* MBAR_ETH + 0x2B8 */ + uint32_t rmon_r_p1024to2047; /* MBAR_ETH + 0x2BC */ + uint32_t rmon_r_p_gte2048; /* MBAR_ETH + 0x2C0 */ + uint32_t rmon_r_octets; /* MBAR_ETH + 0x2C4 */ + uint32_t ieee_r_drop; /* MBAR_ETH + 0x2C8 */ + uint32_t ieee_r_frame_ok; /* MBAR_ETH + 0x2CC */ + uint32_t ieee_r_crc; /* MBAR_ETH + 0x2D0 */ + uint32_t ieee_r_align; /* MBAR_ETH + 0x2D4 */ + uint32_t r_macerr; /* MBAR_ETH + 0x2D8 */ + uint32_t r_fdxfc; /* MBAR_ETH + 0x2DC */ + uint32_t ieee_r_octets_ok; /* MBAR_ETH + 0x2E0 */ + uint32_t res14[7]; /* MBAR_ETH + 0x2E4-2FC */ +} fec_mib_t; + +COMPILETIME_ASSERT (sizeof (fec_mib_t) == 0x100); + +/* Register read/write struct */ +typedef struct reg_fec +{ + uint32_t resv0; + uint32_t eir; + uint32_t eimr; + uint32_t resv1; + uint32_t rdar; + uint32_t tdar; + uint32_t resv2[3]; + uint32_t ecr; + uint32_t resv3[6]; + uint32_t mmfr; + uint32_t mscr; + uint32_t resv4[7]; + uint32_t mibc; + uint32_t resv5[7]; + uint32_t rcr; + uint32_t resv6[15]; + uint32_t tcr; + uint32_t resv7[7]; + fec_mac_t pa; /* Physical address. PALR/PAUR */ + uint32_t opd; + uint32_t resv8[10]; + uint32_t iaur; + uint32_t ialr; + uint32_t gaur; + uint32_t galr; + uint32_t resv9[7]; + uint32_t tfwr; + uint32_t resv10; + uint32_t frbr; + uint32_t frsr; + uint32_t resv11[11]; + uint32_t erdsr; + uint32_t etdsr; + uint32_t emrbr; + uint32_t resv14; + uint32_t rx_section_full; /* Not present on all controllers */ + uint32_t rx_section_empty; /* Not present on all controllers */ + uint32_t rx_almost_empty; /* Not present on all controllers */ + uint32_t rx_almost_full; /* Not present on all controllers */ + uint32_t tx_section_empty; /* Not present on all controllers */ + uint32_t tx_almost_empty; /* Not present on all controllers */ + uint32_t tx_almost_full; /* Not present on all controllers */ + uint32_t resv12[21]; + const fec_mib_t mib; + uint32_t fec_miigsk_cfgr; /* Not present on all controllers */ + uint32_t resv13; + uint32_t fec_miigsk_enr; /* Not present on all controllers */ + uint32_t resv15[0x7d]; + fec_mac_t smac[4]; /* Not present on all controllers */ +} reg_fec_t; + +COMPILETIME_ASSERT (offsetof (reg_fec_t, emrbr) == 0x188); +COMPILETIME_ASSERT (offsetof (reg_fec_t, tx_almost_full) == 0x1A8); +COMPILETIME_ASSERT (offsetof (reg_fec_t, fec_miigsk_cfgr) == 0x300); +COMPILETIME_ASSERT (offsetof (reg_fec_t, smac) == 0x500); +COMPILETIME_ASSERT (offsetof (reg_fec_t, smac[3]) == 0x518); + +//----------------------------------------------------------------------------// + +/* Bit definitions and macros for FEC_EIR */ +#define FEC_EIR_CLEAR_ALL (0xFFF80000) +#define FEC_EIR_HBERR (0x80000000) +#define FEC_EIR_BABR (0x40000000) +#define FEC_EIR_BABT (0x20000000) +#define FEC_EIR_GRA (0x10000000) +#define FEC_EIR_TXF (0x08000000) +#define FEC_EIR_TXB (0x04000000) +#define FEC_EIR_RXF (0x02000000) +#define FEC_EIR_RXB (0x01000000) +#define FEC_EIR_MII (0x00800000) +#define FEC_EIR_EBERR (0x00400000) +#define FEC_EIR_LC (0x00200000) +#define FEC_EIR_RL (0x00100000) +#define FEC_EIR_UN (0x00080000) + +/* Bit definitions and macros for FEC_RDAR */ +#define FEC_RDAR_R_DES_ACTIVE (0x01000000) + +/* Bit definitions and macros for FEC_TDAR */ +#define FEC_TDAR_X_DES_ACTIVE (0x01000000) + +/* Bit definitions and macros for FEC_ECR */ +#define FEC_ECR_DBSWP (0x00000100) +#define FEC_ECR_ETHER_EN (0x00000002) +#define FEC_ECR_RESET (0x00000001) + +/* Bit definitions and macros for FEC_MMFR */ +#define FEC_MMFR_DATA(x) (((x)&0xFFFF)) +#define FEC_MMFR_ST(x) (((x)&0x03)<<30) +#define FEC_MMFR_ST_01 (0x40000000) +#define FEC_MMFR_OP_RD (0x20000000) +#define FEC_MMFR_OP_WR (0x10000000) +#define FEC_MMFR_PA(x) (((x)&0x1F)<<23) +#define FEC_MMFR_RA(x) (((x)&0x1F)<<18) +#define FEC_MMFR_TA(x) (((x)&0x03)<<16) +#define FEC_MMFR_TA_10 (0x00020000) + +/* Bit definitions and macros for FEC_MSCR */ +#define FEC_MSCR_DIS_PREAMBLE (0x00000080) +#define FEC_MSCR_MII_SPEED(x) (((x)&0x3F)<<1) + +/* Bit definitions and macros for FEC_MIBC */ +#define FEC_MIBC_MIB_DISABLE (0x80000000) +#define FEC_MIBC_MIB_IDLE (0x40000000) + +/* Bit definitions and macros for FEC_RCR */ +#define FEC_RCR_GRS (0x80000000) +#define FEC_RCR_NO_LGTH_CHECK (0x40000000) +#define FEC_RCR_MAX_FL(x) (((x)&0x7FF)<<16) +#define FEC_RCR_CNTL_FRM_ENA (0x00008000) +#define FEC_RCR_CRC_FWD (0x00004000) +#define FEC_RCR_PAUSE_FWD (0x00002000) +#define FEC_RCR_PAD_EN (0x00001000) +#define FEC_RCR_RMII_ECHO (0x00000800) +#define FEC_RCR_RMII_LOOP (0x00000400) +#define FEC_RCR_RMII_10T (0x00000200) +#define FEC_RCR_RMII_MODE (0x00000100) +#define FEC_RCR_SGMII_ENA (0x00000080) +#define FEC_RCR_RGMII_ENA (0x00000040) +#define FEC_RCR_FCE (0x00000020) +#define FEC_RCR_BC_REJ (0x00000010) +#define FEC_RCR_PROM (0x00000008) +#define FEC_RCR_MII_MODE (0x00000004) +#define FEC_RCR_DRT (0x00000002) +#define FEC_RCR_LOOP (0x00000001) + +/* Bit definitions and macros for FEC_TCR */ +#define FEC_TCR_RFC_PAUSE (0x00000010) +#define FEC_TCR_TFC_PAUSE (0x00000008) +#define FEC_TCR_FDEN (0x00000004) +#define FEC_TCR_HBC (0x00000002) +#define FEC_TCR_GTS (0x00000001) + +/* Bit definitions and macros for FEC_PAUR */ +#define FEC_PAUR_PADDR2(x) (((x)&0xFFFF)<<16) +#define FEC_PAUR_TYPE(x) ((x)&0xFFFF) + +/* Bit definitions and macros for FEC_OPD */ +#define FEC_OPD_PAUSE_DUR(x) (((x)&0x0000FFFF)<<0) +#define FEC_OPD_OPCODE(x) (((x)&0x0000FFFF)<<16) + +/* Bit definitions and macros for FEC_TFWR */ +#define FEC_TFWR_STR_FWD BIT (8) /* Present on ENET but not on FEC */ +#define FEC_TFWR_X_WMRK(x) ((x)&0x03) +#define FEC_TFWR_X_WMRK_64 (0x01) +#define FEC_TFWR_X_WMRK_128 (0x02) +#define FEC_TFWR_X_WMRK_192 (0x03) + +/* Bit definitions and macros for FEC_FRBR */ +#define FEC_FRBR_R_BOUND(x) (((x)&0xFF)<<2) + +/* Bit definitions and macros for FEC_FRSR */ +#define FEC_FRSR_R_FSTART(x) (((x)&0xFF)<<2) + +/* Bit definitions and macros for FEC_ERDSR */ +#define FEC_ERDSR_R_DES_START(x) (((x)&0x3FFFFFFF)<<2) + +/* Bit definitions and macros for FEC_ETDSR */ +#define FEC_ETDSR_X_DES_START(x) (((x)&0x3FFFFFFF)<<2) + +/* Bit definitions and macros for FEC_EMRBR */ +#define FEC_EMRBR_R_BUF_SIZE(x) (((x)&0x7F)<<4) + +//----------------------------------------------------------------------------// + +/* the defines of MII operation */ +#define FEC_MII_ST 0x40000000 +#define FEC_MII_OP_OFF 28 +#define FEC_MII_OP_MASK 0x03 +#define FEC_MII_OP_RD 0x02 +#define FEC_MII_OP_WR 0x01 +#define FEC_MII_PA_OFF 23 +#define FEC_MII_PA_MASK 0xFF +#define FEC_MII_RA_OFF 18 +#define FEC_MII_RA_MASK 0xFF +#define FEC_MII_TA 0x00020000 +#define FEC_MII_DATA_OFF 0 +#define FEC_MII_DATA_MASK 0x0000FFFF + +#define FEC_MII_FRAME (FEC_MII_ST | FEC_MII_TA) +#define FEC_MII_OP(x) (((x) & FEC_MII_OP_MASK) << FEC_MII_OP_OFF) +#define FEC_MII_PA(pa) (((pa) & FEC_MII_PA_MASK) << FEC_MII_PA_OFF) +#define FEC_MII_RA(ra) (((ra) & FEC_MII_RA_MASK) << FEC_MII_RA_OFF) +#define FEC_MII_SET_DATA(v) (((v) & FEC_MII_DATA_MASK) << FEC_MII_DATA_OFF) +#define FEC_MII_GET_DATA(v) (((v) >> FEC_MII_DATA_OFF) & FEC_MII_DATA_MASK) +#define FEC_MII_READ(pa, ra) ((FEC_MII_FRAME | FEC_MII_OP(FEC_MII_OP_RD)) |\ + FEC_MII_PA(pa) | FEC_MII_RA(ra)) +#define FEC_MII_WRITE(pa, ra, v) (FEC_MII_FRAME | FEC_MII_OP(FEC_MII_OP_WR)|\ + FEC_MII_PA(pa) | FEC_MII_RA(ra) | FEC_MII_SET_DATA(v)) + +#define FEC_MII_TIMEOUT 10 // 1 * 10 ms +#define FEC_MII_TICK 1 // 1 ms + +//----------------------------------------------------------------------------// + +/* Ethernet Transmit and Receive Buffers */ +#define PKT_MAXBUF_SIZE 1518 + +//----------------------------------------------------------------------------// + +typedef enum fec_phy_inteface +{ + /** + * MII, Media Independent Interface + * 4-bit operating at 25 Mhz + */ + FEC_PHY_MII, + /** + * RMII, Reduced Media Independent Interface + * 2-bit operating at 50 Mhz + */ + FEC_PHY_RMII, + FEC_PHY_RGMII +} fec_phy_inteface_t; + +typedef struct fec_cfg +{ + uint32_t base; + uint32_t clock; + fec_buffer_bd_t * tx_bd_base; + fec_buffer_bd_t * rx_bd_base; + fec_phy_inteface_t phy_interface; +} fec_cfg_t; + +typedef struct fec +{ + uint32_t clock; + fec_buffer_bd_t * tx_bd_base; + fec_buffer_bd_t * rx_bd_base; + volatile reg_fec_t *base; + phy_t * phy; + fec_phy_inteface_t phy_interface; +} fec_t; + +/* Buffer descriptors. */ +static fec_buffer_bd_t fec_tx_bd[NUM_BUFFERS] FEC_ALLIGNED; +static fec_buffer_bd_t fec_rx_bd[NUM_BUFFERS] FEC_ALLIGNED; + +/* Data buffers. */ +static uint8_t fec_tx_data[NUM_BUFFERS * TX_BUFFER_SIZE] FEC_ALLIGNED; +static uint8_t fec_rx_data[NUM_BUFFERS * RX_BUFFER_SIZE] FEC_ALLIGNED; + +static fec_t *fec; + +//----------------------------------------------------------------------------// + +#ifdef RTK_DEBUG +const char * fec_ecat_link_duplex_name (uint8_t link_state) +{ + if (link_state & PHY_LINK_OK) + { + if (link_state & PHY_LINK_FULL_DUPLEX) + { + return "full duplex"; + } + else + { + return "half duplex"; + } + } + else + { + return "no duplex (link down)"; + } +} + +const char * fec_ecat_link_speed_name (uint8_t link_state) +{ + if (link_state & PHY_LINK_OK) + { + if (link_state & PHY_LINK_10MBIT) + { + return "10 Mbps"; + } + else if (link_state & PHY_LINK_100MBIT) + { + return "100 Mbps"; + } + else if (link_state & PHY_LINK_1000MBIT) + { + return "1 Gbps"; + } + else + { + return "unknown speed"; + } + } + else + { + return "0 Mbps (link down)"; + } +} +#endif /* RTK_DEBUG */ + +#ifdef RTK_DEBUG_DATA +#include +static void fec_ecat_dump_packet (const uint8_t * payload, size_t len) +{ + size_t i, j, n; + char s[80]; + + ASSERT (payload != NULL); + + for (i = 0; i < len; i += 16) + { + n = 0; + for (j = 0; j < 16 && (i + j) < len; j++) + { + ASSERT (n <= sizeof(s)); + n += rsnprintf (s + n, sizeof(s) - n, "%02x ", payload[i + j]); + } + for (; j < 16; j++) + { + ASSERT (n <= sizeof(s)); + n += rsnprintf (s + n, sizeof(s) - n, " "); + } + ASSERT (n <= sizeof(s)); + n += rsnprintf (s + n, sizeof(s) - n, "|"); + for (j = 0; j < 16 && (i + j) < len; j++) + { + uint8_t c = payload[i + j]; + c = (isprint (c)) ? c : '.'; + ASSERT (n <= sizeof(s)); + n += rsnprintf (s + n, sizeof(s) - n, "%c", c); + } + ASSERT (n <= sizeof(s)); + n += rsnprintf (s + n, sizeof(s) - n, "|\n"); + ASSERT (n <= sizeof(s)); + DPRINT ("%s", s); + } +} +#endif /* DEBUG_DATA */ + +static uint16_t fec_ecat_read_phy (void * arg, uint8_t address, uint8_t reg) +{ + fec->base->eir = FEC_EIR_MII; // Clear interrupt. + fec->base->mmfr = FEC_MII_READ(address, reg); // Write read command. + while (!(fec->base->eir & FEC_EIR_MII)) + { + ; // Wait for interrupt. + } + fec->base->eir = FEC_EIR_MII; // Clear interrupt. + uint16_t data = FEC_MII_GET_DATA(fec->base->mmfr); + return data; // Return read data. +} + +static void fec_ecat_write_phy (void * arg, uint8_t address, uint8_t reg, + uint16_t value) +{ + fec->base->eir = FEC_EIR_MII; // Clear interrupt. + fec->base->mmfr = FEC_MII_WRITE(address, reg, value); // Write data. + while (!(fec->base->eir & FEC_EIR_MII)) + { + ; // Wait for interrupt. + } + fec->base->eir = FEC_EIR_MII; // Clear interrupt. +} + +static void fec_ecat_init_hw (const fec_mac_address_t * mac_address) +{ + uint32_t mii_speed; + + /* Hard reset */ + fec->base->ecr = FEC_ECR_RESET; + while (fec->base->ecr & FEC_ECR_RESET) + { + ; + } + + // Configure MDC clock. + mii_speed = (fec->clock + 499999) / 5000000; + fec->base->mscr = (fec->base->mscr & (~0x7E)) | (mii_speed << 1); + + // Receive control register + fec->base->rcr = FEC_RCR_MAX_FL(PKT_MAXBUF_SIZE) | FEC_RCR_MII_MODE; + + // set RMII mode in RCR register. + if (fec->phy_interface == FEC_PHY_RMII) + { + fec->base->rcr |= FEC_RCR_RMII_MODE; + } + else if(fec->phy_interface == FEC_PHY_RGMII) + { + fec->base->rcr &= ~(FEC_RCR_RMII_MODE | FEC_RCR_MII_MODE ); + fec->base->rcr |= (FEC_RCR_RGMII_ENA | FEC_RCR_MII_MODE); + } + + // Reset phy + if (fec->phy->ops->reset) + { + fec->phy->ops->reset (fec->phy); + } + + /* Don't receive any unicast frames except those whose destination address + * equals our physical address. + */ + fec->base->iaur = 0; + fec->base->ialr = 0; + + /* Receive all multicast frames. */ + fec->base->gaur = UINT32_MAX; + fec->base->galr = UINT32_MAX; + + /* Set our physical address. */ + fec->base->pa.lower = (mac_address->octet[0] << 24) + + (mac_address->octet[1] << 16) + + (mac_address->octet[2] << 8) + + (mac_address->octet[3] << 0); + fec->base->pa.upper = (mac_address->octet[4] << 24) + + (mac_address->octet[5] << 16) + + 0x8808; + + /* Start link autonegotiation */ + fec->phy->ops->start (fec->phy); +} + +int fec_ecat_send (const void *payload, size_t tot_len) +{ + fec_buffer_bd_t * bd; + + /* Frames larger than the maximum Ethernet frame size are not allowed. */ + ASSERT (tot_len <= PKT_MAXBUF_SIZE); + ASSERT (tot_len <= TX_BUFFER_SIZE); + + /* Bus errors should never occur, unless the MPU is enabled and forbids + * the Ethernet MAC DMA from accessing the descriptors or buffers. + */ + ASSERT ((fec->base->eir & FEC_EIR_EBERR) == false); + + /* Allocate a transmit buffer. We wait here until the MAC has released at + * least one buffer. This could take a couple of microseconds. + */ + bd = NULL; + while (bd == NULL) + { + bd = fec_buffer_get_tx (); + } + + DPRINT ("out (%u):\n", tot_len); + DUMP_PACKET (payload, tot_len); + + /* Copy frame to allocated buffer */ + memcpy (bd->data, payload, tot_len); + bd->length = tot_len; + + fec_buffer_produce_tx (bd); + + /* Wait for previous transmissions to complete. + * + * This is a workaround for Freescale Kinetis errata e6358. + * See "Mask Set Errata for Mask 3N96B". + */ + while (fec->base->tdar) + { + ; + } + + /* Transmit frame */ + fec->base->tdar = 1; + + return tot_len; +} + +int fec_ecat_recv (void * buffer, size_t buffer_length) +{ + fec_buffer_bd_t * bd; + int return_value; + size_t frame_length_without_fcs; + + /* Bus errors should never occur, unless the MPU is enabled and forbids + * the Ethernet MAC DMA from accessing the descriptors or buffers. + */ + ASSERT ((fec->base->eir & FEC_EIR_EBERR) == false); + + bd = fec_buffer_get_rx (); + if (bd == NULL) + { + /* No frame received. Not an error. */ + return_value = 0; + return return_value; + } + + /* The FCS CRC should not be returned to user, so subtract that. */ + frame_length_without_fcs = bd->length - ETHERNET_FCS_SIZE_IN_BYTES; + + /* A frame was received. Handle it and then return its buffer to hardware */ + if ((bd->status & BD_RX_L)== 0){ + /* Buffer is not last buffer in frame. This really should never + * happen as our buffers are large enough to contain any + * Ethernet frame. Nevertheless, is has been observed to happen. + * Drop the packet and hope for the best. + */ + DPRINT ("recv(): End of frame not found. Status: 0x%x\n", bd->status); + return_value = -1; + } + else if (bd->status & (BD_RX_LG | BD_RX_NO | BD_RX_CR | BD_RX_OV)) + { + DPRINT ("recv(): Frame is damaged. Status: 0x%x\n", bd->status); + return_value = -1; + } + else if (buffer_length >= frame_length_without_fcs) + { + /* No errors detected, so frame should be valid. */ + ASSERT (frame_length_without_fcs > 0); + + memcpy (buffer, bd->data, frame_length_without_fcs); + DPRINT ("in (%u):\n", frame_length_without_fcs); + DUMP_PACKET (buffer, frame_length_without_fcs); + return_value = frame_length_without_fcs; + } + else + { + DPRINT ("received_frame: User buffer is too small.\n"); + return_value = -1; + } + + /* Tell the HW that there are new free RX buffers. */ + fec_buffer_produce_rx (bd); + fec->base->rdar = 1; + + return return_value; +} + +static void fec_ecat_hotplug (void) +{ + uint8_t link_state = 0; + + /* Disable frame reception/transmission */ + fec->base->ecr &= ~FEC_ECR_ETHER_EN; + + /* Set duplex mode according to link state */ + + link_state = fec->phy->ops->get_link_state (fec->phy); + + /* Set duplex */ + if (link_state & PHY_LINK_FULL_DUPLEX) + { + fec->base->tcr |= FEC_TCR_FDEN; + fec->base->rcr &= ~FEC_RCR_DRT; + } + else + { + fec->base->tcr &= ~FEC_TCR_FDEN; + fec->base->rcr |= FEC_RCR_DRT; + } + + /* Set RMII 10-Mbps mode. */ + if (link_state & PHY_LINK_10MBIT) + { + fec->base->rcr |= FEC_RCR_RMII_10T; + } + else + { + fec->base->rcr &= ~FEC_RCR_RMII_10T; + } + + /* Clear any pending interrupt */ + fec->base->eir = 0xffffffff; + + // Disable all interrupts. + fec->base->eimr = 0x0; + + /* Configure the amount of data required in the + * transmit FIFO before transmission of a frame can begin. + */ + /* All data in frame (i.e. store-and-forward mode). */ + fec->base->tfwr = FEC_TFWR_STR_FWD; + + // Reset buffers. + fec_buffer_reset (); + + // FEC_ERDSR - Receive buffer descriptor ring start register + fec->base->erdsr = (uint32_t)fec->rx_bd_base; + + // FEC_ETDSR - Transmit buffer descriptor ring start register + fec->base->etdsr = (uint32_t)fec->tx_bd_base; + + // FEC_EMRBR - Maximum receive buffer size register + fec->base->emrbr = RX_BUFFER_SIZE - 1; + + /* Let controller count frames. Useful for debugging */ + fec->base->mibc = 0x0; + + /* Now enable the transmit and receive processing. + * + * Also let MAC hardware convert DMA descriptor fields between little endian + * (as accessed by ARM core) and big endian (as used internally by the + * MAC hardware. Software would otherwise need to do the conversions. + */ + fec->base->ecr = FEC_ECR_ETHER_EN | FEC_ECR_DBSWP; + + /* Indicate that there have been empty receive buffers produced */ + // FEC_RDAR - Receive Descriptor ring - Receive descriptor active register + fec->base->rdar = 1; + + DPRINT ("Link up. Speed: %s. Mode: %s.\n", fec_ecat_link_speed_name (link_state), + fec_ecat_link_duplex_name (link_state)); +} + +static dev_state_t fec_ecat_probe (void) +{ + uint8_t link_state; + + link_state = fec->phy->ops->get_link_state (fec->phy); + + return (link_state & PHY_LINK_OK) ? StateAttached : StateDetached; +} + +COMPILETIME_ASSERT (FEC_MODULE_CLOCK_Hz >= FEC_MIN_MODULE_CLOCK_Hz); + +int fec_ecat_init (const fec_mac_address_t * mac_address, + bool phy_loopback_mode) +{ + dev_state_t state; + static const fec_cfg_t eth_cfg = + { + .base = ENET_BASE, + .clock = FEC_MODULE_CLOCK_Hz, + .tx_bd_base = fec_tx_bd, + .rx_bd_base = fec_rx_bd, + .phy_interface = FEC_PHY_RMII, + }; + static phy_cfg_t phy_cfg = + { + .address = ETH_PHY_ADDRESS, + .read = NULL, /* Set by MAC driver */ + .write = NULL, /* Set by MAC driver */ + }; + + phy_cfg.loopback_mode = phy_loopback_mode; + + /* Initialise buffers and buffer descriptors.*/ + fec_buffer_init_tx (fec_tx_bd, fec_tx_data, NUM_BUFFERS); + fec_buffer_init_rx (fec_rx_bd, fec_rx_data, NUM_BUFFERS); + + fec = malloc (sizeof (fec_t)); + UASSERT (fec != NULL, EMEM); + + /* Initialise driver state */ + fec->rx_bd_base = eth_cfg.rx_bd_base; + fec->tx_bd_base = eth_cfg.tx_bd_base; + fec->clock = eth_cfg.clock; + fec->base = (reg_fec_t *)eth_cfg.base; + fec->phy_interface = eth_cfg.phy_interface; + fec->phy = mii_init (&phy_cfg); + fec->phy->arg = fec; + fec->phy->read = fec_ecat_read_phy; + fec->phy->write = fec_ecat_write_phy; + + /* Initialize hardware */ + fec_ecat_init_hw (mac_address); + state = StateDetached; + while (state == StateDetached) + { + state = fec_ecat_probe (); + } + fec_ecat_hotplug (); + + return 0; +} diff --git a/soem-src/oshw/rtk/fec/fec_ecat.h b/soem-src/oshw/rtk/fec/fec_ecat.h new file mode 100644 index 0000000..aded90d --- /dev/null +++ b/soem-src/oshw/rtk/fec/fec_ecat.h @@ -0,0 +1,56 @@ +/****************************************************************************** +* * *** *** +* *** *** *** +* *** **** ********** *** ***** *** **** ***** +* ********* ********** *** ********* ************ ********* +* **** *** *** *** *** **** *** +* *** *** ****** *** *********** *** **** ***** +* *** *** ****** *** ************* *** **** ***** +* *** **** **** *** *** *** **** *** +* *** ******* ***** ************** ************* ********* +* *** ***** *** ******* ** ** ****** ***** +* t h e r e a l t i m e t a r g e t e x p e r t s +* +* http://www.rt-labs.com +* Copyright (C) 2007. rt-labs AB, Sweden. All rights reserved. +*------------------------------------------------------------------------------ +* $Id: fec_ecat.h 91 2014-04-02 13:32:29Z rtlfrm $ +*------------------------------------------------------------------------------ +*/ + +/** + * \defgroup fec EtherCat Ethernet MAC driver for Frescale K60 SoCs. + * + * \{ + */ + +#ifndef FEC_H +#define FEC_H + +#include + +#ifdef __cplusplus +extern "C" +{ +#endif + +typedef struct fec_mac_address +{ + uint8_t octet[6]; +} fec_mac_address_t; + +int fec_ecat_init (const fec_mac_address_t * mac_address, bool phy_loopback_mode); + +int fec_ecat_send (const void *payload, size_t tot_len); + +int fec_ecat_recv (void * buffer, size_t buffer_length); + +#ifdef __cplusplus +} +#endif + +#endif /* FEC_H */ + +/** + * \} + */ diff --git a/soem-src/oshw/rtk/lw_mac/lw_emac.c b/soem-src/oshw/rtk/lw_mac/lw_emac.c new file mode 100644 index 0000000..46d3928 --- /dev/null +++ b/soem-src/oshw/rtk/lw_mac/lw_emac.c @@ -0,0 +1,423 @@ +/****************************************************************************** + * * *** *** + * *** *** *** + * *** **** ********** *** ***** *** **** ***** + * ********* ********** *** ********* ************ ********* + * **** *** *** *** *** **** *** + * *** *** ****** *** *********** *** **** ***** + * *** *** ****** *** ************* *** **** ***** + * *** **** **** *** *** *** **** *** + * *** ******* ***** ************** ************* ********* + * *** ***** *** ******* ** ** ****** ***** + * t h e r e a l t i m e t a r g e t e x p e r t s + * + * http://www.rt-labs.com + * Copyright (C) 2006. rt-labs AB, Sweden. All rights reserved. + *------------------------------------------------------------------------------ + * $Id: lw_emac.c 348 2012-10-18 16:41:14Z rtlfrm $ + * + * + *------------------------------------------------------------------------------ + */ + +#include +#include +#include +#include +#include + +#include "lw_emac.h" + +/* MII standard registers. + See IEEE Std 802.3-2005 clause 22: + "Reconciliation Sublayer (RS) and Media Independent Interface (MII)", + section 2.4.1 to 3. + http://standards.ieee.org/getieee802/download/802.3-2005_section2.pdf */ +#define MII_BMCR 0x00 /* Basic Mode Control Register */ +#define MII_BMSR 0x01 /* Basic Mode Status Register */ +#define MII_PHYIDR1 0x02 /* PHY Identifier Register 1 */ +#define MII_PHYIDR2 0x03 /* PHY Identifier Register 2 */ +#define MII_LPAR 0x05 /* Auto-Negotiation Link Partner Ability Register */ + +/* MII basic mode control register */ +#define MII_BMCR_RST BIT (15) +#define MII_BMCR_ANEG_EN BIT (12) +#define MII_BMCR_ANEG_RST BIT (9) + +/* MII basic mode status register */ +#define MII_BMSR_ANEGACK BIT (5) +#define MII_BMSR_LINK BIT (2) + +/* MII auto-negotiation advertisement register */ +#define MII_ANAR_100_FD BIT (8) /* Can do 100BASE-TX full duplex */ +#define MII_ANAR_10_FD BIT (6) /* Can do 10BASE-T full duplex */ + +/* Management Data Clock (MDC) frequency. + * 2.5Mz corresponds to the minimum period for MDC as defined by the standard. + * This is independent of whether the PHY is in 10Mbit/s or 100Mbit/s mode. + * See Std. IEEE 802.3 (Ethernet) Clause 22.2.2.11 ("management data clock"). */ +#define PHY_MDC_Hz 2500000 +#define PHY_RETRIES 3000 +/* TODO: This might need changing for different boards. Fix? */ +#define PHY_ADDR 0 + +#define ETH_RX_BUF_SIZE 2 +#define ETH_TX_BUF_SIZE 1 +#define ETH_FRAME_SIZE 0x614 + +/* Ethernet MAC register structure */ +typedef struct bfin_emac_regs +{ + /* Control-Status Register Group */ + uint32_t opmode; /* operating mode */ + uint32_t addrlo; /* address low */ + uint32_t addrhi; /* address high */ + uint32_t hashlo; /* multicast hash table low */ + uint32_t hashhi; /* multicast hash table high */ + uint32_t staadd; /* station management address */ + uint32_t stadat; /* station management data */ + uint32_t flc; /* flow control */ + uint32_t vlan1; /* VLAN1 tag */ + uint32_t vlan2; /* VLAN2 tag */ + uint32_t _reserved1; + uint32_t wkup_ctl; /* wakeup frame control and status */ + uint32_t wkup_ffmsk[4]; /* wakeup frame n byte mask (n == 0,..,3) */ + uint32_t wkup_ffcmd; /* wakeup frame filter commands */ + uint32_t wkup_ffoff; /* wakeup frame filter offsets */ + uint32_t wkup_ffcrc0; /* wakeup frame filter CRC0/1 */ + uint32_t wkup_ffcrc1; /* wakeup frame filter CRC2/3 */ + uint32_t _reserved2[4]; + /* System Interface Register Group */ + uint32_t sysctl; /* system control */ + uint32_t systat; /* system status */ + /* Ethernet Frame Status Register Group */ + uint32_t rx_stat; /* RX Current Frame Status */ + uint32_t rx_stky; /* RX Sticky Frame Status */ + uint32_t rx_irqe; /* RX Frame Status Interrupt Enable */ + uint32_t tx_stat; /* TX Current Frame Status Register */ + uint32_t tx_stky; /* TT Sticky Frame Status */ + uint32_t tx_irqe; /* TX Frame Status Interrupt Enable */ + /* MAC Management Counter Register Group */ + uint32_t mmc_ctl; /* Management Counters Control */ + uint32_t mmc_rirqs; /* MMC RX Interrupt Status */ + uint32_t mmc_rirqe; /* MMC RX Interrupt Enable */ + uint32_t mmc_tirqs; /* MMC TX Interrupt Status */ + uint32_t mmc_tirqe; /* MMC TX Interrupt Enable */ + uint32_t _reserved3[27]; + uint32_t mmc_rxc_ok; /* FramesReceivedOK */ + uint32_t mmc_rxc_fcs; /* FrameCheckSequenceErrors */ + uint32_t mmc_rxc_align; /* AlignmentErrors */ + uint32_t mmc_rxc_octet; /* OctetsReceivedOK */ + uint32_t mmc_rxc_dmaovf; /* FramesLostDueToIntMACRcvError */ + uint32_t mmc_rxc_unicst; /* UnicastFramesReceivedOK */ + uint32_t mmc_rxc_multi; /* MulticastFramesReceivedOK */ + uint32_t mmc_rxc_broad; /* BroadcastFramesReceivedOK */ + uint32_t mmc_rxc_lnerri; /* InRangeLengthErrors */ + uint32_t mmc_rxc_lnerro; /* OutOfRangeLengthField */ + uint32_t mmc_rxc_long; /* FrameTooLongErrors */ + uint32_t mmc_rxc_macctl; /* MACControlFramesReceived */ + uint32_t mmc_rxc_opcode; /* UnsupportedOpcodesReceived */ + uint32_t mmc_rxc_pause; /* PAUSEMACCtrlFramesReceived */ + uint32_t mmc_rxc_allfrm; /* FramesReceivedAll */ + uint32_t mmc_rxc_alloct; /* OctetsReceivedAll */ + uint32_t mmc_rxc_typed; /* TypedFramesReceived */ + uint32_t mmc_rxc_short; /* FramesLenLt64Received */ + uint32_t mmc_rxc_eq64; /* FramesLenEq64Received */ + uint32_t mmc_rxc_lt128; /* FramesLen65_127Received */ + uint32_t mmc_rxc_lt256; /* FramesLen128_255Received */ + uint32_t mmc_rxc_lt512; /* FramesLen256_511Received */ + uint32_t mmc_rxc_lt1024; /* FramesLen512_1023Received */ + uint32_t mmc_rxc_ge1024; /* FramesLen1024_MaxReceived */ + /* TODO: add all registers */ +} bfin_emac_regs_t; + +COMPILETIME_ASSERT (offsetof (bfin_emac_regs_t, opmode) == 0x0); +COMPILETIME_ASSERT (offsetof (bfin_emac_regs_t, staadd) == 0x14); +COMPILETIME_ASSERT (offsetof (bfin_emac_regs_t, wkup_ctl) == 0x2c); +COMPILETIME_ASSERT (offsetof (bfin_emac_regs_t, wkup_ffcrc1) == 0x4c); +COMPILETIME_ASSERT (offsetof (bfin_emac_regs_t, sysctl) == 0x60); +COMPILETIME_ASSERT (offsetof (bfin_emac_regs_t, rx_stat) == 0x68); +COMPILETIME_ASSERT (offsetof (bfin_emac_regs_t, mmc_ctl) == 0x80); +COMPILETIME_ASSERT (offsetof (bfin_emac_regs_t, mmc_tirqe) == 0x90); +COMPILETIME_ASSERT (offsetof (bfin_emac_regs_t, mmc_rxc_ok) == 0x100); +COMPILETIME_ASSERT (offsetof (bfin_emac_regs_t, mmc_rxc_dmaovf) == 0x110); +COMPILETIME_ASSERT (offsetof (bfin_emac_regs_t, mmc_rxc_ge1024) == 0x15c); + +/* 32-bit aligned struct for ethernet data */ +typedef struct ethernet_data { + uint16_t length; /* When TX = frame data length in bytes, not including 'length' + * When RX = 0x0000 padding when using RXDWA + */ + uint8_t data[ETH_FRAME_SIZE]; /* Hold the ethernet frame */ + uint16_t padding; /* To make the struct 32-bit aligned */ +} ethernet_data_t; + +COMPILETIME_ASSERT (sizeof (ethernet_data_t) == 0x618) ; + +static volatile bfin_emac_regs_t * pEth = (bfin_emac_regs_t *) EMAC_OPMODE; + +/* Buffers for rx and tx DMA operations */ +static ethernet_data_t rxBuffer[ETH_RX_BUF_SIZE] __attribute__((section(".dma"))); +static ethernet_data_t txBuffer[ETH_TX_BUF_SIZE] __attribute__((section(".dma"))); +static volatile uint32_t rxStatusWord[ETH_RX_BUF_SIZE] __attribute__((section(".dma"))); +static volatile uint32_t txStatusWord[ETH_TX_BUF_SIZE] __attribute__((section(".dma"))); +static bfin_dma_descriptor_t rxDMADesc[ETH_RX_BUF_SIZE * 2] __attribute__((section(".dma"))); +static bfin_dma_descriptor_t txDMADesc[ETH_TX_BUF_SIZE * 2] __attribute__((section(".dma"))); + +/* Hold the current index of rx and tx buffers */ +static uint8_t rxIdx, txIdx; + + +/* Internal function that writes to a PHY register */ +static void lw_emac_write_phy_reg(uint8_t phy_addr, uint8_t reg_addr, uint32_t data) { + /* Set the flags that should be set reg_addr on PHY */ + pEth->stadat = data; + + /* Wait for our turn then initiate writing of stadat to PHY register */ + pEth->staadd = SET_PHYAD(phy_addr) | SET_REGAD(reg_addr) | STAOP | STABUSY; + while (pEth->staadd & STABUSY) ; +} + +/* Internal function that reads from; and returns a PHY register */ +static uint32_t lw_emac_read_phy_reg(uint8_t phy_addr, uint8_t reg_addr) { + pEth->staadd = SET_PHYAD(phy_addr) | SET_REGAD(reg_addr) | STABUSY; + while (pEth->staadd & STABUSY) ; + + return pEth->stadat; +} + +/* Internal function that sets the MAC address */ +static void lw_emac_set_mac_addr(uint8_t * ethAddr) +{ + pEth->addrlo = + ethAddr[0] | + ethAddr[1] << 8 | + ethAddr[2] << 16 | + ethAddr[3] << 24; + pEth->addrhi = + ethAddr[4] | + ethAddr[5] << 8; +} + +static uint8_t lw_emac_init_registers(uint8_t * ethAddr) { + uint32_t clock_divisor, sysctl_mdcdiv, phy_stadat, counter; + + /* CONFIGURE MAC MII PINS */ + + /* Enable PHY Clock Output */ + *(volatile uint16_t *) VR_CTL |= PHYCLKOE; + + /* Set all bits to 1 to use MII mode */ + *(uint16_t *) PORTH_FER = 0xFFFF; + + /* CONFIGURE MAC REGISTERS */ + + /* + * Set the MMC (MAC Management Counter) control register + * RTSC = Clear all counters + * + * Note that counters are not enabled at this time + */ + pEth->mmc_ctl = RSTC; + + /* Set MAC address */ + lw_emac_set_mac_addr (ethAddr); + + clock_divisor = SCLK / PHY_MDC_Hz; + sysctl_mdcdiv = clock_divisor / 2 - 1; + ASSERT (sysctl_mdcdiv <= 0x3f); + + rprintp ("PHY ID: %04x %04x\n", + lw_emac_read_phy_reg (PHY_ADDR, MII_PHYIDR1), + lw_emac_read_phy_reg (PHY_ADDR, MII_PHYIDR2) + ); + + /* + * Set the system control register + * SET_MDCDIV(x) = Set MDC to 2.5 MHz + * RXDWA = Pad incoming frame with 0x0000 as to make the data-part 32-bit aligned + * RXCKS = Enable Receive Frame TCP/UDP Checksum Computation + */ + pEth->sysctl = SET_MDCDIV(sysctl_mdcdiv) | RXDWA ; + + /* CONFIGURE PHY */ + + /* + * Set the PHY basic control register + * MII_BMCR_ANEG_EN = Auto negotiation on + * MII_BMCR_ANEG_RST = Restart the auto-neg process by setting + * Speed handled by auto negotiation + */ + phy_stadat = MII_BMCR_ANEG_EN | MII_BMCR_ANEG_RST; + lw_emac_write_phy_reg(PHY_ADDR, MII_BMCR, phy_stadat); + + /* Loop until link is up or time out after PHY_RETRIES */ + counter = 0; + do { + if (counter > PHY_RETRIES) { + rprintp("Ethernet link is down\n"); + return -1; + } + + task_delay (tick_from_ms (10)); + phy_stadat = lw_emac_read_phy_reg(PHY_ADDR, MII_BMSR); + + ++counter; + } while (!(phy_stadat & MII_BMSR_LINK)) ; + + /* Check whether link partner can do full duplex or not */ + phy_stadat = lw_emac_read_phy_reg(PHY_ADDR, MII_LPAR); + + if (phy_stadat & (MII_ANAR_100_FD | MII_ANAR_10_FD) ) { + pEth->opmode = FDMODE; + } + else { + pEth->opmode = 0; + } + + /* + * Setup DMA MAC receive/transfer channels with XCOUNT 0 (which we use + * together with description based DMA) and XMODIFY 4 (bytes per transfer) + */ + bfin_dma_channel_init(DMA_CHANNEL_EMAC_RX, 0, 4); + bfin_dma_channel_init(DMA_CHANNEL_EMAC_TX, 0, 4); + + return 0; +} + +int bfin_EMAC_init (uint8_t *ethAddr) +{ + rxIdx = txIdx = 0; + + if (lw_emac_init_registers(ethAddr) != 0) { + return -1; + } + + // Reset status words + memset ((uint8_t *)rxStatusWord, 0, sizeof(rxStatusWord)); + memset ((uint8_t *)txStatusWord, 0, sizeof(txStatusWord)); + + txDMADesc[0].next = &txDMADesc[1]; + txDMADesc[0].start_addr = &txBuffer[0]; + txDMADesc[0].config = DMA_CONFIG_DMA_EN | + DMA_CONFIG_WDSIZE(DMA_WDSIZE_32BIT) | + DMA_CONFIG_NDSIZE(5) | + DMA_CONFIG_FLOW(DMA_FLOW_DESCRIPTOR_LIST_LARGE); + + txDMADesc[1].next = &txDMADesc[0]; + txDMADesc[1].start_addr = &txStatusWord[0]; + txDMADesc[1].config = DMA_CONFIG_DMA_EN | + DMA_CONFIG_WNR | + DMA_CONFIG_WDSIZE(DMA_WDSIZE_32BIT) | + DMA_CONFIG_NDSIZE(0) | + DMA_CONFIG_FLOW(DMA_FLOW_STOP); + + rxDMADesc[0].next = &rxDMADesc[1]; + rxDMADesc[0].start_addr = &rxBuffer[0]; + rxDMADesc[0].config = DMA_CONFIG_DMA_EN | + DMA_CONFIG_WNR | + DMA_CONFIG_WDSIZE(DMA_WDSIZE_32BIT) | + DMA_CONFIG_NDSIZE(5) | + DMA_CONFIG_FLOW(DMA_FLOW_DESCRIPTOR_LIST_LARGE); + + rxDMADesc[1].next = &rxDMADesc[2]; + rxDMADesc[1].start_addr = &rxStatusWord[0]; + rxDMADesc[1].config = DMA_CONFIG_DMA_EN | + DMA_CONFIG_WNR | + DMA_CONFIG_WDSIZE(DMA_WDSIZE_32BIT) | + DMA_CONFIG_NDSIZE(5) | + DMA_CONFIG_FLOW(DMA_FLOW_DESCRIPTOR_LIST_LARGE); + + rxDMADesc[2].next = &rxDMADesc[3]; + rxDMADesc[2].start_addr = &rxBuffer[1]; + rxDMADesc[2].config = DMA_CONFIG_DMA_EN | + DMA_CONFIG_WNR | + DMA_CONFIG_WDSIZE(DMA_WDSIZE_32BIT) | + DMA_CONFIG_NDSIZE(5) | + DMA_CONFIG_FLOW(DMA_FLOW_DESCRIPTOR_LIST_LARGE); + + rxDMADesc[3].next = &rxDMADesc[0]; + rxDMADesc[3].start_addr = &rxStatusWord[1]; + rxDMADesc[3].config = DMA_CONFIG_DMA_EN | + DMA_CONFIG_WNR | + DMA_CONFIG_WDSIZE(DMA_WDSIZE_32BIT) | + DMA_CONFIG_NDSIZE(5) | + DMA_CONFIG_FLOW(DMA_FLOW_DESCRIPTOR_LIST_LARGE); + + bfin_dma_channel_enable(DMA_CHANNEL_EMAC_RX, &rxDMADesc[0]); + + /* Enable Receiving, Automatic Pad Stripping and receiving of frames with length < 64 butes */ + pEth->opmode |= ASTP | PSF | RE; + + return 0; +} +int bfin_EMAC_send (void *packet, int length) +{ + UASSERT(length > 0, EARG); + UASSERT(length < ETH_FRAME_SIZE, EARG); + + /* TODO: Check DMA Error in IRQ status */ + + while (bfin_dma_channel_interrupt_is_active (DMA_CHANNEL_EMAC_TX)); + + txBuffer[txIdx].length = length; + memcpy(txBuffer[txIdx].data, packet, length); + + bfin_dma_channel_enable(DMA_CHANNEL_EMAC_TX, txDMADesc); + pEth->opmode |= TE; + + while ((txStatusWord[txIdx] & TX_COMP) == 0); + + ASSERT(txStatusWord[txIdx] & TX_OK); + + txStatusWord[txIdx] = 0; + + ++txIdx; + + if (txIdx == ETH_TX_BUF_SIZE) { + txIdx = 0; + } + + return 0; +} + +int bfin_EMAC_recv (uint8_t * packet, size_t size) +{ + uint32_t length; + uint32_t status = rxStatusWord[rxIdx]; + + /* Check if rx frame is completed */ + if ((status & RX_COMP) == 0) { + return -1; + } + else if ((status & RX_OK) == 0) { + ASSERT(0); + /* TODO: Handle error */ + return -1; + } + else if (status & RX_DMAO) { + ASSERT(0); + /* TODO: Handle overrun */ + return -1; + } + + length = status & RX_FRLEN; + + if (size < length) { + length = size; + } + + memcpy(packet, rxBuffer[rxIdx].data, length); + + bfin_dma_channel_interrupt_clear (DMA_CHANNEL_EMAC_RX); + rxStatusWord[rxIdx] = 0; + + ++rxIdx; + + if (rxIdx == ETH_RX_BUF_SIZE) { + rxIdx = 0; + } + + return length; +} + diff --git a/soem-src/oshw/rtk/lw_mac/lw_emac.h b/soem-src/oshw/rtk/lw_mac/lw_emac.h new file mode 100644 index 0000000..24f98df --- /dev/null +++ b/soem-src/oshw/rtk/lw_mac/lw_emac.h @@ -0,0 +1,12 @@ +/* + * author: Tomas Vestelind + */ + +#ifndef LWIP_MAC_H +#define LWIP_MAC_H + +int bfin_EMAC_init(uint8_t *enetaddr); +int bfin_EMAC_send(void *packet, int length); +int bfin_EMAC_recv(uint8_t * packet, size_t size); + +#endif /* LWIP_MAC_H */ diff --git a/soem-src/oshw/rtk/nicdrv.c b/soem-src/oshw/rtk/nicdrv.c new file mode 100644 index 0000000..5568d84 --- /dev/null +++ b/soem-src/oshw/rtk/nicdrv.c @@ -0,0 +1,628 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +/** \file + * \brief + * EtherCAT RAW socket driver. + * + * Low level interface functions to send and receive EtherCAT packets. + * EtherCAT has the property that packets are only send by the master, + * and the send packets always return in the receive buffer. + * There can be multiple packets "on the wire" before they return. + * To combine the received packets with the original send packets a buffer + * system is installed. The identifier is put in the index item of the + * EtherCAT header. The index is stored and compared when a frame is received. + * If there is a match the packet can be combined with the transmit packet + * and returned to the higher level function. + * + * The socket layer can exhibit a reversal in the packet order (rare). + * If the Tx order is A-B-C the return order could be A-C-B. The indexed buffer + * will reorder the packets automatically. + * + * The "redundant" option will configure two sockets and two NIC interfaces. + * Slaves are connected to both interfaces, one on the IN port and one on the + * OUT port. Packets are send via both interfaces. Any one of the connections + * (also an interconnect) can be removed and the slaves are still serviced with + * packets. The software layer will detect the possible failure modes and + * compensate. If needed the packets from interface A are resent through interface B. + * This layer is fully transparent for the higher layers. + */ + +#include +#include +#include +#include + +#include "osal.h" +#include "oshw.h" + + +#include "lw_mac/lw_emac.h" + +#ifndef MAX +#define MAX(a,b) (((a) > (b)) ? (a) : (b)) +#define MIN(a,b) (((a) < (b)) ? (a) : (b)) +#endif + +/** Redundancy modes */ +enum +{ + /** No redundancy, single NIC mode */ + ECT_RED_NONE, + /** Double redundant NIC connection */ + ECT_RED_DOUBLE +}; + +/** Primary source MAC address used for EtherCAT. + * This address is not the MAC address used from the NIC. + * EtherCAT does not care about MAC addressing, but it is used here to + * differentiate the route the packet traverses through the EtherCAT + * segment. This is needed to find out the packet flow in redundant + * configurations. */ +const uint16 priMAC[3] = { 0x0101, 0x0101, 0x0101 }; +/** Secondary source MAC address used for EtherCAT. */ +const uint16 secMAC[3] = { 0x0404, 0x0404, 0x0404 }; + +/** second MAC word is used for identification */ +#define RX_PRIM priMAC[1] +/** second MAC word is used for identification */ +#define RX_SEC secMAC[1] + +static void ecx_clear_rxbufstat(int *rxbufstat) +{ + int i; + for(i = 0; i < EC_MAXBUF; i++) + { + rxbufstat[i] = EC_BUF_EMPTY; + } +} + +/** Basic setup to connect NIC to socket. + * @param[in] port = port context struct + * @param[in] ifname = Name of NIC device, f.e. "eth0" + * @param[in] secondary = if >0 then use secondary stack instead of primary + * @return >0 if succeeded + */ +int ecx_setupnic(ecx_portt *port, const char *ifname, int secondary) +{ + int i; + int rVal; + int *psock; + + port->getindex_mutex = mtx_create(); + port->tx_mutex = mtx_create(); + port->rx_mutex = mtx_create(); + + rVal = bfin_EMAC_init((uint8_t *)priMAC); + if (rVal != 0) + return 0; + + if (secondary) + { + /* secondary port struct available? */ + if (port->redport) + { + /* when using secondary socket it is automatically a redundant setup */ + psock = &(port->redport->sockhandle); + *psock = -1; + port->redstate = ECT_RED_DOUBLE; + port->redport->stack.sock = &(port->redport->sockhandle); + port->redport->stack.txbuf = &(port->txbuf); + port->redport->stack.txbuflength = &(port->txbuflength); + port->redport->stack.tempbuf = &(port->redport->tempinbuf); + port->redport->stack.rxbuf = &(port->redport->rxbuf); + port->redport->stack.rxbufstat = &(port->redport->rxbufstat); + port->redport->stack.rxsa = &(port->redport->rxsa); + ecx_clear_rxbufstat(&(port->redport->rxbufstat[0])); + } + else + { + /* fail */ + return 0; + } + } + else + { + port->getindex_mutex = mtx_create(); + port->tx_mutex = mtx_create(); + port->rx_mutex = mtx_create(); + port->sockhandle = -1; + port->lastidx = 0; + port->redstate = ECT_RED_NONE; + port->stack.sock = &(port->sockhandle); + port->stack.txbuf = &(port->txbuf); + port->stack.txbuflength = &(port->txbuflength); + port->stack.tempbuf = &(port->tempinbuf); + port->stack.rxbuf = &(port->rxbuf); + port->stack.rxbufstat = &(port->rxbufstat); + port->stack.rxsa = &(port->rxsa); + ecx_clear_rxbufstat(&(port->rxbufstat[0])); + psock = &(port->sockhandle); + } + + /* setup ethernet headers in tx buffers so we don't have to repeat it */ + for (i = 0; i < EC_MAXBUF; i++) + { + ec_setupheader(&(port->txbuf[i])); + port->rxbufstat[i] = EC_BUF_EMPTY; + } + ec_setupheader(&(port->txbuf2)); + + return 1; +} + +/** Close sockets used + * @param[in] port = port context struct + * @return 0 + */ +int ecx_closenic(ecx_portt *port) +{ + if (port->sockhandle >= 0) + { + close(port->sockhandle); + } + if ((port->redport) && (port->redport->sockhandle >= 0)) + { + close(port->redport->sockhandle); + } + return 0; +} + +/** Fill buffer with ethernet header structure. + * Destination MAC is always broadcast. + * Ethertype is always ETH_P_ECAT. + * @param[out] p = buffer + */ +void ec_setupheader(void *p) +{ + ec_etherheadert *bp; + bp = p; + bp->da0 = oshw_htons(0xffff); + bp->da1 = oshw_htons(0xffff); + bp->da2 = oshw_htons(0xffff); + bp->sa0 = oshw_htons(priMAC[0]); + bp->sa1 = oshw_htons(priMAC[1]); + bp->sa2 = oshw_htons(priMAC[2]); + bp->etype = oshw_htons(ETH_P_ECAT); +} + +/** Get new frame identifier index and allocate corresponding rx buffer. + * @param[in] port = port context struct + * @return new index. + */ +uint8 ecx_getindex(ecx_portt *port) +{ + uint8 idx; + uint8 cnt; + + mtx_lock (port->getindex_mutex); + + idx = port->lastidx + 1; + /* index can't be larger than buffer array */ + if (idx >= EC_MAXBUF) + { + idx = 0; + } + cnt = 0; + /* try to find unused index */ + while ((port->rxbufstat[idx] != EC_BUF_EMPTY) && (cnt < EC_MAXBUF)) + { + idx++; + cnt++; + if (idx >= EC_MAXBUF) + { + idx = 0; + } + } + port->rxbufstat[idx] = EC_BUF_ALLOC; + if (port->redstate != ECT_RED_NONE) + { + port->redport->rxbufstat[idx] = EC_BUF_ALLOC; + } + port->lastidx = idx; + + mtx_unlock (port->getindex_mutex); + + return idx; +} + +/** Set rx buffer status. + * @param[in] port = port context struct + * @param[in] idx = index in buffer array + * @param[in] bufstat = status to set + */ +void ecx_setbufstat(ecx_portt *port, uint8 idx, int bufstat) +{ + port->rxbufstat[idx] = bufstat; + if (port->redstate != ECT_RED_NONE) + { + port->redport->rxbufstat[idx] = bufstat; + } +} + +/** Transmit buffer over socket (non blocking). + * @param[in] port = port context struct + * @param[in] idx = index in tx buffer array + * @param[in] stacknumber = 0=Primary 1=Secondary stack + * @return socket send result + */ +int ecx_outframe(ecx_portt *port, uint8 idx, int stacknumber) +{ + int lp, rval; + ec_stackT *stack; + + if (!stacknumber) + { + stack = &(port->stack); + } + else + { + stack = &(port->redport->stack); + } + lp = (*stack->txbuflength)[idx]; + (*stack->rxbufstat)[idx] = EC_BUF_TX; + rval = bfin_EMAC_send((*stack->txbuf)[idx], lp); + + return rval; +} + +/** Transmit buffer over socket (non blocking). + * @param[in] port = port context struct + * @param[in] idx = index in tx buffer array + * @return socket send result + */ +int ecx_outframe_red(ecx_portt *port, uint8 idx) +{ + ec_comt *datagramP; + ec_etherheadert *ehp; + int rval; + + ehp = (ec_etherheadert *)&(port->txbuf[idx]); + /* rewrite MAC source address 1 to primary */ + ehp->sa1 = oshw_htons(priMAC[1]); + /* transmit over primary socket*/ + rval = ecx_outframe(port, idx, 0); + if (port->redstate != ECT_RED_NONE) + { + mtx_lock (port->tx_mutex); + ehp = (ec_etherheadert *)&(port->txbuf2); + /* use dummy frame for secondary socket transmit (BRD) */ + datagramP = (ec_comt*)&(port->txbuf2[ETH_HEADERSIZE]); + /* write index to frame */ + datagramP->index = idx; + /* rewrite MAC source address 1 to secondary */ + ehp->sa1 = oshw_htons(secMAC[1]); + /* transmit over secondary socket */ + //send(sockhandle2, &ec_txbuf2, ec_txbuflength2 , 0); + // OBS! redundant not ACTIVE for BFIN, just added to compile + ASSERT (0); + port->redport->rxbufstat[idx] = EC_BUF_TX; + bfin_EMAC_send(&(port->txbuf2), port->txbuflength2); + mtx_unlock (port->tx_mutex); + } + + return rval; +} + +/** Non blocking read of socket. Put frame in temporary buffer. + * @param[in] port = port context struct + * @param[in] stacknumber = 0=primary 1=secondary stack + * @return >0 if frame is available and read + */ +static int ecx_recvpkt(ecx_portt *port, int stacknumber) +{ + int lp, bytesrx; + ec_stackT *stack; + + if (!stacknumber) + { + stack = &(port->stack); + } + else + { + stack = &(port->redport->stack); + } + lp = sizeof(port->tempinbuf); + bytesrx = bfin_EMAC_recv((*stack->tempbuf), lp); + port->tempinbufs = bytesrx; + + return (bytesrx > 0); +} + +/** Non blocking receive frame function. Uses RX buffer and index to combine + * read frame with transmitted frame. To compensate for received frames that + * are out-of-order all frames are stored in their respective indexed buffer. + * If a frame was placed in the buffer previously, the function retrieves it + * from that buffer index without calling ec_recvpkt. If the requested index + * is not already in the buffer it calls ec_recvpkt to fetch it. There are + * three options now, 1 no frame read, so exit. 2 frame read but other + * than requested index, store in buffer and exit. 3 frame read with matching + * index, store in buffer, set completed flag in buffer status and exit. + * + * @param[in] port = port context struct + * @param[in] idx = requested index of frame + * @param[in] stacknumber = 0=primary 1=secondary stack + * @return Workcounter if a frame is found with corresponding index, otherwise + * EC_NOFRAME or EC_OTHERFRAME. + */ +int ecx_inframe(ecx_portt *port, uint8 idx, int stacknumber) +{ + uint16 l; + int rval; + uint8 idxf; + ec_etherheadert *ehp; + ec_comt *ecp; + ec_stackT *stack; + ec_bufT *rxbuf; + + if (!stacknumber) + { + stack = &(port->stack); + } + else + { + stack = &(port->redport->stack); + } + rval = EC_NOFRAME; + rxbuf = &(*stack->rxbuf)[idx]; + /* check if requested index is already in buffer ? */ + if ((idx < EC_MAXBUF) && ( (*stack->rxbufstat)[idx] == EC_BUF_RCVD)) + { + l = (*rxbuf)[0] + ((uint16)((*rxbuf)[1] & 0x0f) << 8); + /* return WKC */ + rval = ((*rxbuf)[l] + ((uint16)(*rxbuf)[l + 1] << 8)); + /* mark as completed */ + (*stack->rxbufstat)[idx] = EC_BUF_COMPLETE; + } + else + { + mtx_lock (port->rx_mutex); + /* non blocking call to retrieve frame from socket */ + if (ecx_recvpkt(port, stacknumber)) + { + rval = EC_OTHERFRAME; + ehp =(ec_etherheadert*)(stack->tempbuf); + /* check if it is an EtherCAT frame */ + if (ehp->etype == oshw_htons(ETH_P_ECAT)) + { + ecp =(ec_comt*)(&(*stack->tempbuf)[ETH_HEADERSIZE]); + l = etohs(ecp->elength) & 0x0fff; + idxf = ecp->index; + /* found index equals requested index ? */ + if (idxf == idx) + { + /* yes, put it in the buffer array (strip ethernet header) */ + memcpy(rxbuf, &(*stack->tempbuf)[ETH_HEADERSIZE], (*stack->txbuflength)[idx] - ETH_HEADERSIZE); + /* return WKC */ + rval = ((*rxbuf)[l] + ((uint16)((*rxbuf)[l + 1]) << 8)); + /* mark as completed */ + (*stack->rxbufstat)[idx] = EC_BUF_COMPLETE; + /* store MAC source word 1 for redundant routing info */ + (*stack->rxsa)[idx] = oshw_ntohs(ehp->sa1); + } + else + { + /* check if index exist and someone is waiting for it */ + if (idxf < EC_MAXBUF && (*stack->rxbufstat)[idxf] == EC_BUF_TX) + { + rxbuf = &(*stack->rxbuf)[idxf]; + /* put it in the buffer array (strip ethernet header) */ + memcpy(rxbuf, &(*stack->tempbuf)[ETH_HEADERSIZE], (*stack->txbuflength)[idxf] - ETH_HEADERSIZE); + /* mark as received */ + (*stack->rxbufstat)[idxf] = EC_BUF_RCVD; + (*stack->rxsa)[idxf] = oshw_ntohs(ehp->sa1); + } + else + { + /* strange things happened */ + } + } + } + } + mtx_unlock (port->rx_mutex); + + } + + /* WKC if matching frame found */ + return rval; +} + +/** Blocking redundant receive frame function. If redundant mode is not active then + * it skips the secondary stack and redundancy functions. In redundant mode it waits + * for both (primary and secondary) frames to come in. The result goes in an decision + * tree that decides, depending on the route of the packet and its possible missing arrival, + * how to reroute the original packet to get the data in an other try. + * + * @param[in] port = port context struct + * @param[in] idx = requested index of frame + * @param[in] timer = absolute timeout time + * @return Workcounter if a frame is found with corresponding index, otherwise + * EC_NOFRAME. + */ +static int ecx_waitinframe_red(ecx_portt *port, uint8 idx, osal_timert timer) +{ + int wkc = EC_NOFRAME; + int wkc2 = EC_NOFRAME; + int primrx, secrx; + + /* if not in redundant mode then always assume secondary is OK */ + if (port->redstate == ECT_RED_NONE) + { + wkc2 = 0; + } + do + { + /* only read frame if not already in */ + if (wkc <= EC_NOFRAME) + { + wkc = ecx_inframe(port, idx, 0); + } + /* only try secondary if in redundant mode */ + if (port->redstate != ECT_RED_NONE) + { + /* only read frame if not already in */ + if (wkc2 <= EC_NOFRAME) + wkc2 = ecx_inframe(port, idx, 1); + } + /* wait for both frames to arrive or timeout */ + } while (((wkc <= EC_NOFRAME) || (wkc2 <= EC_NOFRAME)) && (osal_timer_is_expired(&timer) == FALSE)); + /* only do redundant functions when in redundant mode */ + if (port->redstate != ECT_RED_NONE) + { + /* primrx if the received MAC source on primary socket */ + primrx = 0; + if (wkc > EC_NOFRAME) + { + primrx = port->rxsa[idx]; + } + /* secrx if the received MAC source on psecondary socket */ + secrx = 0; + if (wkc2 > EC_NOFRAME) + { + secrx = port->redport->rxsa[idx]; + } + /* primary socket got secondary frame and secondary socket got primary frame */ + /* normal situation in redundant mode */ + if ( ((primrx == RX_SEC) && (secrx == RX_PRIM)) ) + { + /* copy secondary buffer to primary */ + memcpy(&(port->rxbuf[idx]), &(port->redport->rxbuf[idx]), port->txbuflength[idx] - ETH_HEADERSIZE); + wkc = wkc2; + } + /* primary socket got nothing or primary frame, and secondary socket got secondary frame */ + /* we need to resend TX packet */ + if ( ((primrx == 0) && (secrx == RX_SEC)) || + ((primrx == RX_PRIM) && (secrx == RX_SEC)) ) + { + osal_timert read_timer; + + /* If both primary and secondary have partial connection retransmit the primary received + * frame over the secondary socket. The result from the secondary received frame is a combined + * frame that traversed all slaves in standard order. */ + if ( (primrx == RX_PRIM) && (secrx == RX_SEC) ) + { + /* copy primary rx to tx buffer */ + memcpy(&(port->txbuf[idx][ETH_HEADERSIZE]), &(port->rxbuf[idx]), port->txbuflength[idx] - ETH_HEADERSIZE); + } + osal_timer_start(&read_timer, EC_TIMEOUTRET); + /* resend secondary tx */ + ecx_outframe(port, idx, 1); + do + { + /* retrieve frame */ + wkc2 = ecx_inframe(port, idx, 1); + } while ((wkc2 <= EC_NOFRAME) && (osal_timer_is_expired(&read_timer) == FALSE)); + if (wkc2 > EC_NOFRAME) + { + /* copy secondary result to primary rx buffer */ + memcpy(&(port->rxbuf[idx]), &(port->redport->rxbuf[idx]), port->txbuflength[idx] - ETH_HEADERSIZE); + wkc = wkc2; + } + } + } + + /* return WKC or EC_NOFRAME */ + return wkc; +} + +/** Blocking receive frame function. Calls ec_waitinframe_red(). + * @param[in] port = port context struct + * @param[in] idx = requested index of frame + * @param[in] timeout = timeout in us + * @return Workcounter if a frame is found with corresponding index, otherwise + * EC_NOFRAME. + */ +int ecx_waitinframe(ecx_portt *port, uint8 idx, int timeout) +{ + int wkc; + osal_timert timer; + + osal_timer_start (&timer, timeout); + wkc = ecx_waitinframe_red(port, idx, timer); + + return wkc; +} + +/** Blocking send and receive frame function. Used for non processdata frames. + * A datagram is build into a frame and transmitted via this function. It waits + * for an answer and returns the workcounter. The function retries if time is + * left and the result is WKC=0 or no frame received. + * + * The function calls ec_outframe_red() and ec_waitinframe_red(). + * + * @param[in] port = port context struct + * @param[in] idx = index of frame + * @param[in] timeout = timeout in us + * @return Workcounter or EC_NOFRAME + */ +int ecx_srconfirm(ecx_portt *port, uint8 idx, int timeout) +{ + int wkc = EC_NOFRAME; + osal_timert timer; + + osal_timer_start(&timer, timeout); + do + { + osal_timert read_timer; + + /* tx frame on primary and if in redundant mode a dummy on secondary */ + ecx_outframe_red(port, idx); + osal_timer_start(&read_timer, MIN(timeout, EC_TIMEOUTRET)); + /* get frame from primary or if in redundant mode possibly from secondary */ + wkc = ecx_waitinframe_red(port, idx, read_timer); + /* wait for answer with WKC>0 or otherwise retry until timeout */ + } while ((wkc <= EC_NOFRAME) && (osal_timer_is_expired(&timer) == FALSE)); + + return wkc; +} + + +#ifdef EC_VER1 +int ec_setupnic(const char *ifname, int secondary) +{ + return ecx_setupnic(&ecx_port, ifname, secondary); +} + +int ec_closenic(void) +{ + return ecx_closenic(&ecx_port); +} + +uint8 ec_getindex(void) +{ + return ecx_getindex(&ecx_port); +} + +void ec_setbufstat(uint8 idx, int bufstat) +{ + ecx_setbufstat(&ecx_port, idx, bufstat); +} + +int ec_outframe(uint8 idx, int stacknumber) +{ + return ecx_outframe(&ecx_port, idx, stacknumber); +} + +int ec_outframe_red(uint8 idx) +{ + return ecx_outframe_red(&ecx_port, idx); +} + +int ec_inframe(uint8 idx, int stacknumber) +{ + return ecx_inframe(&ecx_port, idx, stacknumber); +} + +int ec_waitinframe(uint8 idx, int timeout) +{ + return ecx_waitinframe(&ecx_port, idx, timeout); +} + +int ec_srconfirm(uint8 idx, int timeout) +{ + return ecx_srconfirm(&ecx_port, idx, timeout); +} +#endif + diff --git a/soem-src/oshw/rtk/nicdrv.h b/soem-src/oshw/rtk/nicdrv.h new file mode 100644 index 0000000..b51fab8 --- /dev/null +++ b/soem-src/oshw/rtk/nicdrv.h @@ -0,0 +1,109 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +/** \file + * \brief + * Headerfile for nicdrv.c + */ + +#ifndef _nicdrvh_ +#define _nicdrvh_ + +/** pointer structure to Tx and Rx stacks */ +typedef struct +{ + /** socket connection used */ + int *sock; + /** tx buffer */ + ec_bufT (*txbuf)[EC_MAXBUF]; + /** tx buffer lengths */ + int (*txbuflength)[EC_MAXBUF]; + /** temporary receive buffer */ + ec_bufT *tempbuf; + /** rx buffers */ + ec_bufT (*rxbuf)[EC_MAXBUF]; + /** rx buffer status fields */ + int (*rxbufstat)[EC_MAXBUF]; + /** received MAC source address (middle word) */ + int (*rxsa)[EC_MAXBUF]; +} ec_stackT; + +/** pointer structure to buffers for redundant port */ +typedef struct +{ + ec_stackT stack; + int sockhandle; + /** rx buffers */ + ec_bufT rxbuf[EC_MAXBUF]; + /** rx buffer status */ + int rxbufstat[EC_MAXBUF]; + /** rx MAC source address */ + int rxsa[EC_MAXBUF]; + /** temporary rx buffer */ + ec_bufT tempinbuf; +} ecx_redportt; + +/** pointer structure to buffers, vars and mutexes for port instantiation */ +typedef struct +{ + ec_stackT stack; + int sockhandle; + /** rx buffers */ + ec_bufT rxbuf[EC_MAXBUF]; + /** rx buffer status */ + int rxbufstat[EC_MAXBUF]; + /** rx MAC source address */ + int rxsa[EC_MAXBUF]; + /** temporary rx buffer */ + ec_bufT tempinbuf; + /** temporary rx buffer status */ + int tempinbufs; + /** transmit buffers */ + ec_bufT txbuf[EC_MAXBUF]; + /** transmit buffer lengths */ + int txbuflength[EC_MAXBUF]; + /** temporary tx buffer */ + ec_bufT txbuf2; + /** temporary tx buffer length */ + int txbuflength2; + /** last used frame index */ + uint8 lastidx; + /** current redundancy state */ + int redstate; + /** pointer to redundancy port and buffers */ + ecx_redportt *redport; + mtx_t * getindex_mutex; + mtx_t * tx_mutex; + mtx_t * rx_mutex; +} ecx_portt; + +extern const uint16 priMAC[3]; +extern const uint16 secMAC[3]; + +#ifdef EC_VER1 +extern ecx_portt ecx_port; +extern ecx_redportt ecx_redport; + +int ec_setupnic(const char * ifname, int secondary); +int ec_closenic(void); +void ec_setbufstat(uint8 idx, int bufstat); +uint8 ec_getindex(void); +int ec_outframe(uint8 idx, int stacknumber); +int ec_outframe_red(uint8 idx); +int ec_waitinframe(uint8 idx, int timeout); +int ec_srconfirm(uint8 idx,int timeout); +#endif + +void ec_setupheader(void *p); +int ecx_setupnic(ecx_portt *port, const char * ifname, int secondary); +int ecx_closenic(ecx_portt *port); +void ecx_setbufstat(ecx_portt *port, uint8 idx, int bufstat); +uint8 ecx_getindex(ecx_portt *port); +int ecx_outframe(ecx_portt *port, uint8 idx, int stacknumber); +int ecx_outframe_red(ecx_portt *port, uint8 idx); +int ecx_waitinframe(ecx_portt *port, uint8 idx, int timeout); +int ecx_srconfirm(ecx_portt *port, uint8 idx,int timeout); + +#endif diff --git a/soem-src/oshw/rtk/oshw.c b/soem-src/oshw/rtk/oshw.c new file mode 100644 index 0000000..98a26e7 --- /dev/null +++ b/soem-src/oshw/rtk/oshw.c @@ -0,0 +1,53 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +#include "oshw.h" +#include +#include + +/** + * Host to Network byte order (i.e. to big endian). + * + * Note that Ethercat uses little endian byte order, except for the Ethernet + * header which is big endian as usual. + */ +uint16 oshw_htons(const uint16 host) +{ + uint16 network = htons (host); + return network; +} + +/** + * Network (i.e. big endian) to Host byte order. + * + * Note that Ethercat uses little endian byte order, except for the Ethernet + * header which is big endian as usual. + */ +uint16 oshw_ntohs(const uint16 network) +{ + uint16 host = ntohs (network); + return host; +} + +/* Create list over available network adapters. + * @return First element in linked list of adapters + */ +ec_adaptert * oshw_find_adapters(void) +{ + ec_adaptert * ret_adapter = NULL; + + /* TODO if needed */ + + return ret_adapter; +} + +/** Free memory allocated memory used by adapter collection. + * @param[in] adapter = First element in linked list of adapters + * EC_NOFRAME. + */ +void oshw_free_adapters(ec_adaptert * adapter) +{ + /* TODO if needed */ +} diff --git a/soem-src/oshw/rtk/oshw.h b/soem-src/oshw/rtk/oshw.h new file mode 100644 index 0000000..0eb3c0b --- /dev/null +++ b/soem-src/oshw/rtk/oshw.h @@ -0,0 +1,35 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +/** \file + * \brief + * Headerfile for oshw.c + */ + +#ifndef _oshw_ +#define _oshw_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include +#include "ethercattype.h" +#include "nicdrv.h" +#include "ethercatmain.h" + + +uint16 oshw_htons(uint16 host); +uint16 oshw_ntohs(uint16 network); + +ec_adaptert * oshw_find_adapters(void); +void oshw_free_adapters(ec_adaptert * adapter); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/soem-src/oshw/vxworks/nicdrv.c b/soem-src/oshw/vxworks/nicdrv.c new file mode 100644 index 0000000..3e37118 --- /dev/null +++ b/soem-src/oshw/vxworks/nicdrv.c @@ -0,0 +1,980 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +/** \file + * \brief + * EtherCAT VxWorks SNARF Mux driver. + * + * Low level interface functions to send and receive EtherCAT packets. + * EtherCAT has the property that packets are only send by the master, + * and the send packets always return in the receive buffer. + * There can be multiple packets "on the wire" before they return. + * To combine the received packets with the original send packets a buffer + * system is installed. The identifier is put in the index item of the + * EtherCAT header. The index is stored and compared when a frame is received. + * If there is a match the packet can be combined with the transmit packet + * and returned to the higher level function. + * + * If EtherCAT is run in parallel with normal IP traffic and EtherCAT have a + * dedicated NIC, instantiate an extra tNetX task and redirect the NIC workQ + * to be handle by the extra tNetX task, if needed raise the tNetX task prio. + * This prevents from having tNet0 becoming a bottleneck. + * + * The "redundant" option will configure two Mux drivers and two NIC interfaces. + * Slaves are connected to both interfaces, one on the IN port and one on the + * OUT port. Packets are send via both interfaces. Any one of the connections + * (also an interconnect) can be removed and the slaves are still serviced with + * packets. The software layer will detect the possible failure modes and + * compensate. If needed the packets from interface A are resend through interface B. + * This layer if fully transparent for the higher layers. + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "oshw.h" +#include "osal.h" +#include "nicdrv.h" + +#define NIC_DEBUG /* Print debugging info? */ + +// wvEvent flags +#define ECAT_RECV_FAILED 0x664 +#define ECAT_RECV_OK 0x665 +#define ECAT_RECV_RETRY_OK 0x666 +#define ECAT_STACK_RECV 0x667 +#define ECAT_SEND_START 0x675 +#define ECAT_SEND_COMPLETE 0x676 +#define ECAT_SEND_FAILED 0x677 + +#ifdef NIC_DEBUG + +#define NIC_LOGMSG(x,a,b,c,d,e,f) \ + do { \ + logMsg (x,a,b,c,d,e,f); \ + } while (0) +#define NIC_WVEVENT(a,b,c) \ + do { \ + wvEvent(a, b, c); \ + } while (0) + + +#else +#define NIC_LOGMSG(x,a,b,c,d,e,f) +#define NIC_WVEVENT(a,b,c) +#endif /* NIC_DEBUG */ + +#define IF_NAME_SIZE 8 + +/** Redundancy modes */ +enum +{ + /** No redundancy, single NIC mode */ + ECT_RED_NONE, + /** Double redundant NIC connection */ + ECT_RED_DOUBLE +}; + + +/** Primary source MAC address used for EtherCAT. + * This address is not the MAC address used from the NIC. + * EtherCAT does not care about MAC addressing, but it is used here to + * differentiate the route the packet traverses through the EtherCAT + * segment. This is needed to find out the packet flow in redundant + * configurations. */ +const uint16 priMAC[3] = { 0x0101, 0x0101, 0x0101 }; +/** Secondary source MAC address used for EtherCAT. */ +const uint16 secMAC[3] = { 0x0404, 0x0404, 0x0404 }; + +/** second MAC word is used for identification */ +#define RX_PRIM priMAC[1] +/** second MAC word is used for identification */ +#define RX_SEC secMAC[1] + +/* usec per tick for timeconversion, default to 1kHz */ +#define USECS_PER_SEC 1000000 +static unsigned int usec_per_tick = 1000; + +/** Receive hook called by Mux driver. */ +static int mux_rx_callback(void* pCookie, long type, M_BLK_ID pMblk, LL_HDR_INFO *llHdrInfo, void *muxUserArg); + +static void ecx_clear_rxbufstat(int *rxbufstat) +{ + int i; + for(i = 0; i < EC_MAXBUF; i++) + { + rxbufstat[i] = EC_BUF_EMPTY; + } +} + +void print_nicversion(void) +{ + printf("Generic is used\n"); +} + +/** Basic setup to connect NIC to socket. +* @param[in] port = port context struct +* @param[in] ifname = Name of NIC device, f.e. "gei0" +* @param[in] secondary = if >0 then use secondary stack instead of primary +* @return >0 if succeeded +*/ +int ecx_setupnic(ecx_portt *port, const char *ifname, int secondary) +{ + int i; + char ifn[IF_NAME_SIZE]; + int unit_no = -1; + ETHERCAT_PKT_DEV * pPktDev; + + /* Get systick info, sysClkRateGet return ticks per second */ + usec_per_tick = USECS_PER_SEC / sysClkRateGet(); + /* Don't allow 0 since it is used in DIV */ + if(usec_per_tick == 0) + usec_per_tick = 1; + /* Make reference to packet device struct, keep track if the packet + * device is the redundant or not. + */ + if (secondary) + { + pPktDev = &(port->redport->pktDev); + pPktDev->redundant = 1; + } + else + { + pPktDev = &(port->pktDev); + pPktDev->redundant = 0; + } + + /* Clear frame counters*/ + pPktDev->tx_count = 0; + pPktDev->rx_count = 0; + pPktDev->overrun_count = 0; + + /* Create multi-thread support semaphores */ + port->sem_get_index = semMCreate(SEM_Q_PRIORITY | SEM_INVERSION_SAFE); + + /* Get the dev name and unit from ifname + * We assume form gei1, fei0... + */ + memset(ifn,0x0,sizeof(ifn)); + + for(i=0; i < strlen(ifname);i++) + { + if(isdigit(ifname[i])) + { + strncpy(ifn, ifname, i); + unit_no = atoi(&ifname[i]); + break; + } + } + + /* Detach IP stack */ + //ipDetach(pktDev.unit,pktDev.name); + + pPktDev->port = port; + + /* Bind to mux driver for given interface, include ethercat driver pointer + * as user reference + */ + /* Bind to mux */ + pPktDev->pCookie = muxBind(ifn, + unit_no, + mux_rx_callback, + NULL, + NULL, + NULL, + MUX_PROTO_SNARF, + "ECAT SNARF", + pPktDev); + + if (pPktDev->pCookie == NULL) + { + /* fail */ + NIC_LOGMSG("ecx_setupnic: muxBind init for gei: %d failed\n", + unit_no, 2, 3, 4, 5, 6); + goto exit; + } + + /* Get reference tp END obje */ + pPktDev->endObj = endFindByName(ifn, unit_no); + + if (port->pktDev.endObj == NULL) + { + /* fail */ + NIC_LOGMSG("error_hook: endFindByName failed, device gei: %d not found\n", + unit_no, 2, 3, 4, 5, 6); + goto exit; + } + + if (secondary) + { + /* secondary port struct available? */ + if (port->redport) + { + port->redstate = ECT_RED_DOUBLE; + port->redport->stack.txbuf = &(port->txbuf); + port->redport->stack.txbuflength = &(port->txbuflength); + port->redport->stack.rxbuf = &(port->redport->rxbuf); + port->redport->stack.rxbufstat = &(port->redport->rxbufstat); + port->redport->stack.rxsa = &(port->redport->rxsa); + /* Create mailboxes for each potential EtherCAT frame index */ + for (i = 0; i < EC_MAXBUF; i++) + { + port->redport->msgQId[i] = msgQCreate(1, sizeof(M_BLK_ID), MSG_Q_FIFO); + if (port->redport->msgQId[i] == MSG_Q_ID_NULL) + { + NIC_LOGMSG("ecx_setupnic: Failed to create redundant MsgQ[%d]", + i, 2, 3, 4, 5, 6); + goto exit; + } + } + ecx_clear_rxbufstat(&(port->redport->rxbufstat[0])); + } + else + { + /* fail */ + NIC_LOGMSG("ecx_setupnic: Redundant port not allocated", + unit_no, 2, 3, 4, 5, 6); + goto exit; + } + } + else + { + port->lastidx = 0; + port->redstate = ECT_RED_NONE; + port->stack.txbuf = &(port->txbuf); + port->stack.txbuflength = &(port->txbuflength); + port->stack.rxbuf = &(port->rxbuf); + port->stack.rxbufstat = &(port->rxbufstat); + port->stack.rxsa = &(port->rxsa); + /* Create mailboxes for each potential EtherCAT frame index */ + for (i = 0; i < EC_MAXBUF; i++) + { + port->msgQId[i] = msgQCreate(1, sizeof(M_BLK_ID), MSG_Q_FIFO); + if (port->msgQId[i] == MSG_Q_ID_NULL) + { + NIC_LOGMSG("ecx_setupnic: Failed to create MsgQ[%d]", + i, 2, 3, 4, 5, 6); + goto exit; + } + } + ecx_clear_rxbufstat(&(port->rxbufstat[0])); + } + + /* setup ethernet headers in tx buffers so we don't have to repeat it */ + for (i = 0; i < EC_MAXBUF; i++) + { + ec_setupheader(&(port->txbuf[i])); + port->rxbufstat[i] = EC_BUF_EMPTY; + } + ec_setupheader(&(port->txbuf2)); + + return 1; + +exit: + + return 0; + +} + +/** Close sockets used + * @param[in] port = port context struct + * @return 0 + */ +int ecx_closenic(ecx_portt *port) +{ + int i; + ETHERCAT_PKT_DEV * pPktDev; + M_BLK_ID trash_can; + + pPktDev = &(port->pktDev); + + for (i = 0; i < EC_MAXBUF; i++) + { + if (port->msgQId[i] != MSG_Q_ID_NULL) + { + if (msgQReceive(port->msgQId[i], + (char *)&trash_can, + sizeof(M_BLK_ID), + NO_WAIT) != ERROR) + { + NIC_LOGMSG("ecx_closenic: idx %d MsgQ close\n", i, + 2, 3, 4, 5, 6); + /* Free resources */ + netMblkClChainFree(trash_can); + } + msgQDelete(port->msgQId[i]); + } + } + + if (pPktDev->pCookie != NULL) + { + muxUnbind(pPktDev->pCookie, MUX_PROTO_SNARF, mux_rx_callback); + } + + /* Clean redundant resources if available*/ + if (port->redport) + { + pPktDev = &(port->redport->pktDev); + for (i = 0; i < EC_MAXBUF; i++) + { + if (port->redport->msgQId[i] != MSG_Q_ID_NULL) + { + if (msgQReceive(port->redport->msgQId[i], + (char *)&trash_can, + sizeof(M_BLK_ID), + NO_WAIT) != ERROR) + { + NIC_LOGMSG("ecx_closenic: idx %d MsgQ close\n", i, + 2, 3, 4, 5, 6); + /* Free resources */ + netMblkClChainFree(trash_can); + } + msgQDelete(port->redport->msgQId[i]); + } + } + if (pPktDev->pCookie != NULL) + { + muxUnbind(pPktDev->pCookie, MUX_PROTO_SNARF, mux_rx_callback); + } + } + + return 0; +} + +/** Fill buffer with ethernet header structure. + * Destination MAC is always broadcast. + * Ethertype is always ETH_P_ECAT. + * @param[out] p = buffer + */ +void ec_setupheader(void *p) +{ + ec_etherheadert *bp; + bp = p; + bp->da0 = htons(0xffff); + bp->da1 = htons(0xffff); + bp->da2 = htons(0xffff); + bp->sa0 = htons(priMAC[0]); + bp->sa1 = htons(priMAC[1]); + bp->sa2 = htons(priMAC[2]); + bp->etype = htons(ETH_P_ECAT); +} + +/** Get new frame identifier index and allocate corresponding rx buffer. + * @param[in] port = port context struct + * @return new index. + */ +uint8 ecx_getindex(ecx_portt *port) +{ + uint8 idx; + uint8 cnt; + + semTake(port->sem_get_index, WAIT_FOREVER); + + idx = port->lastidx + 1; + /* index can't be larger than buffer array */ + if (idx >= EC_MAXBUF) + { + idx = 0; + } + cnt = 0; + /* try to find unused index */ + while ((port->rxbufstat[idx] != EC_BUF_EMPTY) && (cnt < EC_MAXBUF)) + { + idx++; + cnt++; + if (idx >= EC_MAXBUF) + { + idx = 0; + } + } + port->rxbufstat[idx] = EC_BUF_ALLOC; + port->lastidx = idx; + + semGive(port->sem_get_index); + + return idx; +} + +/** Set rx buffer status. + * @param[in] port = port context struct + * @param[in] idx = index in buffer array + * @param[in] bufstat = status to set + */ +void ecx_setbufstat(ecx_portt *port, uint8 idx, int bufstat) +{ + port->rxbufstat[idx] = bufstat; + if (port->redstate != ECT_RED_NONE) + port->redport->rxbufstat[idx] = bufstat; +} + +/** Low level transmit buffer over mux layer 2 driver +* +* @param[in] pPktDev = packet device to send buffer over +* @param[in] idx = index in tx buffer array +* @param[in] buf = buff to send +* @param[in] len = bytes to send +* @return driver send result +*/ +static int ec_outfram_send(ETHERCAT_PKT_DEV * pPktDev, uint8 idx, void * buf, int len) +{ + STATUS status = OK; + M_BLK_ID pPacket = NULL; + int rval = 0; + END_OBJ *endObj = (END_OBJ *)pPktDev->endObj; + MSG_Q_ID msgQId; + + /* Clean up any abandoned frames and re-use the allocated buffer*/ + msgQId = pPktDev->port->msgQId[idx]; + if(msgQNumMsgs(msgQId) > 0) + { + pPktDev->abandoned_count++; + NIC_LOGMSG("ec_outfram_send: idx %d MsgQ abandoned\n", idx, + 2, 3, 4, 5, 6); + if (msgQReceive(msgQId, + (char *)&pPacket, + sizeof(M_BLK_ID), + NO_WAIT) == ERROR) + { + pPacket = NULL; + NIC_LOGMSG("ec_outfram_send: idx %d MsgQ mBlk handled by receiver\n", idx, + 2, 3, 4, 5, 6); + } + } + + if (pPacket == NULL) + { + /* Allocate m_blk to send */ + if ((pPacket = netTupleGet(endObj->pNetPool, + len, + M_DONTWAIT, + MT_DATA, + TRUE)) == NULL) + { + NIC_LOGMSG("ec_outfram_send: Could not allocate MBLK memory!\n", 1, 2, 3, 4, 5, 6); + return ERROR; + } + } + + pPacket->mBlkHdr.mLen = len; + pPacket->mBlkHdr.mFlags |= M_PKTHDR; + pPacket->mBlkHdr.mData = pPacket->pClBlk->clNode.pClBuf; + pPacket->mBlkPktHdr.len = pPacket->m_len; + + netMblkFromBufCopy(pPacket, buf, 0, pPacket->mBlkHdr.mLen, M_DONTWAIT, NULL); + status = muxSend(endObj, pPacket); + + if (status == OK) + { + rval = len; + NIC_WVEVENT(ECAT_SEND_COMPLETE, (char *)&rval, sizeof(rval)); + } + else + { + netMblkClChainFree(pPacket); + /* fail */ + static int print_once; + if (print_once == 0) + { + NIC_LOGMSG("ec_outfram_send: failed\n", + 1, 2, 3, 4, 5, 6); + print_once = 1; + } + NIC_WVEVENT(ECAT_SEND_FAILED, (char *)&rval, sizeof(rval)); + } + + return rval; +} + +/** High level transmit buffer over mux layer 2 driver, passing buffer +* and packet device to send on as arguments +* @param[in] port = port context holding reference to packet device +* @param[in] idx = index in tx buffer array +* @param[in] stacknumber = 0=Primary 1=Secondary stack +* @return socket send result +*/ +int ecx_outframe(ecx_portt *port, uint8 idx, int stacknumber) +{ + int rval = 0; + ec_stackT *stack; + ETHERCAT_PKT_DEV * pPktDev; + + if (!stacknumber) + { + stack = &(port->stack); + pPktDev = &(port->pktDev); + } + else + { + stack = &(port->redport->stack); + pPktDev = &(port->redport->pktDev); + } + + (*stack->rxbufstat)[idx] = EC_BUF_TX; + rval = ec_outfram_send(pPktDev, idx, (char*)(*stack->txbuf)[idx], + (*stack->txbuflength)[idx]); + if (rval > 0) + { + port->pktDev.tx_count++; + } + else + { + (*stack->rxbufstat)[idx] = EC_BUF_EMPTY; + } + + return rval; +} + +/** High level transmit frame to send as index. + * @param[in] port = port context + * @param[in] idx = index in tx buffer array + * @return socket send result + */ +int ecx_outframe_red(ecx_portt *port, uint8 idx) +{ + ec_comt *datagramP; + ec_etherheadert *ehp; + int rval; + + ehp = (ec_etherheadert *)&(port->txbuf[idx]); + /* rewrite MAC source address 1 to primary */ + ehp->sa1 = htons(priMAC[1]); + /* transmit over primary socket*/ + rval = ecx_outframe(port, idx, 0); + if (port->redstate != ECT_RED_NONE) + { + ehp = (ec_etherheadert *)&(port->txbuf2); + /* use dummy frame for secondary socket transmit (BRD) */ + datagramP = (ec_comt*)&(port->txbuf2[ETH_HEADERSIZE]); + /* write index to frame */ + datagramP->index = idx; + /* rewrite MAC source address 1 to secondary */ + ehp->sa1 = htons(secMAC[1]); + /* transmit over secondary interface */ + port->redport->rxbufstat[idx] = EC_BUF_TX; + rval = ec_outfram_send(&(port->redport->pktDev), idx, &(port->txbuf2), port->txbuflength2); + if (rval <= 0) + { + port->redport->rxbufstat[idx] = EC_BUF_EMPTY; + } + } + + return rval; +} + + +/** Call back routine registered as hook with mux layer 2 driver +* @param[in] pCookie = Mux cookie +* @param[in] type = received type +* @param[in] pMblk = the received packet reference +* @param[in] llHdrInfo = header info +* @param[in] muxUserArg = assigned reference to packet device when init called +* @return TRUE if frame was successfully read and passed to MsgQ +*/ +static int mux_rx_callback(void* pCookie, long type, M_BLK_ID pMblk, LL_HDR_INFO *llHdrInfo, void *muxUserArg) +{ + BOOL ret = FALSE; + uint8 idxf; + ec_comt *ecp; + ec_bufT * tempbuf; + ecx_portt * port; + MSG_Q_ID msgQId; + ETHERCAT_PKT_DEV * pPktDev; + int length; + int bufstat; + + /* check if it is an EtherCAT frame */ + if (type == ETH_P_ECAT) + { + length = pMblk->mBlkHdr.mLen; + tempbuf = (ec_bufT *)pMblk->mBlkHdr.mData; + pPktDev = (ETHERCAT_PKT_DEV *)muxUserArg; + port = pPktDev->port; + + /* Get ethercat frame header */ + ecp = (ec_comt*)&(*tempbuf)[ETH_HEADERSIZE]; + idxf = ecp->index; + if (idxf >= EC_MAXBUF) + { + NIC_LOGMSG("mux_rx_callback: idx %d out of bounds\n", idxf, + 2, 3, 4, 5, 6); + return ret; + } + + /* Check if it is the redundant port or not */ + if (pPktDev->redundant == 1) + { + bufstat = port->redport->rxbufstat[idxf]; + msgQId = port->redport->msgQId[idxf]; + } + else + { + bufstat = port->rxbufstat[idxf]; + msgQId = port->msgQId[idxf]; + } + + /* Check length and if someone expects the frame */ + if (length > 0 && bufstat == EC_BUF_TX) + { + /* Post the frame to the receive Q for the EtherCAT stack */ + STATUS status; + status = msgQSend(msgQId, (char *)&pMblk, sizeof(M_BLK_ID), + NO_WAIT, MSG_PRI_NORMAL); + if (status == OK) + { + NIC_WVEVENT(ECAT_RECV_OK, (char *)&length, sizeof(length)); + ret = TRUE; + } + else if ((status == ERROR) && (errno == S_objLib_OBJ_UNAVAILABLE)) + { + /* Try to empty the MSGQ since we for some strange reason + * already have a frame in the MsqQ, + * is it due to timeout when receiving? + * We want the latest received frame in the buffer + */ + port->pktDev.overrun_count++; + NIC_LOGMSG("mux_rx_callback: idx %d MsgQ overrun\n", idxf, + 2, 3, 4, 5, 6); + M_BLK_ID trash_can; + if (msgQReceive(msgQId, + (char *)&trash_can, + sizeof(M_BLK_ID), + NO_WAIT) != ERROR) + { + /* Free resources */ + netMblkClChainFree(trash_can); + } + status = msgQSend(msgQId, + (char *)&pMblk, + sizeof(M_BLK_ID), + NO_WAIT, + MSG_PRI_NORMAL); + if (status == OK) + { + NIC_WVEVENT(ECAT_RECV_RETRY_OK, (char *)&length, sizeof(length)); + ret = TRUE; + } + } + else + { + NIC_WVEVENT(ECAT_RECV_FAILED, (char *)&length, sizeof(length)); + } + } + } + + return ret; +} + +/** Non blocking or blocking read, if we use timeout we pass minimum 1 tick. + * @param[in] port = port context struct + * @param[in] pMblk = mBlk for received frame + * @param[in] timeout = timeout in us + * @return >0 if frame is available and read + */ +static int ecx_recvpkt(ecx_portt *port, uint8 idx, int stacknumber, M_BLK_ID * pMblk, int timeout) +{ + int bytesrx = 0; + MSG_Q_ID msgQId; + int tick_timeout = max((timeout / usec_per_tick), 1); + + if (stacknumber == 1) + { + msgQId = port->redport->msgQId[idx]; + } + else + { + msgQId = port->msgQId[idx]; + } + + if (timeout == 0) + { + bytesrx = msgQReceive(msgQId, (void *)pMblk, + sizeof(M_BLK_ID), NO_WAIT); + } + else + { + bytesrx = msgQReceive(msgQId, (void *)pMblk, + sizeof(M_BLK_ID), tick_timeout); + } + + if (bytesrx > 0) + { + bytesrx = (*pMblk)->mBlkHdr.mLen; + NIC_WVEVENT(ECAT_STACK_RECV, (char *)&bytesrx, sizeof(bytesrx)); + } + + + return (bytesrx > 0); +} + +/** Non blocking receive frame function. Uses RX buffer and index to combine + * read frame with transmitted frame. Frames are received by separate receiver + * task tNet0 (default), tNet0 fetch what frame index and store a reference to the + * received frame in matching MsgQ. The stack user tasks fetch the frame + * reference and copies the frame the the RX buffer, when done it free + * the frame buffer allocated by the Mux. + * + * @param[in] port = port context struct + * @param[in] idx = requested index of frame + * @param[in] stacknumber = 0=primary 1=secondary stack + * @param[in] timeout = timeout in us + * @return Workcounter if a frame is found with corresponding index, otherwise + * EC_NOFRAME or EC_OTHERFRAME. + */ +int ecx_inframe(ecx_portt *port, uint8 idx, int stacknumber, int timeout) +{ + uint16 l; + int rval; + ec_etherheadert *ehp; + ec_comt *ecp; + ec_stackT *stack; + ec_bufT *rxbuf; + ec_bufT *tempinbuf; + M_BLK_ID pMblk; + + if (!stacknumber) + { + stack = &(port->stack); + } + else + { + stack = &(port->redport->stack); + } + rval = EC_NOFRAME; + rxbuf = &(*stack->rxbuf)[idx]; + /* (non-) blocking call to retrieve frame from Muxlayer */ + if (ecx_recvpkt(port, idx, stacknumber, &pMblk, timeout)) + { + rval = EC_OTHERFRAME; + + /* Get pointer to the frame */ + tempinbuf = (ec_bufT *)pMblk->mBlkHdr.mData; + /* Get pointer to the Ethernet header */ + ehp = (ec_etherheadert*)tempinbuf; + /* Get pointer to the EtherCAT frame as ethernet payload */ + ecp = (ec_comt*)&(*tempinbuf)[ETH_HEADERSIZE]; + l = etohs(ecp->elength) & 0x0fff; + /* yes, put it in the buffer array (strip ethernet header) */ + netMblkOffsetToBufCopy(pMblk, ETH_HEADERSIZE,(void *) rxbuf, + (*stack->txbuflength)[idx] - ETH_HEADERSIZE, NULL); + + /* return WKC */ + rval = ((*rxbuf)[l] + ((uint16)((*rxbuf)[l + 1]) << 8)); + /* mark as completed */ + (*stack->rxbufstat)[idx] = EC_BUF_COMPLETE; + /* store MAC source word 1 for redundant routing info */ + (*stack->rxsa)[idx] = ntohs(ehp->sa1); + netMblkClChainFree(pMblk); + port->pktDev.rx_count++; + } + + /* WKC if matching frame found */ + return rval; +} + +/** Blocking redundant receive frame function. If redundant mode is not active then + * it skips the secondary stack and redundancy functions. In redundant mode it waits + * for both (primary and secondary) frames to come in. The result goes in an decision + * tree that decides, depending on the route of the packet and its possible missing arrival, + * how to reroute the original packet to get the data in an other try. + * + * @param[in] port = port context struct + * @param[in] idx = requested index of frame + * @param[in] timer = absolute timeout time + * @param[in] timeout = timeout in us + * @return Workcounter if a frame is found with corresponding index, otherwise + * EC_NOFRAME. + */ +static int ecx_waitinframe_red(ecx_portt *port, uint8 idx, osal_timert *timer, int timeout) +{ + osal_timert timer2; + int wkc = EC_NOFRAME; + int wkc2 = EC_NOFRAME; + int primrx, secrx; + + /* if not in redundant mode then always assume secondary is OK */ + if (port->redstate == ECT_RED_NONE) + { + wkc2 = 0; + } + do + { + /* only read frame if not already in */ + if (wkc <= EC_NOFRAME) + { + wkc = ecx_inframe(port, idx, 0, timeout); + } + /* only try secondary if in redundant mode */ + if (port->redstate != ECT_RED_NONE) + { + /* only read frame if not already in */ + if (wkc2 <= EC_NOFRAME) + { + wkc2 = ecx_inframe(port, idx, 1, timeout); + } + } + /* wait for both frames to arrive or timeout */ + } while (((wkc <= EC_NOFRAME) || (wkc2 <= EC_NOFRAME)) && !osal_timer_is_expired(timer)); + /* only do redundant functions when in redundant mode */ + if (port->redstate != ECT_RED_NONE) + { + /* primrx if the received MAC source on primary socket */ + primrx = 0; + if (wkc > EC_NOFRAME) primrx = port->rxsa[idx]; + /* secrx if the received MAC source on psecondary socket */ + secrx = 0; + if (wkc2 > EC_NOFRAME) secrx = port->redport->rxsa[idx]; + + /* primary socket got secondary frame and secondary socket got primary frame */ + /* normal situation in redundant mode */ + if ( ((primrx == RX_SEC) && (secrx == RX_PRIM)) ) + { + /* copy secondary buffer to primary */ + memcpy(&(port->rxbuf[idx]), &(port->redport->rxbuf[idx]), port->txbuflength[idx] - ETH_HEADERSIZE); + wkc = wkc2; + } + /* primary socket got nothing or primary frame, and secondary socket got secondary frame */ + /* we need to resend TX packet */ + if ( ((primrx == 0) && (secrx == RX_SEC)) || + ((primrx == RX_PRIM) && (secrx == RX_SEC)) ) + { + /* If both primary and secondary have partial connection retransmit the primary received + * frame over the secondary socket. The result from the secondary received frame is a combined + * frame that traversed all slaves in standard order. */ + if ( (primrx == RX_PRIM) && (secrx == RX_SEC) ) + { + /* copy primary rx to tx buffer */ + memcpy(&(port->txbuf[idx][ETH_HEADERSIZE]), &(port->rxbuf[idx]), port->txbuflength[idx] - ETH_HEADERSIZE); + } + osal_timer_start (&timer2, EC_TIMEOUTRET); + /* resend secondary tx */ + ecx_outframe(port, idx, 1); + do + { + /* retrieve frame */ + wkc2 = ecx_inframe(port, idx, 1, timeout); + } while ((wkc2 <= EC_NOFRAME) && !osal_timer_is_expired(&timer2)); + if (wkc2 > EC_NOFRAME) + { + /* copy secondary result to primary rx buffer */ + memcpy(&(port->rxbuf[idx]), &(port->redport->rxbuf[idx]), port->txbuflength[idx] - ETH_HEADERSIZE); + wkc = wkc2; + } + } + } + + /* return WKC or EC_NOFRAME */ + return wkc; +} + +/** Blocking receive frame function. Calls ec_waitinframe_red(). + * @param[in] port = port context struct + * @param[in] idx = requested index of frame + * @param[in] timeout = timeout in us + * @return Workcounter if a frame is found with corresponding index, otherwise + * EC_NOFRAME. + */ +int ecx_waitinframe(ecx_portt *port, uint8 idx, int timeout) +{ + int wkc; + osal_timert timer; + + osal_timer_start (&timer, timeout); + wkc = ecx_waitinframe_red(port, idx, &timer, timeout); + + return wkc; +} + +/** Blocking send and receive frame function. Used for non processdata frames. + * A datagram is build into a frame and transmitted via this function. It waits + * for an answer and returns the workcounter. The function retries if time is + * left and the result is WKC=0 or no frame received. + * + * The function calls ec_outframe_red() and ec_waitinframe_red(). + * + * @param[in] port = port context struct + * @param[in] idx = index of frame + * @param[in] timeout = timeout in us + * @return Workcounter or EC_NOFRAME + */ +int ecx_srconfirm(ecx_portt *port, uint8 idx, int timeout) +{ + int wkc = EC_NOFRAME; + osal_timert timer1, timer2; + + osal_timer_start (&timer1, timeout); + do + { + /* tx frame on primary and if in redundant mode a dummy on secondary */ + ecx_outframe_red(port, idx); + if (timeout < EC_TIMEOUTRET) + { + osal_timer_start (&timer2, timeout); + } + else + { + /* normally use partial timeout for rx */ + osal_timer_start (&timer2, EC_TIMEOUTRET); + } + /* get frame from primary or if in redundant mode possibly from secondary */ + wkc = ecx_waitinframe_red(port, idx, &timer2, timeout); + /* wait for answer with WKC>=0 or otherwise retry until timeout */ + } while ((wkc <= EC_NOFRAME) && !osal_timer_is_expired (&timer1)); + + + return wkc; +} + +#ifdef EC_VER1 +int ec_setupnic(const char *ifname, int secondary) +{ + return ecx_setupnic(&ecx_port, ifname, secondary); +} + +int ec_closenic(void) +{ + return ecx_closenic(&ecx_port); +} + +uint8 ec_getindex(void) +{ + return ecx_getindex(&ecx_port); +} + +void ec_setbufstat(uint8 idx, int bufstat) +{ + ecx_setbufstat(&ecx_port, idx, bufstat); +} + +int ec_outframe(uint8 idx, int stacknumber) +{ + return ecx_outframe(&ecx_port, idx, stacknumber); +} + +int ec_outframe_red(uint8 idx) +{ + return ecx_outframe_red(&ecx_port, idx); +} + +int ec_inframe(uint8 idx, int stacknumber, int timeout) +{ + return ecx_inframe(&ecx_port, idx, stacknumber, timeout); +} + +int ec_waitinframe(uint8 idx, int timeout) +{ + return ecx_waitinframe(&ecx_port, idx, timeout); +} + +int ec_srconfirm(uint8 idx, int timeout) +{ + return ecx_srconfirm(&ecx_port, idx, timeout); +} +#endif diff --git a/soem-src/oshw/vxworks/nicdrv.h b/soem-src/oshw/vxworks/nicdrv.h new file mode 100644 index 0000000..6bb34e5 --- /dev/null +++ b/soem-src/oshw/vxworks/nicdrv.h @@ -0,0 +1,130 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +/** \file + * \brief + * Headerfile for nicdrv.c + */ + +#ifndef _nicdrvh_ +#define _nicdrvh_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include + +/** structure to connect EtherCAT stack and VxWorks device */ +typedef struct ETHERCAT_PKT_DEV +{ + struct ecx_port *port; + void *pCookie; + void *endObj; + UINT32 redundant; + UINT32 tx_count; + UINT32 rx_count; + UINT32 overrun_count; + UINT32 abandoned_count; +}ETHERCAT_PKT_DEV; + +/** pointer structure to Tx and Rx stacks */ +typedef struct +{ + /** tx buffer */ + ec_bufT (*txbuf)[EC_MAXBUF]; + /** tx buffer lengths */ + int (*txbuflength)[EC_MAXBUF]; + /** rx buffers */ + ec_bufT (*rxbuf)[EC_MAXBUF]; + /** rx buffer status fields */ + int (*rxbufstat)[EC_MAXBUF]; + /** received MAC source address (middle word) */ + int (*rxsa)[EC_MAXBUF]; +} ec_stackT; + +/** pointer structure to buffers for redundant port */ +typedef struct ecx_redport +{ + /** Stack reference */ + ec_stackT stack; + /** Packet device instance */ + ETHERCAT_PKT_DEV pktDev; + /** rx buffers */ + ec_bufT rxbuf[EC_MAXBUF]; + /** rx buffer status */ + int rxbufstat[EC_MAXBUF]; + /** rx MAC source address */ + int rxsa[EC_MAXBUF]; + /** MSG Q for receive callbacks to post into */ + MSG_Q_ID msgQId[EC_MAXBUF]; +} ecx_redportt; + +/** pointer structure to buffers, vars and mutexes for port instantiation */ +typedef struct ecx_port +{ + /** Stack reference */ + ec_stackT stack; + /** Packet device instance */ + ETHERCAT_PKT_DEV pktDev; + /** rx buffers */ + ec_bufT rxbuf[EC_MAXBUF]; + /** rx buffer status */ + int rxbufstat[EC_MAXBUF]; + /** rx MAC source address */ + int rxsa[EC_MAXBUF]; + /** transmit buffers */ + ec_bufT txbuf[EC_MAXBUF]; + /** transmit buffer lengths */ + int txbuflength[EC_MAXBUF]; + /** temporary tx buffer */ + ec_bufT txbuf2; + /** temporary tx buffer length */ + int txbuflength2; + /** last used frame index */ + uint8 lastidx; + /** current redundancy state */ + int redstate; + /** pointer to redundancy port and buffers */ + ecx_redportt *redport; + /** Semaphore to protect single resources */ + SEM_ID sem_get_index; + /** MSG Q for receive callbacks to post into */ + MSG_Q_ID msgQId[EC_MAXBUF]; +} ecx_portt; + +extern const uint16 priMAC[3]; +extern const uint16 secMAC[3]; + +#ifdef EC_VER1 +extern ecx_portt ecx_port; +extern ecx_redportt ecx_redport; + +int ec_setupnic(const char * ifname, int secondary); +int ec_closenic(void); +void ec_setbufstat(uint8 idx, int bufstat); +uint8 ec_getindex(void); +int ec_outframe(uint8 idx, int sock); +int ec_outframe_red(uint8 idx); +int ec_waitinframe(uint8 idx, int timeout); +int ec_srconfirm(uint8 idx,int timeout); +#endif + +void ec_setupheader(void *p); +int ecx_setupnic(ecx_portt *port, const char * ifname, int secondary); +int ecx_closenic(ecx_portt *port); +void ecx_setbufstat(ecx_portt *port, uint8 idx, int bufstat); +uint8 ecx_getindex(ecx_portt *port); +int ecx_outframe(ecx_portt *port, uint8 idx, int sock); +int ecx_outframe_red(ecx_portt *port, uint8 idx); +int ecx_waitinframe(ecx_portt *port, uint8 idx, int timeout); +int ecx_srconfirm(ecx_portt *port, uint8 idx,int timeout); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/soem-src/oshw/vxworks/oshw.c b/soem-src/oshw/vxworks/oshw.c new file mode 100644 index 0000000..2b5b259 --- /dev/null +++ b/soem-src/oshw/vxworks/oshw.c @@ -0,0 +1,60 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include "oshw.h" + +/** + * Host to Network byte order (i.e. to big endian). + * + * Note that Ethercat uses little endian byte order, except for the Ethernet + * header which is big endian as usual. + */ +uint16 oshw_htons(uint16 host) +{ + uint16 network = htons (host); + return network; +} + +/** + * Network (i.e. big endian) to Host byte order. + * + * Note that Ethercat uses little endian byte order, except for the Ethernet + * header which is big endian as usual. + */ +uint16 oshw_ntohs(uint16 network) +{ + uint16 host = ntohs (network); + return host; +} + +/** Create list over available network adapters. + * @return First element in linked list of adapters + */ +ec_adaptert * oshw_find_adapters(void) +{ + ec_adaptert * ret_adapter = NULL; + /* Not implemented */ + assert(0); + + return ret_adapter; +} + +/** Free memory allocated memory used by adapter collection. + * @param[in] adapter = First element in linked list of adapters + * EC_NOFRAME. + */ +void oshw_free_adapters(ec_adaptert * adapter) +{ + /* Not implemented */ + assert(0); +} diff --git a/soem-src/oshw/vxworks/oshw.h b/soem-src/oshw/vxworks/oshw.h new file mode 100644 index 0000000..3f36af6 --- /dev/null +++ b/soem-src/oshw/vxworks/oshw.h @@ -0,0 +1,23 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +/** \file + * \brief + * Headerfile for oshw.c + */ + +#ifndef _oshw_ +#define _oshw_ + +#include "ethercattype.h" +#include "nicdrv.h" +#include "ethercatmain.h" + +uint16 oshw_htons(uint16 hostshort); +uint16 oshw_ntohs(uint16 networkshort); +ec_adaptert * oshw_find_adapters(void); +void oshw_free_adapters(ec_adaptert * adapter); + +#endif diff --git a/soem-src/oshw/win32/nicdrv.c b/soem-src/oshw/win32/nicdrv.c new file mode 100644 index 0000000..96d2f0f --- /dev/null +++ b/soem-src/oshw/win32/nicdrv.c @@ -0,0 +1,648 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +/** \file + * \brief + * EtherCAT RAW socket driver. + * + * Low level interface functions to send and receive EtherCAT packets. + * EtherCAT has the property that packets are only send by the master, + * and the send packets always return in the receive buffer. + * There can be multiple packets "on the wire" before they return. + * To combine the received packets with the original send packets a buffer + * system is installed. The identifier is put in the index item of the + * EtherCAT header. The index is stored and compared when a frame is received. + * If there is a match the packet can be combined with the transmit packet + * and returned to the higher level function. + * + * The socket layer can exhibit a reversal in the packet order (rare). + * If the Tx order is A-B-C the return order could be A-C-B. The indexed buffer + * will reorder the packets automatically. + * + * The "redundant" option will configure two sockets and two NIC interfaces. + * Slaves are connected to both interfaces, one on the IN port and one on the + * OUT port. Packets are send via both interfaces. Any one of the connections + * (also an interconnect) can be removed and the slaves are still serviced with + * packets. The software layer will detect the possible failure modes and + * compensate. If needed the packets from interface A are resent through interface B. + * This layer is fully transparent for the higher layers. + */ + +#ifdef WIN32 + + +#include +#include +#include +#include + +#include +#include "ethercattype.h" +#include +#include "nicdrv.h" +#include "osal_win32.h" + +#endif + +/** Redundancy modes */ +enum +{ + /** No redundancy, single NIC mode */ + ECT_RED_NONE, + /** Double redundant NIC connection */ + ECT_RED_DOUBLE +}; + +/** Primary source MAC address used for EtherCAT. + * This address is not the MAC address used from the NIC. + * EtherCAT does not care about MAC addressing, but it is used here to + * differentiate the route the packet traverses through the EtherCAT + * segment. This is needed to fund out the packet flow in redundant + * configurations. */ +const uint16 priMAC[3] = { 0x0101, 0x0101, 0x0101 }; +/** Secondary source MAC address used for EtherCAT. */ +const uint16 secMAC[3] = { 0x0404, 0x0404, 0x0404 }; + +/** second MAC word is used for identification */ +#define RX_PRIM priMAC[1] +/** second MAC word is used for identification */ +#define RX_SEC secMAC[1] + +static char errbuf[PCAP_ERRBUF_SIZE]; + +static void ecx_clear_rxbufstat(int *rxbufstat) +{ + int i; + for(i = 0; i < EC_MAXBUF; i++) + { + rxbufstat[i] = EC_BUF_EMPTY; + } +} + +/** Basic setup to connect NIC to socket. + * @param[in] port = port context struct + * @param[in] ifname = Name of NIC device, f.e. "eth0" + * @param[in] secondary = if >0 then use secondary stack instead of primary + * @return >0 if succeeded + */ +int ecx_setupnic(ecx_portt *port, const char *ifname, int secondary) +{ + int i; + pcap_t **psock; + + if (secondary) + { + /* secondary port struct available? */ + if (port->redport) + { + /* when using secondary socket it is automatically a redundant setup */ + psock = &(port->redport->sockhandle); + *psock = NULL; + port->redstate = ECT_RED_DOUBLE; + port->redport->stack.sock = &(port->redport->sockhandle); + port->redport->stack.txbuf = &(port->txbuf); + port->redport->stack.txbuflength = &(port->txbuflength); + port->redport->stack.tempbuf = &(port->redport->tempinbuf); + port->redport->stack.rxbuf = &(port->redport->rxbuf); + port->redport->stack.rxbufstat = &(port->redport->rxbufstat); + port->redport->stack.rxsa = &(port->redport->rxsa); + ecx_clear_rxbufstat(&(port->redport->rxbufstat[0])); + } + else + { + /* fail */ + return 0; + } + } + else + { + InitializeCriticalSection(&(port->getindex_mutex)); + InitializeCriticalSection(&(port->tx_mutex)); + InitializeCriticalSection(&(port->rx_mutex)); + port->sockhandle = NULL; + port->lastidx = 0; + port->redstate = ECT_RED_NONE; + port->stack.sock = &(port->sockhandle); + port->stack.txbuf = &(port->txbuf); + port->stack.txbuflength = &(port->txbuflength); + port->stack.tempbuf = &(port->tempinbuf); + port->stack.rxbuf = &(port->rxbuf); + port->stack.rxbufstat = &(port->rxbufstat); + port->stack.rxsa = &(port->rxsa); + ecx_clear_rxbufstat(&(port->rxbufstat[0])); + psock = &(port->sockhandle); + } + /* we use pcap socket to send RAW packets in windows user mode*/ + *psock = pcap_open(ifname, 65536, PCAP_OPENFLAG_PROMISCUOUS | + PCAP_OPENFLAG_MAX_RESPONSIVENESS | + PCAP_OPENFLAG_NOCAPTURE_LOCAL, -1, NULL , errbuf); + if (NULL == *psock) + { + printf("interface %s could not open with pcap\n", ifname); + return 0; + } + + for (i = 0; i < EC_MAXBUF; i++) + { + ec_setupheader(&(port->txbuf[i])); + port->rxbufstat[i] = EC_BUF_EMPTY; + } + ec_setupheader(&(port->txbuf2)); + + return 1; +} + +/** Close sockets used + * @param[in] port = port context struct + * @return 0 + */ +int ecx_closenic(ecx_portt *port) +{ + timeEndPeriod(1); + + if (port->sockhandle != NULL) + { + DeleteCriticalSection(&(port->getindex_mutex)); + DeleteCriticalSection(&(port->tx_mutex)); + DeleteCriticalSection(&(port->rx_mutex)); + pcap_close(port->sockhandle); + port->sockhandle = NULL; + } + if ((port->redport) && (port->redport->sockhandle != NULL)) + { + pcap_close(port->redport->sockhandle); + port->redport->sockhandle = NULL; + } + + return 0; +} + +/** Fill buffer with ethernet header structure. + * Destination MAC is always broadcast. + * Ethertype is always ETH_P_ECAT. + * @param[out] p = buffer + */ +void ec_setupheader(void *p) +{ + ec_etherheadert *bp; + bp = p; + bp->da0 = htons(0xffff); + bp->da1 = htons(0xffff); + bp->da2 = htons(0xffff); + bp->sa0 = htons(priMAC[0]); + bp->sa1 = htons(priMAC[1]); + bp->sa2 = htons(priMAC[2]); + bp->etype = htons(ETH_P_ECAT); +} + +/** Get new frame identifier index and allocate corresponding rx buffer. + * @param[in] port = port context struct + * @return new index. + */ +uint8 ecx_getindex(ecx_portt *port) +{ + uint8 idx; + uint8 cnt; + + EnterCriticalSection(&(port->getindex_mutex)); + + idx = port->lastidx + 1; + /* index can't be larger than buffer array */ + if (idx >= EC_MAXBUF) + { + idx = 0; + } + cnt = 0; + /* try to find unused index */ + while ((port->rxbufstat[idx] != EC_BUF_EMPTY) && (cnt < EC_MAXBUF)) + { + idx++; + cnt++; + if (idx >= EC_MAXBUF) + { + idx = 0; + } + } + port->rxbufstat[idx] = EC_BUF_ALLOC; + if (port->redstate != ECT_RED_NONE) + port->redport->rxbufstat[idx] = EC_BUF_ALLOC; + port->lastidx = idx; + + LeaveCriticalSection(&(port->getindex_mutex)); + + return idx; +} + +/** Set rx buffer status. + * @param[in] port = port context struct + * @param[in] idx = index in buffer array + * @param[in] bufstat = status to set + */ +void ecx_setbufstat(ecx_portt *port, uint8 idx, int bufstat) +{ + port->rxbufstat[idx] = bufstat; + if (port->redstate != ECT_RED_NONE) + port->redport->rxbufstat[idx] = bufstat; +} + +/** Transmit buffer over socket (non blocking). + * @param[in] port = port context struct + * @param[in] idx = index in tx buffer array + * @param[in] stacknumber = 0=Primary 1=Secondary stack + * @return socket send result + */ +int ecx_outframe(ecx_portt *port, uint8 idx, int stacknumber) +{ + int lp, rval; + ec_stackT *stack; + + if (!stacknumber) + { + stack = &(port->stack); + } + else + { + stack = &(port->redport->stack); + } + lp = (*stack->txbuflength)[idx]; + (*stack->rxbufstat)[idx] = EC_BUF_TX; + rval = pcap_sendpacket(*stack->sock, (*stack->txbuf)[idx], lp); + if (rval == PCAP_ERROR) + { + (*stack->rxbufstat)[idx] = EC_BUF_EMPTY; + } + + return rval; +} + +/** Transmit buffer over socket (non blocking). + * @param[in] port = port context struct + * @param[in] idx = index in tx buffer array + * @return socket send result + */ +int ecx_outframe_red(ecx_portt *port, uint8 idx) +{ + ec_comt *datagramP; + ec_etherheadert *ehp; + int rval; + + ehp = (ec_etherheadert *)&(port->txbuf[idx]); + /* rewrite MAC source address 1 to primary */ + ehp->sa1 = htons(priMAC[1]); + /* transmit over primary socket*/ + rval = ecx_outframe(port, idx, 0); + if (port->redstate != ECT_RED_NONE) + { + EnterCriticalSection( &(port->tx_mutex) ); + ehp = (ec_etherheadert *)&(port->txbuf2); + /* use dummy frame for secondary socket transmit (BRD) */ + datagramP = (ec_comt*)&(port->txbuf2[ETH_HEADERSIZE]); + /* write index to frame */ + datagramP->index = idx; + /* rewrite MAC source address 1 to secondary */ + ehp->sa1 = htons(secMAC[1]); + /* transmit over secondary socket */ + port->redport->rxbufstat[idx] = EC_BUF_TX; + if (pcap_sendpacket(port->redport->sockhandle, (u_char const *)&(port->txbuf2), port->txbuflength2) == PCAP_ERROR) + { + port->redport->rxbufstat[idx] = EC_BUF_EMPTY; + } + LeaveCriticalSection( &(port->tx_mutex) ); + } + + return rval; +} + +/** Non blocking read of socket. Put frame in temporary buffer. + * @param[in] port = port context struct + * @param[in] stacknumber = 0=primary 1=secondary stack + * @return >0 if frame is available and read + */ +static int ecx_recvpkt(ecx_portt *port, int stacknumber) +{ + int lp, bytesrx; + ec_stackT *stack; + struct pcap_pkthdr * header; + unsigned char const * pkt_data; + int res; + + if (!stacknumber) + { + stack = &(port->stack); + } + else + { + stack = &(port->redport->stack); + } + lp = sizeof(port->tempinbuf); + + res = pcap_next_ex(*stack->sock, &header, &pkt_data); + if (res <=0 ) + { + port->tempinbufs = 0; + return 0; + } + bytesrx = header->len; + if (bytesrx > lp) + { + bytesrx = lp; + } + memcpy(*stack->tempbuf, pkt_data, bytesrx); + port->tempinbufs = bytesrx; + return (bytesrx > 0); +} + +/** Non blocking receive frame function. Uses RX buffer and index to combine + * read frame with transmitted frame. To compensate for received frames that + * are out-of-order all frames are stored in their respective indexed buffer. + * If a frame was placed in the buffer previously, the function retrieves it + * from that buffer index without calling ec_recvpkt. If the requested index + * is not already in the buffer it calls ec_recvpkt to fetch it. There are + * three options now, 1 no frame read, so exit. 2 frame read but other + * than requested index, store in buffer and exit. 3 frame read with matching + * index, store in buffer, set completed flag in buffer status and exit. + * + * @param[in] port = port context struct + * @param[in] idx = requested index of frame + * @param[in] stacknumber = 0=primary 1=secondary stack + * @return Workcounter if a frame is found with corresponding index, otherwise + * EC_NOFRAME or EC_OTHERFRAME. + */ +int ecx_inframe(ecx_portt *port, uint8 idx, int stacknumber) +{ + uint16 l; + int rval; + uint8 idxf; + ec_etherheadert *ehp; + ec_comt *ecp; + ec_stackT *stack; + ec_bufT *rxbuf; + + if (!stacknumber) + { + stack = &(port->stack); + } + else + { + stack = &(port->redport->stack); + } + rval = EC_NOFRAME; + rxbuf = &(*stack->rxbuf)[idx]; + /* check if requested index is already in buffer ? */ + if ((idx < EC_MAXBUF) && ((*stack->rxbufstat)[idx] == EC_BUF_RCVD)) + { + l = (*rxbuf)[0] + ((uint16)((*rxbuf)[1] & 0x0f) << 8); + /* return WKC */ + rval = ((*rxbuf)[l] + ((uint16)(*rxbuf)[l + 1] << 8)); + /* mark as completed */ + (*stack->rxbufstat)[idx] = EC_BUF_COMPLETE; + } + else + { + EnterCriticalSection(&(port->rx_mutex)); + /* non blocking call to retrieve frame from socket */ + if (ecx_recvpkt(port, stacknumber)) + { + rval = EC_OTHERFRAME; + ehp =(ec_etherheadert*)(stack->tempbuf); + /* check if it is an EtherCAT frame */ + if (ehp->etype == htons(ETH_P_ECAT)) + { + ecp =(ec_comt*)(&(*stack->tempbuf)[ETH_HEADERSIZE]); + l = etohs(ecp->elength) & 0x0fff; + idxf = ecp->index; + /* found index equals requested index ? */ + if (idxf == idx) + { + /* yes, put it in the buffer array (strip ethernet header) */ + memcpy(rxbuf, &(*stack->tempbuf)[ETH_HEADERSIZE], (*stack->txbuflength)[idx] - ETH_HEADERSIZE); + /* return WKC */ + rval = ((*rxbuf)[l] + ((uint16)((*rxbuf)[l + 1]) << 8)); + /* mark as completed */ + (*stack->rxbufstat)[idx] = EC_BUF_COMPLETE; + /* store MAC source word 1 for redundant routing info */ + (*stack->rxsa)[idx] = ntohs(ehp->sa1); + } + else + { + /* check if index exist and someone is waiting for it */ + if (idxf < EC_MAXBUF && (*stack->rxbufstat)[idxf] == EC_BUF_TX) + { + rxbuf = &(*stack->rxbuf)[idxf]; + /* put it in the buffer array (strip ethernet header) */ + memcpy(rxbuf, &(*stack->tempbuf)[ETH_HEADERSIZE], (*stack->txbuflength)[idxf] - ETH_HEADERSIZE); + /* mark as received */ + (*stack->rxbufstat)[idxf] = EC_BUF_RCVD; + (*stack->rxsa)[idxf] = ntohs(ehp->sa1); + } + else + { + /* strange things happened */ + } + } + } + } + LeaveCriticalSection( &(port->rx_mutex) ); + + } + + /* WKC if matching frame found */ + return rval; +} + +/** Blocking redundant receive frame function. If redundant mode is not active then + * it skips the secondary stack and redundancy functions. In redundant mode it waits + * for both (primary and secondary) frames to come in. The result goes in an decision + * tree that decides, depending on the route of the packet and its possible missing arrival, + * how to reroute the original packet to get the data in an other try. + * + * @param[in] port = port context struct + * @param[in] idx = requested index of frame + * @param[in] tvs = timeout + * @return Workcounter if a frame is found with corresponding index, otherwise + * EC_NOFRAME. + */ +static int ecx_waitinframe_red(ecx_portt *port, uint8 idx, osal_timert *timer) +{ + osal_timert timer2; + int wkc = EC_NOFRAME; + int wkc2 = EC_NOFRAME; + int primrx, secrx; + + /* if not in redundant mode then always assume secondary is OK */ + if (port->redstate == ECT_RED_NONE) + wkc2 = 0; + do + { + /* only read frame if not already in */ + if (wkc <= EC_NOFRAME) + wkc = ecx_inframe(port, idx, 0); + /* only try secondary if in redundant mode */ + if (port->redstate != ECT_RED_NONE) + { + /* only read frame if not already in */ + if (wkc2 <= EC_NOFRAME) + wkc2 = ecx_inframe(port, idx, 1); + } + /* wait for both frames to arrive or timeout */ + } while (((wkc <= EC_NOFRAME) || (wkc2 <= EC_NOFRAME)) && !osal_timer_is_expired(timer)); + /* only do redundant functions when in redundant mode */ + if (port->redstate != ECT_RED_NONE) + { + /* primrx if the received MAC source on primary socket */ + primrx = 0; + if (wkc > EC_NOFRAME) primrx = port->rxsa[idx]; + /* secrx if the received MAC source on psecondary socket */ + secrx = 0; + if (wkc2 > EC_NOFRAME) secrx = port->redport->rxsa[idx]; + + /* primary socket got secondary frame and secondary socket got primary frame */ + /* normal situation in redundant mode */ + if ( ((primrx == RX_SEC) && (secrx == RX_PRIM)) ) + { + /* copy secondary buffer to primary */ + memcpy(&(port->rxbuf[idx]), &(port->redport->rxbuf[idx]), port->txbuflength[idx] - ETH_HEADERSIZE); + wkc = wkc2; + } + /* primary socket got nothing or primary frame, and secondary socket got secondary frame */ + /* we need to resend TX packet */ + if ( ((primrx == 0) && (secrx == RX_SEC)) || + ((primrx == RX_PRIM) && (secrx == RX_SEC)) ) + { + /* If both primary and secondary have partial connection retransmit the primary received + * frame over the secondary socket. The result from the secondary received frame is a combined + * frame that traversed all slaves in standard order. */ + if ( (primrx == RX_PRIM) && (secrx == RX_SEC) ) + { + /* copy primary rx to tx buffer */ + memcpy(&(port->txbuf[idx][ETH_HEADERSIZE]), &(port->rxbuf[idx]), port->txbuflength[idx] - ETH_HEADERSIZE); + } + osal_timer_start (&timer2, EC_TIMEOUTRET); + /* resend secondary tx */ + ecx_outframe(port, idx, 1); + do + { + /* retrieve frame */ + wkc2 = ecx_inframe(port, idx, 1); + } while ((wkc2 <= EC_NOFRAME) && !osal_timer_is_expired(&timer2)); + if (wkc2 > EC_NOFRAME) + { + /* copy secondary result to primary rx buffer */ + memcpy(&(port->rxbuf[idx]), &(port->redport->rxbuf[idx]), port->txbuflength[idx] - ETH_HEADERSIZE); + wkc = wkc2; + } + } + } + + /* return WKC or EC_NOFRAME */ + return wkc; +} + +/** Blocking receive frame function. Calls ec_waitinframe_red(). + * @param[in] port = port context struct + * @param[in] idx = requested index of frame + * @param[in] timeout = timeout in us + * @return Workcounter if a frame is found with corresponding index, otherwise + * EC_NOFRAME. + */ +int ecx_waitinframe(ecx_portt *port, uint8 idx, int timeout) +{ + int wkc; + osal_timert timer; + + osal_timer_start (&timer, timeout); + wkc = ecx_waitinframe_red(port, idx, &timer); + + return wkc; +} + +/** Blocking send and receive frame function. Used for non processdata frames. + * A datagram is build into a frame and transmitted via this function. It waits + * for an answer and returns the workcounter. The function retries if time is + * left and the result is WKC=0 or no frame received. + * + * The function calls ec_outframe_red() and ec_waitinframe_red(). + * + * @param[in] port = port context struct + * @param[in] idx = index of frame + * @param[in] timeout = timeout in us + * @return Workcounter or EC_NOFRAME + */ +int ecx_srconfirm(ecx_portt *port, uint8 idx, int timeout) +{ + int wkc = EC_NOFRAME; + osal_timert timer1, timer2; + + osal_timer_start (&timer1, timeout); + do + { + /* tx frame on primary and if in redundant mode a dummy on secondary */ + ecx_outframe_red(port, idx); + if (timeout < EC_TIMEOUTRET) + { + osal_timer_start (&timer2, timeout); + } + else + { + /* normally use partial timeout for rx */ + osal_timer_start (&timer2, EC_TIMEOUTRET); + } + /* get frame from primary or if in redundant mode possibly from secondary */ + wkc = ecx_waitinframe_red(port, idx, &timer2); + /* wait for answer with WKC>=0 or otherwise retry until timeout */ + } while ((wkc <= EC_NOFRAME) && !osal_timer_is_expired (&timer1)); + + return wkc; +} + + +#ifdef EC_VER1 + +int ec_setupnic(const char *ifname, int secondary) +{ + return ecx_setupnic(&ecx_port, ifname, secondary); +} + +int ec_closenic(void) +{ + return ecx_closenic(&ecx_port); +} + +uint8 ec_getindex(void) +{ + return ecx_getindex(&ecx_port); +} + +void ec_setbufstat(uint8 idx, int bufstat) +{ + ecx_setbufstat(&ecx_port, idx, bufstat); +} + +int ec_outframe(uint8 idx, int stacknumber) +{ + return ecx_outframe(&ecx_port, idx, stacknumber); +} + +int ec_outframe_red(uint8 idx) +{ + return ecx_outframe_red(&ecx_port, idx); +} + +int ec_inframe(uint8 idx, int stacknumber) +{ + return ecx_inframe(&ecx_port, idx, stacknumber); +} + +int ec_waitinframe(uint8 idx, int timeout) +{ + return ecx_waitinframe(&ecx_port, idx, timeout); +} + +int ec_srconfirm(uint8 idx, int timeout) +{ + return ecx_srconfirm(&ecx_port, idx, timeout); +} + +#endif diff --git a/soem-src/oshw/win32/nicdrv.h b/soem-src/oshw/win32/nicdrv.h new file mode 100644 index 0000000..32103a3 --- /dev/null +++ b/soem-src/oshw/win32/nicdrv.h @@ -0,0 +1,123 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +/** \file + * \brief + * Headerfile for nicdrv.c + */ + +#ifndef _nicdrvh_ +#define _nicdrvh_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#define HAVE_REMOTE + +#include +#include + +/** pointer structure to Tx and Rx stacks */ +typedef struct +{ + /** socket connection used */ + pcap_t **sock; + /** tx buffer */ + ec_bufT (*txbuf)[EC_MAXBUF]; + /** tx buffer lengths */ + int (*txbuflength)[EC_MAXBUF]; + /** temporary receive buffer */ + ec_bufT *tempbuf; + /** rx buffers */ + ec_bufT (*rxbuf)[EC_MAXBUF]; + /** rx buffer status fields */ + int (*rxbufstat)[EC_MAXBUF]; + /** received MAC source address (middle word) */ + int (*rxsa)[EC_MAXBUF]; +} ec_stackT; + +/** pointer structure to buffers for redundant port */ +typedef struct +{ + ec_stackT stack; + pcap_t *sockhandle; + /** rx buffers */ + ec_bufT rxbuf[EC_MAXBUF]; + /** rx buffer status */ + int rxbufstat[EC_MAXBUF]; + /** rx MAC source address */ + int rxsa[EC_MAXBUF]; + /** temporary rx buffer */ + ec_bufT tempinbuf; +} ecx_redportt; + +/** pointer structure to buffers, vars and mutexes for port instantiation */ +typedef struct +{ + ec_stackT stack; + pcap_t *sockhandle; + /** rx buffers */ + ec_bufT rxbuf[EC_MAXBUF]; + /** rx buffer status */ + int rxbufstat[EC_MAXBUF]; + /** rx MAC source address */ + int rxsa[EC_MAXBUF]; + /** temporary rx buffer */ + ec_bufT tempinbuf; + /** temporary rx buffer status */ + int tempinbufs; + /** transmit buffers */ + ec_bufT txbuf[EC_MAXBUF]; + /** transmit buffer lengths */ + int txbuflength[EC_MAXBUF]; + /** temporary tx buffer */ + ec_bufT txbuf2; + /** temporary tx buffer length */ + int txbuflength2; + /** last used frame index */ + uint8 lastidx; + /** current redundancy state */ + int redstate; + /** pointer to redundancy port and buffers */ + ecx_redportt *redport; + CRITICAL_SECTION getindex_mutex; + CRITICAL_SECTION tx_mutex; + CRITICAL_SECTION rx_mutex; +} ecx_portt; + +extern const uint16 priMAC[3]; +extern const uint16 secMAC[3]; + +#ifdef EC_VER1 +extern ecx_portt ecx_port; +extern ecx_redportt ecx_redport; + +int ec_setupnic(const char * ifname, int secondary); +int ec_closenic(void); +void ec_setbufstat(uint8 idx, int bufstat); +uint8 ec_getindex(void); +int ec_outframe(uint8 idx, int sock); +int ec_outframe_red(uint8 idx); +int ec_waitinframe(uint8 idx, int timeout); +int ec_srconfirm(uint8 idx,int timeout); +#endif + +void ec_setupheader(void *p); +int ecx_setupnic(ecx_portt *port, const char * ifname, int secondary); +int ecx_closenic(ecx_portt *port); +void ecx_setbufstat(ecx_portt *port, uint8 idx, int bufstat); +uint8 ecx_getindex(ecx_portt *port); +int ecx_outframe(ecx_portt *port, uint8 idx, int sock); +int ecx_outframe_red(ecx_portt *port, uint8 idx); +int ecx_waitinframe(ecx_portt *port, uint8 idx, int timeout); +int ecx_srconfirm(ecx_portt *port, uint8 idx,int timeout); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/soem-src/oshw/win32/oshw.c b/soem-src/oshw/win32/oshw.c new file mode 100644 index 0000000..3a48f6b --- /dev/null +++ b/soem-src/oshw/win32/oshw.c @@ -0,0 +1,121 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +#include "oshw.h" +#include + +/** + * Host to Network byte order (i.e. to big endian). + * + * Note that Ethercat uses little endian byte order, except for the Ethernet + * header which is big endian as usual. + */ +uint16 oshw_htons (uint16 host) +{ + uint16 network = htons (host); + return network; +} + +/** + * Network (i.e. big endian) to Host byte order. + * + * Note that Ethercat uses little endian byte order, except for the Ethernet + * header which is big endian as usual. + */ +uint16 oshw_ntohs (uint16 network) +{ + uint16 host = ntohs (network); + return host; +} + +/* Create list over available network adapters. + * @return First element in linked list of adapters + */ +ec_adaptert * oshw_find_adapters (void) +{ + int i = 0; + pcap_if_t *alldevs; + pcap_if_t *d; + ec_adaptert * adapter; + ec_adaptert * prev_adapter; + ec_adaptert * ret_adapter = NULL; + char errbuf[PCAP_ERRBUF_SIZE]; + + /* find all devices */ + if (pcap_findalldevs(&alldevs, errbuf) == -1) + { + fprintf(stderr,"Error in pcap_findalldevs_ex: %s\n", errbuf); + return (NULL); + } + /* Iterate all devices and create a local copy holding the name and + * description. + */ + for(d= alldevs; d != NULL; d= d->next) + { + adapter = (ec_adaptert *)malloc(sizeof(ec_adaptert)); + /* If we got more than one adapter save link list pointer to previous + * adapter. + * Else save as pointer to return. + */ + if (i) + { + prev_adapter->next = adapter; + } + else + { + ret_adapter = adapter; + } + + /* fetch description and name of the device from libpcap */ + adapter->next = NULL; + if (d->name) + { + strncpy(adapter->name, d->name, EC_MAXLEN_ADAPTERNAME); + adapter->name[EC_MAXLEN_ADAPTERNAME-1] = '\0'; + } + else + { + adapter->name[0] = '\0'; + } + if (d->description) + { + strncpy(adapter->desc, d->description, EC_MAXLEN_ADAPTERNAME); + adapter->desc[EC_MAXLEN_ADAPTERNAME-1] = '\0'; + } + else + { + adapter->desc[0] = '\0'; + } + prev_adapter = adapter; + i++; + } + /* free all devices allocated */ + pcap_freealldevs(alldevs); + + return ret_adapter; +} + +/** Free memory allocated memory used by adapter collection. + * @param[in] adapter = First element in linked list of adapters + * EC_NOFRAME. + */ +void oshw_free_adapters (ec_adaptert * adapter) +{ + ec_adaptert * next_adapter; + /* Iterate the linked list and free all elements holding + * adapter information + */ + if(adapter) + { + next_adapter = adapter->next; + free (adapter); + while (next_adapter) + { + adapter = next_adapter; + next_adapter = adapter->next; + free (adapter); + } + } +} diff --git a/soem-src/oshw/win32/oshw.h b/soem-src/oshw/win32/oshw.h new file mode 100644 index 0000000..dfe2efd --- /dev/null +++ b/soem-src/oshw/win32/oshw.h @@ -0,0 +1,32 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +/** \file + * \brief + * Headerfile for ethercatbase.c + */ + +#ifndef _oshw_ +#define _oshw_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include "ethercattype.h" +#include "nicdrv.h" +#include "ethercatmain.h" + +uint16 oshw_htons (uint16 hostshort); +uint16 oshw_ntohs (uint16 networkshort); +ec_adaptert * oshw_find_adapters (void); +void oshw_free_adapters (ec_adaptert * adapter); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/soem-src/oshw/win32/wpcap/Include/Packet32.h b/soem-src/oshw/win32/wpcap/Include/Packet32.h new file mode 100644 index 0000000..d47b2b5 --- /dev/null +++ b/soem-src/oshw/win32/wpcap/Include/Packet32.h @@ -0,0 +1,359 @@ +/* + * Copyright (c) 1999 - 2005 NetGroup, Politecnico di Torino (Italy) + * Copyright (c) 2005 - 2007 CACE Technologies, Davis (California) + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * 3. Neither the name of the Politecnico di Torino, CACE Technologies + * nor the names of its contributors may be used to endorse or promote + * products derived from this software without specific prior written + * permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +/** @ingroup packetapi + * @{ + */ + +/** @defgroup packet32h Packet.dll definitions and data structures + * Packet32.h contains the data structures and the definitions used by packet.dll. + * The file is used both by the Win9x and the WinNTx versions of packet.dll, and can be included + * by the applications that use the functions of this library + * @{ + */ + +#ifndef __PACKET32 +#define __PACKET32 + +#include + +#ifdef HAVE_AIRPCAP_API +#include +#else +#if !defined(AIRPCAP_HANDLE__EAE405F5_0171_9592_B3C2_C19EC426AD34__DEFINED_) +#define AIRPCAP_HANDLE__EAE405F5_0171_9592_B3C2_C19EC426AD34__DEFINED_ +typedef struct _AirpcapHandle *PAirpcapHandle; +#endif /* AIRPCAP_HANDLE__EAE405F5_0171_9592_B3C2_C19EC426AD34__DEFINED_ */ +#endif /* HAVE_AIRPCAP_API */ + +#ifdef HAVE_DAG_API +#include +#endif /* HAVE_DAG_API */ + +// Working modes +#define PACKET_MODE_CAPT 0x0 ///< Capture mode +#define PACKET_MODE_STAT 0x1 ///< Statistical mode +#define PACKET_MODE_MON 0x2 ///< Monitoring mode +#define PACKET_MODE_DUMP 0x10 ///< Dump mode +#define PACKET_MODE_STAT_DUMP MODE_DUMP | MODE_STAT ///< Statistical dump Mode + + +/// Alignment macro. Defines the alignment size. +#define Packet_ALIGNMENT sizeof(int) +/// Alignment macro. Rounds up to the next even multiple of Packet_ALIGNMENT. +#define Packet_WORDALIGN(x) (((x)+(Packet_ALIGNMENT-1))&~(Packet_ALIGNMENT-1)) + +#define NdisMediumNull -1 ///< Custom linktype: NDIS doesn't provide an equivalent +#define NdisMediumCHDLC -2 ///< Custom linktype: NDIS doesn't provide an equivalent +#define NdisMediumPPPSerial -3 ///< Custom linktype: NDIS doesn't provide an equivalent +#define NdisMediumBare80211 -4 ///< Custom linktype: NDIS doesn't provide an equivalent +#define NdisMediumRadio80211 -5 ///< Custom linktype: NDIS doesn't provide an equivalent +#define NdisMediumPpi -6 ///< Custom linktype: NDIS doesn't provide an equivalent + +// Loopback behaviour definitions +#define NPF_DISABLE_LOOPBACK 1 ///< Drop the packets sent by the NPF driver +#define NPF_ENABLE_LOOPBACK 2 ///< Capture the packets sent by the NPF driver + +/*! + \brief Network type structure. + + This structure is used by the PacketGetNetType() function to return information on the current adapter's type and speed. +*/ +typedef struct NetType +{ + UINT LinkType; ///< The MAC of the current network adapter (see function PacketGetNetType() for more information) + ULONGLONG LinkSpeed; ///< The speed of the network in bits per second +}NetType; + + +//some definitions stolen from libpcap + +#ifndef BPF_MAJOR_VERSION + +/*! + \brief A BPF pseudo-assembly program. + + The program will be injected in the kernel by the PacketSetBPF() function and applied to every incoming packet. +*/ +struct bpf_program +{ + UINT bf_len; ///< Indicates the number of instructions of the program, i.e. the number of struct bpf_insn that will follow. + struct bpf_insn *bf_insns; ///< A pointer to the first instruction of the program. +}; + +/*! + \brief A single BPF pseudo-instruction. + + bpf_insn contains a single instruction for the BPF register-machine. It is used to send a filter program to the driver. +*/ +struct bpf_insn +{ + USHORT code; ///< Instruction type and addressing mode. + UCHAR jt; ///< Jump if true + UCHAR jf; ///< Jump if false + int k; ///< Generic field used for various purposes. +}; + +/*! + \brief Structure that contains a couple of statistics values on the current capture. + + It is used by packet.dll to return statistics about a capture session. +*/ +struct bpf_stat +{ + UINT bs_recv; ///< Number of packets that the driver received from the network adapter + ///< from the beginning of the current capture. This value includes the packets + ///< lost by the driver. + UINT bs_drop; ///< number of packets that the driver lost from the beginning of a capture. + ///< Basically, a packet is lost when the the buffer of the driver is full. + ///< In this situation the packet cannot be stored and the driver rejects it. + UINT ps_ifdrop; ///< drops by interface. XXX not yet supported + UINT bs_capt; ///< number of packets that pass the filter, find place in the kernel buffer and + ///< thus reach the application. +}; + +/*! + \brief Packet header. + + This structure defines the header associated with every packet delivered to the application. +*/ +struct bpf_hdr +{ + struct timeval bh_tstamp; ///< The timestamp associated with the captured packet. + ///< It is stored in a TimeVal structure. + UINT bh_caplen; ///< Length of captured portion. The captured portion can be different + ///< from the original packet, because it is possible (with a proper filter) + ///< to instruct the driver to capture only a portion of the packets. + UINT bh_datalen; ///< Original length of packet + USHORT bh_hdrlen; ///< Length of bpf header (this struct plus alignment padding). In some cases, + ///< a padding could be added between the end of this structure and the packet + ///< data for performance reasons. This filed can be used to retrieve the actual data + ///< of the packet. +}; + +/*! + \brief Dump packet header. + + This structure defines the header associated with the packets in a buffer to be used with PacketSendPackets(). + It is simpler than the bpf_hdr, because it corresponds to the header associated by WinPcap and libpcap to a + packet in a dump file. This makes straightforward sending WinPcap dump files to the network. +*/ +struct dump_bpf_hdr{ + struct timeval ts; ///< Time stamp of the packet + UINT caplen; ///< Length of captured portion. The captured portion can smaller than the + ///< the original packet, because it is possible (with a proper filter) to + ///< instruct the driver to capture only a portion of the packets. + UINT len; ///< Length of the original packet (off wire). +}; + + +#endif + +struct bpf_stat; + +#define DOSNAMEPREFIX TEXT("Packet_") ///< Prefix added to the adapters device names to create the WinPcap devices +#define MAX_LINK_NAME_LENGTH 64 //< Maximum length of the devices symbolic links +#define NMAX_PACKET 65535 + +/*! + \brief Addresses of a network adapter. + + This structure is used by the PacketGetNetInfoEx() function to return the IP addresses associated with + an adapter. +*/ +typedef struct npf_if_addr { + struct sockaddr_storage IPAddress; ///< IP address. + struct sockaddr_storage SubnetMask; ///< Netmask for that address. + struct sockaddr_storage Broadcast; ///< Broadcast address. +}npf_if_addr; + + +#define ADAPTER_NAME_LENGTH 256 + 12 ///< Maximum length for the name of an adapter. The value is the same used by the IP Helper API. +#define ADAPTER_DESC_LENGTH 128 ///< Maximum length for the description of an adapter. The value is the same used by the IP Helper API. +#define MAX_MAC_ADDR_LENGTH 8 ///< Maximum length for the link layer address of an adapter. The value is the same used by the IP Helper API. +#define MAX_NETWORK_ADDRESSES 16 ///< Maximum length for the link layer address of an adapter. The value is the same used by the IP Helper API. + + +typedef struct WAN_ADAPTER_INT WAN_ADAPTER; ///< Describes an opened wan (dialup, VPN...) network adapter using the NetMon API +typedef WAN_ADAPTER *PWAN_ADAPTER; ///< Describes an opened wan (dialup, VPN...) network adapter using the NetMon API + +#define INFO_FLAG_NDIS_ADAPTER 0 ///< Flag for ADAPTER_INFO: this is a traditional ndis adapter +#define INFO_FLAG_NDISWAN_ADAPTER 1 ///< Flag for ADAPTER_INFO: this is a NdisWan adapter, and it's managed by WANPACKET +#define INFO_FLAG_DAG_CARD 2 ///< Flag for ADAPTER_INFO: this is a DAG card +#define INFO_FLAG_DAG_FILE 6 ///< Flag for ADAPTER_INFO: this is a DAG file +#define INFO_FLAG_DONT_EXPORT 8 ///< Flag for ADAPTER_INFO: when this flag is set, the adapter will not be listed or openend by winpcap. This allows to prevent exporting broken network adapters, like for example FireWire ones. +#define INFO_FLAG_AIRPCAP_CARD 16 ///< Flag for ADAPTER_INFO: this is an airpcap card +#define INFO_FLAG_NPFIM_DEVICE 32 + +/*! + \brief Describes an opened network adapter. + + This structure is the most important for the functioning of packet.dll, but the great part of its fields + should be ignored by the user, since the library offers functions that avoid to cope with low-level parameters +*/ +typedef struct _ADAPTER { + HANDLE hFile; ///< \internal Handle to an open instance of the NPF driver. + CHAR SymbolicLink[MAX_LINK_NAME_LENGTH]; ///< \internal A string containing the name of the network adapter currently opened. + int NumWrites; ///< \internal Number of times a packets written on this adapter will be repeated + ///< on the wire. + HANDLE ReadEvent; ///< A notification event associated with the read calls on the adapter. + ///< It can be passed to standard Win32 functions (like WaitForSingleObject + ///< or WaitForMultipleObjects) to wait until the driver's buffer contains some + ///< data. It is particularly useful in GUI applications that need to wait + ///< concurrently on several events. In Windows NT/2000 the PacketSetMinToCopy() + ///< function can be used to define the minimum amount of data in the kernel buffer + ///< that will cause the event to be signalled. + + UINT ReadTimeOut; ///< \internal The amount of time after which a read on the driver will be released and + ///< ReadEvent will be signaled, also if no packets were captured + CHAR Name[ADAPTER_NAME_LENGTH]; + PWAN_ADAPTER pWanAdapter; + UINT Flags; ///< Adapter's flags. Tell if this adapter must be treated in a different way, using the Netmon API or the dagc API. + +#ifdef HAVE_AIRPCAP_API + PAirpcapHandle AirpcapAd; +#endif // HAVE_AIRPCAP_API + +#ifdef HAVE_NPFIM_API + void* NpfImHandle; +#endif // HAVE_NPFIM_API + +#ifdef HAVE_DAG_API + dagc_t *pDagCard; ///< Pointer to the dagc API adapter descriptor for this adapter + PCHAR DagBuffer; ///< Pointer to the buffer with the packets that is received from the DAG card + struct timeval DagReadTimeout; ///< Read timeout. The dagc API requires a timeval structure + unsigned DagFcsLen; ///< Length of the frame check sequence attached to any packet by the card. Obtained from the registry + DWORD DagFastProcess; ///< True if the user requests fast capture processing on this card. Higher level applications can use this value to provide a faster but possibly unprecise capture (for example, libpcap doesn't convert the timestamps). +#endif // HAVE_DAG_API +} ADAPTER, *LPADAPTER; + +/*! + \brief Structure that contains a group of packets coming from the driver. + + This structure defines the header associated with every packet delivered to the application. +*/ +typedef struct _PACKET { + HANDLE hEvent; ///< \deprecated Still present for compatibility with old applications. + OVERLAPPED OverLapped; ///< \deprecated Still present for compatibility with old applications. + PVOID Buffer; ///< Buffer with containing the packets. See the PacketReceivePacket() for + ///< details about the organization of the data in this buffer + UINT Length; ///< Length of the buffer + DWORD ulBytesReceived; ///< Number of valid bytes present in the buffer, i.e. amount of data + ///< received by the last call to PacketReceivePacket() + BOOLEAN bIoComplete; ///< \deprecated Still present for compatibility with old applications. +} PACKET, *LPPACKET; + +/*! + \brief Structure containing an OID request. + + It is used by the PacketRequest() function to send an OID to the interface card driver. + It can be used, for example, to retrieve the status of the error counters on the adapter, its MAC address, + the list of the multicast groups defined on it, and so on. +*/ +struct _PACKET_OID_DATA { + ULONG Oid; ///< OID code. See the Microsoft DDK documentation or the file ntddndis.h + ///< for a complete list of valid codes. + ULONG Length; ///< Length of the data field + UCHAR Data[1]; ///< variable-length field that contains the information passed to or received + ///< from the adapter. +}; +typedef struct _PACKET_OID_DATA PACKET_OID_DATA, *PPACKET_OID_DATA; + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * @} + */ + +/* +BOOLEAN QueryWinPcapRegistryStringA(CHAR *SubKeyName, + CHAR *Value, + UINT *pValueLen, + CHAR *DefaultVal); + +BOOLEAN QueryWinPcapRegistryStringW(WCHAR *SubKeyName, + WCHAR *Value, + UINT *pValueLen, + WCHAR *DefaultVal); +*/ + +//--------------------------------------------------------------------------- +// EXPORTED FUNCTIONS +//--------------------------------------------------------------------------- + +PCHAR PacketGetVersion(); +PCHAR PacketGetDriverVersion(); +BOOLEAN PacketSetMinToCopy(LPADAPTER AdapterObject,int nbytes); +BOOLEAN PacketSetNumWrites(LPADAPTER AdapterObject,int nwrites); +BOOLEAN PacketSetMode(LPADAPTER AdapterObject,int mode); +BOOLEAN PacketSetReadTimeout(LPADAPTER AdapterObject,int timeout); +BOOLEAN PacketSetBpf(LPADAPTER AdapterObject,struct bpf_program *fp); +BOOLEAN PacketSetLoopbackBehavior(LPADAPTER AdapterObject, UINT LoopbackBehavior); +INT PacketSetSnapLen(LPADAPTER AdapterObject,int snaplen); +BOOLEAN PacketGetStats(LPADAPTER AdapterObject,struct bpf_stat *s); +BOOLEAN PacketGetStatsEx(LPADAPTER AdapterObject,struct bpf_stat *s); +BOOLEAN PacketSetBuff(LPADAPTER AdapterObject,int dim); +BOOLEAN PacketGetNetType (LPADAPTER AdapterObject,NetType *type); +LPADAPTER PacketOpenAdapter(PCHAR AdapterName); +BOOLEAN PacketSendPacket(LPADAPTER AdapterObject,LPPACKET pPacket,BOOLEAN Sync); +INT PacketSendPackets(LPADAPTER AdapterObject,PVOID PacketBuff,ULONG Size, BOOLEAN Sync); +LPPACKET PacketAllocatePacket(void); +VOID PacketInitPacket(LPPACKET lpPacket,PVOID Buffer,UINT Length); +VOID PacketFreePacket(LPPACKET lpPacket); +BOOLEAN PacketReceivePacket(LPADAPTER AdapterObject,LPPACKET lpPacket,BOOLEAN Sync); +BOOLEAN PacketSetHwFilter(LPADAPTER AdapterObject,ULONG Filter); +BOOLEAN PacketGetAdapterNames(PTSTR pStr,PULONG BufferSize); +BOOLEAN PacketGetNetInfoEx(PCHAR AdapterName, npf_if_addr* buffer, PLONG NEntries); +BOOLEAN PacketRequest(LPADAPTER AdapterObject,BOOLEAN Set,PPACKET_OID_DATA OidData); +HANDLE PacketGetReadEvent(LPADAPTER AdapterObject); +BOOLEAN PacketSetDumpName(LPADAPTER AdapterObject, void *name, int len); +BOOLEAN PacketSetDumpLimits(LPADAPTER AdapterObject, UINT maxfilesize, UINT maxnpacks); +BOOLEAN PacketIsDumpEnded(LPADAPTER AdapterObject, BOOLEAN sync); +BOOL PacketStopDriver(); +VOID PacketCloseAdapter(LPADAPTER lpAdapter); +BOOLEAN PacketStartOem(PCHAR errorString, UINT errorStringLength); +BOOLEAN PacketStartOemEx(PCHAR errorString, UINT errorStringLength, ULONG flags); +PAirpcapHandle PacketGetAirPcapHandle(LPADAPTER AdapterObject); + +// +// Used by PacketStartOemEx +// +#define PACKET_START_OEM_NO_NETMON 0x00000001 + +#ifdef __cplusplus +} +#endif + +#endif //__PACKET32 diff --git a/soem-src/oshw/win32/wpcap/Include/Win32-Extensions.h b/soem-src/oshw/win32/wpcap/Include/Win32-Extensions.h new file mode 100644 index 0000000..968ac7f --- /dev/null +++ b/soem-src/oshw/win32/wpcap/Include/Win32-Extensions.h @@ -0,0 +1,113 @@ +/* + * Copyright (c) 1999 - 2005 NetGroup, Politecnico di Torino (Italy) + * Copyright (c) 2005 - 2006 CACE Technologies, Davis (California) + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * 3. Neither the name of the Politecnico di Torino, CACE Technologies + * nor the names of its contributors may be used to endorse or promote + * products derived from this software without specific prior written + * permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef __WIN32_EXTENSIONS_H__ +#define __WIN32_EXTENSIONS_H__ + +#ifdef __cplusplus +extern "C" { +#endif + +/* Definitions */ + +/*! + \brief A queue of raw packets that will be sent to the network with pcap_sendqueue_transmit(). +*/ +struct pcap_send_queue +{ + u_int maxlen; ///< Maximum size of the the queue, in bytes. This variable contains the size of the buffer field. + u_int len; ///< Current size of the queue, in bytes. + char *buffer; ///< Buffer containing the packets to be sent. +}; + +typedef struct pcap_send_queue pcap_send_queue; + +/*! + \brief This typedef is a support for the pcap_get_airpcap_handle() function +*/ +#if !defined(AIRPCAP_HANDLE__EAE405F5_0171_9592_B3C2_C19EC426AD34__DEFINED_) +#define AIRPCAP_HANDLE__EAE405F5_0171_9592_B3C2_C19EC426AD34__DEFINED_ +typedef struct _AirpcapHandle *PAirpcapHandle; +#endif + +#define BPF_MEM_EX_IMM 0xc0 +#define BPF_MEM_EX_IND 0xe0 + +/*used for ST*/ +#define BPF_MEM_EX 0xc0 +#define BPF_TME 0x08 + +#define BPF_LOOKUP 0x90 +#define BPF_EXECUTE 0xa0 +#define BPF_INIT 0xb0 +#define BPF_VALIDATE 0xc0 +#define BPF_SET_ACTIVE 0xd0 +#define BPF_RESET 0xe0 +#define BPF_SET_MEMORY 0x80 +#define BPF_GET_REGISTER_VALUE 0x70 +#define BPF_SET_REGISTER_VALUE 0x60 +#define BPF_SET_WORKING 0x50 +#define BPF_SET_ACTIVE_READ 0x40 +#define BPF_SET_AUTODELETION 0x30 +#define BPF_SEPARATION 0xff + +/* Prototypes */ +pcap_send_queue* pcap_sendqueue_alloc(u_int memsize); + +void pcap_sendqueue_destroy(pcap_send_queue* queue); + +int pcap_sendqueue_queue(pcap_send_queue* queue, const struct pcap_pkthdr *pkt_header, const u_char *pkt_data); + +u_int pcap_sendqueue_transmit(pcap_t *p, pcap_send_queue* queue, int sync); + +HANDLE pcap_getevent(pcap_t *p); + +struct pcap_stat *pcap_stats_ex(pcap_t *p, int *pcap_stat_size); + +int pcap_setuserbuffer(pcap_t *p, int size); + +int pcap_live_dump(pcap_t *p, char *filename, int maxsize, int maxpacks); + +int pcap_live_dump_ended(pcap_t *p, int sync); + +int pcap_offline_filter(struct bpf_program *prog, const struct pcap_pkthdr *header, const u_char *pkt_data); + +int pcap_start_oem(char* err_str, int flags); + +PAirpcapHandle pcap_get_airpcap_handle(pcap_t *p); + +#ifdef __cplusplus +} +#endif + +#endif //__WIN32_EXTENSIONS_H__ diff --git a/soem-src/oshw/win32/wpcap/Include/bittypes.h b/soem-src/oshw/win32/wpcap/Include/bittypes.h new file mode 100644 index 0000000..46e55e6 --- /dev/null +++ b/soem-src/oshw/win32/wpcap/Include/bittypes.h @@ -0,0 +1,137 @@ +/* + * Copyright (C) 1999 WIDE Project. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * 3. Neither the name of the project nor the names of its contributors + * may be used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE PROJECT AND CONTRIBUTORS ``AS IS'' AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE PROJECT OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS + * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY + * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF + * SUCH DAMAGE. + */ +#ifndef _BITTYPES_H +#define _BITTYPES_H + +#ifndef HAVE_U_INT8_T + +#if SIZEOF_CHAR == 1 +typedef unsigned char u_int8_t; +typedef signed char int8_t; +#elif SIZEOF_INT == 1 +typedef unsigned int u_int8_t; +typedef signed int int8_t; +#else /* XXX */ +#error "there's no appropriate type for u_int8_t" +#endif +#define HAVE_U_INT8_T 1 +#define HAVE_INT8_T 1 + +#endif /* HAVE_U_INT8_T */ + +#ifndef HAVE_U_INT16_T + +#if SIZEOF_SHORT == 2 +typedef unsigned short u_int16_t; +typedef signed short int16_t; +#elif SIZEOF_INT == 2 +typedef unsigned int u_int16_t; +typedef signed int int16_t; +#elif SIZEOF_CHAR == 2 +typedef unsigned char u_int16_t; +typedef signed char int16_t; +#else /* XXX */ +#error "there's no appropriate type for u_int16_t" +#endif +#define HAVE_U_INT16_T 1 +#define HAVE_INT16_T 1 + +#endif /* HAVE_U_INT16_T */ + +#ifndef HAVE_U_INT32_T + +#if SIZEOF_INT == 4 +typedef unsigned int u_int32_t; +typedef signed int int32_t; +#elif SIZEOF_LONG == 4 +typedef unsigned long u_int32_t; +typedef signed long int32_t; +#elif SIZEOF_SHORT == 4 +typedef unsigned short u_int32_t; +typedef signed short int32_t; +#else /* XXX */ +#error "there's no appropriate type for u_int32_t" +#endif +#define HAVE_U_INT32_T 1 +#define HAVE_INT32_T 1 + +#endif /* HAVE_U_INT32_T */ + +#ifndef HAVE_U_INT64_T +#if SIZEOF_LONG_LONG == 8 +typedef unsigned long long u_int64_t; +typedef long long int64_t; +#elif defined(_MSC_EXTENSIONS) +typedef unsigned _int64 u_int64_t; +typedef _int64 int64_t; +#elif SIZEOF_INT == 8 +typedef unsigned int u_int64_t; +#elif SIZEOF_LONG == 8 +typedef unsigned long u_int64_t; +#elif SIZEOF_SHORT == 8 +typedef unsigned short u_int64_t; +#else /* XXX */ +#error "there's no appropriate type for u_int64_t" +#endif + +#endif /* HAVE_U_INT64_T */ + +#ifndef PRId64 +#ifdef _MSC_EXTENSIONS +#define PRId64 "I64d" +#else /* _MSC_EXTENSIONS */ +#define PRId64 "lld" +#endif /* _MSC_EXTENSIONS */ +#endif /* PRId64 */ + +#ifndef PRIo64 +#ifdef _MSC_EXTENSIONS +#define PRIo64 "I64o" +#else /* _MSC_EXTENSIONS */ +#define PRIo64 "llo" +#endif /* _MSC_EXTENSIONS */ +#endif /* PRIo64 */ + +#ifndef PRIx64 +#ifdef _MSC_EXTENSIONS +#define PRIx64 "I64x" +#else /* _MSC_EXTENSIONS */ +#define PRIx64 "llx" +#endif /* _MSC_EXTENSIONS */ +#endif /* PRIx64 */ + +#ifndef PRIu64 +#ifdef _MSC_EXTENSIONS +#define PRIu64 "I64u" +#else /* _MSC_EXTENSIONS */ +#define PRIu64 "llu" +#endif /* _MSC_EXTENSIONS */ +#endif /* PRIu64 */ + +#endif /* _BITTYPES_H */ diff --git a/soem-src/oshw/win32/wpcap/Include/ip6_misc.h b/soem-src/oshw/win32/wpcap/Include/ip6_misc.h new file mode 100644 index 0000000..562fa61 --- /dev/null +++ b/soem-src/oshw/win32/wpcap/Include/ip6_misc.h @@ -0,0 +1,163 @@ +/* + * Copyright (c) 1993, 1994, 1997 + * The Regents of the University of California. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that: (1) source code distributions + * retain the above copyright notice and this paragraph in its entirety, (2) + * distributions including binary code include the above copyright notice and + * this paragraph in its entirety in the documentation or other materials + * provided with the distribution, and (3) all advertising materials mentioning + * features or use of this software display the following acknowledgement: + * ``This product includes software developed by the University of California, + * Lawrence Berkeley Laboratory and its contributors.'' Neither the name of + * the University nor the names of its contributors may be used to endorse + * or promote products derived from this software without specific prior + * written permission. + * THIS SOFTWARE IS PROVIDED ``AS IS'' AND WITHOUT ANY EXPRESS OR IMPLIED + * WARRANTIES, INCLUDING, WITHOUT LIMITATION, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. + * + * @(#) $Header: /tcpdump/master/libpcap/Win32/Include/ip6_misc.h,v 1.5 2006-01-22 18:02:18 gianluca Exp $ (LBL) + */ + +/* + * This file contains a collage of declarations for IPv6 from FreeBSD not present in Windows + */ + +#include + +#include + +#ifndef __MINGW32__ +#define IN_MULTICAST(a) IN_CLASSD(a) +#endif + +#define IN_EXPERIMENTAL(a) ((((u_int32_t) (a)) & 0xf0000000) == 0xf0000000) + +#define IN_LOOPBACKNET 127 + +#if defined(__MINGW32__) && defined(DEFINE_ADDITIONAL_IPV6_STUFF) +/* IPv6 address */ +struct in6_addr + { + union + { + u_int8_t u6_addr8[16]; + u_int16_t u6_addr16[8]; + u_int32_t u6_addr32[4]; + } in6_u; +#define s6_addr in6_u.u6_addr8 +#define s6_addr16 in6_u.u6_addr16 +#define s6_addr32 in6_u.u6_addr32 +#define s6_addr64 in6_u.u6_addr64 + }; + +#define IN6ADDR_ANY_INIT { 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 } +#define IN6ADDR_LOOPBACK_INIT { 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1 } +#endif /* __MINGW32__ */ + + +#if (defined _MSC_VER) || (defined(__MINGW32__) && defined(DEFINE_ADDITIONAL_IPV6_STUFF)) +typedef unsigned short sa_family_t; +#endif + + +#if defined(__MINGW32__) && defined(DEFINE_ADDITIONAL_IPV6_STUFF) + +#define __SOCKADDR_COMMON(sa_prefix) \ + sa_family_t sa_prefix##family + +/* Ditto, for IPv6. */ +struct sockaddr_in6 + { + __SOCKADDR_COMMON (sin6_); + u_int16_t sin6_port; /* Transport layer port # */ + u_int32_t sin6_flowinfo; /* IPv6 flow information */ + struct in6_addr sin6_addr; /* IPv6 address */ + }; + +#define IN6_IS_ADDR_V4MAPPED(a) \ + ((((u_int32_t *) (a))[0] == 0) && (((u_int32_t *) (a))[1] == 0) && \ + (((u_int32_t *) (a))[2] == htonl (0xffff))) + +#define IN6_IS_ADDR_MULTICAST(a) (((u_int8_t *) (a))[0] == 0xff) + +#define IN6_IS_ADDR_LINKLOCAL(a) \ + ((((u_int32_t *) (a))[0] & htonl (0xffc00000)) == htonl (0xfe800000)) + +#define IN6_IS_ADDR_LOOPBACK(a) \ + (((u_int32_t *) (a))[0] == 0 && ((u_int32_t *) (a))[1] == 0 && \ + ((u_int32_t *) (a))[2] == 0 && ((u_int32_t *) (a))[3] == htonl (1)) +#endif /* __MINGW32__ */ + +#define ip6_vfc ip6_ctlun.ip6_un2_vfc +#define ip6_flow ip6_ctlun.ip6_un1.ip6_un1_flow +#define ip6_plen ip6_ctlun.ip6_un1.ip6_un1_plen +#define ip6_nxt ip6_ctlun.ip6_un1.ip6_un1_nxt +#define ip6_hlim ip6_ctlun.ip6_un1.ip6_un1_hlim +#define ip6_hops ip6_ctlun.ip6_un1.ip6_un1_hlim + +#define nd_rd_type nd_rd_hdr.icmp6_type +#define nd_rd_code nd_rd_hdr.icmp6_code +#define nd_rd_cksum nd_rd_hdr.icmp6_cksum +#define nd_rd_reserved nd_rd_hdr.icmp6_data32[0] + +/* + * IPV6 extension headers + */ +#define IPPROTO_HOPOPTS 0 /* IPv6 hop-by-hop options */ +#define IPPROTO_IPV6 41 /* IPv6 header. */ +#define IPPROTO_ROUTING 43 /* IPv6 routing header */ +#define IPPROTO_FRAGMENT 44 /* IPv6 fragmentation header */ +#define IPPROTO_ESP 50 /* encapsulating security payload */ +#define IPPROTO_AH 51 /* authentication header */ +#define IPPROTO_ICMPV6 58 /* ICMPv6 */ +#define IPPROTO_NONE 59 /* IPv6 no next header */ +#define IPPROTO_DSTOPTS 60 /* IPv6 destination options */ +#define IPPROTO_PIM 103 /* Protocol Independent Multicast. */ + +#define IPV6_RTHDR_TYPE_0 0 + +/* Option types and related macros */ +#define IP6OPT_PAD1 0x00 /* 00 0 00000 */ +#define IP6OPT_PADN 0x01 /* 00 0 00001 */ +#define IP6OPT_JUMBO 0xC2 /* 11 0 00010 = 194 */ +#define IP6OPT_JUMBO_LEN 6 +#define IP6OPT_ROUTER_ALERT 0x05 /* 00 0 00101 */ + +#define IP6OPT_RTALERT_LEN 4 +#define IP6OPT_RTALERT_MLD 0 /* Datagram contains an MLD message */ +#define IP6OPT_RTALERT_RSVP 1 /* Datagram contains an RSVP message */ +#define IP6OPT_RTALERT_ACTNET 2 /* contains an Active Networks msg */ +#define IP6OPT_MINLEN 2 + +#define IP6OPT_BINDING_UPDATE 0xc6 /* 11 0 00110 */ +#define IP6OPT_BINDING_ACK 0x07 /* 00 0 00111 */ +#define IP6OPT_BINDING_REQ 0x08 /* 00 0 01000 */ +#define IP6OPT_HOME_ADDRESS 0xc9 /* 11 0 01001 */ +#define IP6OPT_EID 0x8a /* 10 0 01010 */ + +#define IP6OPT_TYPE(o) ((o) & 0xC0) +#define IP6OPT_TYPE_SKIP 0x00 +#define IP6OPT_TYPE_DISCARD 0x40 +#define IP6OPT_TYPE_FORCEICMP 0x80 +#define IP6OPT_TYPE_ICMP 0xC0 + +#define IP6OPT_MUTABLE 0x20 + + +#if defined(__MINGW32__) && defined(DEFINE_ADDITIONAL_IPV6_STUFF) +#ifndef EAI_ADDRFAMILY +struct addrinfo { + int ai_flags; /* AI_PASSIVE, AI_CANONNAME */ + int ai_family; /* PF_xxx */ + int ai_socktype; /* SOCK_xxx */ + int ai_protocol; /* 0 or IPPROTO_xxx for IPv4 and IPv6 */ + size_t ai_addrlen; /* length of ai_addr */ + char *ai_canonname; /* canonical name for hostname */ + struct sockaddr *ai_addr; /* binary address */ + struct addrinfo *ai_next; /* next structure in linked list */ +}; +#endif +#endif /* __MINGW32__ */ diff --git a/soem-src/oshw/win32/wpcap/Include/pcap-bpf.h b/soem-src/oshw/win32/wpcap/Include/pcap-bpf.h new file mode 100644 index 0000000..2657827 --- /dev/null +++ b/soem-src/oshw/win32/wpcap/Include/pcap-bpf.h @@ -0,0 +1,47 @@ +/*- + * Copyright (c) 1990, 1991, 1992, 1993, 1994, 1995, 1996, 1997 + * The Regents of the University of California. All rights reserved. + * + * This code is derived from the Stanford/CMU enet packet filter, + * (net/enet.c) distributed as part of 4.3BSD, and code contributed + * to Berkeley by Steven McCanne and Van Jacobson both of Lawrence + * Berkeley Laboratory. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * 3. All advertising materials mentioning features or use of this software + * must display the following acknowledgement: + * This product includes software developed by the University of + * California, Berkeley and its contributors. + * 4. Neither the name of the University nor the names of its contributors + * may be used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS + * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY + * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF + * SUCH DAMAGE. + * + * @(#) $Header: /tcpdump/master/libpcap/pcap-bpf.h,v 1.50 2007/04/01 21:43:55 guy Exp $ (LBL) + */ + +/* + * For backwards compatibility. + * + * Note to OS vendors: do NOT get rid of this file! Some applications + * might expect to be able to include . + */ +#include diff --git a/soem-src/oshw/win32/wpcap/Include/pcap-namedb.h b/soem-src/oshw/win32/wpcap/Include/pcap-namedb.h new file mode 100644 index 0000000..80a2f00 --- /dev/null +++ b/soem-src/oshw/win32/wpcap/Include/pcap-namedb.h @@ -0,0 +1,42 @@ +/* + * Copyright (c) 1994, 1996 + * The Regents of the University of California. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * 3. All advertising materials mentioning features or use of this software + * must display the following acknowledgement: + * This product includes software developed by the Computer Systems + * Engineering Group at Lawrence Berkeley Laboratory. + * 4. Neither the name of the University nor of the Laboratory may be used + * to endorse or promote products derived from this software without + * specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS + * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY + * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF + * SUCH DAMAGE. + * + * @(#) $Header: /tcpdump/master/libpcap/pcap-namedb.h,v 1.13 2006/10/04 18:13:32 guy Exp $ (LBL) + */ + +/* + * For backwards compatibility. + * + * Note to OS vendors: do NOT get rid of this file! Some applications + * might expect to be able to include . + */ +#include diff --git a/soem-src/oshw/win32/wpcap/Include/pcap-stdinc.h b/soem-src/oshw/win32/wpcap/Include/pcap-stdinc.h new file mode 100644 index 0000000..46100cc --- /dev/null +++ b/soem-src/oshw/win32/wpcap/Include/pcap-stdinc.h @@ -0,0 +1,93 @@ +/* + * Copyright (c) 2002 - 2005 NetGroup, Politecnico di Torino (Italy) + * Copyright (c) 2005 - 2009 CACE Technologies, Inc. Davis (California) + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * 3. Neither the name of the Politecnico di Torino nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * @(#) $Header: /tcpdump/master/libpcap/pcap-stdinc.h,v 1.10.2.1 2008-10-06 15:38:39 gianluca Exp $ (LBL) + */ + +#define SIZEOF_CHAR 1 +#define SIZEOF_SHORT 2 +#define SIZEOF_INT 4 +#ifndef _MSC_EXTENSIONS +#define SIZEOF_LONG_LONG 8 +#endif + +/* + * Avoids a compiler warning in case this was already defined + * (someone defined _WINSOCKAPI_ when including 'windows.h', in order + * to prevent it from including 'winsock.h') + */ +#ifdef _WINSOCKAPI_ +#undef _WINSOCKAPI_ +#endif +#include + +#include + +#include "bittypes.h" +#include +#include + +#ifndef __MINGW32__ +#include "IP6_misc.h" +#endif + +#define caddr_t char* + +#if _MSC_VER < 1500 +#define snprintf _snprintf +#define vsnprintf _vsnprintf +#define strdup _strdup +#endif + +#define inline __inline + +#ifdef __MINGW32__ +#include +#else /*__MINGW32__*/ +/* MSVC compiler */ +#ifndef _UINTPTR_T_DEFINED +#ifdef _WIN64 +typedef unsigned __int64 uintptr_t; +#else +typedef _W64 unsigned int uintptr_t; +#endif +#define _UINTPTR_T_DEFINED +#endif + +#ifndef _INTPTR_T_DEFINED +#ifdef _WIN64 +typedef __int64 intptr_t; +#else +typedef _W64 int intptr_t; +#endif +#define _INTPTR_T_DEFINED +#endif + +#endif /*__MINGW32__*/ diff --git a/soem-src/oshw/win32/wpcap/Include/pcap.h b/soem-src/oshw/win32/wpcap/Include/pcap.h new file mode 100644 index 0000000..935f949 --- /dev/null +++ b/soem-src/oshw/win32/wpcap/Include/pcap.h @@ -0,0 +1,45 @@ +/* + * Copyright (c) 1993, 1994, 1995, 1996, 1997 + * The Regents of the University of California. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * 3. All advertising materials mentioning features or use of this software + * must display the following acknowledgement: + * This product includes software developed by the Computer Systems + * Engineering Group at Lawrence Berkeley Laboratory. + * 4. Neither the name of the University nor of the Laboratory may be used + * to endorse or promote products derived from this software without + * specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS + * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY + * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF + * SUCH DAMAGE. + * + * @(#) $Header: /tcpdump/master/libpcap/pcap.h,v 1.59 2006/10/04 18:09:22 guy Exp $ (LBL) + */ + +/* + * For backwards compatibility. + * + * Note to OS vendors: do NOT get rid of this file! Many applications + * expect to be able to include , and at least some of them + * go through contortions in their configure scripts to try to detect + * OSes that have "helpfully" moved pcap.h to without + * leaving behind a file. + */ +#include diff --git a/soem-src/oshw/win32/wpcap/Include/pcap/bluetooth.h b/soem-src/oshw/win32/wpcap/Include/pcap/bluetooth.h new file mode 100644 index 0000000..45354b3 --- /dev/null +++ b/soem-src/oshw/win32/wpcap/Include/pcap/bluetooth.h @@ -0,0 +1,48 @@ +/* + * Copyright (c) 2006 Paolo Abeni (Italy) + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * 3. The name of the author may not be used to endorse or promote + * products derived from this software without specific prior written + * permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * bluetooth data struct + * By Paolo Abeni + * + * @(#) $Header: /tcpdump/master/libpcap/pcap/bluetooth.h,v 1.1 2007/09/22 02:10:17 guy Exp $ + */ + +#ifndef _PCAP_BLUETOOTH_STRUCTS_H__ +#define _PCAP_BLUETOOTH_STRUCTS_H__ + +/* + * Header prepended libpcap to each bluetooth h:4 frame. + * fields are in network byte order + */ +typedef struct _pcap_bluetooth_h4_header { + u_int32_t direction; /* if first bit is set direction is incoming */ +} pcap_bluetooth_h4_header; + + +#endif diff --git a/soem-src/oshw/win32/wpcap/Include/pcap/bpf.h b/soem-src/oshw/win32/wpcap/Include/pcap/bpf.h new file mode 100644 index 0000000..4e7e19a --- /dev/null +++ b/soem-src/oshw/win32/wpcap/Include/pcap/bpf.h @@ -0,0 +1,934 @@ +/*- + * Copyright (c) 1990, 1991, 1992, 1993, 1994, 1995, 1996, 1997 + * The Regents of the University of California. All rights reserved. + * + * This code is derived from the Stanford/CMU enet packet filter, + * (net/enet.c) distributed as part of 4.3BSD, and code contributed + * to Berkeley by Steven McCanne and Van Jacobson both of Lawrence + * Berkeley Laboratory. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * 3. All advertising materials mentioning features or use of this software + * must display the following acknowledgement: + * This product includes software developed by the University of + * California, Berkeley and its contributors. + * 4. Neither the name of the University nor the names of its contributors + * may be used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS + * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY + * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF + * SUCH DAMAGE. + * + * @(#)bpf.h 7.1 (Berkeley) 5/7/91 + * + * @(#) $Header: /tcpdump/master/libpcap/pcap/bpf.h,v 1.19.2.8 2008-09-22 20:16:01 guy Exp $ (LBL) + */ + +/* + * This is libpcap's cut-down version of bpf.h; it includes only + * the stuff needed for the code generator and the userland BPF + * interpreter, and the libpcap APIs for setting filters, etc.. + * + * "pcap-bpf.c" will include the native OS version, as it deals with + * the OS's BPF implementation. + * + * XXX - should this all just be moved to "pcap.h"? + */ + +#ifndef BPF_MAJOR_VERSION + +#ifdef __cplusplus +extern "C" { +#endif + +/* BSD style release date */ +#define BPF_RELEASE 199606 + +#ifdef MSDOS /* must be 32-bit */ +typedef long bpf_int32; +typedef unsigned long bpf_u_int32; +#else +typedef int bpf_int32; +typedef u_int bpf_u_int32; +#endif + +/* + * Alignment macros. BPF_WORDALIGN rounds up to the next + * even multiple of BPF_ALIGNMENT. + */ +#ifndef __NetBSD__ +#define BPF_ALIGNMENT sizeof(bpf_int32) +#else +#define BPF_ALIGNMENT sizeof(long) +#endif +#define BPF_WORDALIGN(x) (((x)+(BPF_ALIGNMENT-1))&~(BPF_ALIGNMENT-1)) + +#define BPF_MAXBUFSIZE 0x8000 +#define BPF_MINBUFSIZE 32 + +/* + * Structure for "pcap_compile()", "pcap_setfilter()", etc.. + */ +struct bpf_program { + u_int bf_len; + struct bpf_insn *bf_insns; +}; + +/* + * Struct return by BIOCVERSION. This represents the version number of + * the filter language described by the instruction encodings below. + * bpf understands a program iff kernel_major == filter_major && + * kernel_minor >= filter_minor, that is, if the value returned by the + * running kernel has the same major number and a minor number equal + * equal to or less than the filter being downloaded. Otherwise, the + * results are undefined, meaning an error may be returned or packets + * may be accepted haphazardly. + * It has nothing to do with the source code version. + */ +struct bpf_version { + u_short bv_major; + u_short bv_minor; +}; +/* Current version number of filter architecture. */ +#define BPF_MAJOR_VERSION 1 +#define BPF_MINOR_VERSION 1 + +/* + * Data-link level type codes. + * + * Do *NOT* add new values to this list without asking + * "tcpdump-workers@lists.tcpdump.org" for a value. Otherwise, you run + * the risk of using a value that's already being used for some other + * purpose, and of having tools that read libpcap-format captures not + * being able to handle captures with your new DLT_ value, with no hope + * that they will ever be changed to do so (as that would destroy their + * ability to read captures using that value for that other purpose). + */ + +/* + * These are the types that are the same on all platforms, and that + * have been defined by for ages. + */ +#define DLT_NULL 0 /* BSD loopback encapsulation */ +#define DLT_EN10MB 1 /* Ethernet (10Mb) */ +#define DLT_EN3MB 2 /* Experimental Ethernet (3Mb) */ +#define DLT_AX25 3 /* Amateur Radio AX.25 */ +#define DLT_PRONET 4 /* Proteon ProNET Token Ring */ +#define DLT_CHAOS 5 /* Chaos */ +#define DLT_IEEE802 6 /* 802.5 Token Ring */ +#define DLT_ARCNET 7 /* ARCNET, with BSD-style header */ +#define DLT_SLIP 8 /* Serial Line IP */ +#define DLT_PPP 9 /* Point-to-point Protocol */ +#define DLT_FDDI 10 /* FDDI */ + +/* + * These are types that are different on some platforms, and that + * have been defined by for ages. We use #ifdefs to + * detect the BSDs that define them differently from the traditional + * libpcap + * + * XXX - DLT_ATM_RFC1483 is 13 in BSD/OS, and DLT_RAW is 14 in BSD/OS, + * but I don't know what the right #define is for BSD/OS. + */ +#define DLT_ATM_RFC1483 11 /* LLC-encapsulated ATM */ + +#ifdef __OpenBSD__ +#define DLT_RAW 14 /* raw IP */ +#else +#define DLT_RAW 12 /* raw IP */ +#endif + +/* + * Given that the only OS that currently generates BSD/OS SLIP or PPP + * is, well, BSD/OS, arguably everybody should have chosen its values + * for DLT_SLIP_BSDOS and DLT_PPP_BSDOS, which are 15 and 16, but they + * didn't. So it goes. + */ +#if defined(__NetBSD__) || defined(__FreeBSD__) +#ifndef DLT_SLIP_BSDOS +#define DLT_SLIP_BSDOS 13 /* BSD/OS Serial Line IP */ +#define DLT_PPP_BSDOS 14 /* BSD/OS Point-to-point Protocol */ +#endif +#else +#define DLT_SLIP_BSDOS 15 /* BSD/OS Serial Line IP */ +#define DLT_PPP_BSDOS 16 /* BSD/OS Point-to-point Protocol */ +#endif + +/* + * 17 is used for DLT_OLD_PFLOG in OpenBSD; + * OBSOLETE: DLT_PFLOG is 117 in OpenBSD now as well. See below. + * 18 is used for DLT_PFSYNC in OpenBSD; don't use it for anything else. + */ + +#define DLT_ATM_CLIP 19 /* Linux Classical-IP over ATM */ + +/* + * Apparently Redback uses this for its SmartEdge 400/800. I hope + * nobody else decided to use it, too. + */ +#define DLT_REDBACK_SMARTEDGE 32 + +/* + * These values are defined by NetBSD; other platforms should refrain from + * using them for other purposes, so that NetBSD savefiles with link + * types of 50 or 51 can be read as this type on all platforms. + */ +#define DLT_PPP_SERIAL 50 /* PPP over serial with HDLC encapsulation */ +#define DLT_PPP_ETHER 51 /* PPP over Ethernet */ + +/* + * The Axent Raptor firewall - now the Symantec Enterprise Firewall - uses + * a link-layer type of 99 for the tcpdump it supplies. The link-layer + * header has 6 bytes of unknown data, something that appears to be an + * Ethernet type, and 36 bytes that appear to be 0 in at least one capture + * I've seen. + */ +#define DLT_SYMANTEC_FIREWALL 99 + +/* + * Values between 100 and 103 are used in capture file headers as + * link-layer types corresponding to DLT_ types that differ + * between platforms; don't use those values for new DLT_ new types. + */ + +/* + * This value was defined by libpcap 0.5; platforms that have defined + * it with a different value should define it here with that value - + * a link type of 104 in a save file will be mapped to DLT_C_HDLC, + * whatever value that happens to be, so programs will correctly + * handle files with that link type regardless of the value of + * DLT_C_HDLC. + * + * The name DLT_C_HDLC was used by BSD/OS; we use that name for source + * compatibility with programs written for BSD/OS. + * + * libpcap 0.5 defined it as DLT_CHDLC; we define DLT_CHDLC as well, + * for source compatibility with programs written for libpcap 0.5. + */ +#define DLT_C_HDLC 104 /* Cisco HDLC */ +#define DLT_CHDLC DLT_C_HDLC + +#define DLT_IEEE802_11 105 /* IEEE 802.11 wireless */ + +/* + * 106 is reserved for Linux Classical IP over ATM; it's like DLT_RAW, + * except when it isn't. (I.e., sometimes it's just raw IP, and + * sometimes it isn't.) We currently handle it as DLT_LINUX_SLL, + * so that we don't have to worry about the link-layer header.) + */ + +/* + * Frame Relay; BSD/OS has a DLT_FR with a value of 11, but that collides + * with other values. + * DLT_FR and DLT_FRELAY packets start with the Q.922 Frame Relay header + * (DLCI, etc.). + */ +#define DLT_FRELAY 107 + +/* + * OpenBSD DLT_LOOP, for loopback devices; it's like DLT_NULL, except + * that the AF_ type in the link-layer header is in network byte order. + * + * DLT_LOOP is 12 in OpenBSD, but that's DLT_RAW in other OSes, so + * we don't use 12 for it in OSes other than OpenBSD. + */ +#ifdef __OpenBSD__ +#define DLT_LOOP 12 +#else +#define DLT_LOOP 108 +#endif + +/* + * Encapsulated packets for IPsec; DLT_ENC is 13 in OpenBSD, but that's + * DLT_SLIP_BSDOS in NetBSD, so we don't use 13 for it in OSes other + * than OpenBSD. + */ +#ifdef __OpenBSD__ +#define DLT_ENC 13 +#else +#define DLT_ENC 109 +#endif + +/* + * Values between 110 and 112 are reserved for use in capture file headers + * as link-layer types corresponding to DLT_ types that might differ + * between platforms; don't use those values for new DLT_ types + * other than the corresponding DLT_ types. + */ + +/* + * This is for Linux cooked sockets. + */ +#define DLT_LINUX_SLL 113 + +/* + * Apple LocalTalk hardware. + */ +#define DLT_LTALK 114 + +/* + * Acorn Econet. + */ +#define DLT_ECONET 115 + +/* + * Reserved for use with OpenBSD ipfilter. + */ +#define DLT_IPFILTER 116 + +/* + * OpenBSD DLT_PFLOG; DLT_PFLOG is 17 in OpenBSD, but that's DLT_LANE8023 + * in SuSE 6.3, so we can't use 17 for it in capture-file headers. + * + * XXX: is there a conflict with DLT_PFSYNC 18 as well? + */ +#ifdef __OpenBSD__ +#define DLT_OLD_PFLOG 17 +#define DLT_PFSYNC 18 +#endif +#define DLT_PFLOG 117 + +/* + * Registered for Cisco-internal use. + */ +#define DLT_CISCO_IOS 118 + +/* + * For 802.11 cards using the Prism II chips, with a link-layer + * header including Prism monitor mode information plus an 802.11 + * header. + */ +#define DLT_PRISM_HEADER 119 + +/* + * Reserved for Aironet 802.11 cards, with an Aironet link-layer header + * (see Doug Ambrisko's FreeBSD patches). + */ +#define DLT_AIRONET_HEADER 120 + +/* + * Reserved for Siemens HiPath HDLC. + */ +#define DLT_HHDLC 121 + +/* + * This is for RFC 2625 IP-over-Fibre Channel. + * + * This is not for use with raw Fibre Channel, where the link-layer + * header starts with a Fibre Channel frame header; it's for IP-over-FC, + * where the link-layer header starts with an RFC 2625 Network_Header + * field. + */ +#define DLT_IP_OVER_FC 122 + +/* + * This is for Full Frontal ATM on Solaris with SunATM, with a + * pseudo-header followed by an AALn PDU. + * + * There may be other forms of Full Frontal ATM on other OSes, + * with different pseudo-headers. + * + * If ATM software returns a pseudo-header with VPI/VCI information + * (and, ideally, packet type information, e.g. signalling, ILMI, + * LANE, LLC-multiplexed traffic, etc.), it should not use + * DLT_ATM_RFC1483, but should get a new DLT_ value, so tcpdump + * and the like don't have to infer the presence or absence of a + * pseudo-header and the form of the pseudo-header. + */ +#define DLT_SUNATM 123 /* Solaris+SunATM */ + +/* + * Reserved as per request from Kent Dahlgren + * for private use. + */ +#define DLT_RIO 124 /* RapidIO */ +#define DLT_PCI_EXP 125 /* PCI Express */ +#define DLT_AURORA 126 /* Xilinx Aurora link layer */ + +/* + * Header for 802.11 plus a number of bits of link-layer information + * including radio information, used by some recent BSD drivers as + * well as the madwifi Atheros driver for Linux. + */ +#define DLT_IEEE802_11_RADIO 127 /* 802.11 plus radiotap radio header */ + +/* + * Reserved for the TZSP encapsulation, as per request from + * Chris Waters + * TZSP is a generic encapsulation for any other link type, + * which includes a means to include meta-information + * with the packet, e.g. signal strength and channel + * for 802.11 packets. + */ +#define DLT_TZSP 128 /* Tazmen Sniffer Protocol */ + +/* + * BSD's ARCNET headers have the source host, destination host, + * and type at the beginning of the packet; that's what's handed + * up to userland via BPF. + * + * Linux's ARCNET headers, however, have a 2-byte offset field + * between the host IDs and the type; that's what's handed up + * to userland via PF_PACKET sockets. + * + * We therefore have to have separate DLT_ values for them. + */ +#define DLT_ARCNET_LINUX 129 /* ARCNET */ + +/* + * Juniper-private data link types, as per request from + * Hannes Gredler . The DLT_s are used + * for passing on chassis-internal metainformation such as + * QOS profiles, etc.. + */ +#define DLT_JUNIPER_MLPPP 130 +#define DLT_JUNIPER_MLFR 131 +#define DLT_JUNIPER_ES 132 +#define DLT_JUNIPER_GGSN 133 +#define DLT_JUNIPER_MFR 134 +#define DLT_JUNIPER_ATM2 135 +#define DLT_JUNIPER_SERVICES 136 +#define DLT_JUNIPER_ATM1 137 + +/* + * Apple IP-over-IEEE 1394, as per a request from Dieter Siegmund + * . The header that's presented is an Ethernet-like + * header: + * + * #define FIREWIRE_EUI64_LEN 8 + * struct firewire_header { + * u_char firewire_dhost[FIREWIRE_EUI64_LEN]; + * u_char firewire_shost[FIREWIRE_EUI64_LEN]; + * u_short firewire_type; + * }; + * + * with "firewire_type" being an Ethernet type value, rather than, + * for example, raw GASP frames being handed up. + */ +#define DLT_APPLE_IP_OVER_IEEE1394 138 + +/* + * Various SS7 encapsulations, as per a request from Jeff Morriss + * and subsequent discussions. + */ +#define DLT_MTP2_WITH_PHDR 139 /* pseudo-header with various info, followed by MTP2 */ +#define DLT_MTP2 140 /* MTP2, without pseudo-header */ +#define DLT_MTP3 141 /* MTP3, without pseudo-header or MTP2 */ +#define DLT_SCCP 142 /* SCCP, without pseudo-header or MTP2 or MTP3 */ + +/* + * DOCSIS MAC frames. + */ +#define DLT_DOCSIS 143 + +/* + * Linux-IrDA packets. Protocol defined at http://www.irda.org. + * Those packets include IrLAP headers and above (IrLMP...), but + * don't include Phy framing (SOF/EOF/CRC & byte stuffing), because Phy + * framing can be handled by the hardware and depend on the bitrate. + * This is exactly the format you would get capturing on a Linux-IrDA + * interface (irdaX), but not on a raw serial port. + * Note the capture is done in "Linux-cooked" mode, so each packet include + * a fake packet header (struct sll_header). This is because IrDA packet + * decoding is dependant on the direction of the packet (incomming or + * outgoing). + * When/if other platform implement IrDA capture, we may revisit the + * issue and define a real DLT_IRDA... + * Jean II + */ +#define DLT_LINUX_IRDA 144 + +/* + * Reserved for IBM SP switch and IBM Next Federation switch. + */ +#define DLT_IBM_SP 145 +#define DLT_IBM_SN 146 + +/* + * Reserved for private use. If you have some link-layer header type + * that you want to use within your organization, with the capture files + * using that link-layer header type not ever be sent outside your + * organization, you can use these values. + * + * No libpcap release will use these for any purpose, nor will any + * tcpdump release use them, either. + * + * Do *NOT* use these in capture files that you expect anybody not using + * your private versions of capture-file-reading tools to read; in + * particular, do *NOT* use them in products, otherwise you may find that + * people won't be able to use tcpdump, or snort, or Ethereal, or... to + * read capture files from your firewall/intrusion detection/traffic + * monitoring/etc. appliance, or whatever product uses that DLT_ value, + * and you may also find that the developers of those applications will + * not accept patches to let them read those files. + * + * Also, do not use them if somebody might send you a capture using them + * for *their* private type and tools using them for *your* private type + * would have to read them. + * + * Instead, ask "tcpdump-workers@lists.tcpdump.org" for a new DLT_ value, + * as per the comment above, and use the type you're given. + */ +#define DLT_USER0 147 +#define DLT_USER1 148 +#define DLT_USER2 149 +#define DLT_USER3 150 +#define DLT_USER4 151 +#define DLT_USER5 152 +#define DLT_USER6 153 +#define DLT_USER7 154 +#define DLT_USER8 155 +#define DLT_USER9 156 +#define DLT_USER10 157 +#define DLT_USER11 158 +#define DLT_USER12 159 +#define DLT_USER13 160 +#define DLT_USER14 161 +#define DLT_USER15 162 + +/* + * For future use with 802.11 captures - defined by AbsoluteValue + * Systems to store a number of bits of link-layer information + * including radio information: + * + * http://www.shaftnet.org/~pizza/software/capturefrm.txt + * + * but it might be used by some non-AVS drivers now or in the + * future. + */ +#define DLT_IEEE802_11_RADIO_AVS 163 /* 802.11 plus AVS radio header */ + +/* + * Juniper-private data link type, as per request from + * Hannes Gredler . The DLT_s are used + * for passing on chassis-internal metainformation such as + * QOS profiles, etc.. + */ +#define DLT_JUNIPER_MONITOR 164 + +/* + * Reserved for BACnet MS/TP. + */ +#define DLT_BACNET_MS_TP 165 + +/* + * Another PPP variant as per request from Karsten Keil . + * + * This is used in some OSes to allow a kernel socket filter to distinguish + * between incoming and outgoing packets, on a socket intended to + * supply pppd with outgoing packets so it can do dial-on-demand and + * hangup-on-lack-of-demand; incoming packets are filtered out so they + * don't cause pppd to hold the connection up (you don't want random + * input packets such as port scans, packets from old lost connections, + * etc. to force the connection to stay up). + * + * The first byte of the PPP header (0xff03) is modified to accomodate + * the direction - 0x00 = IN, 0x01 = OUT. + */ +#define DLT_PPP_PPPD 166 + +/* + * Names for backwards compatibility with older versions of some PPP + * software; new software should use DLT_PPP_PPPD. + */ +#define DLT_PPP_WITH_DIRECTION DLT_PPP_PPPD +#define DLT_LINUX_PPP_WITHDIRECTION DLT_PPP_PPPD + +/* + * Juniper-private data link type, as per request from + * Hannes Gredler . The DLT_s are used + * for passing on chassis-internal metainformation such as + * QOS profiles, cookies, etc.. + */ +#define DLT_JUNIPER_PPPOE 167 +#define DLT_JUNIPER_PPPOE_ATM 168 + +#define DLT_GPRS_LLC 169 /* GPRS LLC */ +#define DLT_GPF_T 170 /* GPF-T (ITU-T G.7041/Y.1303) */ +#define DLT_GPF_F 171 /* GPF-F (ITU-T G.7041/Y.1303) */ + +/* + * Requested by Oolan Zimmer for use in Gcom's T1/E1 line + * monitoring equipment. + */ +#define DLT_GCOM_T1E1 172 +#define DLT_GCOM_SERIAL 173 + +/* + * Juniper-private data link type, as per request from + * Hannes Gredler . The DLT_ is used + * for internal communication to Physical Interface Cards (PIC) + */ +#define DLT_JUNIPER_PIC_PEER 174 + +/* + * Link types requested by Gregor Maier of Endace + * Measurement Systems. They add an ERF header (see + * http://www.endace.com/support/EndaceRecordFormat.pdf) in front of + * the link-layer header. + */ +#define DLT_ERF_ETH 175 /* Ethernet */ +#define DLT_ERF_POS 176 /* Packet-over-SONET */ + +/* + * Requested by Daniele Orlandi for raw LAPD + * for vISDN (http://www.orlandi.com/visdn/). Its link-layer header + * includes additional information before the LAPD header, so it's + * not necessarily a generic LAPD header. + */ +#define DLT_LINUX_LAPD 177 + +/* + * Juniper-private data link type, as per request from + * Hannes Gredler . + * The DLT_ are used for prepending meta-information + * like interface index, interface name + * before standard Ethernet, PPP, Frelay & C-HDLC Frames + */ +#define DLT_JUNIPER_ETHER 178 +#define DLT_JUNIPER_PPP 179 +#define DLT_JUNIPER_FRELAY 180 +#define DLT_JUNIPER_CHDLC 181 + +/* + * Multi Link Frame Relay (FRF.16) + */ +#define DLT_MFR 182 + +/* + * Juniper-private data link type, as per request from + * Hannes Gredler . + * The DLT_ is used for internal communication with a + * voice Adapter Card (PIC) + */ +#define DLT_JUNIPER_VP 183 + +/* + * Arinc 429 frames. + * DLT_ requested by Gianluca Varenni . + * Every frame contains a 32bit A429 label. + * More documentation on Arinc 429 can be found at + * http://www.condoreng.com/support/downloads/tutorials/ARINCTutorial.pdf + */ +#define DLT_A429 184 + +/* + * Arinc 653 Interpartition Communication messages. + * DLT_ requested by Gianluca Varenni . + * Please refer to the A653-1 standard for more information. + */ +#define DLT_A653_ICM 185 + +/* + * USB packets, beginning with a USB setup header; requested by + * Paolo Abeni . + */ +#define DLT_USB 186 + +/* + * Bluetooth HCI UART transport layer (part H:4); requested by + * Paolo Abeni. + */ +#define DLT_BLUETOOTH_HCI_H4 187 + +/* + * IEEE 802.16 MAC Common Part Sublayer; requested by Maria Cruz + * . + */ +#define DLT_IEEE802_16_MAC_CPS 188 + +/* + * USB packets, beginning with a Linux USB header; requested by + * Paolo Abeni . + */ +#define DLT_USB_LINUX 189 + +/* + * Controller Area Network (CAN) v. 2.0B packets. + * DLT_ requested by Gianluca Varenni . + * Used to dump CAN packets coming from a CAN Vector board. + * More documentation on the CAN v2.0B frames can be found at + * http://www.can-cia.org/downloads/?269 + */ +#define DLT_CAN20B 190 + +/* + * IEEE 802.15.4, with address fields padded, as is done by Linux + * drivers; requested by Juergen Schimmer. + */ +#define DLT_IEEE802_15_4_LINUX 191 + +/* + * Per Packet Information encapsulated packets. + * DLT_ requested by Gianluca Varenni . + */ +#define DLT_PPI 192 + +/* + * Header for 802.16 MAC Common Part Sublayer plus a radiotap radio header; + * requested by Charles Clancy. + */ +#define DLT_IEEE802_16_MAC_CPS_RADIO 193 + +/* + * Juniper-private data link type, as per request from + * Hannes Gredler . + * The DLT_ is used for internal communication with a + * integrated service module (ISM). + */ +#define DLT_JUNIPER_ISM 194 + +/* + * IEEE 802.15.4, exactly as it appears in the spec (no padding, no + * nothing); requested by Mikko Saarnivala . + */ +#define DLT_IEEE802_15_4 195 + +/* + * Various link-layer types, with a pseudo-header, for SITA + * (http://www.sita.aero/); requested by Fulko Hew (fulko.hew@gmail.com). + */ +#define DLT_SITA 196 + +/* + * Various link-layer types, with a pseudo-header, for Endace DAG cards; + * encapsulates Endace ERF records. Requested by Stephen Donnelly + * . + */ +#define DLT_ERF 197 + +/* + * Special header prepended to Ethernet packets when capturing from a + * u10 Networks board. Requested by Phil Mulholland + * . + */ +#define DLT_RAIF1 198 + +/* + * IPMB packet for IPMI, beginning with the I2C slave address, followed + * by the netFn and LUN, etc.. Requested by Chanthy Toeung + * . + */ +#define DLT_IPMB 199 + +/* + * Juniper-private data link type, as per request from + * Hannes Gredler . + * The DLT_ is used for capturing data on a secure tunnel interface. + */ +#define DLT_JUNIPER_ST 200 + +/* + * Bluetooth HCI UART transport layer (part H:4), with pseudo-header + * that includes direction information; requested by Paolo Abeni. + */ +#define DLT_BLUETOOTH_HCI_H4_WITH_PHDR 201 + +/* + * AX.25 packet with a 1-byte KISS header; see + * + * http://www.ax25.net/kiss.htm + * + * as per Richard Stearn . + */ +#define DLT_AX25_KISS 202 + +/* + * LAPD packets from an ISDN channel, starting with the address field, + * with no pseudo-header. + * Requested by Varuna De Silva . + */ +#define DLT_LAPD 203 + +/* + * Variants of various link-layer headers, with a one-byte direction + * pseudo-header prepended - zero means "received by this host", + * non-zero (any non-zero value) means "sent by this host" - as per + * Will Barker . + */ +#define DLT_PPP_WITH_DIR 204 /* PPP - don't confuse with DLT_PPP_WITH_DIRECTION */ +#define DLT_C_HDLC_WITH_DIR 205 /* Cisco HDLC */ +#define DLT_FRELAY_WITH_DIR 206 /* Frame Relay */ +#define DLT_LAPB_WITH_DIR 207 /* LAPB */ + +/* + * 208 is reserved for an as-yet-unspecified proprietary link-layer + * type, as requested by Will Barker. + */ + +/* + * IPMB with a Linux-specific pseudo-header; as requested by Alexey Neyman + * . + */ +#define DLT_IPMB_LINUX 209 + +/* + * FlexRay automotive bus - http://www.flexray.com/ - as requested + * by Hannes Kaelber . + */ +#define DLT_FLEXRAY 210 + +/* + * Media Oriented Systems Transport (MOST) bus for multimedia + * transport - http://www.mostcooperation.com/ - as requested + * by Hannes Kaelber . + */ +#define DLT_MOST 211 + +/* + * Local Interconnect Network (LIN) bus for vehicle networks - + * http://www.lin-subbus.org/ - as requested by Hannes Kaelber + * . + */ +#define DLT_LIN 212 + +/* + * X2E-private data link type used for serial line capture, + * as requested by Hannes Kaelber . + */ +#define DLT_X2E_SERIAL 213 + +/* + * X2E-private data link type used for the Xoraya data logger + * family, as requested by Hannes Kaelber . + */ +#define DLT_X2E_XORAYA 214 + +/* + * IEEE 802.15.4, exactly as it appears in the spec (no padding, no + * nothing), but with the PHY-level data for non-ASK PHYs (4 octets + * of 0 as preamble, one octet of SFD, one octet of frame length+ + * reserved bit, and then the MAC-layer data, starting with the + * frame control field). + * + * Requested by Max Filippov . + */ +#define DLT_IEEE802_15_4_NONASK_PHY 215 + + +/* + * DLT and savefile link type values are split into a class and + * a member of that class. A class value of 0 indicates a regular + * DLT_/LINKTYPE_ value. + */ +#define DLT_CLASS(x) ((x) & 0x03ff0000) + +/* + * NetBSD-specific generic "raw" link type. The class value indicates + * that this is the generic raw type, and the lower 16 bits are the + * address family we're dealing with. Those values are NetBSD-specific; + * do not assume that they correspond to AF_ values for your operating + * system. + */ +#define DLT_CLASS_NETBSD_RAWAF 0x02240000 +#define DLT_NETBSD_RAWAF(af) (DLT_CLASS_NETBSD_RAWAF | (af)) +#define DLT_NETBSD_RAWAF_AF(x) ((x) & 0x0000ffff) +#define DLT_IS_NETBSD_RAWAF(x) (DLT_CLASS(x) == DLT_CLASS_NETBSD_RAWAF) + + +/* + * The instruction encodings. + */ +/* instruction classes */ +#define BPF_CLASS(code) ((code) & 0x07) +#define BPF_LD 0x00 +#define BPF_LDX 0x01 +#define BPF_ST 0x02 +#define BPF_STX 0x03 +#define BPF_ALU 0x04 +#define BPF_JMP 0x05 +#define BPF_RET 0x06 +#define BPF_MISC 0x07 + +/* ld/ldx fields */ +#define BPF_SIZE(code) ((code) & 0x18) +#define BPF_W 0x00 +#define BPF_H 0x08 +#define BPF_B 0x10 +#define BPF_MODE(code) ((code) & 0xe0) +#define BPF_IMM 0x00 +#define BPF_ABS 0x20 +#define BPF_IND 0x40 +#define BPF_MEM 0x60 +#define BPF_LEN 0x80 +#define BPF_MSH 0xa0 + +/* alu/jmp fields */ +#define BPF_OP(code) ((code) & 0xf0) +#define BPF_ADD 0x00 +#define BPF_SUB 0x10 +#define BPF_MUL 0x20 +#define BPF_DIV 0x30 +#define BPF_OR 0x40 +#define BPF_AND 0x50 +#define BPF_LSH 0x60 +#define BPF_RSH 0x70 +#define BPF_NEG 0x80 +#define BPF_JA 0x00 +#define BPF_JEQ 0x10 +#define BPF_JGT 0x20 +#define BPF_JGE 0x30 +#define BPF_JSET 0x40 +#define BPF_SRC(code) ((code) & 0x08) +#define BPF_K 0x00 +#define BPF_X 0x08 + +/* ret - BPF_K and BPF_X also apply */ +#define BPF_RVAL(code) ((code) & 0x18) +#define BPF_A 0x10 + +/* misc */ +#define BPF_MISCOP(code) ((code) & 0xf8) +#define BPF_TAX 0x00 +#define BPF_TXA 0x80 + +/* + * The instruction data structure. + */ +struct bpf_insn { + u_short code; + u_char jt; + u_char jf; + bpf_u_int32 k; +}; + +/* + * Macros for insn array initializers. + */ +#define BPF_STMT(code, k) { (u_short)(code), 0, 0, k } +#define BPF_JUMP(code, k, jt, jf) { (u_short)(code), jt, jf, k } + +#if __STDC__ || defined(__cplusplus) +extern int bpf_validate(const struct bpf_insn *, int); +extern u_int bpf_filter(const struct bpf_insn *, const u_char *, u_int, u_int); +#else +extern int bpf_validate(); +extern u_int bpf_filter(); +#endif + +/* + * Number of scratch memory words (for BPF_LD|BPF_MEM and BPF_ST). + */ +#define BPF_MEMWORDS 16 + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/soem-src/oshw/win32/wpcap/Include/pcap/namedb.h b/soem-src/oshw/win32/wpcap/Include/pcap/namedb.h new file mode 100644 index 0000000..9002c75 --- /dev/null +++ b/soem-src/oshw/win32/wpcap/Include/pcap/namedb.h @@ -0,0 +1,89 @@ +/* + * Copyright (c) 1994, 1996 + * The Regents of the University of California. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * 3. All advertising materials mentioning features or use of this software + * must display the following acknowledgement: + * This product includes software developed by the Computer Systems + * Engineering Group at Lawrence Berkeley Laboratory. + * 4. Neither the name of the University nor of the Laboratory may be used + * to endorse or promote products derived from this software without + * specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS + * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY + * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF + * SUCH DAMAGE. + * + * @(#) $Header: /tcpdump/master/libpcap/pcap/namedb.h,v 1.1 2006/10/04 18:09:22 guy Exp $ (LBL) + */ + +#ifndef lib_pcap_namedb_h +#define lib_pcap_namedb_h + +#ifdef __cplusplus +extern "C" { +#endif + +/* + * As returned by the pcap_next_etherent() + * XXX this stuff doesn't belong in this interface, but this + * library already must do name to address translation, so + * on systems that don't have support for /etc/ethers, we + * export these hooks since they'll + */ +struct pcap_etherent { + u_char addr[6]; + char name[122]; +}; +#ifndef PCAP_ETHERS_FILE +#define PCAP_ETHERS_FILE "/etc/ethers" +#endif +struct pcap_etherent *pcap_next_etherent(FILE *); +u_char *pcap_ether_hostton(const char*); +u_char *pcap_ether_aton(const char *); + +bpf_u_int32 **pcap_nametoaddr(const char *); +#ifdef INET6 +struct addrinfo *pcap_nametoaddrinfo(const char *); +#endif +bpf_u_int32 pcap_nametonetaddr(const char *); + +int pcap_nametoport(const char *, int *, int *); +int pcap_nametoportrange(const char *, int *, int *, int *); +int pcap_nametoproto(const char *); +int pcap_nametoeproto(const char *); +int pcap_nametollc(const char *); +/* + * If a protocol is unknown, PROTO_UNDEF is returned. + * Also, pcap_nametoport() returns the protocol along with the port number. + * If there are ambiguous entried in /etc/services (i.e. domain + * can be either tcp or udp) PROTO_UNDEF is returned. + */ +#define PROTO_UNDEF -1 + +/* XXX move these to pcap-int.h? */ +int __pcap_atodn(const char *, bpf_u_int32 *); +int __pcap_atoin(const char *, bpf_u_int32 *); +u_short __pcap_nametodnaddr(const char *); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/soem-src/oshw/win32/wpcap/Include/pcap/pcap.h b/soem-src/oshw/win32/wpcap/Include/pcap/pcap.h new file mode 100644 index 0000000..04ade4e --- /dev/null +++ b/soem-src/oshw/win32/wpcap/Include/pcap/pcap.h @@ -0,0 +1,407 @@ +/* -*- Mode: c; tab-width: 8; indent-tabs-mode: 1; c-basic-offset: 8; -*- */ +/* + * Copyright (c) 1993, 1994, 1995, 1996, 1997 + * The Regents of the University of California. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * 3. All advertising materials mentioning features or use of this software + * must display the following acknowledgement: + * This product includes software developed by the Computer Systems + * Engineering Group at Lawrence Berkeley Laboratory. + * 4. Neither the name of the University nor of the Laboratory may be used + * to endorse or promote products derived from this software without + * specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS + * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY + * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF + * SUCH DAMAGE. + * + * @(#) $Header: /tcpdump/master/libpcap/pcap/pcap.h,v 1.4.2.11 2008-10-06 15:38:39 gianluca Exp $ (LBL) + */ + +#ifndef lib_pcap_pcap_h +#define lib_pcap_pcap_h + +#if defined(WIN32) + #include +#elif defined(MSDOS) + #include + #include /* u_int, u_char etc. */ +#else /* UN*X */ + #include + #include +#endif /* WIN32/MSDOS/UN*X */ + +#ifndef PCAP_DONT_INCLUDE_PCAP_BPF_H +#include +#endif + +#include + +#ifdef HAVE_REMOTE + // We have to define the SOCKET here, although it has been defined in sockutils.h + // This is to avoid the distribution of the 'sockutils.h' file around + // (for example in the WinPcap developer's pack) + #ifndef SOCKET + #ifdef WIN32 + #define SOCKET unsigned int + #else + #define SOCKET int + #endif + #endif +#endif + +#ifdef __cplusplus +extern "C" { +#endif + +#define PCAP_VERSION_MAJOR 2 +#define PCAP_VERSION_MINOR 4 + +#define PCAP_ERRBUF_SIZE 256 + +/* + * Compatibility for systems that have a bpf.h that + * predates the bpf typedefs for 64-bit support. + */ +#if BPF_RELEASE - 0 < 199406 +typedef int bpf_int32; +typedef u_int bpf_u_int32; +#endif + +typedef struct pcap pcap_t; +typedef struct pcap_dumper pcap_dumper_t; +typedef struct pcap_if pcap_if_t; +typedef struct pcap_addr pcap_addr_t; + +/* + * The first record in the file contains saved values for some + * of the flags used in the printout phases of tcpdump. + * Many fields here are 32 bit ints so compilers won't insert unwanted + * padding; these files need to be interchangeable across architectures. + * + * Do not change the layout of this structure, in any way (this includes + * changes that only affect the length of fields in this structure). + * + * Also, do not change the interpretation of any of the members of this + * structure, in any way (this includes using values other than + * LINKTYPE_ values, as defined in "savefile.c", in the "linktype" + * field). + * + * Instead: + * + * introduce a new structure for the new format, if the layout + * of the structure changed; + * + * send mail to "tcpdump-workers@lists.tcpdump.org", requesting + * a new magic number for your new capture file format, and, when + * you get the new magic number, put it in "savefile.c"; + * + * use that magic number for save files with the changed file + * header; + * + * make the code in "savefile.c" capable of reading files with + * the old file header as well as files with the new file header + * (using the magic number to determine the header format). + * + * Then supply the changes as a patch at + * + * http://sourceforge.net/projects/libpcap/ + * + * so that future versions of libpcap and programs that use it (such as + * tcpdump) will be able to read your new capture file format. + */ +struct pcap_file_header { + bpf_u_int32 magic; + u_short version_major; + u_short version_minor; + bpf_int32 thiszone; /* gmt to local correction */ + bpf_u_int32 sigfigs; /* accuracy of timestamps */ + bpf_u_int32 snaplen; /* max length saved portion of each pkt */ + bpf_u_int32 linktype; /* data link type (LINKTYPE_*) */ +}; + +/* + * Macros for the value returned by pcap_datalink_ext(). + * + * If LT_FCS_LENGTH_PRESENT(x) is true, the LT_FCS_LENGTH(x) macro + * gives the FCS length of packets in the capture. + */ +#define LT_FCS_LENGTH_PRESENT(x) ((x) & 0x04000000) +#define LT_FCS_LENGTH(x) (((x) & 0xF0000000) >> 28) +#define LT_FCS_DATALINK_EXT(x) ((((x) & 0xF) << 28) | 0x04000000) + +typedef enum { + PCAP_D_INOUT = 0, + PCAP_D_IN, + PCAP_D_OUT +} pcap_direction_t; + +/* + * Generic per-packet information, as supplied by libpcap. + * + * The time stamp can and should be a "struct timeval", regardless of + * whether your system supports 32-bit tv_sec in "struct timeval", + * 64-bit tv_sec in "struct timeval", or both if it supports both 32-bit + * and 64-bit applications. The on-disk format of savefiles uses 32-bit + * tv_sec (and tv_usec); this structure is irrelevant to that. 32-bit + * and 64-bit versions of libpcap, even if they're on the same platform, + * should supply the appropriate version of "struct timeval", even if + * that's not what the underlying packet capture mechanism supplies. + */ +struct pcap_pkthdr { + struct timeval ts; /* time stamp */ + bpf_u_int32 caplen; /* length of portion present */ + bpf_u_int32 len; /* length this packet (off wire) */ +}; + +/* + * As returned by the pcap_stats() + */ +struct pcap_stat { + u_int ps_recv; /* number of packets received */ + u_int ps_drop; /* number of packets dropped */ + u_int ps_ifdrop; /* drops by interface XXX not yet supported */ +#ifdef HAVE_REMOTE + u_int ps_capt; /* number of packets that are received by the application; please get rid off the Win32 ifdef */ + u_int ps_sent; /* number of packets sent by the server on the network */ + u_int ps_netdrop; /* number of packets lost on the network */ +#endif /* HAVE_REMOTE */ +}; + +#ifdef MSDOS +/* + * As returned by the pcap_stats_ex() + */ +struct pcap_stat_ex { + u_long rx_packets; /* total packets received */ + u_long tx_packets; /* total packets transmitted */ + u_long rx_bytes; /* total bytes received */ + u_long tx_bytes; /* total bytes transmitted */ + u_long rx_errors; /* bad packets received */ + u_long tx_errors; /* packet transmit problems */ + u_long rx_dropped; /* no space in Rx buffers */ + u_long tx_dropped; /* no space available for Tx */ + u_long multicast; /* multicast packets received */ + u_long collisions; + + /* detailed rx_errors: */ + u_long rx_length_errors; + u_long rx_over_errors; /* receiver ring buff overflow */ + u_long rx_crc_errors; /* recv'd pkt with crc error */ + u_long rx_frame_errors; /* recv'd frame alignment error */ + u_long rx_fifo_errors; /* recv'r fifo overrun */ + u_long rx_missed_errors; /* recv'r missed packet */ + + /* detailed tx_errors */ + u_long tx_aborted_errors; + u_long tx_carrier_errors; + u_long tx_fifo_errors; + u_long tx_heartbeat_errors; + u_long tx_window_errors; + }; +#endif + +/* + * Item in a list of interfaces. + */ +struct pcap_if { + struct pcap_if *next; + char *name; /* name to hand to "pcap_open_live()" */ + char *description; /* textual description of interface, or NULL */ + struct pcap_addr *addresses; + bpf_u_int32 flags; /* PCAP_IF_ interface flags */ +}; + +#define PCAP_IF_LOOPBACK 0x00000001 /* interface is loopback */ + +/* + * Representation of an interface address. + */ +struct pcap_addr { + struct pcap_addr *next; + struct sockaddr *addr; /* address */ + struct sockaddr *netmask; /* netmask for that address */ + struct sockaddr *broadaddr; /* broadcast address for that address */ + struct sockaddr *dstaddr; /* P2P destination address for that address */ +}; + +typedef void (*pcap_handler)(u_char *, const struct pcap_pkthdr *, + const u_char *); + +/* + * Error codes for the pcap API. + * These will all be negative, so you can check for the success or + * failure of a call that returns these codes by checking for a + * negative value. + */ +#define PCAP_ERROR -1 /* generic error code */ +#define PCAP_ERROR_BREAK -2 /* loop terminated by pcap_breakloop */ +#define PCAP_ERROR_NOT_ACTIVATED -3 /* the capture needs to be activated */ +#define PCAP_ERROR_ACTIVATED -4 /* the operation can't be performed on already activated captures */ +#define PCAP_ERROR_NO_SUCH_DEVICE -5 /* no such device exists */ +#define PCAP_ERROR_RFMON_NOTSUP -6 /* this device doesn't support rfmon (monitor) mode */ +#define PCAP_ERROR_NOT_RFMON -7 /* operation supported only in monitor mode */ +#define PCAP_ERROR_PERM_DENIED -8 /* no permission to open the device */ +#define PCAP_ERROR_IFACE_NOT_UP -9 /* interface isn't up */ + +/* + * Warning codes for the pcap API. + * These will all be positive and non-zero, so they won't look like + * errors. + */ +#define PCAP_WARNING 1 /* generic warning code */ +#define PCAP_WARNING_PROMISC_NOTSUP 2 /* this device doesn't support promiscuous mode */ + +char *pcap_lookupdev(char *); +int pcap_lookupnet(const char *, bpf_u_int32 *, bpf_u_int32 *, char *); + +pcap_t *pcap_create(const char *, char *); +int pcap_set_snaplen(pcap_t *, int); +int pcap_set_promisc(pcap_t *, int); +int pcap_can_set_rfmon(pcap_t *); +int pcap_set_rfmon(pcap_t *, int); +int pcap_set_timeout(pcap_t *, int); +int pcap_set_buffer_size(pcap_t *, int); +int pcap_activate(pcap_t *); + +pcap_t *pcap_open_live(const char *, int, int, int, char *); +pcap_t *pcap_open_dead(int, int); +pcap_t *pcap_open_offline(const char *, char *); +#if defined(WIN32) +pcap_t *pcap_hopen_offline(intptr_t, char *); +#if !defined(LIBPCAP_EXPORTS) +#define pcap_fopen_offline(f,b) \ + pcap_hopen_offline(_get_osfhandle(_fileno(f)), b) +#else /*LIBPCAP_EXPORTS*/ +static pcap_t *pcap_fopen_offline(FILE *, char *); +#endif +#else /*WIN32*/ +pcap_t *pcap_fopen_offline(FILE *, char *); +#endif /*WIN32*/ + +void pcap_close(pcap_t *); +int pcap_loop(pcap_t *, int, pcap_handler, u_char *); +int pcap_dispatch(pcap_t *, int, pcap_handler, u_char *); +const u_char* + pcap_next(pcap_t *, struct pcap_pkthdr *); +int pcap_next_ex(pcap_t *, struct pcap_pkthdr **, const u_char **); +void pcap_breakloop(pcap_t *); +int pcap_stats(pcap_t *, struct pcap_stat *); +int pcap_setfilter(pcap_t *, struct bpf_program *); +int pcap_setdirection(pcap_t *, pcap_direction_t); +int pcap_getnonblock(pcap_t *, char *); +int pcap_setnonblock(pcap_t *, int, char *); +int pcap_inject(pcap_t *, const void *, size_t); +int pcap_sendpacket(pcap_t *, const u_char *, int); +const char *pcap_statustostr(int); +const char *pcap_strerror(int); +char *pcap_geterr(pcap_t *); +void pcap_perror(pcap_t *, char *); +int pcap_compile(pcap_t *, struct bpf_program *, const char *, int, + bpf_u_int32); +int pcap_compile_nopcap(int, int, struct bpf_program *, + const char *, int, bpf_u_int32); +void pcap_freecode(struct bpf_program *); +int pcap_offline_filter(struct bpf_program *, const struct pcap_pkthdr *, + const u_char *); +int pcap_datalink(pcap_t *); +int pcap_datalink_ext(pcap_t *); +int pcap_list_datalinks(pcap_t *, int **); +int pcap_set_datalink(pcap_t *, int); +void pcap_free_datalinks(int *); +int pcap_datalink_name_to_val(const char *); +const char *pcap_datalink_val_to_name(int); +const char *pcap_datalink_val_to_description(int); +int pcap_snapshot(pcap_t *); +int pcap_is_swapped(pcap_t *); +int pcap_major_version(pcap_t *); +int pcap_minor_version(pcap_t *); + +/* XXX */ +FILE *pcap_file(pcap_t *); +int pcap_fileno(pcap_t *); + +pcap_dumper_t *pcap_dump_open(pcap_t *, const char *); +pcap_dumper_t *pcap_dump_fopen(pcap_t *, FILE *fp); +FILE *pcap_dump_file(pcap_dumper_t *); +long pcap_dump_ftell(pcap_dumper_t *); +int pcap_dump_flush(pcap_dumper_t *); +void pcap_dump_close(pcap_dumper_t *); +void pcap_dump(u_char *, const struct pcap_pkthdr *, const u_char *); + +int pcap_findalldevs(pcap_if_t **, char *); +void pcap_freealldevs(pcap_if_t *); + +const char *pcap_lib_version(void); + +/* XXX this guy lives in the bpf tree */ +u_int bpf_filter(const struct bpf_insn *, const u_char *, u_int, u_int); +int bpf_validate(const struct bpf_insn *f, int len); +char *bpf_image(const struct bpf_insn *, int); +void bpf_dump(const struct bpf_program *, int); + +#if defined(WIN32) + +/* + * Win32 definitions + */ + +int pcap_setbuff(pcap_t *p, int dim); +int pcap_setmode(pcap_t *p, int mode); +int pcap_setmintocopy(pcap_t *p, int size); + +#ifdef WPCAP +/* Include file with the wpcap-specific extensions */ +#include +#endif /* WPCAP */ + +#define MODE_CAPT 0 +#define MODE_STAT 1 +#define MODE_MON 2 + +#elif defined(MSDOS) + +/* + * MS-DOS definitions + */ + +int pcap_stats_ex (pcap_t *, struct pcap_stat_ex *); +void pcap_set_wait (pcap_t *p, void (*yield)(void), int wait); +u_long pcap_mac_packets (void); + +#else /* UN*X */ + +/* + * UN*X definitions + */ + +int pcap_get_selectable_fd(pcap_t *); + +#endif /* WIN32/MSDOS/UN*X */ + +#ifdef HAVE_REMOTE +/* Includes most of the public stuff that is needed for the remote capture */ +#include +#endif /* HAVE_REMOTE */ + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/soem-src/oshw/win32/wpcap/Include/pcap/sll.h b/soem-src/oshw/win32/wpcap/Include/pcap/sll.h new file mode 100644 index 0000000..e9d5452 --- /dev/null +++ b/soem-src/oshw/win32/wpcap/Include/pcap/sll.h @@ -0,0 +1,129 @@ +/*- + * Copyright (c) 1990, 1991, 1992, 1993, 1994, 1995, 1996, 1997 + * The Regents of the University of California. All rights reserved. + * + * This code is derived from the Stanford/CMU enet packet filter, + * (net/enet.c) distributed as part of 4.3BSD, and code contributed + * to Berkeley by Steven McCanne and Van Jacobson both of Lawrence + * Berkeley Laboratory. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * 3. All advertising materials mentioning features or use of this software + * must display the following acknowledgement: + * This product includes software developed by the University of + * California, Berkeley and its contributors. + * 4. Neither the name of the University nor the names of its contributors + * may be used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS + * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY + * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF + * SUCH DAMAGE. + * + * @(#) $Header: /tcpdump/master/libpcap/pcap/sll.h,v 1.2.2.1 2008-05-30 01:36:06 guy Exp $ (LBL) + */ + +/* + * For captures on Linux cooked sockets, we construct a fake header + * that includes: + * + * a 2-byte "packet type" which is one of: + * + * LINUX_SLL_HOST packet was sent to us + * LINUX_SLL_BROADCAST packet was broadcast + * LINUX_SLL_MULTICAST packet was multicast + * LINUX_SLL_OTHERHOST packet was sent to somebody else + * LINUX_SLL_OUTGOING packet was sent *by* us; + * + * a 2-byte Ethernet protocol field; + * + * a 2-byte link-layer type; + * + * a 2-byte link-layer address length; + * + * an 8-byte source link-layer address, whose actual length is + * specified by the previous value. + * + * All fields except for the link-layer address are in network byte order. + * + * DO NOT change the layout of this structure, or change any of the + * LINUX_SLL_ values below. If you must change the link-layer header + * for a "cooked" Linux capture, introduce a new DLT_ type (ask + * "tcpdump-workers@lists.tcpdump.org" for one, so that you don't give it + * a value that collides with a value already being used), and use the + * new header in captures of that type, so that programs that can + * handle DLT_LINUX_SLL captures will continue to handle them correctly + * without any change, and so that capture files with different headers + * can be told apart and programs that read them can dissect the + * packets in them. + */ + +#ifndef lib_pcap_sll_h +#define lib_pcap_sll_h + +/* + * A DLT_LINUX_SLL fake link-layer header. + */ +#define SLL_HDR_LEN 16 /* total header length */ +#define SLL_ADDRLEN 8 /* length of address field */ + +struct sll_header { + u_int16_t sll_pkttype; /* packet type */ + u_int16_t sll_hatype; /* link-layer address type */ + u_int16_t sll_halen; /* link-layer address length */ + u_int8_t sll_addr[SLL_ADDRLEN]; /* link-layer address */ + u_int16_t sll_protocol; /* protocol */ +}; + +/* + * The LINUX_SLL_ values for "sll_pkttype"; these correspond to the + * PACKET_ values on Linux, but are defined here so that they're + * available even on systems other than Linux, and so that they + * don't change even if the PACKET_ values change. + */ +#define LINUX_SLL_HOST 0 +#define LINUX_SLL_BROADCAST 1 +#define LINUX_SLL_MULTICAST 2 +#define LINUX_SLL_OTHERHOST 3 +#define LINUX_SLL_OUTGOING 4 + +/* + * The LINUX_SLL_ values for "sll_protocol"; these correspond to the + * ETH_P_ values on Linux, but are defined here so that they're + * available even on systems other than Linux. We assume, for now, + * that the ETH_P_ values won't change in Linux; if they do, then: + * + * if we don't translate them in "pcap-linux.c", capture files + * won't necessarily be readable if captured on a system that + * defines ETH_P_ values that don't match these values; + * + * if we do translate them in "pcap-linux.c", that makes life + * unpleasant for the BPF code generator, as the values you test + * for in the kernel aren't the values that you test for when + * reading a capture file, so the fixup code run on BPF programs + * handed to the kernel ends up having to do more work. + * + * Add other values here as necessary, for handling packet types that + * might show up on non-Ethernet, non-802.x networks. (Not all the ones + * in the Linux "if_ether.h" will, I suspect, actually show up in + * captures.) + */ +#define LINUX_SLL_P_802_3 0x0001 /* Novell 802.3 frames without 802.2 LLC header */ +#define LINUX_SLL_P_802_2 0x0004 /* 802.2 frames (not D/I/X Ethernet) */ + +#endif diff --git a/soem-src/oshw/win32/wpcap/Include/pcap/usb.h b/soem-src/oshw/win32/wpcap/Include/pcap/usb.h new file mode 100644 index 0000000..337a5f2 --- /dev/null +++ b/soem-src/oshw/win32/wpcap/Include/pcap/usb.h @@ -0,0 +1,90 @@ +/* + * Copyright (c) 2006 Paolo Abeni (Italy) + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * 3. The name of the author may not be used to endorse or promote + * products derived from this software without specific prior written + * permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * Basic USB data struct + * By Paolo Abeni + * + * @(#) $Header: /tcpdump/master/libpcap/pcap/usb.h,v 1.6 2007/09/22 02:06:08 guy Exp $ + */ + +#ifndef _PCAP_USB_STRUCTS_H__ +#define _PCAP_USB_STRUCTS_H__ + +/* + * possible transfer mode + */ +#define URB_TRANSFER_IN 0x80 +#define URB_ISOCHRONOUS 0x0 +#define URB_INTERRUPT 0x1 +#define URB_CONTROL 0x2 +#define URB_BULK 0x3 + +/* + * possible event type + */ +#define URB_SUBMIT 'S' +#define URB_COMPLETE 'C' +#define URB_ERROR 'E' + +/* + * USB setup header as defined in USB specification. + * Appears at the front of each packet in DLT_USB captures. + */ +typedef struct _usb_setup { + u_int8_t bmRequestType; + u_int8_t bRequest; + u_int16_t wValue; + u_int16_t wIndex; + u_int16_t wLength; +} pcap_usb_setup; + + +/* + * Header prepended by linux kernel to each event. + * Appears at the front of each packet in DLT_USB_LINUX captures. + */ +typedef struct _usb_header { + u_int64_t id; + u_int8_t event_type; + u_int8_t transfer_type; + u_int8_t endpoint_number; + u_int8_t device_address; + u_int16_t bus_id; + char setup_flag;/*if !=0 the urb setup header is not present*/ + char data_flag; /*if !=0 no urb data is present*/ + int64_t ts_sec; + int32_t ts_usec; + int32_t status; + u_int32_t urb_len; + u_int32_t data_len; /* amount of urb data really present in this event*/ + pcap_usb_setup setup; +} pcap_usb_header; + + +#endif diff --git a/soem-src/oshw/win32/wpcap/Include/pcap/vlan.h b/soem-src/oshw/win32/wpcap/Include/pcap/vlan.h new file mode 100644 index 0000000..b0cb794 --- /dev/null +++ b/soem-src/oshw/win32/wpcap/Include/pcap/vlan.h @@ -0,0 +1,46 @@ +/*- + * Copyright (c) 1990, 1991, 1992, 1993, 1994, 1995, 1996, 1997 + * The Regents of the University of California. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * 3. All advertising materials mentioning features or use of this software + * must display the following acknowledgement: + * This product includes software developed by the University of + * California, Berkeley and its contributors. + * 4. Neither the name of the University nor the names of its contributors + * may be used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS + * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY + * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF + * SUCH DAMAGE. + * + * @(#) $Header: /tcpdump/master/libpcap/pcap/vlan.h,v 1.1.2.2 2008-08-06 07:45:59 guy Exp $ + */ + +#ifndef lib_pcap_vlan_h +#define lib_pcap_vlan_h + +struct vlan_tag { + u_int16_t vlan_tpid; /* ETH_P_8021Q */ + u_int16_t vlan_tci; /* VLAN TCI */ +}; + +#define VLAN_TAG_LEN 4 + +#endif diff --git a/soem-src/oshw/win32/wpcap/Include/remote-ext.h b/soem-src/oshw/win32/wpcap/Include/remote-ext.h new file mode 100644 index 0000000..e5966e7 --- /dev/null +++ b/soem-src/oshw/win32/wpcap/Include/remote-ext.h @@ -0,0 +1,444 @@ +/* + * Copyright (c) 2002 - 2003 + * NetGroup, Politecnico di Torino (Italy) + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * 3. Neither the name of the Politecnico di Torino nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + + +#ifndef __REMOTE_EXT_H__ +#define __REMOTE_EXT_H__ + + +#ifndef HAVE_REMOTE +#error Please do not include this file directly. Just define HAVE_REMOTE and then include pcap.h +#endif + +// Definition for Microsoft Visual Studio +#if _MSC_VER > 1000 +#pragma once +#endif + +#ifdef __cplusplus +extern "C" { +#endif + +/*! + \file remote-ext.h + + The goal of this file it to include most of the new definitions that should be + placed into the pcap.h file. + + It includes all new definitions (structures and functions like pcap_open(). + Some of the functions are not really a remote feature, but, right now, + they are placed here. +*/ + + + +// All this stuff is public +/*! \addtogroup remote_struct + \{ +*/ + + + + +/*! + \brief Defines the maximum buffer size in which address, port, interface names are kept. + + In case the adapter name or such is larger than this value, it is truncated. + This is not used by the user; however it must be aware that an hostname / interface + name longer than this value will be truncated. +*/ +#define PCAP_BUF_SIZE 1024 + + +/*! \addtogroup remote_source_ID + \{ +*/ + + +/*! + \brief Internal representation of the type of source in use (file, + remote/local interface). + + This indicates a file, i.e. the user want to open a capture from a local file. +*/ +#define PCAP_SRC_FILE 2 +/*! + \brief Internal representation of the type of source in use (file, + remote/local interface). + + This indicates a local interface, i.e. the user want to open a capture from + a local interface. This does not involve the RPCAP protocol. +*/ +#define PCAP_SRC_IFLOCAL 3 +/*! + \brief Internal representation of the type of source in use (file, + remote/local interface). + + This indicates a remote interface, i.e. the user want to open a capture from + an interface on a remote host. This does involve the RPCAP protocol. +*/ +#define PCAP_SRC_IFREMOTE 4 + +/*! + \} +*/ + + + +/*! \addtogroup remote_source_string + + The formats allowed by the pcap_open() are the following: + - file://path_and_filename [opens a local file] + - rpcap://devicename [opens the selected device devices available on the local host, without using the RPCAP protocol] + - rpcap://host/devicename [opens the selected device available on a remote host] + - rpcap://host:port/devicename [opens the selected device available on a remote host, using a non-standard port for RPCAP] + - adaptername [to open a local adapter; kept for compability, but it is strongly discouraged] + - (NULL) [to open the first local adapter; kept for compability, but it is strongly discouraged] + + The formats allowed by the pcap_findalldevs_ex() are the following: + - file://folder/ [lists all the files in the given folder] + - rpcap:// [lists all local adapters] + - rpcap://host:port/ [lists the devices available on a remote host] + + Referring to the 'host' and 'port' paramters, they can be either numeric or literal. Since + IPv6 is fully supported, these are the allowed formats: + + - host (literal): e.g. host.foo.bar + - host (numeric IPv4): e.g. 10.11.12.13 + - host (numeric IPv4, IPv6 style): e.g. [10.11.12.13] + - host (numeric IPv6): e.g. [1:2:3::4] + - port: can be either numeric (e.g. '80') or literal (e.g. 'http') + + Here you find some allowed examples: + - rpcap://host.foo.bar/devicename [everything literal, no port number] + - rpcap://host.foo.bar:1234/devicename [everything literal, with port number] + - rpcap://10.11.12.13/devicename [IPv4 numeric, no port number] + - rpcap://10.11.12.13:1234/devicename [IPv4 numeric, with port number] + - rpcap://[10.11.12.13]:1234/devicename [IPv4 numeric with IPv6 format, with port number] + - rpcap://[1:2:3::4]/devicename [IPv6 numeric, no port number] + - rpcap://[1:2:3::4]:1234/devicename [IPv6 numeric, with port number] + - rpcap://[1:2:3::4]:http/devicename [IPv6 numeric, with literal port number] + + \{ +*/ + + +/*! + \brief String that will be used to determine the type of source in use (file, + remote/local interface). + + This string will be prepended to the interface name in order to create a string + that contains all the information required to open the source. + + This string indicates that the user wants to open a capture from a local file. +*/ +#define PCAP_SRC_FILE_STRING "file://" +/*! + \brief String that will be used to determine the type of source in use (file, + remote/local interface). + + This string will be prepended to the interface name in order to create a string + that contains all the information required to open the source. + + This string indicates that the user wants to open a capture from a network interface. + This string does not necessarily involve the use of the RPCAP protocol. If the + interface required resides on the local host, the RPCAP protocol is not involved + and the local functions are used. +*/ +#define PCAP_SRC_IF_STRING "rpcap://" + +/*! + \} +*/ + + + + + +/*! + \addtogroup remote_open_flags + \{ +*/ + +/*! + \brief Defines if the adapter has to go in promiscuous mode. + + It is '1' if you have to open the adapter in promiscuous mode, '0' otherwise. + Note that even if this parameter is false, the interface could well be in promiscuous + mode for some other reason (for example because another capture process with + promiscuous mode enabled is currently using that interface). + On on Linux systems with 2.2 or later kernels (that have the "any" device), this + flag does not work on the "any" device; if an argument of "any" is supplied, + the 'promisc' flag is ignored. +*/ +#define PCAP_OPENFLAG_PROMISCUOUS 1 + +/*! + \brief Defines if the data trasfer (in case of a remote + capture) has to be done with UDP protocol. + + If it is '1' if you want a UDP data connection, '0' if you want + a TCP data connection; control connection is always TCP-based. + A UDP connection is much lighter, but it does not guarantee that all + the captured packets arrive to the client workstation. Moreover, + it could be harmful in case of network congestion. + This flag is meaningless if the source is not a remote interface. + In that case, it is simply ignored. +*/ +#define PCAP_OPENFLAG_DATATX_UDP 2 + + +/*! + \brief Defines if the remote probe will capture its own generated traffic. + + In case the remote probe uses the same interface to capture traffic and to send + data back to the caller, the captured traffic includes the RPCAP traffic as well. + If this flag is turned on, the RPCAP traffic is excluded from the capture, so that + the trace returned back to the collector is does not include this traffic. +*/ +#define PCAP_OPENFLAG_NOCAPTURE_RPCAP 4 + +/*! + \brief Defines if the local adapter will capture its own generated traffic. + + This flag tells the underlying capture driver to drop the packets that were sent by itself. + This is usefult when building applications like bridges, that should ignore the traffic + they just sent. +*/ +#define PCAP_OPENFLAG_NOCAPTURE_LOCAL 8 + +/*! + \brief This flag configures the adapter for maximum responsiveness. + + In presence of a large value for nbytes, WinPcap waits for the arrival of several packets before + copying the data to the user. This guarantees a low number of system calls, i.e. lower processor usage, + i.e. better performance, which is good for applications like sniffers. If the user sets the + PCAP_OPENFLAG_MAX_RESPONSIVENESS flag, the capture driver will copy the packets as soon as the application + is ready to receive them. This is suggested for real time applications (like, for example, a bridge) + that need the best responsiveness.*/ +#define PCAP_OPENFLAG_MAX_RESPONSIVENESS 16 + +/*! + \} +*/ + + +/*! + \addtogroup remote_samp_methods + \{ +*/ + +/*! + \brief No sampling has to be done on the current capture. + + In this case, no sampling algorithms are applied to the current capture. +*/ +#define PCAP_SAMP_NOSAMP 0 + +/*! + \brief It defines that only 1 out of N packets must be returned to the user. + + In this case, the 'value' field of the 'pcap_samp' structure indicates the + number of packets (minus 1) that must be discarded before one packet got accepted. + In other words, if 'value = 10', the first packet is returned to the caller, while + the following 9 are discarded. +*/ +#define PCAP_SAMP_1_EVERY_N 1 + +/*! + \brief It defines that we have to return 1 packet every N milliseconds. + + In this case, the 'value' field of the 'pcap_samp' structure indicates the 'waiting + time' in milliseconds before one packet got accepted. + In other words, if 'value = 10', the first packet is returned to the caller; the next + returned one will be the first packet that arrives when 10ms have elapsed. +*/ +#define PCAP_SAMP_FIRST_AFTER_N_MS 2 + +/*! + \} +*/ + + +/*! + \addtogroup remote_auth_methods + \{ +*/ + +/*! + \brief It defines the NULL authentication. + + This value has to be used within the 'type' member of the pcap_rmtauth structure. + The 'NULL' authentication has to be equal to 'zero', so that old applications + can just put every field of struct pcap_rmtauth to zero, and it does work. +*/ +#define RPCAP_RMTAUTH_NULL 0 +/*! + \brief It defines the username/password authentication. + + With this type of authentication, the RPCAP protocol will use the username/ + password provided to authenticate the user on the remote machine. If the + authentication is successful (and the user has the right to open network devices) + the RPCAP connection will continue; otherwise it will be dropped. + + This value has to be used within the 'type' member of the pcap_rmtauth structure. +*/ +#define RPCAP_RMTAUTH_PWD 1 + +/*! + \} +*/ + + + + +/*! + + \brief This structure keeps the information needed to autheticate + the user on a remote machine. + + The remote machine can either grant or refuse the access according + to the information provided. + In case the NULL authentication is required, both 'username' and + 'password' can be NULL pointers. + + This structure is meaningless if the source is not a remote interface; + in that case, the functions which requires such a structure can accept + a NULL pointer as well. +*/ +struct pcap_rmtauth +{ + /*! + \brief Type of the authentication required. + + In order to provide maximum flexibility, we can support different types + of authentication based on the value of this 'type' variable. The currently + supported authentication methods are defined into the + \link remote_auth_methods Remote Authentication Methods Section\endlink. + + */ + int type; + /*! + \brief Zero-terminated string containing the username that has to be + used on the remote machine for authentication. + + This field is meaningless in case of the RPCAP_RMTAUTH_NULL authentication + and it can be NULL. + */ + char *username; + /*! + \brief Zero-terminated string containing the password that has to be + used on the remote machine for authentication. + + This field is meaningless in case of the RPCAP_RMTAUTH_NULL authentication + and it can be NULL. + */ + char *password; +}; + + +/*! + \brief This structure defines the information related to sampling. + + In case the sampling is requested, the capturing device should read + only a subset of the packets coming from the source. The returned packets depend + on the sampling parameters. + + \warning The sampling process is applied after the filtering process. + In other words, packets are filtered first, then the sampling process selects a + subset of the 'filtered' packets and it returns them to the caller. +*/ +struct pcap_samp +{ + /*! + Method used for sampling. Currently, the supported methods are listed in the + \link remote_samp_methods Sampling Methods Section\endlink. + */ + int method; + + /*! + This value depends on the sampling method defined. For its meaning, please check + at the \link remote_samp_methods Sampling Methods Section\endlink. + */ + int value; +}; + + + + +//! Maximum length of an host name (needed for the RPCAP active mode) +#define RPCAP_HOSTLIST_SIZE 1024 + + +/*! + \} +*/ // end of public documentation + + +// Exported functions + + + +/** \name New WinPcap functions + + This section lists the new functions that are able to help considerably in writing + WinPcap programs because of their easiness of use. + */ +//\{ +pcap_t *pcap_open(const char *source, int snaplen, int flags, int read_timeout, struct pcap_rmtauth *auth, char *errbuf); +int pcap_createsrcstr(char *source, int type, const char *host, const char *port, const char *name, char *errbuf); +int pcap_parsesrcstr(const char *source, int *type, char *host, char *port, char *name, char *errbuf); +int pcap_findalldevs_ex(char *source, struct pcap_rmtauth *auth, pcap_if_t **alldevs, char *errbuf); +struct pcap_samp *pcap_setsampling(pcap_t *p); + +//\} +// End of new winpcap functions + + + +/** \name Remote Capture functions + */ +//\{ +SOCKET pcap_remoteact_accept(const char *address, const char *port, const char *hostlist, char *connectinghost, struct pcap_rmtauth *auth, char *errbuf); +int pcap_remoteact_list(char *hostlist, char sep, int size, char *errbuf); +int pcap_remoteact_close(const char *host, char *errbuf); +void pcap_remoteact_cleanup(); +//\} +// End of remote capture functions + +#ifdef __cplusplus +} +#endif + + +#endif + diff --git a/soem-src/soem/ethercat.h b/soem-src/soem/ethercat.h new file mode 100644 index 0000000..d87d748 --- /dev/null +++ b/soem-src/soem/ethercat.h @@ -0,0 +1,26 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +/** \file + * \brief + * Headerfile for all ethercat headers + */ + +#ifndef _EC_ETHERCAT_H +#define _EC_ETHERCAT_H + +#include "ethercattype.h" +#include "nicdrv.h" +#include "ethercatbase.h" +#include "ethercatmain.h" +#include "ethercatdc.h" +#include "ethercatcoe.h" +#include "ethercatfoe.h" +#include "ethercatsoe.h" +#include "ethercateoe.h" +#include "ethercatconfig.h" +#include "ethercatprint.h" + +#endif /* _EC_ETHERCAT_H */ diff --git a/soem-src/soem/ethercatbase.c b/soem-src/soem/ethercatbase.c new file mode 100644 index 0000000..ba92563 --- /dev/null +++ b/soem-src/soem/ethercatbase.c @@ -0,0 +1,637 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +/** \file + * \brief + * Base EtherCAT functions. + * + * Setting up a datagram in an ethernet frame. + * EtherCAT datagram primitives, broadcast, auto increment, configured and + * logical addressed data transfers. All base transfers are blocking, so + * wait for the frame to be returned to the master or timeout. If this is + * not acceptable build your own datagrams and use the functions from nicdrv.c. + */ + +#include +#include +#include "oshw.h" +#include "osal.h" +#include "ethercattype.h" +#include "ethercatbase.h" + +/** Write data to EtherCAT datagram. + * + * @param[out] datagramdata = data part of datagram + * @param[in] com = command + * @param[in] length = length of databuffer + * @param[in] data = databuffer to be copied into datagram + */ +static void ecx_writedatagramdata(void *datagramdata, ec_cmdtype com, uint16 length, const void * data) +{ + if (length > 0) + { + switch (com) + { + case EC_CMD_NOP: + /* Fall-through */ + case EC_CMD_APRD: + /* Fall-through */ + case EC_CMD_FPRD: + /* Fall-through */ + case EC_CMD_BRD: + /* Fall-through */ + case EC_CMD_LRD: + /* no data to write. initialise data so frame is in a known state */ + memset(datagramdata, 0, length); + break; + default: + memcpy(datagramdata, data, length); + break; + } + } +} + +/** Generate and set EtherCAT datagram in a standard ethernet frame. + * + * @param[in] port = port context struct + * @param[out] frame = framebuffer + * @param[in] com = command + * @param[in] idx = index used for TX and RX buffers + * @param[in] ADP = Address Position + * @param[in] ADO = Address Offset + * @param[in] length = length of datagram excluding EtherCAT header + * @param[in] data = databuffer to be copied in datagram + * @return always 0 + */ +int ecx_setupdatagram(ecx_portt *port, void *frame, uint8 com, uint8 idx, uint16 ADP, uint16 ADO, uint16 length, void *data) +{ + ec_comt *datagramP; + uint8 *frameP; + + frameP = frame; + /* Ethernet header is preset and fixed in frame buffers + EtherCAT header needs to be added after that */ + datagramP = (ec_comt*)&frameP[ETH_HEADERSIZE]; + datagramP->elength = htoes(EC_ECATTYPE + EC_HEADERSIZE + length); + datagramP->command = com; + datagramP->index = idx; + datagramP->ADP = htoes(ADP); + datagramP->ADO = htoes(ADO); + datagramP->dlength = htoes(length); + ecx_writedatagramdata(&frameP[ETH_HEADERSIZE + EC_HEADERSIZE], com, length, data); + /* set WKC to zero */ + frameP[ETH_HEADERSIZE + EC_HEADERSIZE + length] = 0x00; + frameP[ETH_HEADERSIZE + EC_HEADERSIZE + length + 1] = 0x00; + /* set size of frame in buffer array */ + port->txbuflength[idx] = ETH_HEADERSIZE + EC_HEADERSIZE + EC_WKCSIZE + length; + + return 0; +} + +/** Add EtherCAT datagram to a standard ethernet frame with existing datagram(s). + * + * @param[in] port = port context struct + * @param[out] frame = framebuffer + * @param[in] com = command + * @param[in] idx = index used for TX and RX buffers + * @param[in] more = TRUE if still more datagrams to follow + * @param[in] ADP = Address Position + * @param[in] ADO = Address Offset + * @param[in] length = length of datagram excluding EtherCAT header + * @param[in] data = databuffer to be copied in datagram + * @return Offset to data in rx frame, usefull to retrieve data after RX. + */ +uint16 ecx_adddatagram(ecx_portt *port, void *frame, uint8 com, uint8 idx, boolean more, uint16 ADP, uint16 ADO, uint16 length, void *data) +{ + ec_comt *datagramP; + uint8 *frameP; + uint16 prevlength; + + frameP = frame; + /* copy previous frame size */ + prevlength = (uint16)port->txbuflength[idx]; + datagramP = (ec_comt*)&frameP[ETH_HEADERSIZE]; + /* add new datagram to ethernet frame size */ + datagramP->elength = htoes( etohs(datagramP->elength) + EC_HEADERSIZE + length ); + /* add "datagram follows" flag to previous subframe dlength */ + datagramP->dlength = htoes( etohs(datagramP->dlength) | EC_DATAGRAMFOLLOWS ); + /* set new EtherCAT header position */ + datagramP = (ec_comt*)&frameP[prevlength - EC_ELENGTHSIZE]; + datagramP->command = com; + datagramP->index = idx; + datagramP->ADP = htoes(ADP); + datagramP->ADO = htoes(ADO); + if (more) + { + /* this is not the last datagram to add */ + datagramP->dlength = htoes(length | EC_DATAGRAMFOLLOWS); + } + else + { + /* this is the last datagram in the frame */ + datagramP->dlength = htoes(length); + } + ecx_writedatagramdata(&frameP[prevlength + EC_HEADERSIZE - EC_ELENGTHSIZE], com, length, data); + /* set WKC to zero */ + frameP[prevlength + EC_HEADERSIZE - EC_ELENGTHSIZE + length] = 0x00; + frameP[prevlength + EC_HEADERSIZE - EC_ELENGTHSIZE + length + 1] = 0x00; + /* set size of frame in buffer array */ + port->txbuflength[idx] = prevlength + EC_HEADERSIZE - EC_ELENGTHSIZE + EC_WKCSIZE + length; + + /* return offset to data in rx frame + 14 bytes smaller than tx frame due to stripping of ethernet header */ + return prevlength + EC_HEADERSIZE - EC_ELENGTHSIZE - ETH_HEADERSIZE; +} + +/** BRW "broadcast write" primitive. Blocking. + * + * @param[in] port = port context struct + * @param[in] ADP = Address Position, normally 0 + * @param[in] ADO = Address Offset, slave memory address + * @param[in] length = length of databuffer + * @param[in] data = databuffer to be written to slaves + * @param[in] timeout = timeout in us, standard is EC_TIMEOUTRET + * @return Workcounter or EC_NOFRAME + */ +int ecx_BWR (ecx_portt *port, uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout) +{ + uint8 idx; + int wkc; + + /* get fresh index */ + idx = ecx_getindex (port); + /* setup datagram */ + ecx_setupdatagram (port, &(port->txbuf[idx]), EC_CMD_BWR, idx, ADP, ADO, length, data); + /* send data and wait for answer */ + wkc = ecx_srconfirm (port, idx, timeout); + /* clear buffer status */ + ecx_setbufstat (port, idx, EC_BUF_EMPTY); + + return wkc; +} + +/** BRD "broadcast read" primitive. Blocking. + * + * @param[in] port = port context struct + * @param[in] ADP = Address Position, normally 0 + * @param[in] ADO = Address Offset, slave memory address + * @param[in] length = length of databuffer + * @param[out] data = databuffer to put slave data in + * @param[in] timeout = timeout in us, standard is EC_TIMEOUTRET + * @return Workcounter or EC_NOFRAME + */ +int ecx_BRD(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout) +{ + uint8 idx; + int wkc; + + /* get fresh index */ + idx = ecx_getindex(port); + /* setup datagram */ + ecx_setupdatagram(port, &(port->txbuf[idx]), EC_CMD_BRD, idx, ADP, ADO, length, data); + /* send data and wait for answer */ + wkc = ecx_srconfirm (port, idx, timeout); + if (wkc > 0) + { + /* copy datagram to data buffer */ + memcpy(data, &(port->rxbuf[idx][EC_HEADERSIZE]), length); + } + /* clear buffer status */ + ecx_setbufstat(port, idx, EC_BUF_EMPTY); + + return wkc; +} + +/** APRD "auto increment address read" primitive. Blocking. + * + * @param[in] port = port context struct + * @param[in] ADP = Address Position, each slave ++, slave that has 0 executes + * @param[in] ADO = Address Offset, slave memory address + * @param[in] length = length of databuffer + * @param[out] data = databuffer to put slave data in + * @param[in] timeout = timeout in us, standard is EC_TIMEOUTRET + * @return Workcounter or EC_NOFRAME + */ +int ecx_APRD(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout) +{ + int wkc; + uint8 idx; + + idx = ecx_getindex(port); + ecx_setupdatagram(port, &(port->txbuf[idx]), EC_CMD_APRD, idx, ADP, ADO, length, data); + wkc = ecx_srconfirm(port, idx, timeout); + if (wkc > 0) + { + memcpy(data, &(port->rxbuf[idx][EC_HEADERSIZE]), length); + } + ecx_setbufstat(port, idx, EC_BUF_EMPTY); + + return wkc; +} + +/** APRMW "auto increment address read, multiple write" primitive. Blocking. + * + * @param[in] port = port context struct + * @param[in] ADP = Address Position, each slave ++, slave that has 0 reads, + * following slaves write. + * @param[in] ADO = Address Offset, slave memory address + * @param[in] length = length of databuffer + * @param[out] data = databuffer to put slave data in + * @param[in] timeout = timeout in us, standard is EC_TIMEOUTRET + * @return Workcounter or EC_NOFRAME + */ +int ecx_ARMW(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout) +{ + int wkc; + uint8 idx; + + idx = ecx_getindex(port); + ecx_setupdatagram(port, &(port->txbuf[idx]), EC_CMD_ARMW, idx, ADP, ADO, length, data); + wkc = ecx_srconfirm(port, idx, timeout); + if (wkc > 0) + { + memcpy(data, &(port->rxbuf[idx][EC_HEADERSIZE]), length); + } + ecx_setbufstat(port, idx, EC_BUF_EMPTY); + + return wkc; +} + +/** FPRMW "configured address read, multiple write" primitive. Blocking. + * + * @param[in] port = port context struct + * @param[in] ADP = Address Position, slave that has address reads, + * following slaves write. + * @param[in] ADO = Address Offset, slave memory address + * @param[in] length = length of databuffer + * @param[out] data = databuffer to put slave data in + * @param[in] timeout = timeout in us, standard is EC_TIMEOUTRET + * @return Workcounter or EC_NOFRAME + */ +int ecx_FRMW(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout) +{ + int wkc; + uint8 idx; + + idx = ecx_getindex(port); + ecx_setupdatagram(port, &(port->txbuf[idx]), EC_CMD_FRMW, idx, ADP, ADO, length, data); + wkc = ecx_srconfirm(port, idx, timeout); + if (wkc > 0) + { + memcpy(data, &(port->rxbuf[idx][EC_HEADERSIZE]), length); + } + ecx_setbufstat(port, idx, EC_BUF_EMPTY); + + return wkc; +} + +/** APRDw "auto increment address read" word return primitive. Blocking. + * + * @param[in] port = port context struct + * @param[in] ADP = Address Position, each slave ++, slave that has 0 reads. + * @param[in] ADO = Address Offset, slave memory address + * @param[in] timeout = timeout in us, standard is EC_TIMEOUTRET + * @return word data from slave + */ +uint16 ecx_APRDw(ecx_portt *port, uint16 ADP, uint16 ADO, int timeout) +{ + uint16 w; + + w = 0; + ecx_APRD(port, ADP, ADO, sizeof(w), &w, timeout); + + return w; +} + +/** FPRD "configured address read" primitive. Blocking. + * + * @param[in] port = port context struct + * @param[in] ADP = Address Position, slave that has address reads. + * @param[in] ADO = Address Offset, slave memory address + * @param[in] length = length of databuffer + * @param[out] data = databuffer to put slave data in + * @param[in] timeout = timeout in us, standard is EC_TIMEOUTRET + * @return Workcounter or EC_NOFRAME + */ +int ecx_FPRD(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout) +{ + int wkc; + uint8 idx; + + idx = ecx_getindex(port); + ecx_setupdatagram(port, &(port->txbuf[idx]), EC_CMD_FPRD, idx, ADP, ADO, length, data); + wkc = ecx_srconfirm(port, idx, timeout); + if (wkc > 0) + { + memcpy(data, &(port->rxbuf[idx][EC_HEADERSIZE]), length); + } + ecx_setbufstat(port, idx, EC_BUF_EMPTY); + + return wkc; +} + +/** FPRDw "configured address read" word return primitive. Blocking. + * + * @param[in] port = port context struct + * @param[in] ADP = Address Position, slave that has address reads. + * @param[in] ADO = Address Offset, slave memory address + * @param[in] timeout = timeout in us, standard is EC_TIMEOUTRET + * @return word data from slave + */ +uint16 ecx_FPRDw(ecx_portt *port, uint16 ADP, uint16 ADO, int timeout) +{ + uint16 w; + + w = 0; + ecx_FPRD(port, ADP, ADO, sizeof(w), &w, timeout); + return w; +} + +/** APWR "auto increment address write" primitive. Blocking. + * + * @param[in] port = port context struct + * @param[in] ADP = Address Position, each slave ++, slave that has 0 writes. + * @param[in] ADO = Address Offset, slave memory address + * @param[in] length = length of databuffer + * @param[in] data = databuffer to write to slave. + * @param[in] timeout = timeout in us, standard is EC_TIMEOUTRET + * @return Workcounter or EC_NOFRAME + */ +int ecx_APWR(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout) +{ + uint8 idx; + int wkc; + + idx = ecx_getindex(port); + ecx_setupdatagram(port, &(port->txbuf[idx]), EC_CMD_APWR, idx, ADP, ADO, length, data); + wkc = ecx_srconfirm(port, idx, timeout); + ecx_setbufstat(port, idx, EC_BUF_EMPTY); + + return wkc; +} + +/** APWRw "auto increment address write" word primitive. Blocking. + * + * @param[in] port = port context struct + * @param[in] ADP = Address Position, each slave ++, slave that has 0 writes. + * @param[in] ADO = Address Offset, slave memory address + * @param[in] data = word data to write to slave. + * @param[in] timeout = timeout in us, standard is EC_TIMEOUTRET + * @return Workcounter or EC_NOFRAME + */ +int ecx_APWRw(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 data, int timeout) +{ + return ecx_APWR(port, ADP, ADO, sizeof(data), &data, timeout); +} + +/** FPWR "configured address write" primitive. Blocking. + * + * @param[in] port = port context struct + * @param[in] ADP = Address Position, slave that has address writes. + * @param[in] ADO = Address Offset, slave memory address + * @param[in] length = length of databuffer + * @param[in] data = databuffer to write to slave. + * @param[in] timeout = timeout in us, standard is EC_TIMEOUTRET + * @return Workcounter or EC_NOFRAME + */ +int ecx_FPWR(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout) +{ + int wkc; + uint8 idx; + + idx = ecx_getindex(port); + ecx_setupdatagram(port, &(port->txbuf[idx]), EC_CMD_FPWR, idx, ADP, ADO, length, data); + wkc = ecx_srconfirm(port, idx, timeout); + ecx_setbufstat(port, idx, EC_BUF_EMPTY); + + return wkc; +} + +/** FPWR "configured address write" primitive. Blocking. + * + * @param[in] port = port context struct + * @param[in] ADP = Address Position, slave that has address writes. + * @param[in] ADO = Address Offset, slave memory address + * @param[in] data = word to write to slave. + * @param[in] timeout = timeout in us, standard is EC_TIMEOUTRET + * @return Workcounter or EC_NOFRAME + */ +int ecx_FPWRw(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 data, int timeout) +{ + return ecx_FPWR(port, ADP, ADO, sizeof(data), &data, timeout); +} + +/** LRW "logical memory read / write" primitive. Blocking. + * + * @param[in] port = port context struct + * @param[in] LogAdr = Logical memory address + * @param[in] length = length of databuffer + * @param[in,out] data = databuffer to write to and read from slave. + * @param[in] timeout = timeout in us, standard is EC_TIMEOUTRET + * @return Workcounter or EC_NOFRAME + */ +int ecx_LRW(ecx_portt *port, uint32 LogAdr, uint16 length, void *data, int timeout) +{ + uint8 idx; + int wkc; + + idx = ecx_getindex(port); + ecx_setupdatagram(port, &(port->txbuf[idx]), EC_CMD_LRW, idx, LO_WORD(LogAdr), HI_WORD(LogAdr), length, data); + wkc = ecx_srconfirm(port, idx, timeout); + if ((wkc > 0) && (port->rxbuf[idx][EC_CMDOFFSET] == EC_CMD_LRW)) + { + memcpy(data, &(port->rxbuf[idx][EC_HEADERSIZE]), length); + } + ecx_setbufstat(port, idx, EC_BUF_EMPTY); + + return wkc; +} + +/** LRD "logical memory read" primitive. Blocking. + * + * @param[in] port = port context struct + * @param[in] LogAdr = Logical memory address + * @param[in] length = length of bytes to read from slave. + * @param[out] data = databuffer to read from slave. + * @param[in] timeout = timeout in us, standard is EC_TIMEOUTRET + * @return Workcounter or EC_NOFRAME + */ +int ecx_LRD(ecx_portt *port, uint32 LogAdr, uint16 length, void *data, int timeout) +{ + uint8 idx; + int wkc; + + idx = ecx_getindex(port); + ecx_setupdatagram(port, &(port->txbuf[idx]), EC_CMD_LRD, idx, LO_WORD(LogAdr), HI_WORD(LogAdr), length, data); + wkc = ecx_srconfirm(port, idx, timeout); + if ((wkc > 0) && (port->rxbuf[idx][EC_CMDOFFSET]==EC_CMD_LRD)) + { + memcpy(data, &(port->rxbuf[idx][EC_HEADERSIZE]), length); + } + ecx_setbufstat(port, idx, EC_BUF_EMPTY); + + return wkc; +} + +/** LWR "logical memory write" primitive. Blocking. + * + * @param[in] port = port context struct + * @param[in] LogAdr = Logical memory address + * @param[in] length = length of databuffer + * @param[in] data = databuffer to write to slave. + * @param[in] timeout = timeout in us, standard is EC_TIMEOUTRET + * @return Workcounter or EC_NOFRAME + */ +int ecx_LWR(ecx_portt *port, uint32 LogAdr, uint16 length, void *data, int timeout) +{ + uint8 idx; + int wkc; + + idx = ecx_getindex(port); + ecx_setupdatagram(port, &(port->txbuf[idx]), EC_CMD_LWR, idx, LO_WORD(LogAdr), HI_WORD(LogAdr), length, data); + wkc = ecx_srconfirm(port, idx, timeout); + ecx_setbufstat(port, idx, EC_BUF_EMPTY); + + return wkc; +} + +/** LRW "logical memory read / write" primitive plus Clock Distribution. Blocking. + * Frame consists of two datagrams, one LRW and one FPRMW. + * + * @param[in] port = port context struct + * @param[in] LogAdr = Logical memory address + * @param[in] length = length of databuffer + * @param[in,out] data = databuffer to write to and read from slave. + * @param[in] DCrs = Distributed Clock reference slave address. + * @param[out] DCtime = DC time read from reference slave. + * @param[in] timeout = timeout in us, standard is EC_TIMEOUTRET + * @return Workcounter or EC_NOFRAME + */ +int ecx_LRWDC(ecx_portt *port, uint32 LogAdr, uint16 length, void *data, uint16 DCrs, int64 *DCtime, int timeout) +{ + uint16 DCtO; + uint8 idx; + int wkc; + uint64 DCtE; + + idx = ecx_getindex(port); + /* LRW in first datagram */ + ecx_setupdatagram(port, &(port->txbuf[idx]), EC_CMD_LRW, idx, LO_WORD(LogAdr), HI_WORD(LogAdr), length, data); + /* FPRMW in second datagram */ + DCtE = htoell(*DCtime); + DCtO = ecx_adddatagram(port, &(port->txbuf[idx]), EC_CMD_FRMW, idx, FALSE, DCrs, ECT_REG_DCSYSTIME, sizeof(DCtime), &DCtE); + wkc = ecx_srconfirm(port, idx, timeout); + if ((wkc > 0) && (port->rxbuf[idx][EC_CMDOFFSET] == EC_CMD_LRW)) + { + memcpy(data, &(port->rxbuf[idx][EC_HEADERSIZE]), length); + memcpy(&wkc, &(port->rxbuf[idx][EC_HEADERSIZE + length]), EC_WKCSIZE); + memcpy(&DCtE, &(port->rxbuf[idx][DCtO]), sizeof(*DCtime)); + *DCtime = etohll(DCtE); + } + ecx_setbufstat(port, idx, EC_BUF_EMPTY); + + return wkc; +} + +#ifdef EC_VER1 +int ec_setupdatagram(void *frame, uint8 com, uint8 idx, uint16 ADP, uint16 ADO, uint16 length, void *data) +{ + return ecx_setupdatagram (&ecx_port, frame, com, idx, ADP, ADO, length, data); +} + +uint16 ec_adddatagram (void *frame, uint8 com, uint8 idx, boolean more, uint16 ADP, uint16 ADO, uint16 length, void *data) +{ + return ecx_adddatagram (&ecx_port, frame, com, idx, more, ADP, ADO, length, data); +} + +int ec_BWR(uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout) +{ + return ecx_BWR (&ecx_port, ADP, ADO, length, data, timeout); +} + +int ec_BRD(uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout) +{ + return ecx_BRD(&ecx_port, ADP, ADO, length, data, timeout); +} + +int ec_APRD(uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout) +{ + return ecx_APRD(&ecx_port, ADP, ADO, length, data, timeout); +} + +int ec_ARMW(uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout) +{ + return ecx_ARMW(&ecx_port, ADP, ADO, length, data, timeout); +} + +int ec_FRMW(uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout) +{ + return ecx_FRMW(&ecx_port, ADP, ADO, length, data, timeout); +} + +uint16 ec_APRDw(uint16 ADP, uint16 ADO, int timeout) +{ + uint16 w; + + w = 0; + ec_APRD(ADP, ADO, sizeof(w), &w, timeout); + + return w; +} + +int ec_FPRD(uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout) +{ + return ecx_FPRD(&ecx_port, ADP, ADO, length, data, timeout); +} + +uint16 ec_FPRDw(uint16 ADP, uint16 ADO, int timeout) +{ + uint16 w; + + w = 0; + ec_FPRD(ADP, ADO, sizeof(w), &w, timeout); + return w; +} + +int ec_APWR(uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout) +{ + return ecx_APWR(&ecx_port, ADP, ADO, length, data, timeout); +} + +int ec_APWRw(uint16 ADP, uint16 ADO, uint16 data, int timeout) +{ + return ec_APWR(ADP, ADO, sizeof(data), &data, timeout); +} + +int ec_FPWR(uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout) +{ + return ecx_FPWR(&ecx_port, ADP, ADO, length, data, timeout); +} + +int ec_FPWRw(uint16 ADP, uint16 ADO, uint16 data, int timeout) +{ + return ec_FPWR(ADP, ADO, sizeof(data), &data, timeout); +} + +int ec_LRW(uint32 LogAdr, uint16 length, void *data, int timeout) +{ + return ecx_LRW(&ecx_port, LogAdr, length, data, timeout); +} + +int ec_LRD(uint32 LogAdr, uint16 length, void *data, int timeout) +{ + return ecx_LRD(&ecx_port, LogAdr, length, data, timeout); +} + +int ec_LWR(uint32 LogAdr, uint16 length, void *data, int timeout) +{ + return ecx_LWR(&ecx_port, LogAdr, length, data, timeout); +} + +int ec_LRWDC(uint32 LogAdr, uint16 length, void *data, uint16 DCrs, int64 *DCtime, int timeout) +{ + return ecx_LRWDC(&ecx_port, LogAdr, length, data, DCrs, DCtime, timeout); +} +#endif diff --git a/soem-src/soem/ethercatbase.h b/soem-src/soem/ethercatbase.h new file mode 100644 index 0000000..e1ee687 --- /dev/null +++ b/soem-src/soem/ethercatbase.h @@ -0,0 +1,63 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +/** \file + * \brief + * Headerfile for ethercatbase.c + */ + +#ifndef _ethercatbase_ +#define _ethercatbase_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +int ecx_setupdatagram(ecx_portt *port, void *frame, uint8 com, uint8 idx, uint16 ADP, uint16 ADO, uint16 length, void *data); +uint16 ecx_adddatagram(ecx_portt *port, void *frame, uint8 com, uint8 idx, boolean more, uint16 ADP, uint16 ADO, uint16 length, void *data); +int ecx_BWR(ecx_portt *port, uint16 ADP,uint16 ADO,uint16 length,void *data,int timeout); +int ecx_BRD(ecx_portt *port, uint16 ADP,uint16 ADO,uint16 length,void *data,int timeout); +int ecx_APRD(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout); +int ecx_ARMW(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout); +int ecx_FRMW(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout); +uint16 ecx_APRDw(ecx_portt *port, uint16 ADP, uint16 ADO, int timeout); +int ecx_FPRD(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout); +uint16 ecx_FPRDw(ecx_portt *port, uint16 ADP, uint16 ADO, int timeout); +int ecx_APWRw(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 data, int timeout); +int ecx_APWR(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout); +int ecx_FPWRw(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 data, int timeout); +int ecx_FPWR(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout); +int ecx_LRW(ecx_portt *port, uint32 LogAdr, uint16 length, void *data, int timeout); +int ecx_LRD(ecx_portt *port, uint32 LogAdr, uint16 length, void *data, int timeout); +int ecx_LWR(ecx_portt *port, uint32 LogAdr, uint16 length, void *data, int timeout); +int ecx_LRWDC(ecx_portt *port, uint32 LogAdr, uint16 length, void *data, uint16 DCrs, int64 *DCtime, int timeout); + +#ifdef EC_VER1 +int ec_setupdatagram(void *frame, uint8 com, uint8 idx, uint16 ADP, uint16 ADO, uint16 length, void *data); +uint16 ec_adddatagram(void *frame, uint8 com, uint8 idx, boolean more, uint16 ADP, uint16 ADO, uint16 length, void *data); +int ec_BWR(uint16 ADP,uint16 ADO,uint16 length,void *data,int timeout); +int ec_BRD(uint16 ADP,uint16 ADO,uint16 length,void *data,int timeout); +int ec_APRD(uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout); +int ec_ARMW(uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout); +int ec_FRMW(uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout); +uint16 ec_APRDw(uint16 ADP, uint16 ADO, int timeout); +int ec_FPRD(uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout); +uint16 ec_FPRDw(uint16 ADP, uint16 ADO, int timeout); +int ec_APWRw(uint16 ADP, uint16 ADO, uint16 data, int timeout); +int ec_APWR(uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout); +int ec_FPWRw(uint16 ADP, uint16 ADO, uint16 data, int timeout); +int ec_FPWR(uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout); +int ec_LRW(uint32 LogAdr, uint16 length, void *data, int timeout); +int ec_LRD(uint32 LogAdr, uint16 length, void *data, int timeout); +int ec_LWR(uint32 LogAdr, uint16 length, void *data, int timeout); +int ec_LRWDC(uint32 LogAdr, uint16 length, void *data, uint16 DCrs, int64 *DCtime, int timeout); +#endif + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/soem-src/soem/ethercatcoe.c b/soem-src/soem/ethercatcoe.c new file mode 100644 index 0000000..3f29e1f --- /dev/null +++ b/soem-src/soem/ethercatcoe.c @@ -0,0 +1,1505 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +/** \file + * \brief + * CAN over EtherCAT (CoE) module. + * + * SDO read / write and SDO service functions + */ + +#include +#include +#include "osal.h" +#include "oshw.h" +#include "ethercattype.h" +#include "ethercatbase.h" +#include "ethercatmain.h" +#include "ethercatcoe.h" + +/** SDO structure, not to be confused with EcSDOserviceT */ +PACKED_BEGIN +typedef struct PACKED +{ + ec_mbxheadert MbxHeader; + uint16 CANOpen; + uint8 Command; + uint16 Index; + uint8 SubIndex; + union + { + uint8 bdata[0x200]; /* variants for easy data access */ + uint16 wdata[0x100]; + uint32 ldata[0x80]; + }; +} ec_SDOt; +PACKED_END + +/** SDO service structure */ +PACKED_BEGIN +typedef struct PACKED +{ + ec_mbxheadert MbxHeader; + uint16 CANOpen; + uint8 Opcode; + uint8 Reserved; + uint16 Fragments; + union + { + uint8 bdata[0x200]; /* variants for easy data access */ + uint16 wdata[0x100]; + uint32 ldata[0x80]; + }; +} ec_SDOservicet; +PACKED_END + +/** Report SDO error. + * + * @param[in] context = context struct + * @param[in] Slave = Slave number + * @param[in] Index = Index that generated error + * @param[in] SubIdx = Subindex that generated error + * @param[in] AbortCode = Abortcode, see EtherCAT documentation for list + */ +void ecx_SDOerror(ecx_contextt *context, uint16 Slave, uint16 Index, uint8 SubIdx, int32 AbortCode) +{ + ec_errort Ec; + + memset(&Ec, 0, sizeof(Ec)); + Ec.Time = osal_current_time(); + Ec.Slave = Slave; + Ec.Index = Index; + Ec.SubIdx = SubIdx; + *(context->ecaterror) = TRUE; + Ec.Etype = EC_ERR_TYPE_SDO_ERROR; + Ec.AbortCode = AbortCode; + ecx_pusherror(context, &Ec); +} + +/** Report SDO info error + * + * @param[in] context = context struct + * @param[in] Slave = Slave number + * @param[in] Index = Index that generated error + * @param[in] SubIdx = Subindex that generated error + * @param[in] AbortCode = Abortcode, see EtherCAT documentation for list + */ +static void ecx_SDOinfoerror(ecx_contextt *context, uint16 Slave, uint16 Index, uint8 SubIdx, int32 AbortCode) +{ + ec_errort Ec; + + memset(&Ec, 0, sizeof(Ec)); + Ec.Slave = Slave; + Ec.Index = Index; + Ec.SubIdx = SubIdx; + *(context->ecaterror) = TRUE; + Ec.Etype = EC_ERR_TYPE_SDOINFO_ERROR; + Ec.AbortCode = AbortCode; + ecx_pusherror(context, &Ec); +} + +/** CoE SDO read, blocking. Single subindex or Complete Access. + * + * Only a "normal" upload request is issued. If the requested parameter is <= 4bytes + * then a "expedited" response is returned, otherwise a "normal" response. If a "normal" + * response is larger than the mailbox size then the response is segmented. The function + * will combine all segments and copy them to the parameter buffer. + * + * @param[in] context = context struct + * @param[in] slave = Slave number + * @param[in] index = Index to read + * @param[in] subindex = Subindex to read, must be 0 or 1 if CA is used. + * @param[in] CA = FALSE = single subindex. TRUE = Complete Access, all subindexes read. + * @param[in,out] psize = Size in bytes of parameter buffer, returns bytes read from SDO. + * @param[out] p = Pointer to parameter buffer + * @param[in] timeout = Timeout in us, standard is EC_TIMEOUTRXM + * @return Workcounter from last slave response + */ +int ecx_SDOread(ecx_contextt *context, uint16 slave, uint16 index, uint8 subindex, + boolean CA, int *psize, void *p, int timeout) +{ + ec_SDOt *SDOp, *aSDOp; + uint16 bytesize, Framedatasize; + int wkc; + int32 SDOlen; + uint8 *bp; + uint8 *hp; + ec_mbxbuft MbxIn, MbxOut; + uint8 cnt, toggle; + boolean NotLast; + + ec_clearmbx(&MbxIn); + /* Empty slave out mailbox if something is in. Timeout set to 0 */ + wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, 0); + ec_clearmbx(&MbxOut); + aSDOp = (ec_SDOt *)&MbxIn; + SDOp = (ec_SDOt *)&MbxOut; + SDOp->MbxHeader.length = htoes(0x000a); + SDOp->MbxHeader.address = htoes(0x0000); + SDOp->MbxHeader.priority = 0x00; + /* get new mailbox count value, used as session handle */ + cnt = ec_nextmbxcnt(context->slavelist[slave].mbx_cnt); + context->slavelist[slave].mbx_cnt = cnt; + SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + MBX_HDR_SET_CNT(cnt); /* CoE */ + SDOp->CANOpen = htoes(0x000 + (ECT_COES_SDOREQ << 12)); /* number 9bits service upper 4 bits (SDO request) */ + if (CA) + { + SDOp->Command = ECT_SDO_UP_REQ_CA; /* upload request complete access */ + } + else + { + SDOp->Command = ECT_SDO_UP_REQ; /* upload request normal */ + } + SDOp->Index = htoes(index); + if (CA && (subindex > 1)) + { + subindex = 1; + } + SDOp->SubIndex = subindex; + SDOp->ldata[0] = 0; + /* send CoE request to slave */ + wkc = ecx_mbxsend(context, slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM); + if (wkc > 0) /* succeeded to place mailbox in slave ? */ + { + /* clean mailboxbuffer */ + ec_clearmbx(&MbxIn); + /* read slave response */ + wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, timeout); + if (wkc > 0) /* succeeded to read slave response ? */ + { + /* slave response should be CoE, SDO response and the correct index */ + if (((aSDOp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_COE) && + ((etohs(aSDOp->CANOpen) >> 12) == ECT_COES_SDORES) && + (aSDOp->Index == SDOp->Index)) + { + if ((aSDOp->Command & 0x02) > 0) + { + /* expedited frame response */ + bytesize = 4 - ((aSDOp->Command >> 2) & 0x03); + if (*psize >= bytesize) /* parameter buffer big enough ? */ + { + /* copy parameter in parameter buffer */ + memcpy(p, &aSDOp->ldata[0], bytesize); + /* return the real parameter size */ + *psize = bytesize; + } + else + { + wkc = 0; + ecx_packeterror(context, slave, index, subindex, 3); /* data container too small for type */ + } + } + else + { /* normal frame response */ + SDOlen = etohl(aSDOp->ldata[0]); + /* Does parameter fit in parameter buffer ? */ + if (SDOlen <= *psize) + { + bp = p; + hp = p; + /* calculate mailbox transfer size */ + Framedatasize = (etohs(aSDOp->MbxHeader.length) - 10); + if (Framedatasize < SDOlen) /* transfer in segments? */ + { + /* copy parameter data in parameter buffer */ + memcpy(hp, &aSDOp->ldata[1], Framedatasize); + /* increment buffer pointer */ + hp += Framedatasize; + *psize = Framedatasize; + NotLast = TRUE; + toggle= 0x00; + while (NotLast) /* segmented transfer */ + { + SDOp = (ec_SDOt *)&MbxOut; + SDOp->MbxHeader.length = htoes(0x000a); + SDOp->MbxHeader.address = htoes(0x0000); + SDOp->MbxHeader.priority = 0x00; + cnt = ec_nextmbxcnt(context->slavelist[slave].mbx_cnt); + context->slavelist[slave].mbx_cnt = cnt; + SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + MBX_HDR_SET_CNT(cnt); /* CoE */ + SDOp->CANOpen = htoes(0x000 + (ECT_COES_SDOREQ << 12)); /* number 9bits service upper 4 bits (SDO request) */ + SDOp->Command = ECT_SDO_SEG_UP_REQ + toggle; /* segment upload request */ + SDOp->Index = htoes(index); + SDOp->SubIndex = subindex; + SDOp->ldata[0] = 0; + /* send segmented upload request to slave */ + wkc = ecx_mbxsend(context, slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM); + /* is mailbox transferred to slave ? */ + if (wkc > 0) + { + ec_clearmbx(&MbxIn); + /* read slave response */ + wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, timeout); + /* has slave responded ? */ + if (wkc > 0) + { + /* slave response should be CoE, SDO response */ + if ((((aSDOp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_COE) && + ((etohs(aSDOp->CANOpen) >> 12) == ECT_COES_SDORES) && + ((aSDOp->Command & 0xe0) == 0x00))) + { + /* calculate mailbox transfer size */ + Framedatasize = etohs(aSDOp->MbxHeader.length) - 3; + if ((aSDOp->Command & 0x01) > 0) + { /* last segment */ + NotLast = FALSE; + if (Framedatasize == 7) + /* subtract unused bytes from frame */ + Framedatasize = Framedatasize - ((aSDOp->Command & 0x0e) >> 1); + /* copy to parameter buffer */ + memcpy(hp, &(aSDOp->Index), Framedatasize); + } + else /* segments follow */ + { + /* copy to parameter buffer */ + memcpy(hp, &(aSDOp->Index), Framedatasize); + /* increment buffer pointer */ + hp += Framedatasize; + } + /* update parameter size */ + *psize += Framedatasize; + } + /* unexpected frame returned from slave */ + else + { + NotLast = FALSE; + if ((aSDOp->Command) == ECT_SDO_ABORT) /* SDO abort frame received */ + ecx_SDOerror(context, slave, index, subindex, etohl(aSDOp->ldata[0])); + else + ecx_packeterror(context, slave, index, subindex, 1); /* Unexpected frame returned */ + wkc = 0; + } + } + } + toggle = toggle ^ 0x10; /* toggle bit for segment request */ + } + } + /* non segmented transfer */ + else + { + /* copy to parameter buffer */ + memcpy(bp, &aSDOp->ldata[1], SDOlen); + *psize = SDOlen; + } + } + /* parameter buffer too small */ + else + { + wkc = 0; + ecx_packeterror(context, slave, index, subindex, 3); /* data container too small for type */ + } + } + } + /* other slave response */ + else + { + if ((aSDOp->Command) == ECT_SDO_ABORT) /* SDO abort frame received */ + { + ecx_SDOerror(context, slave, index, subindex, etohl(aSDOp->ldata[0])); + } + else + { + ecx_packeterror(context, slave, index, subindex, 1); /* Unexpected frame returned */ + } + wkc = 0; + } + } + } + return wkc; +} + +/** CoE SDO write, blocking. Single subindex or Complete Access. + * + * A "normal" download request is issued, unless we have + * small data, then a "expedited" transfer is used. If the parameter is larger than + * the mailbox size then the download is segmented. The function will split the + * parameter data in segments and send them to the slave one by one. + * + * @param[in] context = context struct + * @param[in] Slave = Slave number + * @param[in] Index = Index to write + * @param[in] SubIndex = Subindex to write, must be 0 or 1 if CA is used. + * @param[in] CA = FALSE = single subindex. TRUE = Complete Access, all subindexes written. + * @param[in] psize = Size in bytes of parameter buffer. + * @param[out] p = Pointer to parameter buffer + * @param[in] Timeout = Timeout in us, standard is EC_TIMEOUTRXM + * @return Workcounter from last slave response + */ +int ecx_SDOwrite(ecx_contextt *context, uint16 Slave, uint16 Index, uint8 SubIndex, + boolean CA, int psize, const void *p, int Timeout) +{ + ec_SDOt *SDOp, *aSDOp; + int wkc, maxdata, framedatasize; + ec_mbxbuft MbxIn, MbxOut; + uint8 cnt, toggle; + boolean NotLast; + const uint8 *hp; + + ec_clearmbx(&MbxIn); + /* Empty slave out mailbox if something is in. Timeout set to 0 */ + wkc = ecx_mbxreceive(context, Slave, (ec_mbxbuft *)&MbxIn, 0); + ec_clearmbx(&MbxOut); + aSDOp = (ec_SDOt *)&MbxIn; + SDOp = (ec_SDOt *)&MbxOut; + maxdata = context->slavelist[Slave].mbx_l - 0x10; /* data section=mailbox size - 6 mbx - 2 CoE - 8 sdo req */ + /* if small data use expedited transfer */ + if ((psize <= 4) && !CA) + { + SDOp->MbxHeader.length = htoes(0x000a); + SDOp->MbxHeader.address = htoes(0x0000); + SDOp->MbxHeader.priority = 0x00; + /* get new mailbox counter, used for session handle */ + cnt = ec_nextmbxcnt(context->slavelist[Slave].mbx_cnt); + context->slavelist[Slave].mbx_cnt = cnt; + SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + MBX_HDR_SET_CNT(cnt); /* CoE */ + SDOp->CANOpen = htoes(0x000 + (ECT_COES_SDOREQ << 12)); /* number 9bits service upper 4 bits */ + SDOp->Command = ECT_SDO_DOWN_EXP | (((4 - psize) << 2) & 0x0c); /* expedited SDO download transfer */ + SDOp->Index = htoes(Index); + SDOp->SubIndex = SubIndex; + hp = p; + /* copy parameter data to mailbox */ + memcpy(&SDOp->ldata[0], hp, psize); + /* send mailbox SDO download request to slave */ + wkc = ecx_mbxsend(context, Slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM); + if (wkc > 0) + { + ec_clearmbx(&MbxIn); + /* read slave response */ + wkc = ecx_mbxreceive(context, Slave, (ec_mbxbuft *)&MbxIn, Timeout); + if (wkc > 0) + { + /* response should be CoE, SDO response, correct index and subindex */ + if (((aSDOp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_COE) && + ((etohs(aSDOp->CANOpen) >> 12) == ECT_COES_SDORES) && + (aSDOp->Index == SDOp->Index) && + (aSDOp->SubIndex == SDOp->SubIndex)) + { + /* all OK */ + } + /* unexpected response from slave */ + else + { + if (aSDOp->Command == ECT_SDO_ABORT) /* SDO abort frame received */ + { + ecx_SDOerror(context, Slave, Index, SubIndex, etohl(aSDOp->ldata[0])); + } + else + { + ecx_packeterror(context, Slave, Index, SubIndex, 1); /* Unexpected frame returned */ + } + wkc = 0; + } + } + } + } + else + { + framedatasize = psize; + NotLast = FALSE; + if (framedatasize > maxdata) + { + framedatasize = maxdata; /* segmented transfer needed */ + NotLast = TRUE; + } + SDOp->MbxHeader.length = htoes((uint16)(0x0a + framedatasize)); + SDOp->MbxHeader.address = htoes(0x0000); + SDOp->MbxHeader.priority = 0x00; + /* get new mailbox counter, used for session handle */ + cnt = ec_nextmbxcnt(context->slavelist[Slave].mbx_cnt); + context->slavelist[Slave].mbx_cnt = cnt; + SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + MBX_HDR_SET_CNT(cnt); /* CoE */ + SDOp->CANOpen = htoes(0x000 + (ECT_COES_SDOREQ << 12)); /* number 9bits service upper 4 bits */ + if (CA) + { + SDOp->Command = ECT_SDO_DOWN_INIT_CA; /* Complete Access, normal SDO init download transfer */ + } + else + { + SDOp->Command = ECT_SDO_DOWN_INIT; /* normal SDO init download transfer */ + } + SDOp->Index = htoes(Index); + SDOp->SubIndex = SubIndex; + if (CA && (SubIndex > 1)) + { + SDOp->SubIndex = 1; + } + SDOp->ldata[0] = htoel(psize); + hp = p; + /* copy parameter data to mailbox */ + memcpy(&SDOp->ldata[1], hp, framedatasize); + hp += framedatasize; + psize -= framedatasize; + /* send mailbox SDO download request to slave */ + wkc = ecx_mbxsend(context, Slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM); + if (wkc > 0) + { + ec_clearmbx(&MbxIn); + /* read slave response */ + wkc = ecx_mbxreceive(context, Slave, (ec_mbxbuft *)&MbxIn, Timeout); + if (wkc > 0) + { + /* response should be CoE, SDO response, correct index and subindex */ + if (((aSDOp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_COE) && + ((etohs(aSDOp->CANOpen) >> 12) == ECT_COES_SDORES) && + (aSDOp->Index == SDOp->Index) && + (aSDOp->SubIndex == SDOp->SubIndex)) + { + /* all ok */ + maxdata += 7; + toggle = 0; + /* repeat while segments left */ + while (NotLast) + { + SDOp = (ec_SDOt *)&MbxOut; + framedatasize = psize; + NotLast = FALSE; + SDOp->Command = 0x01; /* last segment */ + if (framedatasize > maxdata) + { + framedatasize = maxdata; /* more segments needed */ + NotLast = TRUE; + SDOp->Command = 0x00; /* segments follow */ + } + if (!NotLast && (framedatasize < 7)) + { + SDOp->MbxHeader.length = htoes(0x0a); /* minimum size */ + SDOp->Command = (uint8)(0x01 + ((7 - framedatasize) << 1)); /* last segment reduced octets */ + } + else + { + SDOp->MbxHeader.length = htoes((uint16)(framedatasize + 3)); /* data + 2 CoE + 1 SDO */ + } + SDOp->MbxHeader.address = htoes(0x0000); + SDOp->MbxHeader.priority = 0x00; + /* get new mailbox counter value */ + cnt = ec_nextmbxcnt(context->slavelist[Slave].mbx_cnt); + context->slavelist[Slave].mbx_cnt = cnt; + SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + MBX_HDR_SET_CNT(cnt); /* CoE */ + SDOp->CANOpen = htoes(0x000 + (ECT_COES_SDOREQ << 12)); /* number 9bits service upper 4 bits (SDO request) */ + SDOp->Command = SDOp->Command + toggle; /* add toggle bit to command byte */ + /* copy parameter data to mailbox */ + memcpy(&SDOp->Index, hp, framedatasize); + /* update parameter buffer pointer */ + hp += framedatasize; + psize -= framedatasize; + /* send SDO download request */ + wkc = ecx_mbxsend(context, Slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM); + if (wkc > 0) + { + ec_clearmbx(&MbxIn); + /* read slave response */ + wkc = ecx_mbxreceive(context, Slave, (ec_mbxbuft *)&MbxIn, Timeout); + if (wkc > 0) + { + if (((aSDOp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_COE) && + ((etohs(aSDOp->CANOpen) >> 12) == ECT_COES_SDORES) && + ((aSDOp->Command & 0xe0) == 0x20)) + { + /* all OK, nothing to do */ + } + else + { + if (aSDOp->Command == ECT_SDO_ABORT) /* SDO abort frame received */ + { + ecx_SDOerror(context, Slave, Index, SubIndex, etohl(aSDOp->ldata[0])); + } + else + { + ecx_packeterror(context, Slave, Index, SubIndex, 1); /* Unexpected frame returned */ + } + wkc = 0; + NotLast = FALSE; + } + } + } + toggle = toggle ^ 0x10; /* toggle bit for segment request */ + } + } + /* unexpected response from slave */ + else + { + if (aSDOp->Command == ECT_SDO_ABORT) /* SDO abort frame received */ + { + ecx_SDOerror(context, Slave, Index, SubIndex, etohl(aSDOp->ldata[0])); + } + else + { + ecx_packeterror(context, Slave, Index, SubIndex, 1); /* Unexpected frame returned */ + } + wkc = 0; + } + } + } + } + + return wkc; +} + +/** CoE RxPDO write, blocking. + * + * A RxPDO download request is issued. + * + * @param[in] context = context struct + * @param[in] Slave = Slave number + * @param[in] RxPDOnumber = Related RxPDO number + * @param[in] psize = Size in bytes of PDO buffer. + * @param[out] p = Pointer to PDO buffer + * @return Workcounter from last slave response + */ +int ecx_RxPDO(ecx_contextt *context, uint16 Slave, uint16 RxPDOnumber, int psize, const void *p) +{ + ec_SDOt *SDOp; + int wkc, maxdata, framedatasize; + ec_mbxbuft MbxIn, MbxOut; + uint8 cnt; + + ec_clearmbx(&MbxIn); + /* Empty slave out mailbox if something is in. Timeout set to 0 */ + wkc = ecx_mbxreceive(context, Slave, (ec_mbxbuft *)&MbxIn, 0); + ec_clearmbx(&MbxOut); + SDOp = (ec_SDOt *)&MbxOut; + maxdata = context->slavelist[Slave].mbx_l - 0x08; /* data section=mailbox size - 6 mbx - 2 CoE */ + framedatasize = psize; + if (framedatasize > maxdata) + { + framedatasize = maxdata; /* limit transfer */ + } + SDOp->MbxHeader.length = htoes((uint16)(0x02 + framedatasize)); + SDOp->MbxHeader.address = htoes(0x0000); + SDOp->MbxHeader.priority = 0x00; + /* get new mailbox counter, used for session handle */ + cnt = ec_nextmbxcnt(context->slavelist[Slave].mbx_cnt); + context->slavelist[Slave].mbx_cnt = cnt; + SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + MBX_HDR_SET_CNT(cnt); /* CoE */ + SDOp->CANOpen = htoes((RxPDOnumber & 0x01ff) + (ECT_COES_RXPDO << 12)); /* number 9bits service upper 4 bits */ + /* copy PDO data to mailbox */ + memcpy(&SDOp->Command, p, framedatasize); + /* send mailbox RxPDO request to slave */ + wkc = ecx_mbxsend(context, Slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM); + + return wkc; +} + +/** CoE TxPDO read remote request, blocking. + * + * A TxPDO download request is issued. + * + * @param[in] context = context struct + * @param[in] slave = Slave number + * @param[in] TxPDOnumber = Related TxPDO number + * @param[in,out] psize = Size in bytes of PDO buffer, returns bytes read from PDO. + * @param[out] p = Pointer to PDO buffer + * @param[in] timeout = Timeout in us, standard is EC_TIMEOUTRXM + * @return Workcounter from last slave response + */ +int ecx_TxPDO(ecx_contextt *context, uint16 slave, uint16 TxPDOnumber , int *psize, void *p, int timeout) +{ + ec_SDOt *SDOp, *aSDOp; + int wkc; + ec_mbxbuft MbxIn, MbxOut; + uint8 cnt; + uint16 framedatasize; + + ec_clearmbx(&MbxIn); + /* Empty slave out mailbox if something is in. Timeout set to 0 */ + wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, 0); + ec_clearmbx(&MbxOut); + aSDOp = (ec_SDOt *)&MbxIn; + SDOp = (ec_SDOt *)&MbxOut; + SDOp->MbxHeader.length = htoes(0x02); + SDOp->MbxHeader.address = htoes(0x0000); + SDOp->MbxHeader.priority = 0x00; + /* get new mailbox counter, used for session handle */ + cnt = ec_nextmbxcnt(context->slavelist[slave].mbx_cnt); + context->slavelist[slave].mbx_cnt = cnt; + SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + MBX_HDR_SET_CNT(cnt); /* CoE */ + SDOp->CANOpen = htoes((TxPDOnumber & 0x01ff) + (ECT_COES_TXPDO_RR << 12)); /* number 9bits service upper 4 bits */ + wkc = ecx_mbxsend(context, slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM); + if (wkc > 0) + { + /* clean mailboxbuffer */ + ec_clearmbx(&MbxIn); + /* read slave response */ + wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, timeout); + if (wkc > 0) /* succeeded to read slave response ? */ + { + /* slave response should be CoE, TxPDO */ + if (((aSDOp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_COE) && + ((etohs(aSDOp->CANOpen) >> 12) == ECT_COES_TXPDO)) + { + /* TxPDO response */ + framedatasize = (aSDOp->MbxHeader.length - 2); + if (*psize >= framedatasize) /* parameter buffer big enough ? */ + { + /* copy parameter in parameter buffer */ + memcpy(p, &aSDOp->Command, framedatasize); + /* return the real parameter size */ + *psize = framedatasize; + } + /* parameter buffer too small */ + else + { + wkc = 0; + ecx_packeterror(context, slave, 0, 0, 3); /* data container too small for type */ + } + } + /* other slave response */ + else + { + if ((aSDOp->Command) == ECT_SDO_ABORT) /* SDO abort frame received */ + { + ecx_SDOerror(context, slave, 0, 0, etohl(aSDOp->ldata[0])); + } + else + { + ecx_packeterror(context, slave, 0, 0, 1); /* Unexpected frame returned */ + } + wkc = 0; + } + } + } + + return wkc; +} + +/** Read PDO assign structure + * @param[in] context = context struct + * @param[in] Slave = Slave number + * @param[in] PDOassign = PDO assign object + * @return total bitlength of PDO assign + */ +uint32 ecx_readPDOassign(ecx_contextt *context, uint16 Slave, uint16 PDOassign) +{ + uint16 idxloop, nidx, subidxloop, rdat, idx, subidx; + uint8 subcnt; + int wkc, rdl; + int32 rdat2; + uint32 bsize = 0; + + rdl = sizeof(rdat); rdat = 0; + /* read PDO assign subindex 0 ( = number of PDO's) */ + wkc = ecx_SDOread(context, Slave, PDOassign, 0x00, FALSE, &rdl, &rdat, EC_TIMEOUTRXM); + rdat = etohs(rdat); + /* positive result from slave ? */ + if ((wkc > 0) && (rdat > 0)) + { + /* number of available sub indexes */ + nidx = rdat; + bsize = 0; + /* read all PDO's */ + for (idxloop = 1; idxloop <= nidx; idxloop++) + { + rdl = sizeof(rdat); rdat = 0; + /* read PDO assign */ + wkc = ecx_SDOread(context, Slave, PDOassign, (uint8)idxloop, FALSE, &rdl, &rdat, EC_TIMEOUTRXM); + /* result is index of PDO */ + idx = etohs(rdat); + if (idx > 0) + { + rdl = sizeof(subcnt); subcnt = 0; + /* read number of subindexes of PDO */ + wkc = ecx_SDOread(context, Slave,idx, 0x00, FALSE, &rdl, &subcnt, EC_TIMEOUTRXM); + subidx = subcnt; + /* for each subindex */ + for (subidxloop = 1; subidxloop <= subidx; subidxloop++) + { + rdl = sizeof(rdat2); rdat2 = 0; + /* read SDO that is mapped in PDO */ + wkc = ecx_SDOread(context, Slave, idx, (uint8)subidxloop, FALSE, &rdl, &rdat2, EC_TIMEOUTRXM); + rdat2 = etohl(rdat2); + /* extract bitlength of SDO */ + if (LO_BYTE(rdat2) < 0xff) + { + bsize += LO_BYTE(rdat2); + } + else + { + rdl = sizeof(rdat); rdat = htoes(0xff); + /* read Object Entry in Object database */ +// wkc = ec_readOEsingle(idx, (uint8)SubCount, pODlist, pOElist); + bsize += etohs(rdat); + } + } + } + } + } + /* return total found bitlength (PDO) */ + return bsize; +} + +/** Read PDO assign structure in Complete Access mode + * @param[in] context = context struct + * @param[in] Slave = Slave number + * @param[in] Thread_n = Calling thread index + * @param[in] PDOassign = PDO assign object + * @return total bitlength of PDO assign + */ +uint32 ecx_readPDOassignCA(ecx_contextt *context, uint16 Slave, int Thread_n, + uint16 PDOassign) +{ + uint16 idxloop, nidx, subidxloop, idx, subidx; + int wkc, rdl; + uint32 bsize = 0; + + /* find maximum size of PDOassign buffer */ + rdl = sizeof(ec_PDOassignt); + context->PDOassign[Thread_n].n=0; + /* read rxPDOassign in CA mode, all subindexes are read in one struct */ + wkc = ecx_SDOread(context, Slave, PDOassign, 0x00, TRUE, &rdl, + &(context->PDOassign[Thread_n]), EC_TIMEOUTRXM); + /* positive result from slave ? */ + if ((wkc > 0) && (context->PDOassign[Thread_n].n > 0)) + { + nidx = context->PDOassign[Thread_n].n; + bsize = 0; + /* for each PDO do */ + for (idxloop = 1; idxloop <= nidx; idxloop++) + { + /* get index from PDOassign struct */ + idx = etohs(context->PDOassign[Thread_n].index[idxloop - 1]); + if (idx > 0) + { + rdl = sizeof(ec_PDOdesct); context->PDOdesc[Thread_n].n = 0; + /* read SDO's that are mapped in PDO, CA mode */ + wkc = ecx_SDOread(context, Slave,idx, 0x00, TRUE, &rdl, + &(context->PDOdesc[Thread_n]), EC_TIMEOUTRXM); + subidx = context->PDOdesc[Thread_n].n; + /* extract all bitlengths of SDO's */ + for (subidxloop = 1; subidxloop <= subidx; subidxloop++) + { + bsize += LO_BYTE(etohl(context->PDOdesc[Thread_n].PDO[subidxloop -1])); + } + } + } + } + + /* return total found bitlength (PDO) */ + return bsize; +} + +/** CoE read PDO mapping. + * + * CANopen has standard indexes defined for PDO mapping. This function + * tries to read them and collect a full input and output mapping size + * of designated slave. + * + * Principal structure in slave:\n + * 1C00:00 is number of SM defined\n + * 1C00:01 SM0 type -> 1C10\n + * 1C00:02 SM1 type -> 1C11\n + * 1C00:03 SM2 type -> 1C12\n + * 1C00:04 SM3 type -> 1C13\n + * Type 0 = unused, 1 = mailbox in, 2 = mailbox out, + * 3 = outputs (RxPDO), 4 = inputs (TxPDO). + * + * 1C12:00 is number of PDO's defined for SM2\n + * 1C12:01 PDO assign SDO #1 -> f.e. 1A00\n + * 1C12:02 PDO assign SDO #2 -> f.e. 1A04\ + * + * 1A00:00 is number of object defined for this PDO\n + * 1A00:01 object mapping #1, f.e. 60100710 (SDO 6010 SI 07 bitlength 0x10) + * + * @param[in] context = context struct + * @param[in] Slave = Slave number + * @param[out] Osize = Size in bits of output mapping (rxPDO) found + * @param[out] Isize = Size in bits of input mapping (txPDO) found + * @return >0 if mapping successful. + */ +int ecx_readPDOmap(ecx_contextt *context, uint16 Slave, uint32 *Osize, uint32 *Isize) +{ + int wkc, rdl; + int retVal = 0; + uint8 nSM, iSM, tSM; + uint32 Tsize; + uint8 SMt_bug_add; + + *Isize = 0; + *Osize = 0; + SMt_bug_add = 0; + rdl = sizeof(nSM); nSM = 0; + /* read SyncManager Communication Type object count */ + wkc = ecx_SDOread(context, Slave, ECT_SDO_SMCOMMTYPE, 0x00, FALSE, &rdl, &nSM, EC_TIMEOUTRXM); + /* positive result from slave ? */ + if ((wkc > 0) && (nSM > 2)) + { + /* limit to maximum number of SM defined, if true the slave can't be configured */ + if (nSM > EC_MAXSM) + nSM = EC_MAXSM; + /* iterate for every SM type defined */ + for (iSM = 2 ; iSM < nSM ; iSM++) + { + rdl = sizeof(tSM); tSM = 0; + /* read SyncManager Communication Type */ + wkc = ecx_SDOread(context, Slave, ECT_SDO_SMCOMMTYPE, iSM + 1, FALSE, &rdl, &tSM, EC_TIMEOUTRXM); + if (wkc > 0) + { +// start slave bug prevention code, remove if possible + if((iSM == 2) && (tSM == 2)) // SM2 has type 2 == mailbox out, this is a bug in the slave! + { + SMt_bug_add = 1; // try to correct, this works if the types are 0 1 2 3 and should be 1 2 3 4 + } + if(tSM) + { + tSM += SMt_bug_add; // only add if SMt > 0 + } + if((iSM == 2) && (tSM == 0)) // SM2 has type 0, this is a bug in the slave! + { + tSM = 3; + } + if((iSM == 3) && (tSM == 0)) // SM3 has type 0, this is a bug in the slave! + { + tSM = 4; + } +// end slave bug prevention code + + context->slavelist[Slave].SMtype[iSM] = tSM; + /* check if SM is unused -> clear enable flag */ + if (tSM == 0) + { + context->slavelist[Slave].SM[iSM].SMflags = + htoel( etohl(context->slavelist[Slave].SM[iSM].SMflags) & EC_SMENABLEMASK); + } + if ((tSM == 3) || (tSM == 4)) + { + /* read the assign PDO */ + Tsize = ecx_readPDOassign(context, Slave, ECT_SDO_PDOASSIGN + iSM ); + /* if a mapping is found */ + if (Tsize) + { + context->slavelist[Slave].SM[iSM].SMlength = htoes((uint16)((Tsize + 7) / 8)); + if (tSM == 3) + { + /* we are doing outputs */ + *Osize += Tsize; + } + else + { + /* we are doing inputs */ + *Isize += Tsize; + } + } + } + } + } + } + + /* found some I/O bits ? */ + if ((*Isize > 0) || (*Osize > 0)) + { + retVal = 1; + } + + return retVal; +} + +/** CoE read PDO mapping in Complete Access mode (CA). + * + * CANopen has standard indexes defined for PDO mapping. This function + * tries to read them and collect a full input and output mapping size + * of designated slave. Slave has to support CA, otherwise use ec_readPDOmap(). + * + * @param[in] context = context struct + * @param[in] Slave = Slave number + * @param[in] Thread_n = Calling thread index + * @param[out] Osize = Size in bits of output mapping (rxPDO) found + * @param[out] Isize = Size in bits of input mapping (txPDO) found + * @return >0 if mapping successful. + */ +int ecx_readPDOmapCA(ecx_contextt *context, uint16 Slave, int Thread_n, uint32 *Osize, uint32 *Isize) +{ + int wkc, rdl; + int retVal = 0; + uint8 nSM, iSM, tSM; + uint32 Tsize; + uint8 SMt_bug_add; + + *Isize = 0; + *Osize = 0; + SMt_bug_add = 0; + rdl = sizeof(ec_SMcommtypet); + context->SMcommtype[Thread_n].n = 0; + /* read SyncManager Communication Type object count Complete Access*/ + wkc = ecx_SDOread(context, Slave, ECT_SDO_SMCOMMTYPE, 0x00, TRUE, &rdl, + &(context->SMcommtype[Thread_n]), EC_TIMEOUTRXM); + /* positive result from slave ? */ + if ((wkc > 0) && (context->SMcommtype[Thread_n].n > 2)) + { + nSM = context->SMcommtype[Thread_n].n; + /* limit to maximum number of SM defined, if true the slave can't be configured */ + if (nSM > EC_MAXSM) + { + nSM = EC_MAXSM; + ecx_packeterror(context, Slave, 0, 0, 10); /* #SM larger than EC_MAXSM */ + } + /* iterate for every SM type defined */ + for (iSM = 2 ; iSM < nSM ; iSM++) + { + tSM = context->SMcommtype[Thread_n].SMtype[iSM]; + +// start slave bug prevention code, remove if possible + if((iSM == 2) && (tSM == 2)) // SM2 has type 2 == mailbox out, this is a bug in the slave! + { + SMt_bug_add = 1; // try to correct, this works if the types are 0 1 2 3 and should be 1 2 3 4 + } + if(tSM) + { + tSM += SMt_bug_add; // only add if SMt > 0 + } +// end slave bug prevention code + + context->slavelist[Slave].SMtype[iSM] = tSM; + /* check if SM is unused -> clear enable flag */ + if (tSM == 0) + { + context->slavelist[Slave].SM[iSM].SMflags = + htoel( etohl(context->slavelist[Slave].SM[iSM].SMflags) & EC_SMENABLEMASK); + } + if ((tSM == 3) || (tSM == 4)) + { + /* read the assign PDO */ + Tsize = ecx_readPDOassignCA(context, Slave, Thread_n, + ECT_SDO_PDOASSIGN + iSM ); + /* if a mapping is found */ + if (Tsize) + { + context->slavelist[Slave].SM[iSM].SMlength = htoes((uint16)((Tsize + 7) / 8)); + if (tSM == 3) + { + /* we are doing outputs */ + *Osize += Tsize; + } + else + { + /* we are doing inputs */ + *Isize += Tsize; + } + } + } + } + } + + /* found some I/O bits ? */ + if ((*Isize > 0) || (*Osize > 0)) + { + retVal = 1; + } + return retVal; +} + +/** CoE read Object Description List. + * + * @param[in] context = context struct + * @param[in] Slave = Slave number. + * @param[out] pODlist = resulting Object Description list. + * @return Workcounter of slave response. + */ +int ecx_readODlist(ecx_contextt *context, uint16 Slave, ec_ODlistt *pODlist) +{ + ec_SDOservicet *SDOp, *aSDOp; + ec_mbxbuft MbxIn, MbxOut; + int wkc; + uint16 x, n, i, sp, offset; + boolean stop; + uint8 cnt; + boolean First; + + pODlist->Slave = Slave; + pODlist->Entries = 0; + ec_clearmbx(&MbxIn); + /* clear pending out mailbox in slave if available. Timeout is set to 0 */ + wkc = ecx_mbxreceive(context, Slave, &MbxIn, 0); + ec_clearmbx(&MbxOut); + aSDOp = (ec_SDOservicet*)&MbxIn; + SDOp = (ec_SDOservicet*)&MbxOut; + SDOp->MbxHeader.length = htoes(0x0008); + SDOp->MbxHeader.address = htoes(0x0000); + SDOp->MbxHeader.priority = 0x00; + /* Get new mailbox counter value */ + cnt = ec_nextmbxcnt(context->slavelist[Slave].mbx_cnt); + context->slavelist[Slave].mbx_cnt = cnt; + SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + MBX_HDR_SET_CNT(cnt); /* CoE */ + SDOp->CANOpen = htoes(0x000 + (ECT_COES_SDOINFO << 12)); /* number 9bits service upper 4 bits */ + SDOp->Opcode = ECT_GET_ODLIST_REQ; /* get object description list request */ + SDOp->Reserved = 0; + SDOp->Fragments = 0; /* fragments left */ + SDOp->wdata[0] = htoes(0x01); /* all objects */ + /* send get object description list request to slave */ + wkc = ecx_mbxsend(context, Slave, &MbxOut, EC_TIMEOUTTXM); + /* mailbox placed in slave ? */ + if (wkc > 0) + { + x = 0; + sp = 0; + First = TRUE; + offset = 1; /* offset to skip info header in first frame, otherwise set to 0 */ + do + { + stop = TRUE; /* assume this is last iteration */ + ec_clearmbx(&MbxIn); + /* read slave response */ + wkc = ecx_mbxreceive(context, Slave, &MbxIn, EC_TIMEOUTRXM); + /* got response ? */ + if (wkc > 0) + { + /* response should be CoE and "get object description list response" */ + if (((aSDOp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_COE) && + ((aSDOp->Opcode & 0x7f) == ECT_GET_ODLIST_RES)) + { + if (First) + { + /* extract number of indexes from mailbox data size */ + n = (etohs(aSDOp->MbxHeader.length) - (6 + 2)) / 2; + } + else + { + /* extract number of indexes from mailbox data size */ + n = (etohs(aSDOp->MbxHeader.length) - 6) / 2; + } + /* check if indexes fit in buffer structure */ + if ((sp + n) > EC_MAXODLIST) + { + n = EC_MAXODLIST + 1 - sp; + ecx_SDOinfoerror(context, Slave, 0, 0, 0xf000000); /* Too many entries for master buffer */ + stop = TRUE; + } + /* trim to maximum number of ODlist entries defined */ + if ((pODlist->Entries + n) > EC_MAXODLIST) + { + n = EC_MAXODLIST - pODlist->Entries; + } + pODlist->Entries += n; + /* extract indexes one by one */ + for (i = 0; i < n; i++) + { + pODlist->Index[sp + i] = etohs(aSDOp->wdata[i + offset]); + } + sp += n; + /* check if more fragments will follow */ + if (aSDOp->Fragments > 0) + { + stop = FALSE; + } + First = FALSE; + offset = 0; + } + /* got unexpected response from slave */ + else + { + if ((aSDOp->Opcode & 0x7f) == ECT_SDOINFO_ERROR) /* SDO info error received */ + { + ecx_SDOinfoerror(context, Slave, 0, 0, etohl(aSDOp->ldata[0])); + stop = TRUE; + } + else + { + ecx_packeterror(context, Slave, 0, 0, 1); /* Unexpected frame returned */ + } + wkc = 0; + x += 20; + } + } + x++; + } + while ((x <= 128) && !stop); + } + return wkc; +} + +/** CoE read Object Description. Adds textual description to object indexes. + * + * @param[in] context = context struct + * @param[in] Item = Item number in ODlist. + * @param[in,out] pODlist = referencing Object Description list. + * @return Workcounter of slave response. + */ +int ecx_readODdescription(ecx_contextt *context, uint16 Item, ec_ODlistt *pODlist) +{ + ec_SDOservicet *SDOp, *aSDOp; + int wkc; + uint16 n, Slave; + ec_mbxbuft MbxIn, MbxOut; + uint8 cnt; + + Slave = pODlist->Slave; + pODlist->DataType[Item] = 0; + pODlist->ObjectCode[Item] = 0; + pODlist->MaxSub[Item] = 0; + pODlist->Name[Item][0] = 0; + ec_clearmbx(&MbxIn); + /* clear pending out mailbox in slave if available. Timeout is set to 0 */ + wkc = ecx_mbxreceive(context, Slave, &MbxIn, 0); + ec_clearmbx(&MbxOut); + aSDOp = (ec_SDOservicet*)&MbxIn; + SDOp = (ec_SDOservicet*)&MbxOut; + SDOp->MbxHeader.length = htoes(0x0008); + SDOp->MbxHeader.address = htoes(0x0000); + SDOp->MbxHeader.priority = 0x00; + /* Get new mailbox counter value */ + cnt = ec_nextmbxcnt(context->slavelist[Slave].mbx_cnt); + context->slavelist[Slave].mbx_cnt = cnt; + SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + MBX_HDR_SET_CNT(cnt); /* CoE */ + SDOp->CANOpen = htoes(0x000 + (ECT_COES_SDOINFO << 12)); /* number 9bits service upper 4 bits */ + SDOp->Opcode = ECT_GET_OD_REQ; /* get object description request */ + SDOp->Reserved = 0; + SDOp->Fragments = 0; /* fragments left */ + SDOp->wdata[0] = htoes(pODlist->Index[Item]); /* Data of Index */ + /* send get object description request to slave */ + wkc = ecx_mbxsend(context, Slave, &MbxOut, EC_TIMEOUTTXM); + /* mailbox placed in slave ? */ + if (wkc > 0) + { + ec_clearmbx(&MbxIn); + /* read slave response */ + wkc = ecx_mbxreceive(context, Slave, &MbxIn, EC_TIMEOUTRXM); + /* got response ? */ + if (wkc > 0) + { + if (((aSDOp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_COE) && + ((aSDOp->Opcode & 0x7f) == ECT_GET_OD_RES)) + { + n = (etohs(aSDOp->MbxHeader.length) - 12); /* length of string(name of object) */ + if (n > EC_MAXNAME) + { + n = EC_MAXNAME; /* max chars */ + } + pODlist->DataType[Item] = etohs(aSDOp->wdata[1]); + pODlist->ObjectCode[Item] = aSDOp->bdata[5]; + pODlist->MaxSub[Item] = aSDOp->bdata[4]; + + strncpy(pODlist->Name[Item] , (char *)&aSDOp->bdata[6], n); + pODlist->Name[Item][n] = 0x00; /* String terminator */ + } + /* got unexpected response from slave */ + else + { + if (((aSDOp->Opcode & 0x7f) == ECT_SDOINFO_ERROR)) /* SDO info error received */ + { + ecx_SDOinfoerror(context, Slave,pODlist->Index[Item], 0, etohl(aSDOp->ldata[0])); + } + else + { + ecx_packeterror(context, Slave,pODlist->Index[Item], 0, 1); /* Unexpected frame returned */ + } + wkc = 0; + } + } + } + + return wkc; +} + +/** CoE read SDO service object entry, single subindex. + * Used in ec_readOE(). + * + * @param[in] context = context struct + * @param[in] Item = Item in ODlist. + * @param[in] SubI = Subindex of item in ODlist. + * @param[in] pODlist = Object description list for reference. + * @param[out] pOElist = resulting object entry structure. + * @return Workcounter of slave response. + */ +int ecx_readOEsingle(ecx_contextt *context, uint16 Item, uint8 SubI, ec_ODlistt *pODlist, ec_OElistt *pOElist) +{ + ec_SDOservicet *SDOp, *aSDOp; + int wkc; + uint16 Index, Slave; + int16 n; + ec_mbxbuft MbxIn, MbxOut; + uint8 cnt; + + wkc = 0; + Slave = pODlist->Slave; + Index = pODlist->Index[Item]; + ec_clearmbx(&MbxIn); + /* clear pending out mailbox in slave if available. Timeout is set to 0 */ + wkc = ecx_mbxreceive(context, Slave, &MbxIn, 0); + ec_clearmbx(&MbxOut); + aSDOp = (ec_SDOservicet*)&MbxIn; + SDOp = (ec_SDOservicet*)&MbxOut; + SDOp->MbxHeader.length = htoes(0x000a); + SDOp->MbxHeader.address = htoes(0x0000); + SDOp->MbxHeader.priority = 0x00; + /* Get new mailbox counter value */ + cnt = ec_nextmbxcnt(context->slavelist[Slave].mbx_cnt); + context->slavelist[Slave].mbx_cnt = cnt; + SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + MBX_HDR_SET_CNT(cnt); /* CoE */ + SDOp->CANOpen = htoes(0x000 + (ECT_COES_SDOINFO << 12)); /* number 9bits service upper 4 bits */ + SDOp->Opcode = ECT_GET_OE_REQ; /* get object entry description request */ + SDOp->Reserved = 0; + SDOp->Fragments = 0; /* fragments left */ + SDOp->wdata[0] = htoes(Index); /* Index */ + SDOp->bdata[2] = SubI; /* SubIndex */ + SDOp->bdata[3] = 1 + 2 + 4; /* get access rights, object category, PDO */ + /* send get object entry description request to slave */ + wkc = ecx_mbxsend(context, Slave, &MbxOut, EC_TIMEOUTTXM); + /* mailbox placed in slave ? */ + if (wkc > 0) + { + ec_clearmbx(&MbxIn); + /* read slave response */ + wkc = ecx_mbxreceive(context, Slave, &MbxIn, EC_TIMEOUTRXM); + /* got response ? */ + if (wkc > 0) + { + if (((aSDOp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_COE) && + ((aSDOp->Opcode & 0x7f) == ECT_GET_OE_RES)) + { + pOElist->Entries++; + n = (etohs(aSDOp->MbxHeader.length) - 16); /* length of string(name of object) */ + if (n > EC_MAXNAME) + { + n = EC_MAXNAME; /* max string length */ + } + if (n < 0 ) + { + n = 0; + } + pOElist->ValueInfo[SubI] = aSDOp->bdata[3]; + pOElist->DataType[SubI] = etohs(aSDOp->wdata[2]); + pOElist->BitLength[SubI] = etohs(aSDOp->wdata[3]); + pOElist->ObjAccess[SubI] = etohs(aSDOp->wdata[4]); + + strncpy(pOElist->Name[SubI] , (char *)&aSDOp->wdata[5], n); + pOElist->Name[SubI][n] = 0x00; /* string terminator */ + } + /* got unexpected response from slave */ + else + { + if (((aSDOp->Opcode & 0x7f) == ECT_SDOINFO_ERROR)) /* SDO info error received */ + { + ecx_SDOinfoerror(context, Slave, Index, SubI, etohl(aSDOp->ldata[0])); + } + else + { + ecx_packeterror(context, Slave, Index, SubI, 1); /* Unexpected frame returned */ + } + wkc = 0; + } + } + } + + return wkc; +} + +/** CoE read SDO service object entry. + * + * @param[in] context = context struct + * @param[in] Item = Item in ODlist. + * @param[in] pODlist = Object description list for reference. + * @param[out] pOElist = resulting object entry structure. + * @return Workcounter of slave response. + */ +int ecx_readOE(ecx_contextt *context, uint16 Item, ec_ODlistt *pODlist, ec_OElistt *pOElist) +{ + uint16 SubCount; + int wkc; + uint8 SubI; + + wkc = 0; + pOElist->Entries = 0; + SubI = pODlist->MaxSub[Item]; + /* for each entry found in ODlist */ + for (SubCount = 0; SubCount <= SubI; SubCount++) + { + /* read subindex of entry */ + wkc = ecx_readOEsingle(context, Item, (uint8)SubCount, pODlist, pOElist); + } + + return wkc; +} + +#ifdef EC_VER1 +/** Report SDO error. + * + * @param[in] Slave = Slave number + * @param[in] Index = Index that generated error + * @param[in] SubIdx = Subindex that generated error + * @param[in] AbortCode = Abortcode, see EtherCAT documentation for list + * @see ecx_SDOerror + */ +void ec_SDOerror(uint16 Slave, uint16 Index, uint8 SubIdx, int32 AbortCode) +{ + ecx_SDOerror(&ecx_context, Slave, Index, SubIdx, AbortCode); +} + +/** CoE SDO read, blocking. Single subindex or Complete Access. + * + * Only a "normal" upload request is issued. If the requested parameter is <= 4bytes + * then a "expedited" response is returned, otherwise a "normal" response. If a "normal" + * response is larger than the mailbox size then the response is segmented. The function + * will combine all segments and copy them to the parameter buffer. + * + * @param[in] slave = Slave number + * @param[in] index = Index to read + * @param[in] subindex = Subindex to read, must be 0 or 1 if CA is used. + * @param[in] CA = FALSE = single subindex. TRUE = Complete Access, all subindexes read. + * @param[in,out] psize = Size in bytes of parameter buffer, returns bytes read from SDO. + * @param[out] p = Pointer to parameter buffer + * @param[in] timeout = Timeout in us, standard is EC_TIMEOUTRXM + * @return Workcounter from last slave response + * @see ecx_SDOread + */ +int ec_SDOread(uint16 slave, uint16 index, uint8 subindex, + boolean CA, int *psize, void *p, int timeout) +{ + return ecx_SDOread(&ecx_context, slave, index, subindex, CA, psize, p, timeout); +} + +/** CoE SDO write, blocking. Single subindex or Complete Access. + * + * A "normal" download request is issued, unless we have + * small data, then a "expedited" transfer is used. If the parameter is larger than + * the mailbox size then the download is segmented. The function will split the + * parameter data in segments and send them to the slave one by one. + * + * @param[in] Slave = Slave number + * @param[in] Index = Index to write + * @param[in] SubIndex = Subindex to write, must be 0 or 1 if CA is used. + * @param[in] CA = FALSE = single subindex. TRUE = Complete Access, all subindexes written. + * @param[in] psize = Size in bytes of parameter buffer. + * @param[out] p = Pointer to parameter buffer + * @param[in] Timeout = Timeout in us, standard is EC_TIMEOUTRXM + * @return Workcounter from last slave response + * @see ecx_SDOwrite + */ +int ec_SDOwrite(uint16 Slave, uint16 Index, uint8 SubIndex, + boolean CA, int psize, const void *p, int Timeout) +{ + return ecx_SDOwrite(&ecx_context, Slave, Index, SubIndex, CA, psize, p, Timeout); +} + +/** CoE RxPDO write, blocking. + * + * A RxPDO download request is issued. + * + * @param[in] Slave = Slave number + * @param[in] RxPDOnumber = Related RxPDO number + * @param[in] psize = Size in bytes of PDO buffer. + * @param[out] p = Pointer to PDO buffer + * @return Workcounter from last slave response + * @see ecx_RxPDO + */ +int ec_RxPDO(uint16 Slave, uint16 RxPDOnumber, int psize, const void *p) +{ + return ecx_RxPDO(&ecx_context, Slave, RxPDOnumber, psize, p); +} + +/** CoE TxPDO read remote request, blocking. + * + * A TxPDO download request is issued. + * + * @param[in] slave = Slave number + * @param[in] TxPDOnumber = Related TxPDO number + * @param[in,out] psize = Size in bytes of PDO buffer, returns bytes read from PDO. + * @param[out] p = Pointer to PDO buffer + * @param[in] timeout = Timeout in us, standard is EC_TIMEOUTRXM + * @return Workcounter from last slave response + * @see ecx_TxPDO + */ +int ec_TxPDO(uint16 slave, uint16 TxPDOnumber , int *psize, void *p, int timeout) +{ + return ecx_TxPDO(&ecx_context, slave, TxPDOnumber, psize, p, timeout); +} + +/** Read PDO assign structure + * @param[in] Slave = Slave number + * @param[in] PDOassign = PDO assign object + * @return total bitlength of PDO assign + */ +uint32 ec_readPDOassign(uint16 Slave, uint16 PDOassign) +{ + return ecx_readPDOassign(&ecx_context, Slave, PDOassign); +} + +/** Read PDO assign structure in Complete Access mode + * @param[in] Slave = Slave number + * @param[in] PDOassign = PDO assign object + * @param[in] Thread_n = Calling thread index + * @return total bitlength of PDO assign + * @see ecx_readPDOmap + */ +uint32 ec_readPDOassignCA(uint16 Slave, uint16 PDOassign, int Thread_n) +{ + return ecx_readPDOassignCA(&ecx_context, Slave, Thread_n, PDOassign); +} + +/** CoE read PDO mapping. + * + * CANopen has standard indexes defined for PDO mapping. This function + * tries to read them and collect a full input and output mapping size + * of designated slave. + * + * For details, see #ecx_readPDOmap + * + * @param[in] Slave = Slave number + * @param[out] Osize = Size in bits of output mapping (rxPDO) found + * @param[out] Isize = Size in bits of input mapping (txPDO) found + * @return >0 if mapping succesful. + */ +int ec_readPDOmap(uint16 Slave, uint32 *Osize, uint32 *Isize) +{ + return ecx_readPDOmap(&ecx_context, Slave, Osize, Isize); +} + +/** CoE read PDO mapping in Complete Access mode (CA). + * + * CANopen has standard indexes defined for PDO mapping. This function + * tries to read them and collect a full input and output mapping size + * of designated slave. Slave has to support CA, otherwise use ec_readPDOmap(). + * + * @param[in] Slave = Slave number + * @param[in] Thread_n = Calling thread index + * @param[out] Osize = Size in bits of output mapping (rxPDO) found + * @param[out] Isize = Size in bits of input mapping (txPDO) found + * @return >0 if mapping succesful. + * @see ecx_readPDOmap ec_readPDOmapCA + */ +int ec_readPDOmapCA(uint16 Slave, int Thread_n, uint32 *Osize, uint32 *Isize) +{ + return ecx_readPDOmapCA(&ecx_context, Slave, Thread_n, Osize, Isize); +} + +/** CoE read Object Description List. + * + * @param[in] Slave = Slave number. + * @param[out] pODlist = resulting Object Description list. + * @return Workcounter of slave response. + * @see ecx_readODlist + */ +int ec_readODlist(uint16 Slave, ec_ODlistt *pODlist) +{ + return ecx_readODlist(&ecx_context, Slave, pODlist); +} + +/** CoE read Object Description. Adds textual description to object indexes. + * + * @param[in] Item = Item number in ODlist. + * @param[in,out] pODlist = referencing Object Description list. + * @return Workcounter of slave response. + * @see ecx_readODdescription + */ +int ec_readODdescription(uint16 Item, ec_ODlistt *pODlist) +{ + return ecx_readODdescription(&ecx_context, Item, pODlist); +} + +int ec_readOEsingle(uint16 Item, uint8 SubI, ec_ODlistt *pODlist, ec_OElistt *pOElist) +{ + return ecx_readOEsingle(&ecx_context, Item, SubI, pODlist, pOElist); +} + +/** CoE read SDO service object entry. + * + * @param[in] Item = Item in ODlist. + * @param[in] pODlist = Object description list for reference. + * @param[out] pOElist = resulting object entry structure. + * @return Workcounter of slave response. + * @see ecx_readOE + */ +int ec_readOE(uint16 Item, ec_ODlistt *pODlist, ec_OElistt *pOElist) +{ + return ecx_readOE(&ecx_context, Item, pODlist, pOElist); +} +#endif diff --git a/soem-src/soem/ethercatcoe.h b/soem-src/soem/ethercatcoe.h new file mode 100644 index 0000000..a708e0c --- /dev/null +++ b/soem-src/soem/ethercatcoe.h @@ -0,0 +1,95 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +/** \file + * \brief + * Headerfile for ethercatcoe.c + */ + +#ifndef _ethercatcoe_ +#define _ethercatcoe_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +/** max entries in Object Description list */ +#define EC_MAXODLIST 1024 + +/** max entries in Object Entry list */ +#define EC_MAXOELIST 256 + +/* Storage for object description list */ +typedef struct +{ + /** slave number */ + uint16 Slave; + /** number of entries in list */ + uint16 Entries; + /** array of indexes */ + uint16 Index[EC_MAXODLIST]; + /** array of datatypes, see EtherCAT specification */ + uint16 DataType[EC_MAXODLIST]; + /** array of object codes, see EtherCAT specification */ + uint8 ObjectCode[EC_MAXODLIST]; + /** number of subindexes for each index */ + uint8 MaxSub[EC_MAXODLIST]; + /** textual description of each index */ + char Name[EC_MAXODLIST][EC_MAXNAME+1]; +} ec_ODlistt; + +/* storage for object list entry information */ +typedef struct +{ + /** number of entries in list */ + uint16 Entries; + /** array of value infos, see EtherCAT specification */ + uint8 ValueInfo[EC_MAXOELIST]; + /** array of value infos, see EtherCAT specification */ + uint16 DataType[EC_MAXOELIST]; + /** array of bit lengths, see EtherCAT specification */ + uint16 BitLength[EC_MAXOELIST]; + /** array of object access bits, see EtherCAT specification */ + uint16 ObjAccess[EC_MAXOELIST]; + /** textual description of each index */ + char Name[EC_MAXOELIST][EC_MAXNAME+1]; +} ec_OElistt; + +#ifdef EC_VER1 +void ec_SDOerror(uint16 Slave, uint16 Index, uint8 SubIdx, int32 AbortCode); +int ec_SDOread(uint16 slave, uint16 index, uint8 subindex, + boolean CA, int *psize, void *p, int timeout); +int ec_SDOwrite(uint16 Slave, uint16 Index, uint8 SubIndex, + boolean CA, int psize, const void *p, int Timeout); +int ec_RxPDO(uint16 Slave, uint16 RxPDOnumber , int psize, const void *p); +int ec_TxPDO(uint16 slave, uint16 TxPDOnumber , int *psize, void *p, int timeout); +int ec_readPDOmap(uint16 Slave, uint32 *Osize, uint32 *Isize); +int ec_readPDOmapCA(uint16 Slave, int Thread_n, uint32 *Osize, uint32 *Isize); +int ec_readODlist(uint16 Slave, ec_ODlistt *pODlist); +int ec_readODdescription(uint16 Item, ec_ODlistt *pODlist); +int ec_readOEsingle(uint16 Item, uint8 SubI, ec_ODlistt *pODlist, ec_OElistt *pOElist); +int ec_readOE(uint16 Item, ec_ODlistt *pODlist, ec_OElistt *pOElist); +#endif + +void ecx_SDOerror(ecx_contextt *context, uint16 Slave, uint16 Index, uint8 SubIdx, int32 AbortCode); +int ecx_SDOread(ecx_contextt *context, uint16 slave, uint16 index, uint8 subindex, + boolean CA, int *psize, void *p, int timeout); +int ecx_SDOwrite(ecx_contextt *context, uint16 Slave, uint16 Index, uint8 SubIndex, + boolean CA, int psize, const void *p, int Timeout); +int ecx_RxPDO(ecx_contextt *context, uint16 Slave, uint16 RxPDOnumber , int psize, const void *p); +int ecx_TxPDO(ecx_contextt *context, uint16 slave, uint16 TxPDOnumber , int *psize, void *p, int timeout); +int ecx_readPDOmap(ecx_contextt *context, uint16 Slave, uint32 *Osize, uint32 *Isize); +int ecx_readPDOmapCA(ecx_contextt *context, uint16 Slave, int Thread_n, uint32 *Osize, uint32 *Isize); +int ecx_readODlist(ecx_contextt *context, uint16 Slave, ec_ODlistt *pODlist); +int ecx_readODdescription(ecx_contextt *context, uint16 Item, ec_ODlistt *pODlist); +int ecx_readOEsingle(ecx_contextt *context, uint16 Item, uint8 SubI, ec_ODlistt *pODlist, ec_OElistt *pOElist); +int ecx_readOE(ecx_contextt *context, uint16 Item, ec_ODlistt *pODlist, ec_OElistt *pOElist); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/soem-src/soem/ethercatconfig.c b/soem-src/soem/ethercatconfig.c new file mode 100644 index 0000000..9d8d589 --- /dev/null +++ b/soem-src/soem/ethercatconfig.c @@ -0,0 +1,1807 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +/** \file + * \brief + * Configuration module for EtherCAT master. + * + * After successful initialisation with ec_init() or ec_init_redundant() + * the slaves can be auto configured with this module. + */ + +#include +#include +#include "osal.h" +#include "oshw.h" +#include "ethercattype.h" +#include "ethercatbase.h" +#include "ethercatmain.h" +#include "ethercatcoe.h" +#include "ethercatsoe.h" +#include "ethercatconfig.h" + + +typedef struct +{ + int thread_n; + int running; + ecx_contextt *context; + uint16 slave; +} ecx_mapt_t; + +ecx_mapt_t ecx_mapt[EC_MAX_MAPT]; +#if EC_MAX_MAPT > 1 +OSAL_THREAD_HANDLE ecx_threadh[EC_MAX_MAPT]; +#endif + +#ifdef EC_VER1 +/** Slave configuration structure */ +typedef const struct +{ + /** Manufacturer code of slave */ + uint32 man; + /** ID of slave */ + uint32 id; + /** Readable name */ + char name[EC_MAXNAME + 1]; + /** Data type */ + uint8 Dtype; + /** Input bits */ + uint16 Ibits; + /** Output bits */ + uint16 Obits; + /** SyncManager 2 address */ + uint16 SM2a; + /** SyncManager 2 flags */ + uint32 SM2f; + /** SyncManager 3 address */ + uint16 SM3a; + /** SyncManager 3 flags */ + uint32 SM3f; + /** FMMU 0 activation */ + uint8 FM0ac; + /** FMMU 1 activation */ + uint8 FM1ac; +} ec_configlist_t; + +#include "ethercatconfiglist.h" +#endif + +/** standard SM0 flags configuration for mailbox slaves */ +#define EC_DEFAULTMBXSM0 0x00010026 +/** standard SM1 flags configuration for mailbox slaves */ +#define EC_DEFAULTMBXSM1 0x00010022 +/** standard SM0 flags configuration for digital output slaves */ +#define EC_DEFAULTDOSM0 0x00010044 + +#ifdef EC_VER1 +/** Find slave in standard configuration list ec_configlist[] + * + * @param[in] man = manufacturer + * @param[in] id = ID + * @return index in ec_configlist[] when found, otherwise 0 + */ +int ec_findconfig( uint32 man, uint32 id) +{ + int i = 0; + + do + { + i++; + } while ( (ec_configlist[i].man != EC_CONFIGEND) && + ((ec_configlist[i].man != man) || (ec_configlist[i].id != id)) ); + if (ec_configlist[i].man == EC_CONFIGEND) + { + i = 0; + } + return i; +} +#endif + +void ecx_init_context(ecx_contextt *context) +{ + int lp; + *(context->slavecount) = 0; + /* clean ec_slave array */ + memset(context->slavelist, 0x00, sizeof(ec_slavet) * context->maxslave); + memset(context->grouplist, 0x00, sizeof(ec_groupt) * context->maxgroup); + /* clear slave eeprom cache, does not actually read any eeprom */ + ecx_siigetbyte(context, 0, EC_MAXEEPBUF); + for(lp = 0; lp < context->maxgroup; lp++) + { + /* default start address per group entry */ + context->grouplist[lp].logstartaddr = lp << EC_LOGGROUPOFFSET; + } +} + +int ecx_detect_slaves(ecx_contextt *context) +{ + uint8 b; + uint16 w; + int wkc; + + /* make special pre-init register writes to enable MAC[1] local administered bit * + * setting for old netX100 slaves */ + b = 0x00; + ecx_BWR(context->port, 0x0000, ECT_REG_DLALIAS, sizeof(b), &b, EC_TIMEOUTRET3); /* Ignore Alias register */ + w = htoes(EC_STATE_INIT | EC_STATE_ACK); + ecx_BWR(context->port, 0x0000, ECT_REG_ALCTL, sizeof(w), &w, EC_TIMEOUTRET3); /* Reset all slaves to Init */ + /* netX100 should now be happy */ + ecx_BWR(context->port, 0x0000, ECT_REG_ALCTL, sizeof(w), &w, EC_TIMEOUTRET3); /* Reset all slaves to Init */ + wkc = ecx_BRD(context->port, 0x0000, ECT_REG_TYPE, sizeof(w), &w, EC_TIMEOUTSAFE); /* detect number of slaves */ + if (wkc > 0) + { + /* this is strictly "less than" since the master is "slave 0" */ + if (wkc < context->maxslave) + { + *(context->slavecount) = wkc; + } + else + { + EC_PRINT("Error: too many slaves on network: num_slaves=%d, max_slaves=%d\n", + wkc, context->maxslave); + return EC_SLAVECOUNTEXCEEDED; + } + } + return wkc; +} + +static void ecx_set_slaves_to_default(ecx_contextt *context) +{ + uint8 b; + uint16 w; + uint8 zbuf[64]; + memset(&zbuf, 0x00, sizeof(zbuf)); + b = 0x00; + ecx_BWR(context->port, 0x0000, ECT_REG_DLPORT , sizeof(b) , &b, EC_TIMEOUTRET3); /* deact loop manual */ + w = htoes(0x0004); + ecx_BWR(context->port, 0x0000, ECT_REG_IRQMASK , sizeof(w) , &w, EC_TIMEOUTRET3); /* set IRQ mask */ + ecx_BWR(context->port, 0x0000, ECT_REG_RXERR , 8 , &zbuf, EC_TIMEOUTRET3); /* reset CRC counters */ + ecx_BWR(context->port, 0x0000, ECT_REG_FMMU0 , 16 * 3 , &zbuf, EC_TIMEOUTRET3); /* reset FMMU's */ + ecx_BWR(context->port, 0x0000, ECT_REG_SM0 , 8 * 4 , &zbuf, EC_TIMEOUTRET3); /* reset SyncM */ + b = 0x00; + ecx_BWR(context->port, 0x0000, ECT_REG_DCSYNCACT , sizeof(b) , &b, EC_TIMEOUTRET3); /* reset activation register */ + ecx_BWR(context->port, 0x0000, ECT_REG_DCSYSTIME , 4 , &zbuf, EC_TIMEOUTRET3); /* reset system time+ofs */ + w = htoes(0x1000); + ecx_BWR(context->port, 0x0000, ECT_REG_DCSPEEDCNT , sizeof(w) , &w, EC_TIMEOUTRET3); /* DC speedstart */ + w = htoes(0x0c00); + ecx_BWR(context->port, 0x0000, ECT_REG_DCTIMEFILT , sizeof(w) , &w, EC_TIMEOUTRET3); /* DC filt expr */ + b = 0x00; + ecx_BWR(context->port, 0x0000, ECT_REG_DLALIAS , sizeof(b) , &b, EC_TIMEOUTRET3); /* Ignore Alias register */ + w = htoes(EC_STATE_INIT | EC_STATE_ACK); + ecx_BWR(context->port, 0x0000, ECT_REG_ALCTL , sizeof(w) , &w, EC_TIMEOUTRET3); /* Reset all slaves to Init */ + b = 2; + ecx_BWR(context->port, 0x0000, ECT_REG_EEPCFG , sizeof(b) , &b, EC_TIMEOUTRET3); /* force Eeprom from PDI */ + b = 0; + ecx_BWR(context->port, 0x0000, ECT_REG_EEPCFG , sizeof(b) , &b, EC_TIMEOUTRET3); /* set Eeprom to master */ +} + +#ifdef EC_VER1 +static int ecx_config_from_table(ecx_contextt *context, uint16 slave) +{ + int cindex; + ec_slavet *csl; + + csl = &(context->slavelist[slave]); + cindex = ec_findconfig( csl->eep_man, csl->eep_id ); + csl->configindex= cindex; + /* slave found in configuration table ? */ + if (cindex) + { + csl->Dtype = ec_configlist[cindex].Dtype; + strcpy(csl->name ,ec_configlist[cindex].name); + csl->Ibits = ec_configlist[cindex].Ibits; + csl->Obits = ec_configlist[cindex].Obits; + if (csl->Obits) + { + csl->FMMU0func = 1; + } + if (csl->Ibits) + { + csl->FMMU1func = 2; + } + csl->FMMU[0].FMMUactive = ec_configlist[cindex].FM0ac; + csl->FMMU[1].FMMUactive = ec_configlist[cindex].FM1ac; + csl->SM[2].StartAddr = htoes(ec_configlist[cindex].SM2a); + csl->SM[2].SMflags = htoel(ec_configlist[cindex].SM2f); + /* simple (no mailbox) output slave found ? */ + if (csl->Obits && !csl->SM[2].StartAddr) + { + csl->SM[0].StartAddr = htoes(0x0f00); + csl->SM[0].SMlength = htoes((csl->Obits + 7) / 8); + csl->SM[0].SMflags = htoel(EC_DEFAULTDOSM0); + csl->FMMU[0].FMMUactive = 1; + csl->FMMU[0].FMMUtype = 2; + csl->SMtype[0] = 3; + } + /* complex output slave */ + else + { + csl->SM[2].SMlength = htoes((csl->Obits + 7) / 8); + csl->SMtype[2] = 3; + } + csl->SM[3].StartAddr = htoes(ec_configlist[cindex].SM3a); + csl->SM[3].SMflags = htoel(ec_configlist[cindex].SM3f); + /* simple (no mailbox) input slave found ? */ + if (csl->Ibits && !csl->SM[3].StartAddr) + { + csl->SM[1].StartAddr = htoes(0x1000); + csl->SM[1].SMlength = htoes((csl->Ibits + 7) / 8); + csl->SM[1].SMflags = htoel(0x00000000); + csl->FMMU[1].FMMUactive = 1; + csl->FMMU[1].FMMUtype = 1; + csl->SMtype[1] = 4; + } + /* complex input slave */ + else + { + csl->SM[3].SMlength = htoes((csl->Ibits + 7) / 8); + csl->SMtype[3] = 4; + } + } + return cindex; +} +#else +static int ecx_config_from_table(ecx_contextt *context, uint16 slave) +{ + (void)context; + (void)slave; + return 0; +} +#endif + +/* If slave has SII and same slave ID done before, use previous data. + * This is safe because SII is constant for same slave ID. + */ +static int ecx_lookup_prev_sii(ecx_contextt *context, uint16 slave) +{ + int i, nSM; + if ((slave > 1) && (*(context->slavecount) > 0)) + { + i = 1; + while(((context->slavelist[i].eep_man != context->slavelist[slave].eep_man) || + (context->slavelist[i].eep_id != context->slavelist[slave].eep_id ) || + (context->slavelist[i].eep_rev != context->slavelist[slave].eep_rev)) && + (i < slave)) + { + i++; + } + if(i < slave) + { + context->slavelist[slave].CoEdetails = context->slavelist[i].CoEdetails; + context->slavelist[slave].FoEdetails = context->slavelist[i].FoEdetails; + context->slavelist[slave].EoEdetails = context->slavelist[i].EoEdetails; + context->slavelist[slave].SoEdetails = context->slavelist[i].SoEdetails; + if(context->slavelist[i].blockLRW > 0) + { + context->slavelist[slave].blockLRW = 1; + context->slavelist[0].blockLRW++; + } + context->slavelist[slave].Ebuscurrent = context->slavelist[i].Ebuscurrent; + context->slavelist[0].Ebuscurrent += context->slavelist[slave].Ebuscurrent; + memcpy(context->slavelist[slave].name, context->slavelist[i].name, EC_MAXNAME + 1); + for( nSM=0 ; nSM < EC_MAXSM ; nSM++ ) + { + context->slavelist[slave].SM[nSM].StartAddr = context->slavelist[i].SM[nSM].StartAddr; + context->slavelist[slave].SM[nSM].SMlength = context->slavelist[i].SM[nSM].SMlength; + context->slavelist[slave].SM[nSM].SMflags = context->slavelist[i].SM[nSM].SMflags; + } + context->slavelist[slave].FMMU0func = context->slavelist[i].FMMU0func; + context->slavelist[slave].FMMU1func = context->slavelist[i].FMMU1func; + context->slavelist[slave].FMMU2func = context->slavelist[i].FMMU2func; + context->slavelist[slave].FMMU3func = context->slavelist[i].FMMU3func; + EC_PRINT("Copy SII slave %d from %d.\n", slave, i); + return 1; + } + } + return 0; +} + +/** Enumerate and init all slaves. + * + * @param[in] context = context struct + * @param[in] usetable = TRUE when using configtable to init slaves, FALSE otherwise + * @return Workcounter of slave discover datagram = number of slaves found + */ +int ecx_config_init(ecx_contextt *context, uint8 usetable) +{ + uint16 slave, ADPh, configadr, ssigen; + uint16 topology, estat; + int16 topoc, slavec, aliasadr; + uint8 b,h; + uint8 SMc; + uint32 eedat; + int wkc, cindex, nSM; + uint16 val16; + + EC_PRINT("ec_config_init %d\n",usetable); + ecx_init_context(context); + wkc = ecx_detect_slaves(context); + if (wkc > 0) + { + ecx_set_slaves_to_default(context); + for (slave = 1; slave <= *(context->slavecount); slave++) + { + ADPh = (uint16)(1 - slave); + val16 = ecx_APRDw(context->port, ADPh, ECT_REG_PDICTL, EC_TIMEOUTRET3); /* read interface type of slave */ + context->slavelist[slave].Itype = etohs(val16); + /* a node offset is used to improve readability of network frames */ + /* this has no impact on the number of addressable slaves (auto wrap around) */ + ecx_APWRw(context->port, ADPh, ECT_REG_STADR, htoes(slave + EC_NODEOFFSET) , EC_TIMEOUTRET3); /* set node address of slave */ + if (slave == 1) + { + b = 1; /* kill non ecat frames for first slave */ + } + else + { + b = 0; /* pass all frames for following slaves */ + } + ecx_APWRw(context->port, ADPh, ECT_REG_DLCTL, htoes(b), EC_TIMEOUTRET3); /* set non ecat frame behaviour */ + configadr = ecx_APRDw(context->port, ADPh, ECT_REG_STADR, EC_TIMEOUTRET3); + configadr = etohs(configadr); + context->slavelist[slave].configadr = configadr; + ecx_FPRD(context->port, configadr, ECT_REG_ALIAS, sizeof(aliasadr), &aliasadr, EC_TIMEOUTRET3); + context->slavelist[slave].aliasadr = etohs(aliasadr); + ecx_FPRD(context->port, configadr, ECT_REG_EEPSTAT, sizeof(estat), &estat, EC_TIMEOUTRET3); + estat = etohs(estat); + if (estat & EC_ESTAT_R64) /* check if slave can read 8 byte chunks */ + { + context->slavelist[slave].eep_8byte = 1; + } + ecx_readeeprom1(context, slave, ECT_SII_MANUF); /* Manuf */ + } + for (slave = 1; slave <= *(context->slavecount); slave++) + { + eedat = ecx_readeeprom2(context, slave, EC_TIMEOUTEEP); /* Manuf */ + context->slavelist[slave].eep_man = etohl(eedat); + ecx_readeeprom1(context, slave, ECT_SII_SN); /* serial # */ + } + for (slave = 1; slave <= *(context->slavecount); slave++) + { + eedat = ecx_readeeprom2(context, slave, EC_TIMEOUTEEP); /* serial # */ + context->slavelist[slave].eep_sn = etohl(eedat); + ecx_readeeprom1(context, slave, ECT_SII_ID); /* ID */ + } + for (slave = 1; slave <= *(context->slavecount); slave++) + { + eedat = ecx_readeeprom2(context, slave, EC_TIMEOUTEEP); /* ID */ + context->slavelist[slave].eep_id = etohl(eedat); + ecx_readeeprom1(context, slave, ECT_SII_REV); /* revision */ + } + for (slave = 1; slave <= *(context->slavecount); slave++) + { + eedat = ecx_readeeprom2(context, slave, EC_TIMEOUTEEP); /* revision */ + context->slavelist[slave].eep_rev = etohl(eedat); + ecx_readeeprom1(context, slave, ECT_SII_RXMBXADR); /* write mailbox address + mailboxsize */ + } + for (slave = 1; slave <= *(context->slavecount); slave++) + { + eedat = ecx_readeeprom2(context, slave, EC_TIMEOUTEEP); /* write mailbox address and mailboxsize */ + context->slavelist[slave].mbx_wo = (uint16)LO_WORD(etohl(eedat)); + context->slavelist[slave].mbx_l = (uint16)HI_WORD(etohl(eedat)); + if (context->slavelist[slave].mbx_l > 0) + { + ecx_readeeprom1(context, slave, ECT_SII_TXMBXADR); /* read mailbox offset */ + } + } + for (slave = 1; slave <= *(context->slavecount); slave++) + { + if (context->slavelist[slave].mbx_l > 0) + { + eedat = ecx_readeeprom2(context, slave, EC_TIMEOUTEEP); /* read mailbox offset */ + context->slavelist[slave].mbx_ro = (uint16)LO_WORD(etohl(eedat)); /* read mailbox offset */ + context->slavelist[slave].mbx_rl = (uint16)HI_WORD(etohl(eedat)); /*read mailbox length */ + if (context->slavelist[slave].mbx_rl == 0) + { + context->slavelist[slave].mbx_rl = context->slavelist[slave].mbx_l; + } + ecx_readeeprom1(context, slave, ECT_SII_MBXPROTO); + } + configadr = context->slavelist[slave].configadr; + val16 = ecx_FPRDw(context->port, configadr, ECT_REG_ESCSUP, EC_TIMEOUTRET3); + if ((etohs(val16) & 0x04) > 0) /* Support DC? */ + { + context->slavelist[slave].hasdc = TRUE; + } + else + { + context->slavelist[slave].hasdc = FALSE; + } + topology = ecx_FPRDw(context->port, configadr, ECT_REG_DLSTAT, EC_TIMEOUTRET3); /* extract topology from DL status */ + topology = etohs(topology); + h = 0; + b = 0; + if ((topology & 0x0300) == 0x0200) /* port0 open and communication established */ + { + h++; + b |= 0x01; + } + if ((topology & 0x0c00) == 0x0800) /* port1 open and communication established */ + { + h++; + b |= 0x02; + } + if ((topology & 0x3000) == 0x2000) /* port2 open and communication established */ + { + h++; + b |= 0x04; + } + if ((topology & 0xc000) == 0x8000) /* port3 open and communication established */ + { + h++; + b |= 0x08; + } + /* ptype = Physical type*/ + val16 = ecx_FPRDw(context->port, configadr, ECT_REG_PORTDES, EC_TIMEOUTRET3); + context->slavelist[slave].ptype = LO_BYTE(etohs(val16)); + context->slavelist[slave].topology = h; + context->slavelist[slave].activeports = b; + /* 0=no links, not possible */ + /* 1=1 link , end of line */ + /* 2=2 links , one before and one after */ + /* 3=3 links , split point */ + /* 4=4 links , cross point */ + /* search for parent */ + context->slavelist[slave].parent = 0; /* parent is master */ + if (slave > 1) + { + topoc = 0; + slavec = slave - 1; + do + { + topology = context->slavelist[slavec].topology; + if (topology == 1) + { + topoc--; /* endpoint found */ + } + if (topology == 3) + { + topoc++; /* split found */ + } + if (topology == 4) + { + topoc += 2; /* cross found */ + } + if (((topoc >= 0) && (topology > 1)) || + (slavec == 1)) /* parent found */ + { + context->slavelist[slave].parent = slavec; + slavec = 1; + } + slavec--; + } + while (slavec > 0); + } + (void)ecx_statecheck(context, slave, EC_STATE_INIT, EC_TIMEOUTSTATE); //* check state change Init */ + + /* set default mailbox configuration if slave has mailbox */ + if (context->slavelist[slave].mbx_l>0) + { + context->slavelist[slave].SMtype[0] = 1; + context->slavelist[slave].SMtype[1] = 2; + context->slavelist[slave].SMtype[2] = 3; + context->slavelist[slave].SMtype[3] = 4; + context->slavelist[slave].SM[0].StartAddr = htoes(context->slavelist[slave].mbx_wo); + context->slavelist[slave].SM[0].SMlength = htoes(context->slavelist[slave].mbx_l); + context->slavelist[slave].SM[0].SMflags = htoel(EC_DEFAULTMBXSM0); + context->slavelist[slave].SM[1].StartAddr = htoes(context->slavelist[slave].mbx_ro); + context->slavelist[slave].SM[1].SMlength = htoes(context->slavelist[slave].mbx_rl); + context->slavelist[slave].SM[1].SMflags = htoel(EC_DEFAULTMBXSM1); + eedat = ecx_readeeprom2(context, slave, EC_TIMEOUTEEP); + context->slavelist[slave].mbx_proto = (uint16)etohl(eedat); + } + cindex = 0; + /* use configuration table ? */ + if (usetable == 1) + { + cindex = ecx_config_from_table(context, slave); + } + /* slave not in configuration table, find out via SII */ + if (!cindex && !ecx_lookup_prev_sii(context, slave)) + { + ssigen = ecx_siifind(context, slave, ECT_SII_GENERAL); + /* SII general section */ + if (ssigen) + { + context->slavelist[slave].CoEdetails = ecx_siigetbyte(context, slave, ssigen + 0x07); + context->slavelist[slave].FoEdetails = ecx_siigetbyte(context, slave, ssigen + 0x08); + context->slavelist[slave].EoEdetails = ecx_siigetbyte(context, slave, ssigen + 0x09); + context->slavelist[slave].SoEdetails = ecx_siigetbyte(context, slave, ssigen + 0x0a); + if((ecx_siigetbyte(context, slave, ssigen + 0x0d) & 0x02) > 0) + { + context->slavelist[slave].blockLRW = 1; + context->slavelist[0].blockLRW++; + } + context->slavelist[slave].Ebuscurrent = ecx_siigetbyte(context, slave, ssigen + 0x0e); + context->slavelist[slave].Ebuscurrent += ecx_siigetbyte(context, slave, ssigen + 0x0f) << 8; + context->slavelist[0].Ebuscurrent += context->slavelist[slave].Ebuscurrent; + } + /* SII strings section */ + if (ecx_siifind(context, slave, ECT_SII_STRING) > 0) + { + ecx_siistring(context, context->slavelist[slave].name, slave, 1); + } + /* no name for slave found, use constructed name */ + else + { + sprintf(context->slavelist[slave].name, "? M:%8.8x I:%8.8x", + (unsigned int)context->slavelist[slave].eep_man, + (unsigned int)context->slavelist[slave].eep_id); + } + /* SII SM section */ + nSM = ecx_siiSM(context, slave, context->eepSM); + if (nSM>0) + { + context->slavelist[slave].SM[0].StartAddr = htoes(context->eepSM->PhStart); + context->slavelist[slave].SM[0].SMlength = htoes(context->eepSM->Plength); + context->slavelist[slave].SM[0].SMflags = + htoel((context->eepSM->Creg) + (context->eepSM->Activate << 16)); + SMc = 1; + while ((SMc < EC_MAXSM) && ecx_siiSMnext(context, slave, context->eepSM, SMc)) + { + context->slavelist[slave].SM[SMc].StartAddr = htoes(context->eepSM->PhStart); + context->slavelist[slave].SM[SMc].SMlength = htoes(context->eepSM->Plength); + context->slavelist[slave].SM[SMc].SMflags = + htoel((context->eepSM->Creg) + (context->eepSM->Activate << 16)); + SMc++; + } + } + /* SII FMMU section */ + if (ecx_siiFMMU(context, slave, context->eepFMMU)) + { + if (context->eepFMMU->FMMU0 !=0xff) + { + context->slavelist[slave].FMMU0func = context->eepFMMU->FMMU0; + } + if (context->eepFMMU->FMMU1 !=0xff) + { + context->slavelist[slave].FMMU1func = context->eepFMMU->FMMU1; + } + if (context->eepFMMU->FMMU2 !=0xff) + { + context->slavelist[slave].FMMU2func = context->eepFMMU->FMMU2; + } + if (context->eepFMMU->FMMU3 !=0xff) + { + context->slavelist[slave].FMMU3func = context->eepFMMU->FMMU3; + } + } + } + + if (context->slavelist[slave].mbx_l > 0) + { + if (context->slavelist[slave].SM[0].StartAddr == 0x0000) /* should never happen */ + { + EC_PRINT("Slave %d has no proper mailbox in configuration, try default.\n", slave); + context->slavelist[slave].SM[0].StartAddr = htoes(0x1000); + context->slavelist[slave].SM[0].SMlength = htoes(0x0080); + context->slavelist[slave].SM[0].SMflags = htoel(EC_DEFAULTMBXSM0); + context->slavelist[slave].SMtype[0] = 1; + } + if (context->slavelist[slave].SM[1].StartAddr == 0x0000) /* should never happen */ + { + EC_PRINT("Slave %d has no proper mailbox out configuration, try default.\n", slave); + context->slavelist[slave].SM[1].StartAddr = htoes(0x1080); + context->slavelist[slave].SM[1].SMlength = htoes(0x0080); + context->slavelist[slave].SM[1].SMflags = htoel(EC_DEFAULTMBXSM1); + context->slavelist[slave].SMtype[1] = 2; + } + /* program SM0 mailbox in and SM1 mailbox out for slave */ + /* writing both SM in one datagram will solve timing issue in old NETX */ + ecx_FPWR(context->port, configadr, ECT_REG_SM0, sizeof(ec_smt) * 2, + &(context->slavelist[slave].SM[0]), EC_TIMEOUTRET3); + } + /* some slaves need eeprom available to PDI in init->preop transition */ + ecx_eeprom2pdi(context, slave); + /* User may override automatic state change */ + if (context->manualstatechange == 0) + { + /* request pre_op for slave */ + ecx_FPWRw(context->port, + configadr, + ECT_REG_ALCTL, + htoes(EC_STATE_PRE_OP | EC_STATE_ACK), + EC_TIMEOUTRET3); /* set preop status */ + } + } + } + return wkc; +} + +/* If slave has SII mapping and same slave ID done before, use previous mapping. + * This is safe because SII mapping is constant for same slave ID. + */ +static int ecx_lookup_mapping(ecx_contextt *context, uint16 slave, uint32 *Osize, uint32 *Isize) +{ + int i, nSM; + if ((slave > 1) && (*(context->slavecount) > 0)) + { + i = 1; + while(((context->slavelist[i].eep_man != context->slavelist[slave].eep_man) || + (context->slavelist[i].eep_id != context->slavelist[slave].eep_id ) || + (context->slavelist[i].eep_rev != context->slavelist[slave].eep_rev)) && + (i < slave)) + { + i++; + } + if(i < slave) + { + for( nSM=0 ; nSM < EC_MAXSM ; nSM++ ) + { + context->slavelist[slave].SM[nSM].SMlength = context->slavelist[i].SM[nSM].SMlength; + context->slavelist[slave].SMtype[nSM] = context->slavelist[i].SMtype[nSM]; + } + *Osize = context->slavelist[i].Obits; + *Isize = context->slavelist[i].Ibits; + context->slavelist[slave].Obits = (uint16)*Osize; + context->slavelist[slave].Ibits = (uint16)*Isize; + EC_PRINT("Copy mapping slave %d from %d.\n", slave, i); + return 1; + } + } + return 0; +} + +static int ecx_map_coe_soe(ecx_contextt *context, uint16 slave, int thread_n) +{ + uint32 Isize, Osize; + int rval; + + ecx_statecheck(context, slave, EC_STATE_PRE_OP, EC_TIMEOUTSTATE); /* check state change pre-op */ + + EC_PRINT(" >Slave %d, configadr %x, state %2.2x\n", + slave, context->slavelist[slave].configadr, context->slavelist[slave].state); + + /* execute special slave configuration hook Pre-Op to Safe-OP */ + if(context->slavelist[slave].PO2SOconfig) /* only if registered */ + { + context->slavelist[slave].PO2SOconfig(slave, context->slavelist[slave].user); + } + if (context->slavelist[slave].PO2SOconfigx) /* only if registered */ + { + context->slavelist[slave].PO2SOconfigx(context, slave); + } + /* if slave not found in configlist find IO mapping in slave self */ + if (!context->slavelist[slave].configindex) + { + Isize = 0; + Osize = 0; + if (context->slavelist[slave].mbx_proto & ECT_MBXPROT_COE) /* has CoE */ + { + rval = 0; + if (context->slavelist[slave].CoEdetails & ECT_COEDET_SDOCA) /* has Complete Access */ + { + /* read PDO mapping via CoE and use Complete Access */ + rval = ecx_readPDOmapCA(context, slave, thread_n, &Osize, &Isize); + } + if (!rval) /* CA not available or not succeeded */ + { + /* read PDO mapping via CoE */ + rval = ecx_readPDOmap(context, slave, &Osize, &Isize); + } + EC_PRINT(" CoE Osize:%u Isize:%u\n", Osize, Isize); + } + if ((!Isize && !Osize) && (context->slavelist[slave].mbx_proto & ECT_MBXPROT_SOE)) /* has SoE */ + { + /* read AT / MDT mapping via SoE */ + rval = ecx_readIDNmap(context, slave, &Osize, &Isize); + context->slavelist[slave].SM[2].SMlength = htoes((uint16)((Osize + 7) / 8)); + context->slavelist[slave].SM[3].SMlength = htoes((uint16)((Isize + 7) / 8)); + EC_PRINT(" SoE Osize:%u Isize:%u\n", Osize, Isize); + } + context->slavelist[slave].Obits = (uint16)Osize; + context->slavelist[slave].Ibits = (uint16)Isize; + } + + return 1; +} + +static int ecx_map_sii(ecx_contextt *context, uint16 slave) +{ + uint32 Isize, Osize; + int nSM; + ec_eepromPDOt eepPDO; + + Osize = context->slavelist[slave].Obits; + Isize = context->slavelist[slave].Ibits; + + if (!Isize && !Osize) /* find PDO in previous slave with same ID */ + { + (void)ecx_lookup_mapping(context, slave, &Osize, &Isize); + } + if (!Isize && !Osize) /* find PDO mapping by SII */ + { + memset(&eepPDO, 0, sizeof(eepPDO)); + Isize = ecx_siiPDO(context, slave, &eepPDO, 0); + EC_PRINT(" SII Isize:%u\n", Isize); + for( nSM=0 ; nSM < EC_MAXSM ; nSM++ ) + { + if (eepPDO.SMbitsize[nSM] > 0) + { + context->slavelist[slave].SM[nSM].SMlength = htoes((eepPDO.SMbitsize[nSM] + 7) / 8); + context->slavelist[slave].SMtype[nSM] = 4; + EC_PRINT(" SM%d length %d\n", nSM, eepPDO.SMbitsize[nSM]); + } + } + Osize = ecx_siiPDO(context, slave, &eepPDO, 1); + EC_PRINT(" SII Osize:%u\n", Osize); + for( nSM=0 ; nSM < EC_MAXSM ; nSM++ ) + { + if (eepPDO.SMbitsize[nSM] > 0) + { + context->slavelist[slave].SM[nSM].SMlength = htoes((eepPDO.SMbitsize[nSM] + 7) / 8); + context->slavelist[slave].SMtype[nSM] = 3; + EC_PRINT(" SM%d length %d\n", nSM, eepPDO.SMbitsize[nSM]); + } + } + } + context->slavelist[slave].Obits = (uint16)Osize; + context->slavelist[slave].Ibits = (uint16)Isize; + EC_PRINT(" ISIZE:%d %d OSIZE:%d\n", + context->slavelist[slave].Ibits, Isize,context->slavelist[slave].Obits); + + return 1; +} + +static int ecx_map_sm(ecx_contextt *context, uint16 slave) +{ + uint16 configadr; + int nSM; + + configadr = context->slavelist[slave].configadr; + + EC_PRINT(" SM programming\n"); + if (!context->slavelist[slave].mbx_l && context->slavelist[slave].SM[0].StartAddr) + { + ecx_FPWR(context->port, configadr, ECT_REG_SM0, + sizeof(ec_smt), &(context->slavelist[slave].SM[0]), EC_TIMEOUTRET3); + EC_PRINT(" SM0 Type:%d StartAddr:%4.4x Flags:%8.8x\n", + context->slavelist[slave].SMtype[0], + etohs(context->slavelist[slave].SM[0].StartAddr), + etohl(context->slavelist[slave].SM[0].SMflags)); + } + if (!context->slavelist[slave].mbx_l && context->slavelist[slave].SM[1].StartAddr) + { + ecx_FPWR(context->port, configadr, ECT_REG_SM1, + sizeof(ec_smt), &context->slavelist[slave].SM[1], EC_TIMEOUTRET3); + EC_PRINT(" SM1 Type:%d StartAddr:%4.4x Flags:%8.8x\n", + context->slavelist[slave].SMtype[1], + etohs(context->slavelist[slave].SM[1].StartAddr), + etohl(context->slavelist[slave].SM[1].SMflags)); + } + /* program SM2 to SMx */ + for( nSM = 2 ; nSM < EC_MAXSM ; nSM++ ) + { + if (context->slavelist[slave].SM[nSM].StartAddr) + { + /* check if SM length is zero -> clear enable flag */ + if( context->slavelist[slave].SM[nSM].SMlength == 0) + { + context->slavelist[slave].SM[nSM].SMflags = + htoel( etohl(context->slavelist[slave].SM[nSM].SMflags) & EC_SMENABLEMASK); + } + /* if SM length is non zero always set enable flag */ + else + { + context->slavelist[slave].SM[nSM].SMflags = + htoel( etohl(context->slavelist[slave].SM[nSM].SMflags) | ~EC_SMENABLEMASK); + } + ecx_FPWR(context->port, configadr, (uint16)(ECT_REG_SM0 + (nSM * sizeof(ec_smt))), + sizeof(ec_smt), &context->slavelist[slave].SM[nSM], EC_TIMEOUTRET3); + EC_PRINT(" SM%d Type:%d StartAddr:%4.4x Flags:%8.8x\n", nSM, + context->slavelist[slave].SMtype[nSM], + etohs(context->slavelist[slave].SM[nSM].StartAddr), + etohl(context->slavelist[slave].SM[nSM].SMflags)); + } + } + if (context->slavelist[slave].Ibits > 7) + { + context->slavelist[slave].Ibytes = (context->slavelist[slave].Ibits + 7) / 8; + } + if (context->slavelist[slave].Obits > 7) + { + context->slavelist[slave].Obytes = (context->slavelist[slave].Obits + 7) / 8; + } + + return 1; +} + +#if EC_MAX_MAPT > 1 +OSAL_THREAD_FUNC ecx_mapper_thread(void *param) +{ + ecx_mapt_t *maptp; + maptp = param; + ecx_map_coe_soe(maptp->context, maptp->slave, maptp->thread_n); + maptp->running = 0; +} + +static int ecx_find_mapt(void) +{ + int p; + p = 0; + while((p < EC_MAX_MAPT) && ecx_mapt[p].running) + { + p++; + } + if(p < EC_MAX_MAPT) + { + return p; + } + else + { + return -1; + } +} +#endif + +static int ecx_get_threadcount(void) +{ + int thrc, thrn; + thrc = 0; + for(thrn = 0 ; thrn < EC_MAX_MAPT ; thrn++) + { + thrc += ecx_mapt[thrn].running; + } + return thrc; +} + +static void ecx_config_find_mappings(ecx_contextt *context, uint8 group) +{ + int thrn, thrc; + uint16 slave; + + for (thrn = 0; thrn < EC_MAX_MAPT; thrn++) + { + ecx_mapt[thrn].running = 0; + } + /* find CoE and SoE mapping of slaves in multiple threads */ + for (slave = 1; slave <= *(context->slavecount); slave++) + { + if (!group || (group == context->slavelist[slave].group)) + { +#if EC_MAX_MAPT > 1 + /* multi-threaded version */ + while ((thrn = ecx_find_mapt()) < 0) + { + osal_usleep(1000); + } + ecx_mapt[thrn].context = context; + ecx_mapt[thrn].slave = slave; + ecx_mapt[thrn].thread_n = thrn; + ecx_mapt[thrn].running = 1; + osal_thread_create(&(ecx_threadh[thrn]), 128000, + &ecx_mapper_thread, &(ecx_mapt[thrn])); +#else + /* serialised version */ + ecx_map_coe_soe(context, slave, 0); +#endif + } + } + /* wait for all threads to finish */ + do + { + thrc = ecx_get_threadcount(); + if (thrc) + { + osal_usleep(1000); + } + } while (thrc); + /* find SII mapping of slave and program SM */ + for (slave = 1; slave <= *(context->slavecount); slave++) + { + if (!group || (group == context->slavelist[slave].group)) + { + ecx_map_sii(context, slave); + ecx_map_sm(context, slave); + } + } +} + +static void ecx_config_create_input_mappings(ecx_contextt *context, void *pIOmap, + uint8 group, int16 slave, uint32 * LogAddr, uint8 * BitPos) +{ + int BitCount = 0; + int FMMUdone = 0; + uint16 ByteCount = 0; + uint16 FMMUsize = 0; + uint8 SMc = 0; + uint16 EndAddr; + uint16 SMlength; + uint16 configadr; + uint8 FMMUc; + + EC_PRINT(" =Slave %d, INPUT MAPPING\n", slave); + + configadr = context->slavelist[slave].configadr; + FMMUc = context->slavelist[slave].FMMUunused; + if (context->slavelist[slave].Obits) /* find free FMMU */ + { + while (context->slavelist[slave].FMMU[FMMUc].LogStart) + { + FMMUc++; + } + } + /* search for SM that contribute to the input mapping */ + while ((SMc < EC_MAXSM) && (FMMUdone < ((context->slavelist[slave].Ibits + 7) / 8))) + { + EC_PRINT(" FMMU %d\n", FMMUc); + while ((SMc < (EC_MAXSM - 1)) && (context->slavelist[slave].SMtype[SMc] != 4)) + { + SMc++; + } + EC_PRINT(" SM%d\n", SMc); + context->slavelist[slave].FMMU[FMMUc].PhysStart = + context->slavelist[slave].SM[SMc].StartAddr; + SMlength = etohs(context->slavelist[slave].SM[SMc].SMlength); + ByteCount += SMlength; + BitCount += SMlength * 8; + EndAddr = etohs(context->slavelist[slave].SM[SMc].StartAddr) + SMlength; + while ((BitCount < context->slavelist[slave].Ibits) && (SMc < (EC_MAXSM - 1))) /* more SM for input */ + { + SMc++; + while ((SMc < (EC_MAXSM - 1)) && (context->slavelist[slave].SMtype[SMc] != 4)) + { + SMc++; + } + /* if addresses from more SM connect use one FMMU otherwise break up in multiple FMMU */ + if (etohs(context->slavelist[slave].SM[SMc].StartAddr) > EndAddr) + { + break; + } + EC_PRINT(" SM%d\n", SMc); + SMlength = etohs(context->slavelist[slave].SM[SMc].SMlength); + ByteCount += SMlength; + BitCount += SMlength * 8; + EndAddr = etohs(context->slavelist[slave].SM[SMc].StartAddr) + SMlength; + } + + /* bit oriented slave */ + if (!context->slavelist[slave].Ibytes) + { + context->slavelist[slave].FMMU[FMMUc].LogStart = htoel(*LogAddr); + context->slavelist[slave].FMMU[FMMUc].LogStartbit = *BitPos; + *BitPos += context->slavelist[slave].Ibits - 1; + if (*BitPos > 7) + { + *LogAddr += 1; + *BitPos -= 8; + } + FMMUsize = (uint16)(*LogAddr - etohl(context->slavelist[slave].FMMU[FMMUc].LogStart) + 1); + context->slavelist[slave].FMMU[FMMUc].LogLength = htoes(FMMUsize); + context->slavelist[slave].FMMU[FMMUc].LogEndbit = *BitPos; + *BitPos += 1; + if (*BitPos > 7) + { + *LogAddr += 1; + *BitPos -= 8; + } + } + /* byte oriented slave */ + else + { + if (*BitPos) + { + *LogAddr += 1; + *BitPos = 0; + } + context->slavelist[slave].FMMU[FMMUc].LogStart = htoel(*LogAddr); + context->slavelist[slave].FMMU[FMMUc].LogStartbit = *BitPos; + *BitPos = 7; + FMMUsize = ByteCount; + if ((FMMUsize + FMMUdone)> (int)context->slavelist[slave].Ibytes) + { + FMMUsize = (uint16)(context->slavelist[slave].Ibytes - FMMUdone); + } + *LogAddr += FMMUsize; + context->slavelist[slave].FMMU[FMMUc].LogLength = htoes(FMMUsize); + context->slavelist[slave].FMMU[FMMUc].LogEndbit = *BitPos; + *BitPos = 0; + } + FMMUdone += FMMUsize; + if (context->slavelist[slave].FMMU[FMMUc].LogLength) + { + context->slavelist[slave].FMMU[FMMUc].PhysStartBit = 0; + context->slavelist[slave].FMMU[FMMUc].FMMUtype = 1; + context->slavelist[slave].FMMU[FMMUc].FMMUactive = 1; + /* program FMMU for input */ + ecx_FPWR(context->port, configadr, ECT_REG_FMMU0 + (sizeof(ec_fmmut) * FMMUc), + sizeof(ec_fmmut), &(context->slavelist[slave].FMMU[FMMUc]), EC_TIMEOUTRET3); + } + if (!context->slavelist[slave].inputs) + { + if (group) + { + context->slavelist[slave].inputs = + (uint8 *)(pIOmap) + + etohl(context->slavelist[slave].FMMU[FMMUc].LogStart) - + context->grouplist[group].logstartaddr; + } + else + { + context->slavelist[slave].inputs = + (uint8 *)(pIOmap) + + etohl(context->slavelist[slave].FMMU[FMMUc].LogStart); + } + context->slavelist[slave].Istartbit = + context->slavelist[slave].FMMU[FMMUc].LogStartbit; + EC_PRINT(" Inputs %p startbit %d\n", + context->slavelist[slave].inputs, + context->slavelist[slave].Istartbit); + } + FMMUc++; + } + context->slavelist[slave].FMMUunused = FMMUc; +} + +static void ecx_config_create_output_mappings(ecx_contextt *context, void *pIOmap, + uint8 group, int16 slave, uint32 * LogAddr, uint8 * BitPos) +{ + int BitCount = 0; + int FMMUdone = 0; + uint16 ByteCount = 0; + uint16 FMMUsize = 0; + uint8 SMc = 0; + uint16 EndAddr; + uint16 SMlength; + uint16 configadr; + uint8 FMMUc; + + EC_PRINT(" OUTPUT MAPPING\n"); + + FMMUc = context->slavelist[slave].FMMUunused; + configadr = context->slavelist[slave].configadr; + + /* search for SM that contribute to the output mapping */ + while ((SMc < EC_MAXSM) && (FMMUdone < ((context->slavelist[slave].Obits + 7) / 8))) + { + EC_PRINT(" FMMU %d\n", FMMUc); + while ((SMc < (EC_MAXSM - 1)) && (context->slavelist[slave].SMtype[SMc] != 3)) + { + SMc++; + } + EC_PRINT(" SM%d\n", SMc); + context->slavelist[slave].FMMU[FMMUc].PhysStart = + context->slavelist[slave].SM[SMc].StartAddr; + SMlength = etohs(context->slavelist[slave].SM[SMc].SMlength); + ByteCount += SMlength; + BitCount += SMlength * 8; + EndAddr = etohs(context->slavelist[slave].SM[SMc].StartAddr) + SMlength; + while ((BitCount < context->slavelist[slave].Obits) && (SMc < (EC_MAXSM - 1))) /* more SM for output */ + { + SMc++; + while ((SMc < (EC_MAXSM - 1)) && (context->slavelist[slave].SMtype[SMc] != 3)) + { + SMc++; + } + /* if addresses from more SM connect use one FMMU otherwise break up in multiple FMMU */ + if (etohs(context->slavelist[slave].SM[SMc].StartAddr) > EndAddr) + { + break; + } + EC_PRINT(" SM%d\n", SMc); + SMlength = etohs(context->slavelist[slave].SM[SMc].SMlength); + ByteCount += SMlength; + BitCount += SMlength * 8; + EndAddr = etohs(context->slavelist[slave].SM[SMc].StartAddr) + SMlength; + } + + /* bit oriented slave */ + if (!context->slavelist[slave].Obytes) + { + context->slavelist[slave].FMMU[FMMUc].LogStart = htoel(*LogAddr); + context->slavelist[slave].FMMU[FMMUc].LogStartbit = *BitPos; + *BitPos += context->slavelist[slave].Obits - 1; + if (*BitPos > 7) + { + *LogAddr += 1; + *BitPos -= 8; + } + FMMUsize = (uint16)(*LogAddr - etohl(context->slavelist[slave].FMMU[FMMUc].LogStart) + 1); + context->slavelist[slave].FMMU[FMMUc].LogLength = htoes(FMMUsize); + context->slavelist[slave].FMMU[FMMUc].LogEndbit = *BitPos; + *BitPos += 1; + if (*BitPos > 7) + { + *LogAddr += 1; + *BitPos -= 8; + } + } + /* byte oriented slave */ + else + { + if (*BitPos) + { + *LogAddr += 1; + *BitPos = 0; + } + context->slavelist[slave].FMMU[FMMUc].LogStart = htoel(*LogAddr); + context->slavelist[slave].FMMU[FMMUc].LogStartbit = *BitPos; + *BitPos = 7; + FMMUsize = ByteCount; + if ((FMMUsize + FMMUdone)> (int)context->slavelist[slave].Obytes) + { + FMMUsize = (uint16)(context->slavelist[slave].Obytes - FMMUdone); + } + *LogAddr += FMMUsize; + context->slavelist[slave].FMMU[FMMUc].LogLength = htoes(FMMUsize); + context->slavelist[slave].FMMU[FMMUc].LogEndbit = *BitPos; + *BitPos = 0; + } + FMMUdone += FMMUsize; + if (context->slavelist[slave].FMMU[FMMUc].LogLength) + { + context->slavelist[slave].FMMU[FMMUc].PhysStartBit = 0; + context->slavelist[slave].FMMU[FMMUc].FMMUtype = 2; + context->slavelist[slave].FMMU[FMMUc].FMMUactive = 1; + /* program FMMU for output */ + ecx_FPWR(context->port, configadr, ECT_REG_FMMU0 + (sizeof(ec_fmmut) * FMMUc), + sizeof(ec_fmmut), &(context->slavelist[slave].FMMU[FMMUc]), EC_TIMEOUTRET3); + } + if (!context->slavelist[slave].outputs) + { + if (group) + { + context->slavelist[slave].outputs = + (uint8 *)(pIOmap) + + etohl(context->slavelist[slave].FMMU[FMMUc].LogStart) - + context->grouplist[group].logstartaddr; + } + else + { + context->slavelist[slave].outputs = + (uint8 *)(pIOmap) + + etohl(context->slavelist[slave].FMMU[FMMUc].LogStart); + } + context->slavelist[slave].Ostartbit = + context->slavelist[slave].FMMU[FMMUc].LogStartbit; + EC_PRINT(" slave %d Outputs %p startbit %d\n", + slave, + context->slavelist[slave].outputs, + context->slavelist[slave].Ostartbit); + } + FMMUc++; + } + context->slavelist[slave].FMMUunused = FMMUc; +} + +static int ecx_main_config_map_group(ecx_contextt *context, void *pIOmap, uint8 group, boolean forceByteAlignment) +{ + uint16 slave, configadr; + uint8 BitPos; + uint32 LogAddr = 0; + uint32 oLogAddr = 0; + uint32 diff; + uint16 currentsegment = 0; + uint32 segmentsize = 0; + uint32 segmentmaxsize = (EC_MAXLRWDATA - EC_FIRSTDCDATAGRAM); /* first segment must account for DC overhead */ + + if ((*(context->slavecount) > 0) && (group < context->maxgroup)) + { + EC_PRINT("ec_config_map_group IOmap:%p group:%d\n", pIOmap, group); + LogAddr = context->grouplist[group].logstartaddr; + oLogAddr = LogAddr; + BitPos = 0; + context->grouplist[group].nsegments = 0; + context->grouplist[group].outputsWKC = 0; + context->grouplist[group].inputsWKC = 0; + + /* Find mappings and program syncmanagers */ + ecx_config_find_mappings(context, group); + + /* do output mapping of slave and program FMMUs */ + for (slave = 1; slave <= *(context->slavecount); slave++) + { + configadr = context->slavelist[slave].configadr; + + if (!group || (group == context->slavelist[slave].group)) + { + /* create output mapping */ + if (context->slavelist[slave].Obits) + { + ecx_config_create_output_mappings (context, pIOmap, group, slave, &LogAddr, &BitPos); + + if (forceByteAlignment) + { + /* Force byte alignment if the output is < 8 bits */ + if (BitPos) + { + LogAddr++; + BitPos = 0; + } + } + + diff = LogAddr - oLogAddr; + oLogAddr = LogAddr; + if ((segmentsize + diff) > segmentmaxsize && diff <= segmentmaxsize && currentsegment < EC_MAXIOSEGMENTS) + { + context->grouplist[group].IOsegment[currentsegment++] = segmentsize; + segmentsize = 0; + segmentmaxsize = EC_MAXLRWDATA; /* can ignore DC overhead after first segment */ + } + segmentsize += diff; + while (segmentsize > segmentmaxsize && currentsegment < EC_MAXIOSEGMENTS) + { + context->grouplist[group].IOsegment[currentsegment++] = segmentmaxsize; + segmentsize -= segmentmaxsize; + context->grouplist[group].outputsWKC++; + segmentmaxsize = EC_MAXLRWDATA; /* can ignore DC overhead after first segment */ + } + /* if this slave added output data and there is a partial segment still outstanding increment the outputwkc */ + if (segmentsize && diff) + context->grouplist[group].outputsWKC++; + } + } + } + if (BitPos) + { + LogAddr++; + oLogAddr = LogAddr; + BitPos = 0; + if ((segmentsize + 1) > segmentmaxsize && currentsegment < EC_MAXIOSEGMENTS) + { + context->grouplist[group].IOsegment[currentsegment++] = segmentsize; + segmentsize = 0; + context->grouplist[group].outputsWKC++; + segmentmaxsize = EC_MAXLRWDATA; /* can ignore DC overhead after first segment */ + } + segmentsize += 1; + } + context->grouplist[group].outputs = pIOmap; + context->grouplist[group].Obytes = LogAddr - context->grouplist[group].logstartaddr; + context->grouplist[group].nsegments = currentsegment + 1; + context->grouplist[group].Isegment = currentsegment; + context->grouplist[group].Ioffset = (uint16)segmentsize; + if (!group) + { + context->slavelist[0].outputs = pIOmap; + context->slavelist[0].Obytes = LogAddr - + context->grouplist[group].logstartaddr; /* store output bytes in master record */ + } + + /* do input mapping of slave and program FMMUs */ + for (slave = 1; slave <= *(context->slavecount); slave++) + { + configadr = context->slavelist[slave].configadr; + if (!group || (group == context->slavelist[slave].group)) + { + /* create input mapping */ + if (context->slavelist[slave].Ibits) + { + + ecx_config_create_input_mappings(context, pIOmap, group, slave, &LogAddr, &BitPos); + + if (forceByteAlignment) + { + /* Force byte alignment if the input is < 8 bits */ + if (BitPos) + { + LogAddr++; + BitPos = 0; + } + } + + diff = LogAddr - oLogAddr; + oLogAddr = LogAddr; + if ((segmentsize + diff) > segmentmaxsize && diff <= segmentmaxsize && currentsegment < EC_MAXIOSEGMENTS) + { + context->grouplist[group].IOsegment[currentsegment++] = segmentsize; + segmentsize = 0; + segmentmaxsize = EC_MAXLRWDATA; /* can ignore DC overhead after first segment */ + } + segmentsize += diff; + while (segmentsize > segmentmaxsize && currentsegment < EC_MAXIOSEGMENTS) + { + context->grouplist[group].IOsegment[currentsegment++] = segmentmaxsize; + segmentsize -= segmentmaxsize; + context->grouplist[group].inputsWKC++; + segmentmaxsize = EC_MAXLRWDATA; /* can ignore DC overhead after first segment */ + } + /* if this slave added input data and there is a partial segment still outstanding increment the inputwkc */ + if (segmentsize && diff) + context->grouplist[group].inputsWKC++; + } + + ecx_eeprom2pdi(context, slave); /* set Eeprom control to PDI */ + /* User may override automatic state change */ + if (context->manualstatechange == 0) + { + /* request safe_op for slave */ + ecx_FPWRw(context->port, + configadr, + ECT_REG_ALCTL, + htoes(EC_STATE_SAFE_OP), + EC_TIMEOUTRET3); /* set safeop status */ + } + if (context->slavelist[slave].blockLRW) + { + context->grouplist[group].blockLRW++; + } + context->grouplist[group].Ebuscurrent += context->slavelist[slave].Ebuscurrent; + } + } + if (BitPos) + { + LogAddr++; + oLogAddr = LogAddr; + BitPos = 0; + if ((segmentsize + 1) > segmentmaxsize && currentsegment < EC_MAXIOSEGMENTS) + { + context->grouplist[group].IOsegment[currentsegment++] = segmentsize; + segmentsize = 0; + context->grouplist[group].inputsWKC++; + } + segmentsize += 1; + } + context->grouplist[group].IOsegment[currentsegment] = segmentsize; + context->grouplist[group].nsegments = currentsegment + 1; + context->grouplist[group].inputs = (uint8 *)(pIOmap) + context->grouplist[group].Obytes; + context->grouplist[group].Ibytes = LogAddr - + context->grouplist[group].logstartaddr - + context->grouplist[group].Obytes; + if (!group) + { + context->slavelist[0].inputs = (uint8 *)(pIOmap) + context->slavelist[0].Obytes; + context->slavelist[0].Ibytes = LogAddr - + context->grouplist[group].logstartaddr - + context->slavelist[0].Obytes; /* store input bytes in master record */ + } + + EC_PRINT("IOmapSize %d\n", LogAddr - context->grouplist[group].logstartaddr); + + return (LogAddr - context->grouplist[group].logstartaddr); + } + + return 0; +} + +/** Map all PDOs in one group of slaves to IOmap with Outputs/Inputs +* in sequential order (legacy SOEM way). +* + * + * @param[in] context = context struct + * @param[out] pIOmap = pointer to IOmap + * @param[in] group = group to map, 0 = all groups + * @return IOmap size + */ +int ecx_config_map_group(ecx_contextt *context, void *pIOmap, uint8 group) +{ + return ecx_main_config_map_group(context, pIOmap, group, FALSE); +} + +/** Map all PDOs in one group of slaves to IOmap with Outputs/Inputs +* in sequential order (legacy SOEM way) and force byte alignement. +* + * + * @param[in] context = context struct + * @param[out] pIOmap = pointer to IOmap + * @param[in] group = group to map, 0 = all groups + * @return IOmap size + */ +int ecx_config_map_group_aligned(ecx_contextt *context, void *pIOmap, uint8 group) +{ + return ecx_main_config_map_group(context, pIOmap, group, TRUE); +} + +/** Map all PDOs in one group of slaves to IOmap with Outputs/Inputs + * overlapping. NOTE: Must use this for TI ESC when using LRW. + * + * @param[in] context = context struct + * @param[out] pIOmap = pointer to IOmap + * @param[in] group = group to map, 0 = all groups + * @return IOmap size + */ +int ecx_config_overlap_map_group(ecx_contextt *context, void *pIOmap, uint8 group) +{ + uint16 slave, configadr; + uint8 BitPos; + uint32 mLogAddr = 0; + uint32 siLogAddr = 0; + uint32 soLogAddr = 0; + uint32 tempLogAddr; + uint32 diff; + uint16 currentsegment = 0; + uint32 segmentsize = 0; + uint32 segmentmaxsize = (EC_MAXLRWDATA - EC_FIRSTDCDATAGRAM); + + if ((*(context->slavecount) > 0) && (group < context->maxgroup)) + { + EC_PRINT("ec_config_map_group IOmap:%p group:%d\n", pIOmap, group); + mLogAddr = context->grouplist[group].logstartaddr; + siLogAddr = mLogAddr; + soLogAddr = mLogAddr; + BitPos = 0; + context->grouplist[group].nsegments = 0; + context->grouplist[group].outputsWKC = 0; + context->grouplist[group].inputsWKC = 0; + + /* Find mappings and program syncmanagers */ + ecx_config_find_mappings(context, group); + + /* do IO mapping of slave and program FMMUs */ + for (slave = 1; slave <= *(context->slavecount); slave++) + { + configadr = context->slavelist[slave].configadr; + siLogAddr = soLogAddr = mLogAddr; + + if (!group || (group == context->slavelist[slave].group)) + { + /* create output mapping */ + if (context->slavelist[slave].Obits) + { + + ecx_config_create_output_mappings(context, pIOmap, group, + slave, &soLogAddr, &BitPos); + if (BitPos) + { + soLogAddr++; + BitPos = 0; + } + } + + /* create input mapping */ + if (context->slavelist[slave].Ibits) + { + ecx_config_create_input_mappings(context, pIOmap, group, + slave, &siLogAddr, &BitPos); + if (BitPos) + { + siLogAddr++; + BitPos = 0; + } + } + + tempLogAddr = (siLogAddr > soLogAddr) ? siLogAddr : soLogAddr; + diff = tempLogAddr - mLogAddr; + int soLength = soLogAddr - mLogAddr; + int siLength = siLogAddr - mLogAddr; + mLogAddr = tempLogAddr; + if ((segmentsize + diff) > segmentmaxsize && diff <= segmentmaxsize && currentsegment < EC_MAXIOSEGMENTS) + { + context->grouplist[group].IOsegment[currentsegment++] = segmentsize; + segmentsize = 0; + segmentmaxsize = EC_MAXLRWDATA; /* can ignore DC overhead after first segment */ + } + segmentsize += diff; + while (segmentsize > segmentmaxsize && currentsegment < EC_MAXIOSEGMENTS) + { + context->grouplist[group].IOsegment[currentsegment++] = segmentmaxsize; + segmentsize -= segmentmaxsize; + context->grouplist[group].inputsWKC += (siLength > 0); + context->grouplist[group].outputsWKC += (soLength > 0); + siLength -= segmentmaxsize; + soLength -= segmentmaxsize; + segmentmaxsize = EC_MAXLRWDATA; /* can ignore DC overhead after first segment */ + } + /* if this slave added data and there is a partial segment still outstanding increment the relevant wkc */ + if (segmentsize && diff) + { + context->grouplist[group].inputsWKC += (siLength > 0); + context->grouplist[group].outputsWKC += (soLength > 0); + } + + ecx_eeprom2pdi(context, slave); /* set Eeprom control to PDI */ + /* User may override automatic state change */ + if (context->manualstatechange == 0) + { + /* request safe_op for slave */ + ecx_FPWRw(context->port, + configadr, + ECT_REG_ALCTL, + htoes(EC_STATE_SAFE_OP), + EC_TIMEOUTRET3); + } + if (context->slavelist[slave].blockLRW) + { + context->grouplist[group].blockLRW++; + } + context->grouplist[group].Ebuscurrent += context->slavelist[slave].Ebuscurrent; + + } + } + + context->grouplist[group].IOsegment[currentsegment] = segmentsize; + context->grouplist[group].nsegments = currentsegment + 1; + context->grouplist[group].Isegment = 0; + context->grouplist[group].Ioffset = 0; + + context->grouplist[group].Obytes = soLogAddr - context->grouplist[group].logstartaddr; + context->grouplist[group].Ibytes = siLogAddr - context->grouplist[group].logstartaddr; + context->grouplist[group].outputs = pIOmap; + context->grouplist[group].inputs = (uint8 *)pIOmap + context->grouplist[group].Obytes; + + /* Move calculated inputs with OBytes offset*/ + for (slave = 1; slave <= *(context->slavecount); slave++) + { + if (!group || (group == context->slavelist[slave].group)) + { + if(context->slavelist[slave].Ibits > 0) + { + context->slavelist[slave].inputs += context->grouplist[group].Obytes; + } + } + } + + if (!group) + { + /* store output bytes in master record */ + context->slavelist[0].outputs = pIOmap; + context->slavelist[0].Obytes = soLogAddr - context->grouplist[group].logstartaddr; + context->slavelist[0].inputs = (uint8 *)pIOmap + context->slavelist[0].Obytes; + context->slavelist[0].Ibytes = siLogAddr - context->grouplist[group].logstartaddr; + } + + EC_PRINT("IOmapSize %d\n", context->grouplist[group].Obytes + context->grouplist[group].Ibytes); + + return (context->grouplist[group].Obytes + context->grouplist[group].Ibytes); + } + + return 0; +} + + +/** Recover slave. + * + * @param[in] context = context struct + * @param[in] slave = slave to recover + * @param[in] timeout = local timeout f.e. EC_TIMEOUTRET3 + * @return >0 if successful + */ +int ecx_recover_slave(ecx_contextt *context, uint16 slave, int timeout) +{ + int rval; + int wkc; + uint16 ADPh, configadr, readadr; + + rval = 0; + configadr = context->slavelist[slave].configadr; + ADPh = (uint16)(1 - slave); + /* check if we found another slave than the requested */ + readadr = 0xfffe; + wkc = ecx_APRD(context->port, ADPh, ECT_REG_STADR, sizeof(readadr), &readadr, timeout); + /* correct slave found, finished */ + if(readadr == configadr) + { + return 1; + } + /* only try if no config address*/ + if( (wkc > 0) && (readadr == 0)) + { + /* clear possible slaves at EC_TEMPNODE */ + ecx_FPWRw(context->port, EC_TEMPNODE, ECT_REG_STADR, htoes(0) , 0); + /* set temporary node address of slave */ + if(ecx_APWRw(context->port, ADPh, ECT_REG_STADR, htoes(EC_TEMPNODE) , timeout) <= 0) + { + ecx_FPWRw(context->port, EC_TEMPNODE, ECT_REG_STADR, htoes(0) , 0); + return 0; /* slave fails to respond */ + } + + context->slavelist[slave].configadr = EC_TEMPNODE; /* temporary config address */ + ecx_eeprom2master(context, slave); /* set Eeprom control to master */ + + /* check if slave is the same as configured before */ + if ((ecx_FPRDw(context->port, EC_TEMPNODE, ECT_REG_ALIAS, timeout) == + htoes(context->slavelist[slave].aliasadr)) && + (ecx_readeeprom(context, slave, ECT_SII_ID, EC_TIMEOUTEEP) == + htoel(context->slavelist[slave].eep_id)) && + (ecx_readeeprom(context, slave, ECT_SII_MANUF, EC_TIMEOUTEEP) == + htoel(context->slavelist[slave].eep_man)) && + (ecx_readeeprom(context, slave, ECT_SII_REV, EC_TIMEOUTEEP) == + htoel(context->slavelist[slave].eep_rev))) + { + rval = ecx_FPWRw(context->port, EC_TEMPNODE, ECT_REG_STADR, htoes(configadr) , timeout); + context->slavelist[slave].configadr = configadr; + } + else + { + /* slave is not the expected one, remove config address*/ + ecx_FPWRw(context->port, EC_TEMPNODE, ECT_REG_STADR, htoes(0) , timeout); + context->slavelist[slave].configadr = configadr; + } + } + + return rval; +} + +/** Reconfigure slave. + * + * @param[in] context = context struct + * @param[in] slave = slave to reconfigure + * @param[in] timeout = local timeout f.e. EC_TIMEOUTRET3 + * @return Slave state + */ +int ecx_reconfig_slave(ecx_contextt *context, uint16 slave, int timeout) +{ + int state, nSM, FMMUc; + uint16 configadr; + + configadr = context->slavelist[slave].configadr; + if (ecx_FPWRw(context->port, configadr, ECT_REG_ALCTL, htoes(EC_STATE_INIT) , timeout) <= 0) + { + return 0; + } + state = 0; + ecx_eeprom2pdi(context, slave); /* set Eeprom control to PDI */ + /* check state change init */ + state = ecx_statecheck(context, slave, EC_STATE_INIT, EC_TIMEOUTSTATE); + if(state == EC_STATE_INIT) + { + /* program all enabled SM */ + for( nSM = 0 ; nSM < EC_MAXSM ; nSM++ ) + { + if (context->slavelist[slave].SM[nSM].StartAddr) + { + ecx_FPWR(context->port, configadr, (uint16)(ECT_REG_SM0 + (nSM * sizeof(ec_smt))), + sizeof(ec_smt), &context->slavelist[slave].SM[nSM], timeout); + } + } + ecx_FPWRw(context->port, configadr, ECT_REG_ALCTL, htoes(EC_STATE_PRE_OP) , timeout); + state = ecx_statecheck(context, slave, EC_STATE_PRE_OP, EC_TIMEOUTSTATE); /* check state change pre-op */ + if( state == EC_STATE_PRE_OP) + { + /* execute special slave configuration hook Pre-Op to Safe-OP */ + if(context->slavelist[slave].PO2SOconfig) /* only if registered */ + { + context->slavelist[slave].PO2SOconfig(slave, context->slavelist[slave].user); + } + if (context->slavelist[slave].PO2SOconfigx) /* only if registered */ + { + context->slavelist[slave].PO2SOconfigx(context, slave); + } + ecx_FPWRw(context->port, configadr, ECT_REG_ALCTL, htoes(EC_STATE_SAFE_OP) , timeout); /* set safeop status */ + state = ecx_statecheck(context, slave, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE); /* check state change safe-op */ + /* program configured FMMU */ + for( FMMUc = 0 ; FMMUc < context->slavelist[slave].FMMUunused ; FMMUc++ ) + { + ecx_FPWR(context->port, configadr, (uint16)(ECT_REG_FMMU0 + (sizeof(ec_fmmut) * FMMUc)), + sizeof(ec_fmmut), &context->slavelist[slave].FMMU[FMMUc], timeout); + } + } + } + + return state; +} + +#ifdef EC_VER1 +/** Enumerate and init all slaves. + * + * @param[in] usetable = TRUE when using configtable to init slaves, FALSE otherwise + * @return Workcounter of slave discover datagram = number of slaves found + * @see ecx_config_init + */ +int ec_config_init(uint8 usetable) +{ + return ecx_config_init(&ecx_context, usetable); +} + +/** Map all PDOs in one group of slaves to IOmap with Outputs/Inputs + * in sequential order (legacy SOEM way). + * + * @param[out] pIOmap = pointer to IOmap + * @param[in] group = group to map, 0 = all groups + * @return IOmap size + * @see ecx_config_map_group + */ +int ec_config_map_group(void *pIOmap, uint8 group) +{ + return ecx_config_map_group(&ecx_context, pIOmap, group); +} + +/** Map all PDOs in one group of slaves to IOmap with Outputs/Inputs +* overlapping. NOTE: Must use this for TI ESC when using LRW. +* +* @param[out] pIOmap = pointer to IOmap +* @param[in] group = group to map, 0 = all groups +* @return IOmap size +* @see ecx_config_overlap_map_group +*/ +int ec_config_overlap_map_group(void *pIOmap, uint8 group) +{ + return ecx_config_overlap_map_group(&ecx_context, pIOmap, group); +} + +/** Map all PDOs in one group of slaves to IOmap with Outputs/Inputs + * in sequential order (legacy SOEM way) and force byte alignment. + * + * @param[out] pIOmap = pointer to IOmap + * @param[in] group = group to map, 0 = all groups + * @return IOmap size + * @see ecx_config_map_group + */ +int ec_config_map_group_aligned(void *pIOmap, uint8 group) +{ + return ecx_config_map_group_aligned(&ecx_context, pIOmap, group); +} + +/** Map all PDOs from slaves to IOmap with Outputs/Inputs + * in sequential order (legacy SOEM way). + * + * @param[out] pIOmap = pointer to IOmap + * @return IOmap size + */ +int ec_config_map(void *pIOmap) +{ + return ec_config_map_group(pIOmap, 0); +} + +/** Map all PDOs from slaves to IOmap with Outputs/Inputs +* overlapping. NOTE: Must use this for TI ESC when using LRW. +* +* @param[out] pIOmap = pointer to IOmap +* @return IOmap size +*/ +int ec_config_overlap_map(void *pIOmap) +{ + return ec_config_overlap_map_group(pIOmap, 0); +} + +/** Map all PDOs from slaves to IOmap with Outputs/Inputs + * in sequential order (legacy SOEM way) and force byte alignment. + * + * @param[out] pIOmap = pointer to IOmap + * @return IOmap size + */ +int ec_config_map_aligned(void *pIOmap) +{ + return ec_config_map_group_aligned(pIOmap, 0); +} + +/** Enumerate / map and init all slaves. + * + * @param[in] usetable = TRUE when using configtable to init slaves, FALSE otherwise + * @param[out] pIOmap = pointer to IOmap + * @return Workcounter of slave discover datagram = number of slaves found + */ +int ec_config(uint8 usetable, void *pIOmap) +{ + int wkc; + wkc = ec_config_init(usetable); + if (wkc) + { + ec_config_map(pIOmap); + } + return wkc; +} + +/** Enumerate / map and init all slaves. +* +* @param[in] usetable = TRUE when using configtable to init slaves, FALSE otherwise +* @param[out] pIOmap = pointer to IOmap +* @return Workcounter of slave discover datagram = number of slaves found +*/ +int ec_config_overlap(uint8 usetable, void *pIOmap) +{ + int wkc; + wkc = ec_config_init(usetable); + if (wkc) + { + ec_config_overlap_map(pIOmap); + } + return wkc; +} + +/** Recover slave. + * + * @param[in] slave = slave to recover + * @param[in] timeout = local timeout f.e. EC_TIMEOUTRET3 + * @return >0 if successful + * @see ecx_recover_slave + */ +int ec_recover_slave(uint16 slave, int timeout) +{ + return ecx_recover_slave(&ecx_context, slave, timeout); +} + +/** Reconfigure slave. + * + * @param[in] slave = slave to reconfigure + * @param[in] timeout = local timeout f.e. EC_TIMEOUTRET3 + * @return Slave state + * @see ecx_reconfig_slave + */ +int ec_reconfig_slave(uint16 slave, int timeout) +{ + return ecx_reconfig_slave(&ecx_context, slave, timeout); +} +#endif diff --git a/soem-src/soem/ethercatconfig.h b/soem-src/soem/ethercatconfig.h new file mode 100644 index 0000000..fbe544c --- /dev/null +++ b/soem-src/soem/ethercatconfig.h @@ -0,0 +1,47 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +/** \file + * \brief + * Headerfile for ethercatconfig.c + */ + +#ifndef _ethercatconfig_ +#define _ethercatconfig_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#define EC_NODEOFFSET 0x1000 +#define EC_TEMPNODE 0xffff + +#ifdef EC_VER1 +int ec_config_init(uint8 usetable); +int ec_config_map(void *pIOmap); +int ec_config_overlap_map(void *pIOmap); +int ec_config_map_aligned(void *pIOmap); +int ec_config_map_group(void *pIOmap, uint8 group); +int ec_config_overlap_map_group(void *pIOmap, uint8 group); +int ec_config_map_group_aligned(void *pIOmap, uint8 group); +int ec_config(uint8 usetable, void *pIOmap); +int ec_config_overlap(uint8 usetable, void *pIOmap); +int ec_recover_slave(uint16 slave, int timeout); +int ec_reconfig_slave(uint16 slave, int timeout); +#endif + +int ecx_config_init(ecx_contextt *context, uint8 usetable); +int ecx_config_map_group(ecx_contextt *context, void *pIOmap, uint8 group); +int ecx_config_overlap_map_group(ecx_contextt *context, void *pIOmap, uint8 group); +int ecx_config_map_group_aligned(ecx_contextt *context, void *pIOmap, uint8 group); +int ecx_recover_slave(ecx_contextt *context, uint16 slave, int timeout); +int ecx_reconfig_slave(ecx_contextt *context, uint16 slave, int timeout); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/soem-src/soem/ethercatconfiglist.h b/soem-src/soem/ethercatconfiglist.h new file mode 100644 index 0000000..75fe538 --- /dev/null +++ b/soem-src/soem/ethercatconfiglist.h @@ -0,0 +1,66 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +/** \file + * \brief + * DEPRECATED Configuration list of known EtherCAT slave devices. + * + * If a slave is found in this list it is configured according to the parameters + * in the list. Otherwise the configuration info is read directly from the slave + * EEPROM (SII or Slave Information Interface). + */ + +#ifndef _ethercatconfiglist_ +#define _ethercatconfiglist_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +/* + explanation of dev: + 1: static device with no IO mapping ie EK1100 + 2: input device no mailbox ie simple IO device + 3: output device no mailbox + 4: input device with mailbox configuration + 5: output device with mailbox configuration + 6: input/output device no mailbox + 7: input.output device with mailbox configuration +*/ +#define EC_CONFIGEND 0xffffffff + +ec_configlist_t ec_configlist[] = { + {/*Man=*/0x00000000,/*ID=*/0x00000000,/*Name=*/"" ,/*dtype=*/0,/*Ibits=*/ 0,/*Obits=*/ 0,/*SM2a*/ 0,/*SM2f*/ 0,/*SM3a*/ 0,/*SM3f*/ 0,/*FM0ac*/0,/*FM1ac*/0}, + {/*Man=*/0x00000002,/*ID=*/0x044c2c52,/*Name=*/"EK1100" ,/*dtype=*/1,/*Ibits=*/ 0,/*Obits=*/ 0,/*SM2a*/ 0,/*SM2f*/ 0,/*SM3a*/ 0,/*SM3f*/ 0,/*FM0ac*/0,/*FM1ac*/0}, + {/*Man=*/0x00000002,/*ID=*/0x03ea3052,/*Name=*/"EL1002" ,/*dtype=*/2,/*Ibits=*/ 2,/*Obits=*/ 0,/*SM2a*/ 0,/*SM2f*/ 0,/*SM3a*/ 0,/*SM3f*/ 0,/*FM0ac*/0,/*FM1ac*/0}, + {/*Man=*/0x00000002,/*ID=*/0x03ec3052,/*Name=*/"EL1004" ,/*dtype=*/2,/*Ibits=*/ 4,/*Obits=*/ 0,/*SM2a*/ 0,/*SM2f*/ 0,/*SM3a*/ 0,/*SM3f*/ 0,/*FM0ac*/0,/*FM1ac*/0}, + {/*Man=*/0x00000002,/*ID=*/0x03f43052,/*Name=*/"EL1012" ,/*dtype=*/2,/*Ibits=*/ 2,/*Obits=*/ 0,/*SM2a*/ 0,/*SM2f*/ 0,/*SM3a*/ 0,/*SM3f*/ 0,/*FM0ac*/0,/*FM1ac*/0}, + {/*Man=*/0x00000002,/*ID=*/0x03f63052,/*Name=*/"EL1014" ,/*dtype=*/2,/*Ibits=*/ 4,/*Obits=*/ 0,/*SM2a*/ 0,/*SM2f*/ 0,/*SM3a*/ 0,/*SM3f*/ 0,/*FM0ac*/0,/*FM1ac*/0}, + {/*Man=*/0x00000002,/*ID=*/0x03fa3052,/*Name=*/"EL1018" ,/*dtype=*/2,/*Ibits=*/ 8,/*Obits=*/ 0,/*SM2a*/ 0,/*SM2f*/ 0,/*SM3a*/ 0,/*SM3f*/ 0,/*FM0ac*/0,/*FM1ac*/0}, + {/*Man=*/0x00000002,/*ID=*/0x07d23052,/*Name=*/"EL2002" ,/*dtype=*/3,/*Ibits=*/ 0,/*Obits=*/ 2,/*SM2a*/ 0,/*SM2f*/ 0,/*SM3a*/ 0,/*SM3f*/ 0,/*FM0ac*/0,/*FM1ac*/0}, + {/*Man=*/0x00000002,/*ID=*/0x07d43052,/*Name=*/"EL2004" ,/*dtype=*/3,/*Ibits=*/ 0,/*Obits=*/ 4,/*SM2a*/ 0,/*SM2f*/ 0,/*SM3a*/ 0,/*SM3f*/ 0,/*FM0ac*/0,/*FM1ac*/0}, + {/*Man=*/0x00000002,/*ID=*/0x07d83052,/*Name=*/"EL2008" ,/*dtype=*/3,/*Ibits=*/ 0,/*Obits=*/ 8,/*SM2a*/ 0,/*SM2f*/ 0,/*SM3a*/ 0,/*SM3f*/ 0,/*FM0ac*/0,/*FM1ac*/0}, + {/*Man=*/0x00000002,/*ID=*/0x07f03052,/*Name=*/"EL2032" ,/*dtype=*/6,/*Ibits=*/ 2,/*Obits=*/ 2,/*SM2a*/ 0,/*SM2f*/ 0,/*SM3a*/ 0,/*SM3f*/ 0,/*FM0ac*/0,/*FM1ac*/0}, + {/*Man=*/0x00000002,/*ID=*/0x0c1e3052,/*Name=*/"EL3102" ,/*dtype=*/4,/*Ibits=*/48,/*Obits=*/ 0,/*SM2a*/0x1000,/*SM2f*/0x00000024,/*SM3a*/0x1100,/*SM3f*/0x00010020,/*FM0ac*/0,/*FM1ac*/1}, + {/*Man=*/0x00000002,/*ID=*/0x0c283052,/*Name=*/"EL3112" ,/*dtype=*/4,/*Ibits=*/48,/*Obits=*/ 0,/*SM2a*/0x1000,/*SM2f*/0x00000024,/*SM3a*/0x1100,/*SM3f*/0x00010020,/*FM0ac*/0,/*FM1ac*/1}, + {/*Man=*/0x00000002,/*ID=*/0x0c323052,/*Name=*/"EL3122" ,/*dtype=*/4,/*Ibits=*/48,/*Obits=*/ 0,/*SM2a*/0x1000,/*SM2f*/0x00000024,/*SM3a*/0x1100,/*SM3f*/0x00010020,/*FM0ac*/0,/*FM1ac*/1}, + {/*Man=*/0x00000002,/*ID=*/0x0c463052,/*Name=*/"EL3142" ,/*dtype=*/4,/*Ibits=*/48,/*Obits=*/ 0,/*SM2a*/0x1000,/*SM2f*/0x00000024,/*SM3a*/0x1100,/*SM3f*/0x00010020,/*FM0ac*/0,/*FM1ac*/1}, + {/*Man=*/0x00000002,/*ID=*/0x0c503052,/*Name=*/"EL3152" ,/*dtype=*/4,/*Ibits=*/48,/*Obits=*/ 0,/*SM2a*/0x1000,/*SM2f*/0x00000024,/*SM3a*/0x1100,/*SM3f*/0x00010020,/*FM0ac*/0,/*FM1ac*/1}, + {/*Man=*/0x00000002,/*ID=*/0x0c5a3052,/*Name=*/"EL3162" ,/*dtype=*/4,/*Ibits=*/48,/*Obits=*/ 0,/*SM2a*/0x1000,/*SM2f*/0x00000024,/*SM3a*/0x1100,/*SM3f*/0x00010020,/*FM0ac*/0,/*FM1ac*/1}, + {/*Man=*/0x00000002,/*ID=*/0x0fc03052,/*Name=*/"EL4032" ,/*dtype=*/5,/*Ibits=*/ 0,/*Obits=*/32,/*SM2a*/0x1100,/*SM2f*/0x00010024,/*SM3a*/0x1180,/*SM3f*/0x00000022,/*FM0ac*/1,/*FM1ac*/0}, + {/*Man=*/0x00000002,/*ID=*/0x10063052,/*Name=*/"EL4102" ,/*dtype=*/5,/*Ibits=*/ 0,/*Obits=*/32,/*SM2a*/0x1000,/*SM2f*/0x00010024,/*SM3a*/0x1100,/*SM3f*/0x00000022,/*FM0ac*/1,/*FM1ac*/0}, + {/*Man=*/0x00000002,/*ID=*/0x10103052,/*Name=*/"EL4112" ,/*dtype=*/5,/*Ibits=*/ 0,/*Obits=*/32,/*SM2a*/0x1000,/*SM2f*/0x00010024,/*SM3a*/0x1100,/*SM3f*/0x00000022,/*FM0ac*/1,/*FM1ac*/0}, + {/*Man=*/0x00000002,/*ID=*/0x101a3052,/*Name=*/"EL4122" ,/*dtype=*/5,/*Ibits=*/ 0,/*Obits=*/32,/*SM2a*/0x1000,/*SM2f*/0x00010024,/*SM3a*/0x1100,/*SM3f*/0x00000022,/*FM0ac*/1,/*FM1ac*/0}, + {/*Man=*/0x00000002,/*ID=*/0x10243052,/*Name=*/"EL4132" ,/*dtype=*/5,/*Ibits=*/ 0,/*Obits=*/32,/*SM2a*/0x1000,/*SM2f*/0x00010024,/*SM3a*/0x1100,/*SM3f*/0x00000022,/*FM0ac*/1,/*FM1ac*/0}, + {/*Man=*/0x00000002,/*ID=*/0x13ed3052,/*Name=*/"EL5101" ,/*dtype=*/7,/*Ibits=*/40,/*Obits=*/24,/*SM2a*/0x1000,/*SM2f*/0x00010024,/*SM3a*/0x1100,/*SM3f*/0x00010020,/*FM0ac*/1,/*FM1ac*/1}, + {/*Man=*/EC_CONFIGEND,/*ID=*/0x00000000,/*Name=*/"" ,/*dtype=*/0,/*Ibits=*/ 0,/*Obits=*/ 0,/*SM2a*/ 0,/*SM2f*/ 0,/*SM3a*/ 0,/*SM3f*/ 0,/*FM0ac*/0,/*FM1ac*/0} +}; + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/soem-src/soem/ethercatdc.c b/soem-src/soem/ethercatdc.c new file mode 100644 index 0000000..b6220b1 --- /dev/null +++ b/soem-src/soem/ethercatdc.c @@ -0,0 +1,448 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +/** \file + * \brief + * Distributed Clock EtherCAT functions. + * + */ +#include "oshw.h" +#include "osal.h" +#include "ethercattype.h" +#include "ethercatbase.h" +#include "ethercatmain.h" +#include "ethercatdc.h" + +#define PORTM0 0x01 +#define PORTM1 0x02 +#define PORTM2 0x04 +#define PORTM3 0x08 + +/** 1st sync pulse delay in ns here 100ms */ +#define SyncDelay ((int32)100000000) + +/** + * Set DC of slave to fire sync0 at CyclTime interval with CyclShift offset. + * + * @param[in] context = context struct + * @param [in] slave Slave number. + * @param [in] act TRUE = active, FALSE = deactivated + * @param [in] CyclTime Cycltime in ns. + * @param [in] CyclShift CyclShift in ns. + */ +void ecx_dcsync0(ecx_contextt *context, uint16 slave, boolean act, uint32 CyclTime, int32 CyclShift) +{ + uint8 h, RA; + uint16 slaveh; + int64 t, t1; + int32 tc; + + slaveh = context->slavelist[slave].configadr; + RA = 0; + + /* stop cyclic operation, ready for next trigger */ + (void)ecx_FPWR(context->port, slaveh, ECT_REG_DCSYNCACT, sizeof(RA), &RA, EC_TIMEOUTRET); + if (act) + { + RA = 1 + 2; /* act cyclic operation and sync0, sync1 deactivated */ + } + h = 0; + (void)ecx_FPWR(context->port, slaveh, ECT_REG_DCCUC, sizeof(h), &h, EC_TIMEOUTRET); /* write access to ethercat */ + t1 = 0; + (void)ecx_FPRD(context->port, slaveh, ECT_REG_DCSYSTIME, sizeof(t1), &t1, EC_TIMEOUTRET); /* read local time of slave */ + t1 = etohll(t1); + + /* Calculate first trigger time, always a whole multiple of CyclTime rounded up + plus the shifttime (can be negative) + This insures best synchronization between slaves, slaves with the same CyclTime + will sync at the same moment (you can use CyclShift to shift the sync) */ + if (CyclTime > 0) + { + t = ((t1 + SyncDelay) / CyclTime) * CyclTime + CyclTime + CyclShift; + } + else + { + t = t1 + SyncDelay + CyclShift; + /* first trigger at T1 + CyclTime + SyncDelay + CyclShift in ns */ + } + t = htoell(t); + (void)ecx_FPWR(context->port, slaveh, ECT_REG_DCSTART0, sizeof(t), &t, EC_TIMEOUTRET); /* SYNC0 start time */ + tc = htoel(CyclTime); + (void)ecx_FPWR(context->port, slaveh, ECT_REG_DCCYCLE0, sizeof(tc), &tc, EC_TIMEOUTRET); /* SYNC0 cycle time */ + (void)ecx_FPWR(context->port, slaveh, ECT_REG_DCSYNCACT, sizeof(RA), &RA, EC_TIMEOUTRET); /* activate cyclic operation */ + + // update ec_slave state + context->slavelist[slave].DCactive = (uint8)act; + context->slavelist[slave].DCshift = CyclShift; + context->slavelist[slave].DCcycle = CyclTime; +} + +/** + * Set DC of slave to fire sync0 and sync1 at CyclTime interval with CyclShift offset. + * + * @param[in] context = context struct + * @param [in] slave Slave number. + * @param [in] act TRUE = active, FALSE = deactivated + * @param [in] CyclTime0 Cycltime SYNC0 in ns. + * @param [in] CyclTime1 Cycltime SYNC1 in ns. This time is a delta time in relation to + the SYNC0 fire. If CylcTime1 = 0 then SYNC1 fires a the same time + as SYNC0. + * @param [in] CyclShift CyclShift in ns. + */ +void ecx_dcsync01(ecx_contextt *context, uint16 slave, boolean act, uint32 CyclTime0, uint32 CyclTime1, int32 CyclShift) +{ + uint8 h, RA; + uint16 slaveh; + int64 t, t1; + int32 tc; + uint32 TrueCyclTime; + + /* Sync1 can be used as a multiple of Sync0, use true cycle time */ + TrueCyclTime = ((CyclTime1 / CyclTime0) + 1) * CyclTime0; + + slaveh = context->slavelist[slave].configadr; + RA = 0; + + /* stop cyclic operation, ready for next trigger */ + (void)ecx_FPWR(context->port, slaveh, ECT_REG_DCSYNCACT, sizeof(RA), &RA, EC_TIMEOUTRET); + if (act) + { + RA = 1 + 2 + 4; /* act cyclic operation and sync0 + sync1 */ + } + h = 0; + (void)ecx_FPWR(context->port, slaveh, ECT_REG_DCCUC, sizeof(h), &h, EC_TIMEOUTRET); /* write access to ethercat */ + t1 = 0; + (void)ecx_FPRD(context->port, slaveh, ECT_REG_DCSYSTIME, sizeof(t1), &t1, EC_TIMEOUTRET); /* read local time of slave */ + t1 = etohll(t1); + + /* Calculate first trigger time, always a whole multiple of TrueCyclTime rounded up + plus the shifttime (can be negative) + This insures best synchronization between slaves, slaves with the same CyclTime + will sync at the same moment (you can use CyclShift to shift the sync) */ + if (CyclTime0 > 0) + { + t = ((t1 + SyncDelay) / TrueCyclTime) * TrueCyclTime + TrueCyclTime + CyclShift; + } + else + { + t = t1 + SyncDelay + CyclShift; + /* first trigger at T1 + CyclTime + SyncDelay + CyclShift in ns */ + } + t = htoell(t); + (void)ecx_FPWR(context->port, slaveh, ECT_REG_DCSTART0, sizeof(t), &t, EC_TIMEOUTRET); /* SYNC0 start time */ + tc = htoel(CyclTime0); + (void)ecx_FPWR(context->port, slaveh, ECT_REG_DCCYCLE0, sizeof(tc), &tc, EC_TIMEOUTRET); /* SYNC0 cycle time */ + tc = htoel(CyclTime1); + (void)ecx_FPWR(context->port, slaveh, ECT_REG_DCCYCLE1, sizeof(tc), &tc, EC_TIMEOUTRET); /* SYNC1 cycle time */ + (void)ecx_FPWR(context->port, slaveh, ECT_REG_DCSYNCACT, sizeof(RA), &RA, EC_TIMEOUTRET); /* activate cyclic operation */ + + // update ec_slave state + context->slavelist[slave].DCactive = (uint8)act; + context->slavelist[slave].DCshift = CyclShift; + context->slavelist[slave].DCcycle = CyclTime0; +} + +/* latched port time of slave */ +static int32 ecx_porttime(ecx_contextt *context, uint16 slave, uint8 port) +{ + int32 ts; + switch (port) + { + case 0: + ts = context->slavelist[slave].DCrtA; + break; + case 1: + ts = context->slavelist[slave].DCrtB; + break; + case 2: + ts = context->slavelist[slave].DCrtC; + break; + case 3: + ts = context->slavelist[slave].DCrtD; + break; + default: + ts = 0; + break; + } + return ts; +} + +/* calculate previous active port of a slave */ +static uint8 ecx_prevport(ecx_contextt *context, uint16 slave, uint8 port) +{ + uint8 pport = port; + uint8 aport = context->slavelist[slave].activeports; + switch(port) + { + case 0: + if(aport & PORTM2) + pport = 2; + else if (aport & PORTM1) + pport = 1; + else if (aport & PORTM3) + pport = 3; + break; + case 1: + if(aport & PORTM3) + pport = 3; + else if (aport & PORTM0) + pport = 0; + else if (aport & PORTM2) + pport = 2; + break; + case 2: + if(aport & PORTM1) + pport = 1; + else if (aport & PORTM3) + pport = 3; + else if (aport & PORTM0) + pport = 0; + break; + case 3: + if(aport & PORTM0) + pport = 0; + else if (aport & PORTM2) + pport = 2; + else if (aport & PORTM1) + pport = 1; + break; + } + return pport; +} + +/* search unconsumed ports in parent, consume and return first open port */ +static uint8 ecx_parentport(ecx_contextt *context, uint16 parent) +{ + uint8 parentport = 0; + uint8 b; + /* search order is important, here 3 - 1 - 2 - 0 */ + b = context->slavelist[parent].consumedports; + if (b & PORTM3) + { + parentport = 3; + b &= (uint8)~PORTM3; + } + else if (b & PORTM1) + { + parentport = 1; + b &= (uint8)~PORTM1; + } + else if (b & PORTM2) + { + parentport = 2; + b &= (uint8)~PORTM2; + } + else if (b & PORTM0) + { + parentport = 0; + b &= (uint8)~PORTM0; + } + context->slavelist[parent].consumedports = b; + return parentport; +} + +/** + * Locate DC slaves, measure propagation delays. + * + * @param[in] context = context struct + * @return boolean if slaves are found with DC + */ +boolean ecx_configdc(ecx_contextt *context) +{ + uint16 i, slaveh, parent, child; + uint16 parenthold = 0; + uint16 prevDCslave = 0; + int32 ht, dt1, dt2, dt3; + int64 hrt; + uint8 entryport; + int8 nlist; + int8 plist[4]; + int32 tlist[4]; + ec_timet mastertime; + uint64 mastertime64; + + context->slavelist[0].hasdc = FALSE; + context->grouplist[0].hasdc = FALSE; + ht = 0; + + ecx_BWR(context->port, 0, ECT_REG_DCTIME0, sizeof(ht), &ht, EC_TIMEOUTRET); /* latch DCrecvTimeA of all slaves */ + mastertime = osal_current_time(); + mastertime.sec -= 946684800UL; /* EtherCAT uses 2000-01-01 as epoch start instead of 1970-01-01 */ + mastertime64 = (((uint64)mastertime.sec * 1000000) + (uint64)mastertime.usec) * 1000; + for (i = 1; i <= *(context->slavecount); i++) + { + context->slavelist[i].consumedports = context->slavelist[i].activeports; + if (context->slavelist[i].hasdc) + { + if (!context->slavelist[0].hasdc) + { + context->slavelist[0].hasdc = TRUE; + context->slavelist[0].DCnext = i; + context->slavelist[i].DCprevious = 0; + context->grouplist[context->slavelist[i].group].hasdc = TRUE; + context->grouplist[context->slavelist[i].group].DCnext = i; + } + else + { + context->slavelist[prevDCslave].DCnext = i; + context->slavelist[i].DCprevious = prevDCslave; + } + /* this branch has DC slave so remove parenthold */ + parenthold = 0; + prevDCslave = i; + slaveh = context->slavelist[i].configadr; + (void)ecx_FPRD(context->port, slaveh, ECT_REG_DCTIME0, sizeof(ht), &ht, EC_TIMEOUTRET); + context->slavelist[i].DCrtA = etohl(ht); + /* 64bit latched DCrecvTimeA of each specific slave */ + (void)ecx_FPRD(context->port, slaveh, ECT_REG_DCSOF, sizeof(hrt), &hrt, EC_TIMEOUTRET); + /* use it as offset in order to set local time around 0 + mastertime */ + hrt = htoell(-etohll(hrt) + mastertime64); + /* save it in the offset register */ + (void)ecx_FPWR(context->port, slaveh, ECT_REG_DCSYSOFFSET, sizeof(hrt), &hrt, EC_TIMEOUTRET); + (void)ecx_FPRD(context->port, slaveh, ECT_REG_DCTIME1, sizeof(ht), &ht, EC_TIMEOUTRET); + context->slavelist[i].DCrtB = etohl(ht); + (void)ecx_FPRD(context->port, slaveh, ECT_REG_DCTIME2, sizeof(ht), &ht, EC_TIMEOUTRET); + context->slavelist[i].DCrtC = etohl(ht); + (void)ecx_FPRD(context->port, slaveh, ECT_REG_DCTIME3, sizeof(ht), &ht, EC_TIMEOUTRET); + context->slavelist[i].DCrtD = etohl(ht); + + /* make list of active ports and their time stamps */ + nlist = 0; + if (context->slavelist[i].activeports & PORTM0) + { + plist[nlist] = 0; + tlist[nlist] = context->slavelist[i].DCrtA; + nlist++; + } + if (context->slavelist[i].activeports & PORTM3) + { + plist[nlist] = 3; + tlist[nlist] = context->slavelist[i].DCrtD; + nlist++; + } + if (context->slavelist[i].activeports & PORTM1) + { + plist[nlist] = 1; + tlist[nlist] = context->slavelist[i].DCrtB; + nlist++; + } + if (context->slavelist[i].activeports & PORTM2) + { + plist[nlist] = 2; + tlist[nlist] = context->slavelist[i].DCrtC; + nlist++; + } + /* entryport is port with the lowest timestamp */ + entryport = 0; + if((nlist > 1) && (tlist[1] < tlist[entryport])) + { + entryport = 1; + } + if((nlist > 2) && (tlist[2] < tlist[entryport])) + { + entryport = 2; + } + if((nlist > 3) && (tlist[3] < tlist[entryport])) + { + entryport = 3; + } + entryport = plist[entryport]; + context->slavelist[i].entryport = entryport; + /* consume entryport from activeports */ + context->slavelist[i].consumedports &= (uint8)~(1 << entryport); + + /* finding DC parent of current */ + parent = i; + do + { + child = parent; + parent = context->slavelist[parent].parent; + } + while (!((parent == 0) || (context->slavelist[parent].hasdc))); + /* only calculate propagation delay if slave is not the first */ + if (parent > 0) + { + /* find port on parent this slave is connected to */ + context->slavelist[i].parentport = ecx_parentport(context, parent); + if (context->slavelist[parent].topology == 1) + { + context->slavelist[i].parentport = context->slavelist[parent].entryport; + } + + dt1 = 0; + dt2 = 0; + /* delta time of (parentport - 1) - parentport */ + /* note: order of ports is 0 - 3 - 1 -2 */ + /* non active ports are skipped */ + dt3 = ecx_porttime(context, parent, context->slavelist[i].parentport) - + ecx_porttime(context, parent, + ecx_prevport(context, parent, context->slavelist[i].parentport)); + /* current slave has children */ + /* those children's delays need to be subtracted */ + if (context->slavelist[i].topology > 1) + { + dt1 = ecx_porttime(context, i, + ecx_prevport(context, i, context->slavelist[i].entryport)) - + ecx_porttime(context, i, context->slavelist[i].entryport); + } + /* we are only interested in positive difference */ + if (dt1 > dt3) dt1 = -dt1; + /* current slave is not the first child of parent */ + /* previous child's delays need to be added */ + if ((child - parent) > 1) + { + dt2 = ecx_porttime(context, parent, + ecx_prevport(context, parent, context->slavelist[i].parentport)) - + ecx_porttime(context, parent, context->slavelist[parent].entryport); + } + if (dt2 < 0) dt2 = -dt2; + + /* calculate current slave delay from delta times */ + /* assumption : forward delay equals return delay */ + context->slavelist[i].pdelay = ((dt3 - dt1) / 2) + dt2 + + context->slavelist[parent].pdelay; + ht = htoel(context->slavelist[i].pdelay); + /* write propagation delay*/ + (void)ecx_FPWR(context->port, slaveh, ECT_REG_DCSYSDELAY, sizeof(ht), &ht, EC_TIMEOUTRET); + } + } + else + { + context->slavelist[i].DCrtA = 0; + context->slavelist[i].DCrtB = 0; + context->slavelist[i].DCrtC = 0; + context->slavelist[i].DCrtD = 0; + parent = context->slavelist[i].parent; + /* if non DC slave found on first position on branch hold root parent */ + if ( (parent > 0) && (context->slavelist[parent].topology > 2)) + parenthold = parent; + /* if branch has no DC slaves consume port on root parent */ + if ( parenthold && (context->slavelist[i].topology == 1)) + { + ecx_parentport(context, parenthold); + parenthold = 0; + } + } + } + + return context->slavelist[0].hasdc; +} + +#ifdef EC_VER1 +void ec_dcsync0(uint16 slave, boolean act, uint32 CyclTime, int32 CyclShift) +{ + ecx_dcsync0(&ecx_context, slave, act, CyclTime, CyclShift); +} + +void ec_dcsync01(uint16 slave, boolean act, uint32 CyclTime0, uint32 CyclTime1, int32 CyclShift) +{ + ecx_dcsync01(&ecx_context, slave, act, CyclTime0, CyclTime1, CyclShift); +} + +boolean ec_configdc(void) +{ + return ecx_configdc(&ecx_context); +} +#endif diff --git a/soem-src/soem/ethercatdc.h b/soem-src/soem/ethercatdc.h new file mode 100644 index 0000000..b4ad2d1 --- /dev/null +++ b/soem-src/soem/ethercatdc.h @@ -0,0 +1,33 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +/** \file + * \brief + * Headerfile for ethercatdc.c + */ + +#ifndef _EC_ECATDC_H +#define _EC_ECATDC_H + +#ifdef __cplusplus +extern "C" +{ +#endif + +#ifdef EC_VER1 +boolean ec_configdc(); +void ec_dcsync0(uint16 slave, boolean act, uint32 CyclTime, int32 CyclShift); +void ec_dcsync01(uint16 slave, boolean act, uint32 CyclTime0, uint32 CyclTime1, int32 CyclShift); +#endif + +boolean ecx_configdc(ecx_contextt *context); +void ecx_dcsync0(ecx_contextt *context, uint16 slave, boolean act, uint32 CyclTime, int32 CyclShift); +void ecx_dcsync01(ecx_contextt *context, uint16 slave, boolean act, uint32 CyclTime0, uint32 CyclTime1, int32 CyclShift); + +#ifdef __cplusplus +} +#endif + +#endif /* _EC_ECATDC_H */ diff --git a/soem-src/soem/ethercateoe.c b/soem-src/soem/ethercateoe.c new file mode 100644 index 0000000..633987f --- /dev/null +++ b/soem-src/soem/ethercateoe.c @@ -0,0 +1,634 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +/** \file + * \brief + * Ethernet over EtherCAT (EoE) module. + * + * Set / Get IP functions + * Blocking send/receive Ethernet Frame + * Read incoming EoE fragment to Ethernet Frame + */ + +#include +#include +#include "osal.h" +#include "oshw.h" +#include "ethercat.h" + + /** EoE utility function to convert uint32 to eoe ip bytes. + * @param[in] ip = ip in uint32 + * @param[out] byte_ip = eoe ip 4th octet, 3ed octet, 2nd octet, 1st octet + */ +static void EOE_ip_uint32_to_byte(eoe_ip4_addr_t * ip, uint8_t * byte_ip) +{ + byte_ip[3] = eoe_ip4_addr1(ip); /* 1st octet */ + byte_ip[2] = eoe_ip4_addr2(ip); /* 2nd octet */ + byte_ip[1] = eoe_ip4_addr3(ip); /* 3ed octet */ + byte_ip[0] = eoe_ip4_addr4(ip); /* 4th octet */ +} + +/** EoE utility function to convert eoe ip bytes to uint32. +* @param[in] byte_ip = eoe ip 4th octet, 3ed octet, 2nd octet, 1st octet +* @param[out] ip = ip in uint32 +*/ +static void EOE_ip_byte_to_uint32(uint8_t * byte_ip, eoe_ip4_addr_t * ip) +{ + EOE_IP4_ADDR_TO_U32(ip, + byte_ip[3], /* 1st octet */ + byte_ip[2], /* 2nd octet */ + byte_ip[1], /* 3ed octet */ + byte_ip[0]); /* 4th octet */ +} + +/** EoE fragment data handler hook. Should not block. +* +* @param[in] context = context struct +* @param[in] hook = Pointer to hook function. +* @return 1 +*/ +int ecx_EOEdefinehook(ecx_contextt *context, void *hook) +{ + context->EOEhook = hook; + return 1; +} + +/** EoE EOE set IP, blocking. Waits for response from the slave. +* +* @param[in] context = Context struct +* @param[in] slave = Slave number +* @param[in] port = Port number on slave if applicable +* @param[in] ipparam = IP parameter data to be sent +* @param[in] timeout = Timeout in us, standard is EC_TIMEOUTRXM +* @return Workcounter from last slave response or returned result code +*/ +int ecx_EOEsetIp(ecx_contextt *context, uint16 slave, uint8 port, eoe_param_t * ipparam, int timeout) +{ + ec_EOEt *EOEp, *aEOEp; + ec_mbxbuft MbxIn, MbxOut; + uint16 frameinfo1, result; + uint8 cnt, data_offset; + uint8 flags = 0; + int wkc; + + ec_clearmbx(&MbxIn); + /* Empty slave out mailbox if something is in. Timout set to 0 */ + wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, 0); + ec_clearmbx(&MbxOut); + aEOEp = (ec_EOEt *)&MbxIn; + EOEp = (ec_EOEt *)&MbxOut; + EOEp->mbxheader.address = htoes(0x0000); + EOEp->mbxheader.priority = 0x00; + data_offset = EOE_PARAM_OFFSET; + + /* get new mailbox count value, used as session handle */ + cnt = ec_nextmbxcnt(context->slavelist[slave].mbx_cnt); + context->slavelist[slave].mbx_cnt = cnt; + + EOEp->mbxheader.mbxtype = ECT_MBXT_EOE + MBX_HDR_SET_CNT(cnt); /* EoE */ + + EOEp->frameinfo1 = htoes(EOE_HDR_FRAME_TYPE_SET(EOE_INIT_REQ) | + EOE_HDR_FRAME_PORT_SET(port) | + EOE_HDR_LAST_FRAGMENT); + EOEp->frameinfo2 = 0; + + if (ipparam->mac_set) + { + flags |= EOE_PARAM_MAC_INCLUDE; + memcpy(&EOEp->data[data_offset], ipparam->mac.addr, EOE_ETHADDR_LENGTH); + data_offset += EOE_ETHADDR_LENGTH; + } + if (ipparam->ip_set) + { + flags |= EOE_PARAM_IP_INCLUDE; + EOE_ip_uint32_to_byte(&ipparam->ip, &EOEp->data[data_offset]); + data_offset += EOE_IP4_LENGTH; + } + if (ipparam->subnet_set) + { + flags |= EOE_PARAM_SUBNET_IP_INCLUDE; + EOE_ip_uint32_to_byte(&ipparam->subnet, &EOEp->data[data_offset]); + data_offset += EOE_IP4_LENGTH; + } + if (ipparam->default_gateway_set) + { + flags |= EOE_PARAM_DEFAULT_GATEWAY_INCLUDE; + EOE_ip_uint32_to_byte(&ipparam->default_gateway, &EOEp->data[data_offset]); + data_offset += EOE_IP4_LENGTH; + } + if (ipparam->dns_ip_set) + { + flags |= EOE_PARAM_DNS_IP_INCLUDE; + EOE_ip_uint32_to_byte(&ipparam->dns_ip, &EOEp->data[data_offset]); + data_offset += EOE_IP4_LENGTH; + } + if (ipparam->dns_name_set) + { + /* TwinCAT include EOE_DNS_NAME_LENGTH chars even if name is shorter */ + flags |= EOE_PARAM_DNS_NAME_INCLUDE; + memcpy(&EOEp->data[data_offset], (void *)ipparam->dns_name, EOE_DNS_NAME_LENGTH); + data_offset += EOE_DNS_NAME_LENGTH; + } + + EOEp->mbxheader.length = htoes(EOE_PARAM_OFFSET + data_offset); + EOEp->data[0] = flags; + + /* send EoE request to slave */ + wkc = ecx_mbxsend(context, slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM); + + if (wkc > 0) /* succeeded to place mailbox in slave ? */ + { + /* clean mailboxbuffer */ + ec_clearmbx(&MbxIn); + /* read slave response */ + wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, timeout); + if (wkc > 0) /* succeeded to read slave response ? */ + { + /* slave response should be FoE */ + if ((aEOEp->mbxheader.mbxtype & 0x0f) == ECT_MBXT_EOE) + { + frameinfo1 = etohs(aEOEp->frameinfo1); + result = etohs(aEOEp->result); + if ((EOE_HDR_FRAME_TYPE_GET(frameinfo1) != EOE_INIT_RESP) || + (result != EOE_RESULT_SUCCESS)) + { + wkc = -result; + } + } + else + { + /* unexpected mailbox received */ + wkc = -EC_ERR_TYPE_PACKET_ERROR; + } + } + } + return wkc; +} + +/** EoE EOE get IP, blocking. Waits for response from the slave. +* +* @param[in] context = Context struct +* @param[in] slave = Slave number +* @param[in] port = Port number on slave if applicable +* @param[out] ipparam = IP parameter data retrived from slave +* @param[in] timeout = Timeout in us, standard is EC_TIMEOUTRXM +* @return Workcounter from last slave response or returned result code +*/ +int ecx_EOEgetIp(ecx_contextt *context, uint16 slave, uint8 port, eoe_param_t * ipparam, int timeout) +{ + ec_EOEt *EOEp, *aEOEp; + ec_mbxbuft MbxIn, MbxOut; + uint16 frameinfo1, eoedatasize; + uint8 cnt, data_offset; + uint8 flags = 0; + int wkc; + + /* Empty slave out mailbox if something is in. Timout set to 0 */ + wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, 0); + ec_clearmbx(&MbxOut); + aEOEp = (ec_EOEt *)&MbxIn; + EOEp = (ec_EOEt *)&MbxOut; + EOEp->mbxheader.address = htoes(0x0000); + EOEp->mbxheader.priority = 0x00; + data_offset = EOE_PARAM_OFFSET; + + /* get new mailbox count value, used as session handle */ + cnt = ec_nextmbxcnt(context->slavelist[slave].mbx_cnt); + context->slavelist[slave].mbx_cnt = cnt; + + EOEp->mbxheader.mbxtype = ECT_MBXT_EOE + MBX_HDR_SET_CNT(cnt); /* EoE */ + + EOEp->frameinfo1 = htoes(EOE_HDR_FRAME_TYPE_SET(EOE_GET_IP_PARAM_REQ) | + EOE_HDR_FRAME_PORT_SET(port) | + EOE_HDR_LAST_FRAGMENT); + EOEp->frameinfo2 = 0; + + EOEp->mbxheader.length = htoes(0x0004); + EOEp->data[0] = flags; + + /* send EoE request to slave */ + wkc = ecx_mbxsend(context, slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM); + if (wkc > 0) /* succeeded to place mailbox in slave ? */ + { + /* clean mailboxbuffer */ + ec_clearmbx(&MbxIn); + /* read slave response */ + wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, timeout); + if (wkc > 0) /* succeeded to read slave response ? */ + { + /* slave response should be FoE */ + if ((aEOEp->mbxheader.mbxtype & 0x0f) == ECT_MBXT_EOE) + { + frameinfo1 = etohs(aEOEp->frameinfo1); + eoedatasize = etohs(aEOEp->mbxheader.length) - 0x0004; + if (EOE_HDR_FRAME_TYPE_GET(frameinfo1) != EOE_GET_IP_PARAM_RESP) + { + wkc = -EOE_RESULT_UNSUPPORTED_FRAME_TYPE; + } + else + { + flags = aEOEp->data[0]; + if (flags & EOE_PARAM_MAC_INCLUDE) + { + memcpy(ipparam->mac.addr, + &aEOEp->data[data_offset], + EOE_ETHADDR_LENGTH); + ipparam->mac_set = 1; + data_offset += EOE_ETHADDR_LENGTH; + } + if (flags & EOE_PARAM_IP_INCLUDE) + { + EOE_ip_byte_to_uint32(&aEOEp->data[data_offset], + &ipparam->ip); + ipparam->ip_set = 1; + data_offset += EOE_IP4_LENGTH; + } + if (flags & EOE_PARAM_SUBNET_IP_INCLUDE) + { + EOE_ip_byte_to_uint32(&aEOEp->data[data_offset], + &ipparam->subnet); + ipparam->subnet_set = 1; + data_offset += EOE_IP4_LENGTH; + } + if (flags & EOE_PARAM_DEFAULT_GATEWAY_INCLUDE) + { + EOE_ip_byte_to_uint32(&aEOEp->data[data_offset], + &ipparam->default_gateway); + ipparam->default_gateway_set = 1; + data_offset += EOE_IP4_LENGTH; + } + if (flags & EOE_PARAM_DNS_IP_INCLUDE) + { + EOE_ip_byte_to_uint32(&aEOEp->data[data_offset], + &ipparam->dns_ip); + ipparam->dns_ip_set = 1; + data_offset += EOE_IP4_LENGTH; + } + if (flags & EOE_PARAM_DNS_NAME_INCLUDE) + { + uint16_t dns_len; + if ((eoedatasize - data_offset) < EOE_DNS_NAME_LENGTH) + { + dns_len = (eoedatasize - data_offset); + } + else + { + dns_len = EOE_DNS_NAME_LENGTH; + } + /* Assume ZERO terminated string */ + memcpy(ipparam->dns_name, &aEOEp->data[data_offset], dns_len); + ipparam->dns_name_set = 1; + data_offset += EOE_DNS_NAME_LENGTH; + } + /* Something os not correct, flag the error */ + if(data_offset > eoedatasize) + { + wkc = -EC_ERR_TYPE_MBX_ERROR; + } + } + } + else + { + /* unexpected mailbox received */ + wkc = -EC_ERR_TYPE_PACKET_ERROR; + } + } + } + return wkc; +} + +/** EoE ethernet buffer write, blocking. +* +* If the buffer is larger than the mailbox size then the buffer is sent in +* several fragments. The function will split the buf data in fragments and +* send them to the slave one by one. +* +* @param[in] context = context struct +* @param[in] slave = Slave number +* @param[in] port = Port number on slave if applicable +* @param[in] psize = Size in bytes of parameter buffer. +* @param[in] p = Pointer to parameter buffer +* @param[in] timeout = Timeout in us, standard is EC_TIMEOUTRXM +* @return Workcounter from last slave transmission +*/ +int ecx_EOEsend(ecx_contextt *context, uint16 slave, uint8 port, int psize, void *p, int timeout) +{ + ec_EOEt *EOEp; + ec_mbxbuft MbxOut; + uint16 frameinfo1, frameinfo2; + uint8 cnt, txfragmentno; + boolean NotLast; + int wkc, maxdata, txframesize, txframeoffset; + const uint8 * buf = p; + static uint8_t txframeno = 0; + + ec_clearmbx(&MbxOut); + EOEp = (ec_EOEt *)&MbxOut; + EOEp->mbxheader.address = htoes(0x0000); + EOEp->mbxheader.priority = 0x00; + /* data section=mailbox size - 6 mbx - 4 EoEh */ + maxdata = context->slavelist[slave].mbx_l - 0x0A; + txframesize = psize; + txfragmentno = 0; + txframeoffset = 0; + NotLast = TRUE; + + do + { + txframesize = psize - txframeoffset; + if (txframesize > maxdata) + { + /* Adjust to even 32-octect blocks */ + txframesize = ((maxdata >> 5) << 5); + } + + if (txframesize == (psize - txframeoffset)) + { + frameinfo1 = (EOE_HDR_LAST_FRAGMENT_SET(1) | EOE_HDR_FRAME_PORT_SET(port)); + NotLast = FALSE; + } + else + { + frameinfo1 = EOE_HDR_FRAME_PORT_SET(port); + } + + frameinfo2 = EOE_HDR_FRAG_NO_SET(txfragmentno); + if (txfragmentno > 0) + { + frameinfo2 = frameinfo2 | (EOE_HDR_FRAME_OFFSET_SET((txframeoffset >> 5))); + } + else + { + frameinfo2 = frameinfo2 | (EOE_HDR_FRAME_OFFSET_SET(((psize + 31) >> 5))); + txframeno++; + } + frameinfo2 = frameinfo2 | EOE_HDR_FRAME_NO_SET(txframeno); + + /* get new mailbox count value, used as session handle */ + cnt = ec_nextmbxcnt(context->slavelist[slave].mbx_cnt); + context->slavelist[slave].mbx_cnt = cnt; + + EOEp->mbxheader.length = htoes((uint16)(4 + txframesize)); /* no timestamp */ + EOEp->mbxheader.mbxtype = ECT_MBXT_EOE + MBX_HDR_SET_CNT(cnt); /* EoE */ + + EOEp->frameinfo1 = htoes(frameinfo1); + EOEp->frameinfo2 = htoes(frameinfo2); + + memcpy(EOEp->data, &buf[txframeoffset], txframesize); + + /* send EoE request to slave */ + wkc = ecx_mbxsend(context, slave, (ec_mbxbuft *)&MbxOut, timeout); + if ((NotLast == TRUE) && (wkc > 0)) + { + txframeoffset += txframesize; + txfragmentno++; + } + } while ((NotLast == TRUE) && (wkc > 0)); + + return wkc; +} + + +/** EoE ethernet buffer read, blocking. +* +* If the buffer is larger than the mailbox size then the buffer is received +* by several fragments. The function will assamble the fragments into +* a complete Ethernet buffer. +* +* @param[in] context = context struct +* @param[in] slave = Slave number +* @param[in] port = Port number on slave if applicable +* @param[in,out] psize = Size in bytes of parameter buffer. +* @param[in] p = Pointer to parameter buffer +* @param[in] timeout = Timeout in us, standard is EC_TIMEOUTRXM +* @return Workcounter from last slave response or error code +*/ +int ecx_EOErecv(ecx_contextt *context, uint16 slave, uint8 port, int * psize, void *p, int timeout) +{ + ec_EOEt *aEOEp; + ec_mbxbuft MbxIn; + uint16 frameinfo1, frameinfo2; + uint8 rxfragmentno, rxframeno; + boolean NotLast; + int wkc, buffersize, rxframesize, rxframeoffset, eoedatasize; + uint8 * buf = p; + + ec_clearmbx(&MbxIn); + aEOEp = (ec_EOEt *)&MbxIn; + NotLast = TRUE; + buffersize = *psize; + rxfragmentno = 0; + rxframeno = 0xff; + rxframeoffset = 0; + + /* Hang for a while if nothing is in */ + wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, timeout); + + while ((wkc > 0) && (NotLast == TRUE)) + { + /* slave response should be FoE */ + if ((aEOEp->mbxheader.mbxtype & 0x0f) == ECT_MBXT_EOE) + { + + eoedatasize = etohs(aEOEp->mbxheader.length) - 0x00004; + frameinfo1 = etohs(aEOEp->frameinfo1); + frameinfo2 = etohs(aEOEp->frameinfo2); + + if (rxfragmentno != EOE_HDR_FRAG_NO_GET(frameinfo2)) + { + if (EOE_HDR_FRAG_NO_GET(frameinfo2) > 0) + { + wkc = -EC_ERR_TYPE_EOE_INVALID_RX_DATA; + /* Exit here*/ + break; + } + } + + if (rxfragmentno == 0) + { + rxframeno = EOE_HDR_FRAME_NO_GET(frameinfo2); + rxframesize = (EOE_HDR_FRAME_OFFSET_GET(frameinfo2) << 5); + if (rxframesize > buffersize) + { + wkc = -EC_ERR_TYPE_EOE_INVALID_RX_DATA; + /* Exit here*/ + break; + } + if (port != EOE_HDR_FRAME_PORT_GET(frameinfo1)) + { + wkc = -EC_ERR_TYPE_EOE_INVALID_RX_DATA; + /* Exit here*/ + break; + } + } + else + { + if (rxframeno != EOE_HDR_FRAME_NO_GET(frameinfo2)) + { + wkc = -EC_ERR_TYPE_EOE_INVALID_RX_DATA; + /* Exit here*/ + break; + } + else if (rxframeoffset != (EOE_HDR_FRAME_OFFSET_GET(frameinfo2) << 5)) + { + wkc = -EC_ERR_TYPE_EOE_INVALID_RX_DATA; + /* Exit here*/ + break; + } + } + + if ((rxframeoffset + eoedatasize) <= buffersize) + { + memcpy(&buf[rxframeoffset], aEOEp->data, eoedatasize); + rxframeoffset += eoedatasize; + rxfragmentno++; + } + + if (EOE_HDR_LAST_FRAGMENT_GET(frameinfo1)) + { + /* Remove timestamp */ + if (EOE_HDR_TIME_APPEND_GET(frameinfo1)) + { + rxframeoffset -= 4; + } + NotLast = FALSE; + *psize = rxframeoffset; + } + else + { + /* Hang for a while if nothing is in */ + wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, timeout); + } + } + else + { + /* unexpected mailbox received */ + wkc = -EC_ERR_TYPE_PACKET_ERROR; + } + } + return wkc; +} + +/** EoE mailbox fragment read +* +* Will take the data in incoming mailbox buffer and copy to destination +* Ethernet frame buffer at given offset and update current fragment variables +* +* @param[in] MbxIn = Received mailbox containing fragment data +* @param[in,out] rxfragmentno = Fragment number +* @param[in,out] rxframesize = Frame size +* @param[in,out] rxframeoffset = Frame offset +* @param[in,out] rxframeno = Frame number +* @param[in,out] psize = Size in bytes of frame buffer. +* @param[out] p = Pointer to frame buffer +* @return 0= if fragment OK, >0 if last fragment, <0 on error +*/ +int ecx_EOEreadfragment( + ec_mbxbuft * MbxIn, + uint8 * rxfragmentno, + uint16 * rxframesize, + uint16 * rxframeoffset, + uint16 * rxframeno, + int * psize, + void *p) +{ + uint16 frameinfo1, frameinfo2, eoedatasize; + int wkc; + ec_EOEt * aEOEp; + uint8 * buf; + + aEOEp = (ec_EOEt *)MbxIn; + buf = p; + wkc = 0; + + /* slave response should be EoE */ + if ((aEOEp->mbxheader.mbxtype & 0x0f) == ECT_MBXT_EOE) + { + eoedatasize = etohs(aEOEp->mbxheader.length) - 0x00004; + frameinfo1 = etohs(aEOEp->frameinfo1); + frameinfo2 = etohs(aEOEp->frameinfo2); + + /* Retrive fragment number, is it what we expect? */ + if (*rxfragmentno != EOE_HDR_FRAG_NO_GET(frameinfo2)) + { + /* If expected fragment number is not 0, reset working variables */ + if (*rxfragmentno != 0) + { + *rxfragmentno = 0; + *rxframesize = 0; + *rxframeoffset = 0; + *rxframeno = 0; + } + + /* If incoming fragment number is not 0 we can't recover, exit */ + if (EOE_HDR_FRAG_NO_GET(frameinfo2) > 0) + { + wkc = -EC_ERR_TYPE_EOE_INVALID_RX_DATA; + return wkc; + } + } + + /* Is it a new frame?*/ + if (*rxfragmentno == 0) + { + *rxframesize = (uint16)(EOE_HDR_FRAME_OFFSET_GET(frameinfo2) << 5); + *rxframeoffset = 0; + *rxframeno = EOE_HDR_FRAME_NO_GET(frameinfo2); + } + else + { + /* If we're inside a frame, make sure it is the same */ + if (*rxframeno != EOE_HDR_FRAME_NO_GET(frameinfo2)) + { + *rxfragmentno = 0; + *rxframesize = 0; + *rxframeoffset = 0; + *rxframeno = 0; + wkc = -EC_ERR_TYPE_EOE_INVALID_RX_DATA; + return wkc; + } + else if (*rxframeoffset != (EOE_HDR_FRAME_OFFSET_GET(frameinfo2) << 5)) + { + *rxfragmentno = 0; + *rxframesize = 0; + *rxframeoffset = 0; + *rxframeno = 0; + wkc = -EC_ERR_TYPE_EOE_INVALID_RX_DATA; + return wkc; + } + } + + /* Make sure we're inside expected frame size */ + if (((*rxframeoffset + eoedatasize) <= *rxframesize) && + ((*rxframeoffset + eoedatasize) <= *psize)) + { + memcpy(&buf[*rxframeoffset], aEOEp->data, eoedatasize); + *rxframeoffset += eoedatasize; + *rxfragmentno += 1; + } + + /* Is it the last fragment */ + if (EOE_HDR_LAST_FRAGMENT_GET(frameinfo1)) + { + /* Remove timestamp */ + if (EOE_HDR_TIME_APPEND_GET(frameinfo1)) + { + *rxframeoffset -= 4; + } + *psize = *rxframeoffset; + *rxfragmentno = 0; + *rxframesize = 0; + *rxframeoffset = 0; + *rxframeno = 0; + wkc = 1; + } + } + else + { + /* unexpected mailbox received */ + wkc = -EC_ERR_TYPE_PACKET_ERROR; + } + return wkc; +} diff --git a/soem-src/soem/ethercateoe.h b/soem-src/soem/ethercateoe.h new file mode 100644 index 0000000..67ef329 --- /dev/null +++ b/soem-src/soem/ethercateoe.h @@ -0,0 +1,213 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +/** \file + * \brief + * Headerfile for ethercatfoe.c + */ + +#ifndef _ethercateoe_ +#define _ethercateoe_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include "ethercat.h" + +/* use maximum size for EOE mailbox data - mbxheader and 2x frameinfo */ +#define EC_MAXEOEDATA (EC_MAXMBX - (sizeof(ec_mbxheadert) +\ + sizeof(uint16_t) +\ + sizeof(uint16_t))) + +/** DNS length according to ETG 1000.6 */ +#define EOE_DNS_NAME_LENGTH 32 +/** Ethernet address length not including VLAN */ +#define EOE_ETHADDR_LENGTH 6 +/** IPv4 address length */ +#define EOE_IP4_LENGTH sizeof(uint32_t) + +#define EOE_MAKEU32(a,b,c,d) (((uint32_t)((a) & 0xff) << 24) | \ + ((uint32_t)((b) & 0xff) << 16) | \ + ((uint32_t)((c) & 0xff) << 8) | \ + (uint32_t)((d) & 0xff)) + +#if !defined(EC_BIG_ENDIAN) && defined(EC_LITTLE_ENDIAN) + +#define EOE_HTONS(x) ((((x) & 0x00ffUL) << 8) | (((x) & 0xff00UL) >> 8)) +#define EOE_NTOHS(x) EOE_HTONS(x) +#define EOE_HTONL(x) ((((x) & 0x000000ffUL) << 24) | \ + (((x) & 0x0000ff00UL) << 8) | \ + (((x) & 0x00ff0000UL) >> 8) | \ + (((x) & 0xff000000UL) >> 24)) +#define EOE_NTOHL(x) EOE_HTONL(x) +#else +#define EOE_HTONS(x) (x) +#define EOE_NTOHS(x) (x) +#define EOE_HTONL(x) (x) +#define EOE_NTOHL(x) (x) +#endif /* !defined(EC_BIG_ENDIAN) && defined(EC_LITTLE_ENDIAN) */ + +/** Get one byte from the 4-byte address */ +#define eoe_ip4_addr1(ipaddr) (((const uint8_t*)(&(ipaddr)->addr))[0]) +#define eoe_ip4_addr2(ipaddr) (((const uint8_t*)(&(ipaddr)->addr))[1]) +#define eoe_ip4_addr3(ipaddr) (((const uint8_t*)(&(ipaddr)->addr))[2]) +#define eoe_ip4_addr4(ipaddr) (((const uint8_t*)(&(ipaddr)->addr))[3]) + +/** Set an IP address given by the four byte-parts */ +#define EOE_IP4_ADDR_TO_U32(ipaddr,a,b,c,d) \ + (ipaddr)->addr = EOE_HTONL(EOE_MAKEU32(a,b,c,d)) + +/** Header frame info 1 */ +#define EOE_HDR_FRAME_TYPE_OFFSET 0 +#define EOE_HDR_FRAME_TYPE (0xF << 0) +#define EOE_HDR_FRAME_TYPE_SET(x) (((x) & 0xF) << 0) +#define EOE_HDR_FRAME_TYPE_GET(x) (((x) >> 0) & 0xF) +#define EOE_HDR_FRAME_PORT_OFFSET 4 +#define EOE_HDR_FRAME_PORT (0xF << 4) +#define EOE_HDR_FRAME_PORT_SET(x) ((uint16)(((x) & 0xF) << 4)) +#define EOE_HDR_FRAME_PORT_GET(x) (((x) >> 4) & 0xF) +#define EOE_HDR_LAST_FRAGMENT_OFFSET 8 +#define EOE_HDR_LAST_FRAGMENT (0x1 << 8) +#define EOE_HDR_LAST_FRAGMENT_SET(x) (((x) & 0x1) << 8) +#define EOE_HDR_LAST_FRAGMENT_GET(x) (((x) >> 8) & 0x1) +#define EOE_HDR_TIME_APPEND_OFFSET 9 +#define EOE_HDR_TIME_APPEND (0x1 << 9) +#define EOE_HDR_TIME_APPEND_SET(x) (((x) & 0x1) << 9) +#define EOE_HDR_TIME_APPEND_GET(x) (((x) >> 9) & 0x1) +#define EOE_HDR_TIME_REQUEST_OFFSET 10 +#define EOE_HDR_TIME_REQUEST (0x1 << 10) +#define EOE_HDR_TIME_REQUEST_SET(x) (((x) & 0x1) << 10) +#define EOE_HDR_TIME_REQUEST_GET(x) (((x) >> 10) & 0x1) + +/** Header frame info 2 */ +#define EOE_HDR_FRAG_NO_OFFSET 0 +#define EOE_HDR_FRAG_NO (0x3F << 0) +#define EOE_HDR_FRAG_NO_SET(x) ((uint16)(((x) & 0x3F) << 0)) +#define EOE_HDR_FRAG_NO_GET(x) (((x) >> 0) & 0x3F) +#define EOE_HDR_FRAME_OFFSET_OFFSET 6 +#define EOE_HDR_FRAME_OFFSET (0x3F << 6) +#define EOE_HDR_FRAME_OFFSET_SET(x) ((uint16)(((x) & 0x3F) << 6)) +#define EOE_HDR_FRAME_OFFSET_GET(x) (((x) >> 6) & 0x3F) +#define EOE_HDR_FRAME_NO_OFFSET 12 +#define EOE_HDR_FRAME_NO (0xF << 12) +#define EOE_HDR_FRAME_NO_SET(x) ((uint16)(((x) & 0xF) << 12)) +#define EOE_HDR_FRAME_NO_GET(x) (((x) >> 12) & 0xF) + +/** EOE param */ +#define EOE_PARAM_OFFSET 4 +#define EOE_PARAM_MAC_INCLUDE (0x1 << 0) +#define EOE_PARAM_IP_INCLUDE (0x1 << 1) +#define EOE_PARAM_SUBNET_IP_INCLUDE (0x1 << 2) +#define EOE_PARAM_DEFAULT_GATEWAY_INCLUDE (0x1 << 3) +#define EOE_PARAM_DNS_IP_INCLUDE (0x1 << 4) +#define EOE_PARAM_DNS_NAME_INCLUDE (0x1 << 5) + +/** EoE frame types */ +#define EOE_FRAG_DATA 0 +#define EOE_INIT_RESP_TIMESTAMP 1 +#define EOE_INIT_REQ 2 /* Spec SET IP REQ */ +#define EOE_INIT_RESP 3 /* Spec SET IP RESP */ +#define EOE_SET_ADDR_FILTER_REQ 4 +#define EOE_SET_ADDR_FILTER_RESP 5 +#define EOE_GET_IP_PARAM_REQ 6 +#define EOE_GET_IP_PARAM_RESP 7 +#define EOE_GET_ADDR_FILTER_REQ 8 +#define EOE_GET_ADDR_FILTER_RESP 9 + +/** EoE parameter result codes */ +#define EOE_RESULT_SUCCESS 0x0000 +#define EOE_RESULT_UNSPECIFIED_ERROR 0x0001 +#define EOE_RESULT_UNSUPPORTED_FRAME_TYPE 0x0002 +#define EOE_RESULT_NO_IP_SUPPORT 0x0201 +#define EOE_RESULT_NO_DHCP_SUPPORT 0x0202 +#define EOE_RESULT_NO_FILTER_SUPPORT 0x0401 + + +/** EOE ip4 address in network order */ +typedef struct eoe_ip4_addr { + uint32_t addr; +}eoe_ip4_addr_t; + +/** EOE ethernet address */ +PACKED_BEGIN +typedef struct PACKED eoe_ethaddr +{ + uint8_t addr[EOE_ETHADDR_LENGTH]; +} eoe_ethaddr_t; +PACKED_END + +/** EoE IP request structure, storage only, no need to pack */ +typedef struct eoe_param +{ + uint8_t mac_set : 1; + uint8_t ip_set : 1; + uint8_t subnet_set : 1; + uint8_t default_gateway_set : 1; + uint8_t dns_ip_set : 1; + uint8_t dns_name_set : 1; + eoe_ethaddr_t mac; + eoe_ip4_addr_t ip; + eoe_ip4_addr_t subnet; + eoe_ip4_addr_t default_gateway; + eoe_ip4_addr_t dns_ip; + char dns_name[EOE_DNS_NAME_LENGTH]; +} eoe_param_t; + +/** EOE structure. +* Used to interpret EoE mailbox packets. +*/ +PACKED_BEGIN +typedef struct PACKED +{ + ec_mbxheadert mbxheader; + uint16_t frameinfo1; + union + { + uint16_t frameinfo2; + uint16_t result; + }; + uint8 data[EC_MAXEOEDATA]; +} ec_EOEt; +PACKED_END + +int ecx_EOEdefinehook(ecx_contextt *context, void *hook); +int ecx_EOEsetIp(ecx_contextt *context, + uint16 slave, + uint8 port, + eoe_param_t * ipparam, + int timeout); +int ecx_EOEgetIp(ecx_contextt *context, + uint16 slave, + uint8 port, + eoe_param_t * ipparam, + int timeout); +int ecx_EOEsend(ecx_contextt *context, + uint16 slave, + uint8 port, + int psize, + void *p, + int timeout); +int ecx_EOErecv(ecx_contextt *context, + uint16 slave, + uint8 port, + int * psize, + void *p, + int timeout); +int ecx_EOEreadfragment( + ec_mbxbuft * MbxIn, + uint8 * rxfragmentno, + uint16 * rxframesize, + uint16 * rxframeoffset, + uint16 * rxframeno, + int * psize, + void *p); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/soem-src/soem/ethercatfoe.c b/soem-src/soem/ethercatfoe.c new file mode 100644 index 0000000..a832193 --- /dev/null +++ b/soem-src/soem/ethercatfoe.c @@ -0,0 +1,416 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +/** \file + * \brief + * File over EtherCAT (FoE) module. + * + * SDO read / write and SDO service functions + */ + +#include +#include +#include "osal.h" +#include "oshw.h" +#include "ethercattype.h" +#include "ethercatbase.h" +#include "ethercatmain.h" +#include "ethercatfoe.h" + +/* use maximum size for FOE mailbox data - header and metadata */ +#define EC_MAXFOEDATA \ + (EC_MAXMBX - \ + (sizeof(ec_mbxheadert) + \ + sizeof(uint8_t) + \ + sizeof(uint8_t) + \ + sizeof(uint32_t))) + +/** FOE structure. + * Used for Read, Write, Data, Ack and Error mailbox packets. + */ +PACKED_BEGIN +typedef struct PACKED +{ + ec_mbxheadert MbxHeader; + uint8 OpCode; + uint8 Reserved; + union + { + uint32 Password; + uint32 PacketNumber; + uint32 ErrorCode; + }; + union + { + char FileName[EC_MAXFOEDATA]; + uint8 Data[EC_MAXFOEDATA]; + char ErrorText[EC_MAXFOEDATA]; + }; +} ec_FOEt; +PACKED_END + +/** FoE progress hook. + * + * @param[in] context = context struct + * @param[in] hook = Pointer to hook function. + * @return 1 + */ +int ecx_FOEdefinehook(ecx_contextt *context, void *hook) +{ + context->FOEhook = hook; + return 1; +} + +/** FoE read, blocking. + * + * @param[in] context = context struct + * @param[in] slave = Slave number. + * @param[in] filename = Filename of file to read. + * @param[in] password = password. + * @param[in,out] psize = Size in bytes of file buffer, returns bytes read from file. + * @param[out] p = Pointer to file buffer + * @param[in] timeout = Timeout per mailbox cycle in us, standard is EC_TIMEOUTRXM + * @return Workcounter from last slave response + */ +int ecx_FOEread(ecx_contextt *context, uint16 slave, char *filename, uint32 password, int *psize, void *p, int timeout) +{ + ec_FOEt *FOEp, *aFOEp; + int wkc; + int32 dataread = 0; + int32 buffersize, packetnumber, prevpacket = 0; + uint16 fnsize, maxdata, segmentdata; + ec_mbxbuft MbxIn, MbxOut; + uint8 cnt; + boolean worktodo; + + buffersize = *psize; + ec_clearmbx(&MbxIn); + /* Empty slave out mailbox if something is in. Timeout set to 0 */ + wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, 0); + ec_clearmbx(&MbxOut); + aFOEp = (ec_FOEt *)&MbxIn; + FOEp = (ec_FOEt *)&MbxOut; + fnsize = (uint16)strlen(filename); + if (fnsize > EC_MAXFOEDATA) + { + fnsize = EC_MAXFOEDATA; + } + maxdata = context->slavelist[slave].mbx_l - 12; + if (fnsize > maxdata) + { + fnsize = maxdata; + } + FOEp->MbxHeader.length = htoes(0x0006 + fnsize); + FOEp->MbxHeader.address = htoes(0x0000); + FOEp->MbxHeader.priority = 0x00; + /* get new mailbox count value, used as session handle */ + cnt = ec_nextmbxcnt(context->slavelist[slave].mbx_cnt); + context->slavelist[slave].mbx_cnt = cnt; + FOEp->MbxHeader.mbxtype = ECT_MBXT_FOE + MBX_HDR_SET_CNT(cnt); /* FoE */ + FOEp->OpCode = ECT_FOE_READ; + FOEp->Password = htoel(password); + /* copy filename in mailbox */ + memcpy(&FOEp->FileName[0], filename, fnsize); + /* send FoE request to slave */ + wkc = ecx_mbxsend(context, slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM); + if (wkc > 0) /* succeeded to place mailbox in slave ? */ + { + do + { + worktodo = FALSE; + /* clean mailboxbuffer */ + ec_clearmbx(&MbxIn); + /* read slave response */ + wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, timeout); + if (wkc > 0) /* succeeded to read slave response ? */ + { + /* slave response should be FoE */ + if ((aFOEp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_FOE) + { + if(aFOEp->OpCode == ECT_FOE_DATA) + { + segmentdata = etohs(aFOEp->MbxHeader.length) - 0x0006; + packetnumber = etohl(aFOEp->PacketNumber); + if ((packetnumber == ++prevpacket) && (dataread + segmentdata <= buffersize)) + { + memcpy(p, &aFOEp->Data[0], segmentdata); + dataread += segmentdata; + p = (uint8 *)p + segmentdata; + if (segmentdata == maxdata) + { + worktodo = TRUE; + } + FOEp->MbxHeader.length = htoes(0x0006); + FOEp->MbxHeader.address = htoes(0x0000); + FOEp->MbxHeader.priority = 0x00; + /* get new mailbox count value */ + cnt = ec_nextmbxcnt(context->slavelist[slave].mbx_cnt); + context->slavelist[slave].mbx_cnt = cnt; + FOEp->MbxHeader.mbxtype = ECT_MBXT_FOE + MBX_HDR_SET_CNT(cnt); /* FoE */ + FOEp->OpCode = ECT_FOE_ACK; + FOEp->PacketNumber = htoel(packetnumber); + /* send FoE ack to slave */ + wkc = ecx_mbxsend(context, slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM); + if (wkc <= 0) + { + worktodo = FALSE; + } + if (context->FOEhook) + { + context->FOEhook(slave, packetnumber, dataread); + } + } + else + { + /* FoE error */ + wkc = -EC_ERR_TYPE_FOE_BUF2SMALL; + } + } + else + { + if(aFOEp->OpCode == ECT_FOE_ERROR) + { + /* FoE error */ + wkc = -EC_ERR_TYPE_FOE_ERROR; + } + else + { + /* unexpected mailbox received */ + wkc = -EC_ERR_TYPE_PACKET_ERROR; + } + } + } + else + { + /* unexpected mailbox received */ + wkc = -EC_ERR_TYPE_PACKET_ERROR; + } + *psize = dataread; + } + } while (worktodo); + } + + return wkc; +} + +/** FoE write, blocking. + * + * @param[in] context = context struct + * @param[in] slave = Slave number. + * @param[in] filename = Filename of file to write. + * @param[in] password = password. + * @param[in] psize = Size in bytes of file buffer. + * @param[out] p = Pointer to file buffer + * @param[in] timeout = Timeout per mailbox cycle in us, standard is EC_TIMEOUTRXM + * @return Workcounter from last slave response + */ +int ecx_FOEwrite(ecx_contextt *context, uint16 slave, char *filename, uint32 password, int psize, void *p, int timeout) +{ + ec_FOEt *FOEp, *aFOEp; + int wkc; + int32 packetnumber, sendpacket = 0; + uint16 fnsize, maxdata; + int segmentdata; + ec_mbxbuft MbxIn, MbxOut; + uint8 cnt; + boolean worktodo, dofinalzero; + int tsize; + + ec_clearmbx(&MbxIn); + /* Empty slave out mailbox if something is in. Timeout set to 0 */ + wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, 0); + ec_clearmbx(&MbxOut); + aFOEp = (ec_FOEt *)&MbxIn; + FOEp = (ec_FOEt *)&MbxOut; + dofinalzero = TRUE; + fnsize = (uint16)strlen(filename); + if (fnsize > EC_MAXFOEDATA) + { + fnsize = EC_MAXFOEDATA; + } + maxdata = context->slavelist[slave].mbx_l - 12; + if (fnsize > maxdata) + { + fnsize = maxdata; + } + FOEp->MbxHeader.length = htoes(0x0006 + fnsize); + FOEp->MbxHeader.address = htoes(0x0000); + FOEp->MbxHeader.priority = 0x00; + /* get new mailbox count value, used as session handle */ + cnt = ec_nextmbxcnt(context->slavelist[slave].mbx_cnt); + context->slavelist[slave].mbx_cnt = cnt; + FOEp->MbxHeader.mbxtype = ECT_MBXT_FOE + MBX_HDR_SET_CNT(cnt); /* FoE */ + FOEp->OpCode = ECT_FOE_WRITE; + FOEp->Password = htoel(password); + /* copy filename in mailbox */ + memcpy(&FOEp->FileName[0], filename, fnsize); + /* send FoE request to slave */ + wkc = ecx_mbxsend(context, slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM); + if (wkc > 0) /* succeeded to place mailbox in slave ? */ + { + do + { + worktodo = FALSE; + /* clean mailboxbuffer */ + ec_clearmbx(&MbxIn); + /* read slave response */ + wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, timeout); + if (wkc > 0) /* succeeded to read slave response ? */ + { + /* slave response should be FoE */ + if ((aFOEp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_FOE) + { + switch (aFOEp->OpCode) + { + case ECT_FOE_ACK: + { + packetnumber = etohl(aFOEp->PacketNumber); + if (packetnumber == sendpacket) + { + if (context->FOEhook) + { + context->FOEhook(slave, packetnumber, psize); + } + tsize = psize; + if (tsize > maxdata) + { + tsize = maxdata; + } + if(tsize || dofinalzero) + { + worktodo = TRUE; + dofinalzero = FALSE; + segmentdata = tsize; + psize -= segmentdata; + /* if last packet was full size, add a zero size packet as final */ + /* EOF is defined as packetsize < full packetsize */ + if (!psize && (segmentdata == maxdata)) + { + dofinalzero = TRUE; + } + FOEp->MbxHeader.length = htoes((uint16)(0x0006 + segmentdata)); + FOEp->MbxHeader.address = htoes(0x0000); + FOEp->MbxHeader.priority = 0x00; + /* get new mailbox count value */ + cnt = ec_nextmbxcnt(context->slavelist[slave].mbx_cnt); + context->slavelist[slave].mbx_cnt = cnt; + FOEp->MbxHeader.mbxtype = ECT_MBXT_FOE + MBX_HDR_SET_CNT(cnt); /* FoE */ + FOEp->OpCode = ECT_FOE_DATA; + sendpacket++; + FOEp->PacketNumber = htoel(sendpacket); + memcpy(&FOEp->Data[0], p, segmentdata); + p = (uint8 *)p + segmentdata; + /* send FoE data to slave */ + wkc = ecx_mbxsend(context, slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM); + if (wkc <= 0) + { + worktodo = FALSE; + } + } + } + else + { + /* FoE error */ + wkc = -EC_ERR_TYPE_FOE_PACKETNUMBER; + } + break; + } + case ECT_FOE_BUSY: + { + /* resend if data has been send before */ + /* otherwise ignore */ + if (sendpacket) + { + psize += segmentdata; + p = (uint8 *)p - segmentdata; + --sendpacket; + tsize = psize; + if (tsize > maxdata) + { + tsize = maxdata; + } + if(tsize || dofinalzero) + { + worktodo = TRUE; + dofinalzero = FALSE; + segmentdata = tsize; + psize -= segmentdata; + /* if last packet was full size, add a zero size packet as final */ + /* EOF is defined as packetsize < full packetsize */ + if (!psize && (segmentdata == maxdata)) + { + dofinalzero = TRUE; + } + FOEp->MbxHeader.length = htoes((uint16)(0x0006 + segmentdata)); + FOEp->MbxHeader.address = htoes(0x0000); + FOEp->MbxHeader.priority = 0x00; + /* get new mailbox count value */ + cnt = ec_nextmbxcnt(context->slavelist[slave].mbx_cnt); + context->slavelist[slave].mbx_cnt = cnt; + FOEp->MbxHeader.mbxtype = ECT_MBXT_FOE + MBX_HDR_SET_CNT(cnt); /* FoE */ + FOEp->OpCode = ECT_FOE_DATA; + sendpacket++; + FOEp->PacketNumber = htoel(sendpacket); + memcpy(&FOEp->Data[0], p, segmentdata); + p = (uint8 *)p + segmentdata; + /* send FoE data to slave */ + wkc = ecx_mbxsend(context, slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM); + if (wkc <= 0) + { + worktodo = FALSE; + } + } + } + break; + } + case ECT_FOE_ERROR: + { + /* FoE error */ + if (aFOEp->ErrorCode == 0x8001) + { + wkc = -EC_ERR_TYPE_FOE_FILE_NOTFOUND; + } + else + { + wkc = -EC_ERR_TYPE_FOE_ERROR; + } + break; + } + default: + { + /* unexpected mailbox received */ + wkc = -EC_ERR_TYPE_PACKET_ERROR; + break; + } + } + } + else + { + /* unexpected mailbox received */ + wkc = -EC_ERR_TYPE_PACKET_ERROR; + } + } + } while (worktodo); + } + + return wkc; +} + +#ifdef EC_VER1 +int ec_FOEdefinehook(void *hook) +{ + return ecx_FOEdefinehook(&ecx_context, hook); +} + +int ec_FOEread(uint16 slave, char *filename, uint32 password, int *psize, void *p, int timeout) +{ + return ecx_FOEread(&ecx_context, slave, filename, password, psize, p, timeout); +} + +int ec_FOEwrite(uint16 slave, char *filename, uint32 password, int psize, void *p, int timeout) +{ + return ecx_FOEwrite(&ecx_context, slave, filename, password, psize, p, timeout); +} +#endif diff --git a/soem-src/soem/ethercatfoe.h b/soem-src/soem/ethercatfoe.h new file mode 100644 index 0000000..c35a42d --- /dev/null +++ b/soem-src/soem/ethercatfoe.h @@ -0,0 +1,33 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +/** \file + * \brief + * Headerfile for ethercatfoe.c + */ + +#ifndef _ethercatfoe_ +#define _ethercatfoe_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#ifdef EC_VER1 +int ec_FOEdefinehook(void *hook); +int ec_FOEread(uint16 slave, char *filename, uint32 password, int *psize, void *p, int timeout); +int ec_FOEwrite(uint16 slave, char *filename, uint32 password, int psize, void *p, int timeout); +#endif + +int ecx_FOEdefinehook(ecx_contextt *context, void *hook); +int ecx_FOEread(ecx_contextt *context, uint16 slave, char *filename, uint32 password, int *psize, void *p, int timeout); +int ecx_FOEwrite(ecx_contextt *context, uint16 slave, char *filename, uint32 password, int psize, void *p, int timeout); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/soem-src/soem/ethercatmain.c b/soem-src/soem/ethercatmain.c new file mode 100644 index 0000000..a845850 --- /dev/null +++ b/soem-src/soem/ethercatmain.c @@ -0,0 +1,2418 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +/** + * \file + * \brief + * Main EtherCAT functions. + * + * Initialisation, state set and read, mailbox primitives, EEPROM primitives, + * SII reading and processdata exchange. + * + * Defines ec_slave[]. All slave information is put in this structure. + * Needed for most user interaction with slaves. + */ + +#include +#include +#include "osal.h" +#include "oshw.h" +#include "ethercat.h" + + +/** delay in us for eeprom ready loop */ +#define EC_LOCALDELAY 200 + +/** record for ethercat eeprom communications */ +PACKED_BEGIN +typedef struct PACKED +{ + uint16 comm; + uint16 addr; + uint16 d2; +} ec_eepromt; +PACKED_END + +/** mailbox error structure */ +PACKED_BEGIN +typedef struct PACKED +{ + ec_mbxheadert MbxHeader; + uint16 Type; + uint16 Detail; +} ec_mbxerrort; +PACKED_END + +/** emergency request structure */ +PACKED_BEGIN +typedef struct PACKED +{ + ec_mbxheadert MbxHeader; + uint16 CANOpen; + uint16 ErrorCode; + uint8 ErrorReg; + uint8 bData; + uint16 w1,w2; +} ec_emcyt; +PACKED_END + +#ifdef EC_VER1 +/** Main slave data array. + * Each slave found on the network gets its own record. + * ec_slave[0] is reserved for the master. Structure gets filled + * in by the configuration function ec_config(). + */ +ec_slavet ec_slave[EC_MAXSLAVE]; +/** number of slaves found on the network */ +int ec_slavecount; +/** slave group structure */ +ec_groupt ec_group[EC_MAXGROUP]; + +/** cache for EEPROM read functions */ +static uint8 ec_esibuf[EC_MAXEEPBUF]; +/** bitmap for filled cache buffer bytes */ +static uint32 ec_esimap[EC_MAXEEPBITMAP]; +/** current slave for EEPROM cache buffer */ +static ec_eringt ec_elist; +static ec_idxstackT ec_idxstack; + +/** SyncManager Communication Type struct to store data of one slave */ +static ec_SMcommtypet ec_SMcommtype[EC_MAX_MAPT]; +/** PDO assign struct to store data of one slave */ +static ec_PDOassignt ec_PDOassign[EC_MAX_MAPT]; +/** PDO description struct to store data of one slave */ +static ec_PDOdesct ec_PDOdesc[EC_MAX_MAPT]; + +/** buffer for EEPROM SM data */ +static ec_eepromSMt ec_SM; +/** buffer for EEPROM FMMU data */ +static ec_eepromFMMUt ec_FMMU; +/** Global variable TRUE if error available in error stack */ +boolean EcatError = FALSE; + +int64 ec_DCtime; + +ecx_portt ecx_port; +ecx_redportt ecx_redport; + +ecx_contextt ecx_context = { + &ecx_port, // .port = + &ec_slave[0], // .slavelist = + &ec_slavecount, // .slavecount = + EC_MAXSLAVE, // .maxslave = + &ec_group[0], // .grouplist = + EC_MAXGROUP, // .maxgroup = + &ec_esibuf[0], // .esibuf = + &ec_esimap[0], // .esimap = + 0, // .esislave = + &ec_elist, // .elist = + &ec_idxstack, // .idxstack = + &EcatError, // .ecaterror = + 0, // .DCtO = 补丁：SOEM master 给 ecx_context 加了 + 0, // .DCl = DCtO/DCl 两个 uint16 却漏改这个全局初始化器， + // MSVC 下 &ec_DCtime 错位到 2 字节字段 → C1311 + &ec_DCtime, // .DCtime = + &ec_SMcommtype[0], // .SMcommtype = + &ec_PDOassign[0], // .PDOassign = + &ec_PDOdesc[0], // .PDOdesc = + &ec_SM, // .eepSM = + &ec_FMMU, // .eepFMMU = + NULL, // .FOEhook() + NULL, // .EOEhook() + 0, // .manualstatechange + NULL, // .userdata +}; +#endif + +/** Create list over available network adapters. + * + * @return First element in list over available network adapters. + */ +ec_adaptert * ec_find_adapters (void) +{ + ec_adaptert * ret_adapter; + + ret_adapter = oshw_find_adapters (); + + return ret_adapter; +} + +/** Free dynamically allocated list over available network adapters. + * + * @param[in] adapter = Struct holding adapter name, description and pointer to next. + */ +void ec_free_adapters (ec_adaptert * adapter) +{ + oshw_free_adapters (adapter); +} + +/** Pushes an error on the error list. + * + * @param[in] context = context struct + * @param[in] Ec pointer describing the error. + */ +void ecx_pusherror(ecx_contextt *context, const ec_errort *Ec) +{ + context->elist->Error[context->elist->head] = *Ec; + context->elist->Error[context->elist->head].Signal = TRUE; + context->elist->head++; + if (context->elist->head > EC_MAXELIST) + { + context->elist->head = 0; + } + if (context->elist->head == context->elist->tail) + { + context->elist->tail++; + } + if (context->elist->tail > EC_MAXELIST) + { + context->elist->tail = 0; + } + *(context->ecaterror) = TRUE; +} + +/** Pops an error from the list. + * + * @param[in] context = context struct + * @param[out] Ec = Struct describing the error. + * @return TRUE if an error was popped. + */ +boolean ecx_poperror(ecx_contextt *context, ec_errort *Ec) +{ + boolean notEmpty = (context->elist->head != context->elist->tail); + + *Ec = context->elist->Error[context->elist->tail]; + context->elist->Error[context->elist->tail].Signal = FALSE; + if (notEmpty) + { + context->elist->tail++; + if (context->elist->tail > EC_MAXELIST) + { + context->elist->tail = 0; + } + } + else + { + *(context->ecaterror) = FALSE; + } + return notEmpty; +} + +/** Check if error list has entries. + * + * @param[in] context = context struct + * @return TRUE if error list contains entries. + */ +boolean ecx_iserror(ecx_contextt *context) +{ + return (context->elist->head != context->elist->tail); +} + +/** Report packet error + * + * @param[in] context = context struct + * @param[in] Slave = Slave number + * @param[in] Index = Index that generated error + * @param[in] SubIdx = Subindex that generated error + * @param[in] ErrorCode = Error code + */ +void ecx_packeterror(ecx_contextt *context, uint16 Slave, uint16 Index, uint8 SubIdx, uint16 ErrorCode) +{ + ec_errort Ec; + + memset(&Ec, 0, sizeof(Ec)); + Ec.Time = osal_current_time(); + Ec.Slave = Slave; + Ec.Index = Index; + Ec.SubIdx = SubIdx; + *(context->ecaterror) = TRUE; + Ec.Etype = EC_ERR_TYPE_PACKET_ERROR; + Ec.ErrorCode = ErrorCode; + ecx_pusherror(context, &Ec); +} + +/** Report Mailbox Error + * + * @param[in] context = context struct + * @param[in] Slave = Slave number + * @param[in] Detail = Following EtherCAT specification + */ +static void ecx_mbxerror(ecx_contextt *context, uint16 Slave,uint16 Detail) +{ + ec_errort Ec; + + memset(&Ec, 0, sizeof(Ec)); + Ec.Time = osal_current_time(); + Ec.Slave = Slave; + Ec.Index = 0; + Ec.SubIdx = 0; + Ec.Etype = EC_ERR_TYPE_MBX_ERROR; + Ec.ErrorCode = Detail; + ecx_pusherror(context, &Ec); +} + +/** Report Mailbox Emergency Error + * + * @param[in] context = context struct + * @param[in] Slave = Slave number + * @param[in] ErrorCode = Following EtherCAT specification + * @param[in] ErrorReg + * @param[in] b1 + * @param[in] w1 + * @param[in] w2 + */ +static void ecx_mbxemergencyerror(ecx_contextt *context, uint16 Slave,uint16 ErrorCode,uint16 ErrorReg, + uint8 b1, uint16 w1, uint16 w2) +{ + ec_errort Ec; + + memset(&Ec, 0, sizeof(Ec)); + Ec.Time = osal_current_time(); + Ec.Slave = Slave; + Ec.Index = 0; + Ec.SubIdx = 0; + Ec.Etype = EC_ERR_TYPE_EMERGENCY; + Ec.ErrorCode = ErrorCode; + Ec.ErrorReg = (uint8)ErrorReg; + Ec.b1 = b1; + Ec.w1 = w1; + Ec.w2 = w2; + ecx_pusherror(context, &Ec); +} + +/** Initialise lib in single NIC mode + * @param[in] context = context struct + * @param[in] ifname = Dev name, f.e. "eth0" + * @return >0 if OK + */ +int ecx_init(ecx_contextt *context, const char * ifname) +{ + return ecx_setupnic(context->port, ifname, FALSE); +} + +/** Initialise lib in redundant NIC mode + * @param[in] context = context struct + * @param[in] redport = pointer to redport, redundant port data + * @param[in] ifname = Primary Dev name, f.e. "eth0" + * @param[in] if2name = Secondary Dev name, f.e. "eth1" + * @return >0 if OK + */ +int ecx_init_redundant(ecx_contextt *context, ecx_redportt *redport, const char *ifname, char *if2name) +{ + int rval, zbuf; + ec_etherheadert *ehp; + + context->port->redport = redport; + ecx_setupnic(context->port, ifname, FALSE); + rval = ecx_setupnic(context->port, if2name, TRUE); + /* prepare "dummy" BRD tx frame for redundant operation */ + ehp = (ec_etherheadert *)&(context->port->txbuf2); + ehp->sa1 = oshw_htons(secMAC[0]); + zbuf = 0; + ecx_setupdatagram(context->port, &(context->port->txbuf2), EC_CMD_BRD, 0, 0x0000, 0x0000, 2, &zbuf); + context->port->txbuflength2 = ETH_HEADERSIZE + EC_HEADERSIZE + EC_WKCSIZE + 2; + + return rval; +} + +/** Close lib. + * @param[in] context = context struct + */ +void ecx_close(ecx_contextt *context) +{ + ecx_closenic(context->port); +}; + +/** Read one byte from slave EEPROM via cache. + * If the cache location is empty then a read request is made to the slave. + * Depending on the slave capabilities the request is 4 or 8 bytes. + * @param[in] context = context struct + * @param[in] slave = slave number + * @param[in] address = eeprom address in bytes (slave uses words) + * @return requested byte, if not available then 0xff + */ +uint8 ecx_siigetbyte(ecx_contextt *context, uint16 slave, uint16 address) +{ + uint16 configadr, eadr; + uint64 edat64; + uint32 edat32; + uint16 mapw, mapb; + int lp,cnt; + uint8 retval; + + retval = 0xff; + if (slave != context->esislave) /* not the same slave? */ + { + memset(context->esimap, 0x00, EC_MAXEEPBITMAP * sizeof(uint32)); /* clear esibuf cache map */ + context->esislave = slave; + } + if (address < EC_MAXEEPBUF) + { + mapw = address >> 5; + mapb = (uint16)(address - (mapw << 5)); + if (context->esimap[mapw] & (1U << mapb)) + { + /* byte is already in buffer */ + retval = context->esibuf[address]; + } + else + { + /* byte is not in buffer, put it there */ + configadr = context->slavelist[slave].configadr; + ecx_eeprom2master(context, slave); /* set eeprom control to master */ + eadr = address >> 1; + edat64 = ecx_readeepromFP (context, configadr, eadr, EC_TIMEOUTEEP); + /* 8 byte response */ + if (context->slavelist[slave].eep_8byte) + { + put_unaligned64(edat64, &(context->esibuf[eadr << 1])); + cnt = 8; + } + /* 4 byte response */ + else + { + edat32 = (uint32)edat64; + put_unaligned32(edat32, &(context->esibuf[eadr << 1])); + cnt = 4; + } + /* find bitmap location */ + mapw = eadr >> 4; + mapb = (uint16)((eadr << 1) - (mapw << 5)); + for(lp = 0 ; lp < cnt ; lp++) + { + /* set bitmap for each byte that is read */ + context->esimap[mapw] |= (1U << mapb); + mapb++; + if (mapb > 31) + { + mapb = 0; + mapw++; + } + } + retval = context->esibuf[address]; + } + } + + return retval; +} + +/** Find SII section header in slave EEPROM. + * @param[in] context = context struct + * @param[in] slave = slave number + * @param[in] cat = section category + * @return byte address of section at section length entry, if not available then 0 + */ +int16 ecx_siifind(ecx_contextt *context, uint16 slave, uint16 cat) +{ + int16 a; + uint16 p; + uint8 eectl = context->slavelist[slave].eep_pdi; + + a = ECT_SII_START << 1; + /* read first SII section category */ + p = ecx_siigetbyte(context, slave, a++); + p += (ecx_siigetbyte(context, slave, a++) << 8); + /* traverse SII while category is not found and not EOF */ + while ((p != cat) && (p != 0xffff)) + { + /* read section length */ + p = ecx_siigetbyte(context, slave, a++); + p += (ecx_siigetbyte(context, slave, a++) << 8); + /* locate next section category */ + a += p << 1; + /* read section category */ + p = ecx_siigetbyte(context, slave, a++); + p += (ecx_siigetbyte(context, slave, a++) << 8); + } + if (p != cat) + { + a = 0; + } + if (eectl) + { + ecx_eeprom2pdi(context, slave); /* if eeprom control was previously pdi then restore */ + } + + return a; +} + +/** Get string from SII string section in slave EEPROM. + * @param[in] context = context struct + * @param[out] str = requested string, 0x00 if not found + * @param[in] slave = slave number + * @param[in] Sn = string number + */ +void ecx_siistring(ecx_contextt *context, char *str, uint16 slave, uint16 Sn) +{ + uint16 a,i,j,l,n,ba; + char *ptr; + uint8 eectl = context->slavelist[slave].eep_pdi; + + ptr = str; + a = ecx_siifind (context, slave, ECT_SII_STRING); /* find string section */ + if (a > 0) + { + ba = a + 2; /* skip SII section header */ + n = ecx_siigetbyte(context, slave, ba++); /* read number of strings in section */ + if (Sn <= n) /* is req string available? */ + { + for (i = 1; i <= Sn; i++) /* walk through strings */ + { + l = ecx_siigetbyte(context, slave, ba++); /* length of this string */ + if (i < Sn) + { + ba += l; + } + else + { + ptr = str; + for (j = 1; j <= l; j++) /* copy one string */ + { + if(j <= EC_MAXNAME) + { + *ptr = (char)ecx_siigetbyte(context, slave, ba++); + ptr++; + } + else + { + ba++; + } + } + } + } + *ptr = 0; /* add zero terminator */ + } + else + { + ptr = str; + *ptr = 0; /* empty string */ + } + } + if (eectl) + { + ecx_eeprom2pdi(context, slave); /* if eeprom control was previously pdi then restore */ + } +} + +/** Get FMMU data from SII FMMU section in slave EEPROM. + * @param[in] context = context struct + * @param[in] slave = slave number + * @param[out] FMMU = FMMU struct from SII, max. 4 FMMU's + * @return number of FMMU's defined in section + */ +uint16 ecx_siiFMMU(ecx_contextt *context, uint16 slave, ec_eepromFMMUt* FMMU) +{ + uint16 a; + uint8 eectl = context->slavelist[slave].eep_pdi; + + FMMU->nFMMU = 0; + FMMU->FMMU0 = 0; + FMMU->FMMU1 = 0; + FMMU->FMMU2 = 0; + FMMU->FMMU3 = 0; + FMMU->Startpos = ecx_siifind(context, slave, ECT_SII_FMMU); + + if (FMMU->Startpos > 0) + { + a = FMMU->Startpos; + FMMU->nFMMU = ecx_siigetbyte(context, slave, a++); + FMMU->nFMMU += (ecx_siigetbyte(context, slave, a++) << 8); + FMMU->nFMMU *= 2; + FMMU->FMMU0 = ecx_siigetbyte(context, slave, a++); + FMMU->FMMU1 = ecx_siigetbyte(context, slave, a++); + if (FMMU->nFMMU > 2) + { + FMMU->FMMU2 = ecx_siigetbyte(context, slave, a++); + FMMU->FMMU3 = ecx_siigetbyte(context, slave, a++); + } + } + if (eectl) + { + ecx_eeprom2pdi(context, slave); /* if eeprom control was previously pdi then restore */ + } + + return FMMU->nFMMU; +} + +/** Get SM data from SII SM section in slave EEPROM. + * @param[in] context = context struct + * @param[in] slave = slave number + * @param[out] SM = first SM struct from SII + * @return number of SM's defined in section + */ +uint16 ecx_siiSM(ecx_contextt *context, uint16 slave, ec_eepromSMt* SM) +{ + uint16 a,w; + uint8 eectl = context->slavelist[slave].eep_pdi; + + SM->nSM = 0; + SM->Startpos = ecx_siifind(context, slave, ECT_SII_SM); + if (SM->Startpos > 0) + { + a = SM->Startpos; + w = ecx_siigetbyte(context, slave, a++); + w += (ecx_siigetbyte(context, slave, a++) << 8); + SM->nSM = (uint8)(w / 4); + SM->PhStart = ecx_siigetbyte(context, slave, a++); + SM->PhStart += (ecx_siigetbyte(context, slave, a++) << 8); + SM->Plength = ecx_siigetbyte(context, slave, a++); + SM->Plength += (ecx_siigetbyte(context, slave, a++) << 8); + SM->Creg = ecx_siigetbyte(context, slave, a++); + SM->Sreg = ecx_siigetbyte(context, slave, a++); + SM->Activate = ecx_siigetbyte(context, slave, a++); + SM->PDIctrl = ecx_siigetbyte(context, slave, a++); + } + if (eectl) + { + ecx_eeprom2pdi(context, slave); /* if eeprom control was previously pdi then restore */ + } + + return SM->nSM; +} + +/** Get next SM data from SII SM section in slave EEPROM. + * @param[in] context = context struct + * @param[in] slave = slave number + * @param[out] SM = first SM struct from SII + * @param[in] n = SM number + * @return >0 if OK + */ +uint16 ecx_siiSMnext(ecx_contextt *context, uint16 slave, ec_eepromSMt* SM, uint16 n) +{ + uint16 a; + uint16 retVal = 0; + uint8 eectl = context->slavelist[slave].eep_pdi; + + if (n < SM->nSM) + { + a = SM->Startpos + 2 + (n * 8); + SM->PhStart = ecx_siigetbyte(context, slave, a++); + SM->PhStart += (ecx_siigetbyte(context, slave, a++) << 8); + SM->Plength = ecx_siigetbyte(context, slave, a++); + SM->Plength += (ecx_siigetbyte(context, slave, a++) << 8); + SM->Creg = ecx_siigetbyte(context, slave, a++); + SM->Sreg = ecx_siigetbyte(context, slave, a++); + SM->Activate = ecx_siigetbyte(context, slave, a++); + SM->PDIctrl = ecx_siigetbyte(context, slave, a++); + retVal = 1; + } + if (eectl) + { + ecx_eeprom2pdi(context, slave); /* if eeprom control was previously pdi then restore */ + } + + return retVal; +} + +/** Get PDO data from SII PDO section in slave EEPROM. + * @param[in] context = context struct + * @param[in] slave = slave number + * @param[out] PDO = PDO struct from SII + * @param[in] t = 0=RXPDO 1=TXPDO + * @return mapping size in bits of PDO + */ +uint32 ecx_siiPDO(ecx_contextt *context, uint16 slave, ec_eepromPDOt* PDO, uint8 t) +{ + uint16 a , w, c, e, er, Size; + uint8 eectl = context->slavelist[slave].eep_pdi; + + Size = 0; + PDO->nPDO = 0; + PDO->Length = 0; + PDO->Index[1] = 0; + for (c = 0 ; c < EC_MAXSM ; c++) PDO->SMbitsize[c] = 0; + if (t > 1) + t = 1; + PDO->Startpos = ecx_siifind(context, slave, ECT_SII_PDO + t); + if (PDO->Startpos > 0) + { + a = PDO->Startpos; + w = ecx_siigetbyte(context, slave, a++); + w += (ecx_siigetbyte(context, slave, a++) << 8); + PDO->Length = w; + c = 1; + /* traverse through all PDOs */ + do + { + PDO->nPDO++; + PDO->Index[PDO->nPDO] = ecx_siigetbyte(context, slave, a++); + PDO->Index[PDO->nPDO] += (ecx_siigetbyte(context, slave, a++) << 8); + PDO->BitSize[PDO->nPDO] = 0; + c++; + e = ecx_siigetbyte(context, slave, a++); + PDO->SyncM[PDO->nPDO] = ecx_siigetbyte(context, slave, a++); + a += 4; + c += 2; + if (PDO->SyncM[PDO->nPDO] < EC_MAXSM) /* active and in range SM? */ + { + /* read all entries defined in PDO */ + for (er = 1; er <= e; er++) + { + c += 4; + a += 5; + PDO->BitSize[PDO->nPDO] += ecx_siigetbyte(context, slave, a++); + a += 2; + } + PDO->SMbitsize[ PDO->SyncM[PDO->nPDO] ] += PDO->BitSize[PDO->nPDO]; + Size += PDO->BitSize[PDO->nPDO]; + c++; + } + else /* PDO deactivated because SM is 0xff or > EC_MAXSM */ + { + c += 4 * e; + a += 8 * e; + c++; + } + if (PDO->nPDO >= (EC_MAXEEPDO - 1)) + { + c = PDO->Length; /* limit number of PDO entries in buffer */ + } + } + while (c < PDO->Length); + } + if (eectl) + { + ecx_eeprom2pdi(context, slave); /* if eeprom control was previously pdi then restore */ + } + + return (Size); +} + +#define MAX_FPRD_MULTI 64 + +int ecx_FPRD_multi(ecx_contextt *context, int n, uint16 *configlst, ec_alstatust *slstatlst, int timeout) +{ + int wkc; + uint8 idx; + ecx_portt *port; + uint16 sldatapos[MAX_FPRD_MULTI]; + int slcnt; + + port = context->port; + idx = ecx_getindex(port); + slcnt = 0; + ecx_setupdatagram(port, &(port->txbuf[idx]), EC_CMD_FPRD, idx, + *(configlst + slcnt), ECT_REG_ALSTAT, sizeof(ec_alstatust), slstatlst + slcnt); + sldatapos[slcnt] = EC_HEADERSIZE; + while(++slcnt < (n - 1)) + { + sldatapos[slcnt] = ecx_adddatagram(port, &(port->txbuf[idx]), EC_CMD_FPRD, idx, TRUE, + *(configlst + slcnt), ECT_REG_ALSTAT, sizeof(ec_alstatust), slstatlst + slcnt); + } + if(slcnt < n) + { + sldatapos[slcnt] = ecx_adddatagram(port, &(port->txbuf[idx]), EC_CMD_FPRD, idx, FALSE, + *(configlst + slcnt), ECT_REG_ALSTAT, sizeof(ec_alstatust), slstatlst + slcnt); + } + wkc = ecx_srconfirm(port, idx, timeout); + if (wkc >= 0) + { + for(slcnt = 0 ; slcnt < n ; slcnt++) + { + memcpy(slstatlst + slcnt, &(port->rxbuf[idx][sldatapos[slcnt]]), sizeof(ec_alstatust)); + } + } + ecx_setbufstat(port, idx, EC_BUF_EMPTY); + return wkc; +} + +/** Read all slave states in ec_slave. + * @warning The BOOT state is actually higher than INIT and PRE_OP (see state representation) + * @param[in] context = context struct + * @return lowest state found + */ +int ecx_readstate(ecx_contextt *context) +{ + uint16 slave, fslave, lslave, configadr, lowest, rval, bitwisestate; + ec_alstatust sl[MAX_FPRD_MULTI]; + uint16 slca[MAX_FPRD_MULTI]; + boolean noerrorflag, allslavessamestate; + boolean allslavespresent = FALSE; + int wkc; + + /* Try to establish the state of all slaves sending only one broadcast datagram. + * This way a number of datagrams equal to the number of slaves will be sent only if needed.*/ + rval = 0; + wkc = ecx_BRD(context->port, 0, ECT_REG_ALSTAT, sizeof(rval), &rval, EC_TIMEOUTRET); + + if(wkc >= *(context->slavecount)) + { + allslavespresent = TRUE; + } + + rval = etohs(rval); + bitwisestate = (rval & 0x0f); + + if ((rval & EC_STATE_ERROR) == 0) + { + noerrorflag = TRUE; + context->slavelist[0].ALstatuscode = 0; + } + else + { + noerrorflag = FALSE; + } + + switch (bitwisestate) + { + /* Note: BOOT State collides with PRE_OP | INIT and cannot be used here */ + case EC_STATE_INIT: + case EC_STATE_PRE_OP: + case EC_STATE_SAFE_OP: + case EC_STATE_OPERATIONAL: + allslavessamestate = TRUE; + context->slavelist[0].state = bitwisestate; + break; + default: + allslavessamestate = FALSE; + break; + } + + if (noerrorflag && allslavessamestate && allslavespresent) + { + /* No slave has toggled the error flag so the alstatuscode + * (even if different from 0) should be ignored and + * the slaves have reached the same state so the internal state + * can be updated without sending any datagram. */ + for (slave = 1; slave <= *(context->slavecount); slave++) + { + context->slavelist[slave].ALstatuscode = 0x0000; + context->slavelist[slave].state = bitwisestate; + } + lowest = bitwisestate; + } + else + { + /* Not all slaves have the same state or at least one is in error so one datagram per slave + * is needed. */ + context->slavelist[0].ALstatuscode = 0; + lowest = 0xff; + fslave = 1; + do + { + lslave = (uint16)*(context->slavecount); + if ((lslave - fslave) >= MAX_FPRD_MULTI) + { + lslave = fslave + MAX_FPRD_MULTI - 1; + } + for (slave = fslave; slave <= lslave; slave++) + { + const ec_alstatust zero = { 0, 0, 0 }; + + configadr = context->slavelist[slave].configadr; + slca[slave - fslave] = configadr; + sl[slave - fslave] = zero; + } + ecx_FPRD_multi(context, (lslave - fslave) + 1, &(slca[0]), &(sl[0]), EC_TIMEOUTRET3); + for (slave = fslave; slave <= lslave; slave++) + { + configadr = context->slavelist[slave].configadr; + rval = etohs(sl[slave - fslave].alstatus); + context->slavelist[slave].ALstatuscode = etohs(sl[slave - fslave].alstatuscode); + if ((rval & 0xf) < lowest) + { + lowest = (rval & 0xf); + } + context->slavelist[slave].state = rval; + context->slavelist[0].ALstatuscode |= context->slavelist[slave].ALstatuscode; + } + fslave = lslave + 1; + } while (lslave < *(context->slavecount)); + context->slavelist[0].state = lowest; + } + + return lowest; +} + +/** Write slave state, if slave = 0 then write to all slaves. + * The function does not check if the actual state is changed. + * @param[in] context = context struct + * @param[in] slave = Slave number, 0 = master + * @return Workcounter or EC_NOFRAME + */ +int ecx_writestate(ecx_contextt *context, uint16 slave) +{ + int ret; + uint16 configadr, slstate; + + if (slave == 0) + { + slstate = htoes(context->slavelist[slave].state); + ret = ecx_BWR(context->port, 0, ECT_REG_ALCTL, sizeof(slstate), + &slstate, EC_TIMEOUTRET3); + } + else + { + configadr = context->slavelist[slave].configadr; + + ret = ecx_FPWRw(context->port, configadr, ECT_REG_ALCTL, + htoes(context->slavelist[slave].state), EC_TIMEOUTRET3); + } + return ret; +} + +/** Check actual slave state. + * This is a blocking function. + * To refresh the state of all slaves ecx_readstate() should be called + * @warning If this is used for slave 0 (=all slaves), the state of all slaves is read by an bitwise OR operation. + * The returned value is also the bitwise OR state of all slaves. + * This has some implications for the BOOT state. The Boot state representation collides with INIT | PRE_OP so this + * function cannot be used for slave = 0 and reqstate = EC_STATE_BOOT and also, if the returned state is BOOT, some + * slaves might actually be in INIT and PRE_OP and not in BOOT. + * @param[in] context = context struct + * @param[in] slave = Slave number, 0 = all slaves (only the "slavelist[0].state" is refreshed) + * @param[in] reqstate = Requested state + * @param[in] timeout = Timeout value in us + * @return Requested state, or found state after timeout. + */ +uint16 ecx_statecheck(ecx_contextt *context, uint16 slave, uint16 reqstate, int timeout) +{ + uint16 configadr, state, rval; + ec_alstatust slstat; + osal_timert timer; + + if ( slave > *(context->slavecount) ) + { + return 0; + } + osal_timer_start(&timer, timeout); + configadr = context->slavelist[slave].configadr; + do + { + if (slave < 1) + { + rval = 0; + ecx_BRD(context->port, 0, ECT_REG_ALSTAT, sizeof(rval), &rval , EC_TIMEOUTRET); + rval = etohs(rval); + } + else + { + slstat.alstatus = 0; + slstat.alstatuscode = 0; + ecx_FPRD(context->port, configadr, ECT_REG_ALSTAT, sizeof(slstat), &slstat, EC_TIMEOUTRET); + rval = etohs(slstat.alstatus); + context->slavelist[slave].ALstatuscode = etohs(slstat.alstatuscode); + } + state = rval & 0x000f; /* read slave status */ + if (state != reqstate) + { + osal_usleep(1000); + } + } + while ((state != reqstate) && (osal_timer_is_expired(&timer) == FALSE)); + context->slavelist[slave].state = rval; + + return state; +} + +/** Get index of next mailbox counter value. + * Used for Mailbox Link Layer. + * @param[in] cnt = Mailbox counter value [0..7] + * @return next mailbox counter value + */ +uint8 ec_nextmbxcnt(uint8 cnt) +{ + cnt++; + if (cnt > 7) + { + cnt = 1; /* wrap around to 1, not 0 */ + } + + return cnt; +} + +/** Clear mailbox buffer. + * @param[out] Mbx = Mailbox buffer to clear + */ +void ec_clearmbx(ec_mbxbuft *Mbx) +{ + memset(Mbx, 0x00, EC_MAXMBX); +} + +/** Check if IN mailbox of slave is empty. + * @param[in] context = context struct + * @param[in] slave = Slave number + * @param[in] timeout = Timeout in us + * @return >0 is success + */ +int ecx_mbxempty(ecx_contextt *context, uint16 slave, int timeout) +{ + uint16 configadr; + uint8 SMstat; + int wkc; + osal_timert timer; + + osal_timer_start(&timer, timeout); + configadr = context->slavelist[slave].configadr; + do + { + SMstat = 0; + wkc = ecx_FPRD(context->port, configadr, ECT_REG_SM0STAT, sizeof(SMstat), &SMstat, EC_TIMEOUTRET); + SMstat = etohs(SMstat); + if (((SMstat & 0x08) != 0) && (timeout > EC_LOCALDELAY)) + { + osal_usleep(EC_LOCALDELAY); + } + } + while (((wkc <= 0) || ((SMstat & 0x08) != 0)) && (osal_timer_is_expired(&timer) == FALSE)); + + if ((wkc > 0) && ((SMstat & 0x08) == 0)) + { + return 1; + } + + return 0; +} + +/** Write IN mailbox to slave. + * @param[in] context = context struct + * @param[in] slave = Slave number + * @param[out] mbx = Mailbox data + * @param[in] timeout = Timeout in us + * @return Work counter (>0 is success) + */ +int ecx_mbxsend(ecx_contextt *context, uint16 slave,ec_mbxbuft *mbx, int timeout) +{ + uint16 mbxwo,mbxl,configadr; + int wkc; + + wkc = 0; + configadr = context->slavelist[slave].configadr; + mbxl = context->slavelist[slave].mbx_l; + if ((mbxl > 0) && (mbxl <= EC_MAXMBX)) + { + if (ecx_mbxempty(context, slave, timeout)) + { + mbxwo = context->slavelist[slave].mbx_wo; + /* write slave in mailbox */ + wkc = ecx_FPWR(context->port, configadr, mbxwo, mbxl, mbx, EC_TIMEOUTRET3); + } + else + { + wkc = 0; + } + } + + return wkc; +} + +/** Read OUT mailbox from slave. + * Supports Mailbox Link Layer with repeat requests. + * @param[in] context = context struct + * @param[in] slave = Slave number + * @param[out] mbx = Mailbox data + * @param[in] timeout = Timeout in us + * @return Work counter (>0 is success) + */ +int ecx_mbxreceive(ecx_contextt *context, uint16 slave, ec_mbxbuft *mbx, int timeout) +{ + uint16 mbxro,mbxl,configadr; + int wkc=0; + int wkc2; + uint16 SMstat; + uint8 SMcontr; + ec_mbxheadert *mbxh; + ec_emcyt *EMp; + ec_mbxerrort *MBXEp; + + configadr = context->slavelist[slave].configadr; + mbxl = context->slavelist[slave].mbx_rl; + if ((mbxl > 0) && (mbxl <= EC_MAXMBX)) + { + osal_timert timer; + + osal_timer_start(&timer, timeout); + wkc = 0; + do /* wait for read mailbox available */ + { + SMstat = 0; + wkc = ecx_FPRD(context->port, configadr, ECT_REG_SM1STAT, sizeof(SMstat), &SMstat, EC_TIMEOUTRET); + SMstat = etohs(SMstat); + if (((SMstat & 0x08) == 0) && (timeout > EC_LOCALDELAY)) + { + osal_usleep(EC_LOCALDELAY); + } + } + while (((wkc <= 0) || ((SMstat & 0x08) == 0)) && (osal_timer_is_expired(&timer) == FALSE)); + + if ((wkc > 0) && ((SMstat & 0x08) > 0)) /* read mailbox available ? */ + { + mbxro = context->slavelist[slave].mbx_ro; + mbxh = (ec_mbxheadert *)mbx; + do + { + wkc = ecx_FPRD(context->port, configadr, mbxro, mbxl, mbx, EC_TIMEOUTRET); /* get mailbox */ + if ((wkc > 0) && ((mbxh->mbxtype & 0x0f) == 0x00)) /* Mailbox error response? */ + { + MBXEp = (ec_mbxerrort *)mbx; + ecx_mbxerror(context, slave, etohs(MBXEp->Detail)); + wkc = 0; /* prevent emergency to cascade up, it is already handled. */ + } + else if ((wkc > 0) && ((mbxh->mbxtype & 0x0f) == ECT_MBXT_COE)) /* CoE response? */ + { + EMp = (ec_emcyt *)mbx; + if ((etohs(EMp->CANOpen) >> 12) == 0x01) /* Emergency request? */ + { + ecx_mbxemergencyerror(context, slave, etohs(EMp->ErrorCode), EMp->ErrorReg, + EMp->bData, etohs(EMp->w1), etohs(EMp->w2)); + wkc = 0; /* prevent emergency to cascade up, it is already handled. */ + } + } + else if ((wkc > 0) && ((mbxh->mbxtype & 0x0f) == ECT_MBXT_EOE)) /* EoE response? */ + { + ec_EOEt * eoembx = (ec_EOEt *)mbx; + uint16 frameinfo1 = etohs(eoembx->frameinfo1); + /* All non fragment data frame types are expected to be handled by + * slave send/receive API if the EoE hook is set + */ + if (EOE_HDR_FRAME_TYPE_GET(frameinfo1) == EOE_FRAG_DATA) + { + if (context->EOEhook) + { + if (context->EOEhook(context, slave, eoembx) > 0) + { + /* Fragment handled by EoE hook */ + wkc = 0; + } + } + } + } + else + { + if (wkc <= 0) /* read mailbox lost */ + { + SMstat ^= 0x0200; /* toggle repeat request */ + SMstat = htoes(SMstat); + wkc2 = ecx_FPWR(context->port, configadr, ECT_REG_SM1STAT, sizeof(SMstat), &SMstat, EC_TIMEOUTRET); + SMstat = etohs(SMstat); + do /* wait for toggle ack */ + { + wkc2 = ecx_FPRD(context->port, configadr, ECT_REG_SM1CONTR, sizeof(SMcontr), &SMcontr, EC_TIMEOUTRET); + } while (((wkc2 <= 0) || ((SMcontr & 0x02) != (HI_BYTE(SMstat) & 0x02))) && (osal_timer_is_expired(&timer) == FALSE)); + do /* wait for read mailbox available */ + { + wkc2 = ecx_FPRD(context->port, configadr, ECT_REG_SM1STAT, sizeof(SMstat), &SMstat, EC_TIMEOUTRET); + SMstat = etohs(SMstat); + if (((SMstat & 0x08) == 0) && (timeout > EC_LOCALDELAY)) + { + osal_usleep(EC_LOCALDELAY); + } + } while (((wkc2 <= 0) || ((SMstat & 0x08) == 0)) && (osal_timer_is_expired(&timer) == FALSE)); + } + } + } while ((wkc <= 0) && (osal_timer_is_expired(&timer) == FALSE)); /* if WKC<=0 repeat */ + } + else /* no read mailbox available */ + { + if (wkc > 0) + wkc = EC_TIMEOUT; + } + } + + return wkc; +} + +/** Dump complete EEPROM data from slave in buffer. + * @param[in] context = context struct + * @param[in] slave = Slave number + * @param[out] esibuf = EEPROM data buffer, make sure it is big enough. + */ +void ecx_esidump(ecx_contextt *context, uint16 slave, uint8 *esibuf) +{ + uint16 configadr, address, incr; + uint64 *p64; + uint16 *p16; + uint64 edat; + uint8 eectl = context->slavelist[slave].eep_pdi; + + ecx_eeprom2master(context, slave); /* set eeprom control to master */ + configadr = context->slavelist[slave].configadr; + address = ECT_SII_START; + p16=(uint16*)esibuf; + if (context->slavelist[slave].eep_8byte) + { + incr = 4; + } + else + { + incr = 2; + } + do + { + edat = ecx_readeepromFP(context, configadr, address, EC_TIMEOUTEEP); + p64 = (uint64*)p16; + *p64 = edat; + p16 += incr; + address += incr; + } while ((address <= (EC_MAXEEPBUF >> 1)) && ((uint32)edat != 0xffffffff)); + + if (eectl) + { + ecx_eeprom2pdi(context, slave); /* if eeprom control was previously pdi then restore */ + } +} + +/** Read EEPROM from slave bypassing cache. + * @param[in] context = context struct + * @param[in] slave = Slave number + * @param[in] eeproma = (WORD) Address in the EEPROM + * @param[in] timeout = Timeout in us. + * @return EEPROM data 32bit + */ +uint32 ecx_readeeprom(ecx_contextt *context, uint16 slave, uint16 eeproma, int timeout) +{ + uint16 configadr; + + ecx_eeprom2master(context, slave); /* set eeprom control to master */ + configadr = context->slavelist[slave].configadr; + + return ((uint32)ecx_readeepromFP(context, configadr, eeproma, timeout)); +} + +/** Write EEPROM to slave bypassing cache. + * @param[in] context = context struct + * @param[in] slave = Slave number + * @param[in] eeproma = (WORD) Address in the EEPROM + * @param[in] data = 16bit data + * @param[in] timeout = Timeout in us. + * @return >0 if OK + */ +int ecx_writeeeprom(ecx_contextt *context, uint16 slave, uint16 eeproma, uint16 data, int timeout) +{ + uint16 configadr; + + ecx_eeprom2master(context, slave); /* set eeprom control to master */ + configadr = context->slavelist[slave].configadr; + return (ecx_writeeepromFP(context, configadr, eeproma, data, timeout)); +} + +/** Set eeprom control to master. Only if set to PDI. + * @param[in] context = context struct + * @param[in] slave = Slave number + * @return >0 if OK + */ +int ecx_eeprom2master(ecx_contextt *context, uint16 slave) +{ + int wkc = 1, cnt = 0; + uint16 configadr; + uint8 eepctl; + + if ( context->slavelist[slave].eep_pdi ) + { + configadr = context->slavelist[slave].configadr; + eepctl = 2; + do + { + wkc = ecx_FPWR(context->port, configadr, ECT_REG_EEPCFG, sizeof(eepctl), &eepctl , EC_TIMEOUTRET); /* force Eeprom from PDI */ + } + while ((wkc <= 0) && (cnt++ < EC_DEFAULTRETRIES)); + eepctl = 0; + cnt = 0; + do + { + wkc = ecx_FPWR(context->port, configadr, ECT_REG_EEPCFG, sizeof(eepctl), &eepctl , EC_TIMEOUTRET); /* set Eeprom to master */ + } + while ((wkc <= 0) && (cnt++ < EC_DEFAULTRETRIES)); + context->slavelist[slave].eep_pdi = 0; + } + + return wkc; +} + +/** Set eeprom control to PDI. Only if set to master. + * @param[in] context = context struct + * @param[in] slave = Slave number + * @return >0 if OK + */ +int ecx_eeprom2pdi(ecx_contextt *context, uint16 slave) +{ + int wkc = 1, cnt = 0; + uint16 configadr; + uint8 eepctl; + + if ( !context->slavelist[slave].eep_pdi ) + { + configadr = context->slavelist[slave].configadr; + eepctl = 1; + do + { + wkc = ecx_FPWR(context->port, configadr, ECT_REG_EEPCFG, sizeof(eepctl), &eepctl , EC_TIMEOUTRET); /* set Eeprom to PDI */ + } + while ((wkc <= 0) && (cnt++ < EC_DEFAULTRETRIES)); + context->slavelist[slave].eep_pdi = 1; + } + + return wkc; +} + +uint16 ecx_eeprom_waitnotbusyAP(ecx_contextt *context, uint16 aiadr,uint16 *estat, int timeout) +{ + int wkc, cnt = 0; + uint16 retval = 0; + osal_timert timer; + + osal_timer_start(&timer, timeout); + do + { + if (cnt++) + { + osal_usleep(EC_LOCALDELAY); + } + *estat = 0; + wkc=ecx_APRD(context->port, aiadr, ECT_REG_EEPSTAT, sizeof(*estat), estat, EC_TIMEOUTRET); + *estat = etohs(*estat); + } + while (((wkc <= 0) || ((*estat & EC_ESTAT_BUSY) > 0)) && (osal_timer_is_expired(&timer) == FALSE)); /* wait for eeprom ready */ + if ((*estat & EC_ESTAT_BUSY) == 0) + { + retval = 1; + } + + return retval; +} + +/** Read EEPROM from slave bypassing cache. APRD method. + * @param[in] context = context struct + * @param[in] aiadr = auto increment address of slave + * @param[in] eeproma = (WORD) Address in the EEPROM + * @param[in] timeout = Timeout in us. + * @return EEPROM data 64bit or 32bit + */ +uint64 ecx_readeepromAP(ecx_contextt *context, uint16 aiadr, uint16 eeproma, int timeout) +{ + uint16 estat; + uint32 edat32; + uint64 edat64; + ec_eepromt ed; + int wkc, cnt, nackcnt = 0; + + edat64 = 0; + edat32 = 0; + if (ecx_eeprom_waitnotbusyAP(context, aiadr, &estat, timeout)) + { + if (estat & EC_ESTAT_EMASK) /* error bits are set */ + { + estat = htoes(EC_ECMD_NOP); /* clear error bits */ + wkc = ecx_APWR(context->port, aiadr, ECT_REG_EEPCTL, sizeof(estat), &estat, EC_TIMEOUTRET3); + } + + do + { + ed.comm = htoes(EC_ECMD_READ); + ed.addr = htoes(eeproma); + ed.d2 = 0x0000; + cnt = 0; + do + { + wkc = ecx_APWR(context->port, aiadr, ECT_REG_EEPCTL, sizeof(ed), &ed, EC_TIMEOUTRET); + } + while ((wkc <= 0) && (cnt++ < EC_DEFAULTRETRIES)); + if (wkc) + { + osal_usleep(EC_LOCALDELAY); + estat = 0x0000; + if (ecx_eeprom_waitnotbusyAP(context, aiadr, &estat, timeout)) + { + if (estat & EC_ESTAT_NACK) + { + nackcnt++; + osal_usleep(EC_LOCALDELAY * 5); + } + else + { + nackcnt = 0; + if (estat & EC_ESTAT_R64) + { + cnt = 0; + do + { + wkc = ecx_APRD(context->port, aiadr, ECT_REG_EEPDAT, sizeof(edat64), &edat64, EC_TIMEOUTRET); + } + while ((wkc <= 0) && (cnt++ < EC_DEFAULTRETRIES)); + } + else + { + cnt = 0; + do + { + wkc = ecx_APRD(context->port, aiadr, ECT_REG_EEPDAT, sizeof(edat32), &edat32, EC_TIMEOUTRET); + } + while ((wkc <= 0) && (cnt++ < EC_DEFAULTRETRIES)); + edat64=(uint64)edat32; + } + } + } + } + } + while ((nackcnt > 0) && (nackcnt < 3)); + } + + return edat64; +} + +/** Write EEPROM to slave bypassing cache. APWR method. + * @param[in] context = context struct + * @param[in] aiadr = configured address of slave + * @param[in] eeproma = (WORD) Address in the EEPROM + * @param[in] data = 16bit data + * @param[in] timeout = Timeout in us. + * @return >0 if OK + */ +int ecx_writeeepromAP(ecx_contextt *context, uint16 aiadr, uint16 eeproma, uint16 data, int timeout) +{ + uint16 estat; + ec_eepromt ed; + int wkc, rval = 0, cnt = 0, nackcnt = 0; + + if (ecx_eeprom_waitnotbusyAP(context, aiadr, &estat, timeout)) + { + if (estat & EC_ESTAT_EMASK) /* error bits are set */ + { + estat = htoes(EC_ECMD_NOP); /* clear error bits */ + wkc = ecx_APWR(context->port, aiadr, ECT_REG_EEPCTL, sizeof(estat), &estat, EC_TIMEOUTRET3); + } + do + { + cnt = 0; + do + { + wkc = ecx_APWR(context->port, aiadr, ECT_REG_EEPDAT, sizeof(data), &data, EC_TIMEOUTRET); + } + while ((wkc <= 0) && (cnt++ < EC_DEFAULTRETRIES)); + + ed.comm = EC_ECMD_WRITE; + ed.addr = eeproma; + ed.d2 = 0x0000; + cnt = 0; + do + { + wkc = ecx_APWR(context->port, aiadr, ECT_REG_EEPCTL, sizeof(ed), &ed, EC_TIMEOUTRET); + } + while ((wkc <= 0) && (cnt++ < EC_DEFAULTRETRIES)); + if (wkc) + { + osal_usleep(EC_LOCALDELAY * 2); + estat = 0x0000; + if (ecx_eeprom_waitnotbusyAP(context, aiadr, &estat, timeout)) + { + if (estat & EC_ESTAT_NACK) + { + nackcnt++; + osal_usleep(EC_LOCALDELAY * 5); + } + else + { + nackcnt = 0; + rval = 1; + } + } + } + + } + while ((nackcnt > 0) && (nackcnt < 3)); + } + + return rval; +} + +uint16 ecx_eeprom_waitnotbusyFP(ecx_contextt *context, uint16 configadr,uint16 *estat, int timeout) +{ + int wkc, cnt = 0; + uint16 retval = 0; + osal_timert timer; + + osal_timer_start(&timer, timeout); + do + { + if (cnt++) + { + osal_usleep(EC_LOCALDELAY); + } + *estat = 0; + wkc=ecx_FPRD(context->port, configadr, ECT_REG_EEPSTAT, sizeof(*estat), estat, EC_TIMEOUTRET); + *estat = etohs(*estat); + } + while (((wkc <= 0) || ((*estat & EC_ESTAT_BUSY) > 0)) && (osal_timer_is_expired(&timer) == FALSE)); /* wait for eeprom ready */ + if ((*estat & EC_ESTAT_BUSY) == 0) + { + retval = 1; + } + + return retval; +} + +/** Read EEPROM from slave bypassing cache. FPRD method. + * @param[in] context = context struct + * @param[in] configadr = configured address of slave + * @param[in] eeproma = (WORD) Address in the EEPROM + * @param[in] timeout = Timeout in us. + * @return EEPROM data 64bit or 32bit + */ +uint64 ecx_readeepromFP(ecx_contextt *context, uint16 configadr, uint16 eeproma, int timeout) +{ + uint16 estat; + uint32 edat32; + uint64 edat64; + ec_eepromt ed; + int wkc, cnt, nackcnt = 0; + + edat64 = 0; + edat32 = 0; + if (ecx_eeprom_waitnotbusyFP(context, configadr, &estat, timeout)) + { + if (estat & EC_ESTAT_EMASK) /* error bits are set */ + { + estat = htoes(EC_ECMD_NOP); /* clear error bits */ + wkc=ecx_FPWR(context->port, configadr, ECT_REG_EEPCTL, sizeof(estat), &estat, EC_TIMEOUTRET3); + } + + do + { + ed.comm = htoes(EC_ECMD_READ); + ed.addr = htoes(eeproma); + ed.d2 = 0x0000; + cnt = 0; + do + { + wkc=ecx_FPWR(context->port, configadr, ECT_REG_EEPCTL, sizeof(ed), &ed, EC_TIMEOUTRET); + } + while ((wkc <= 0) && (cnt++ < EC_DEFAULTRETRIES)); + if (wkc) + { + osal_usleep(EC_LOCALDELAY); + estat = 0x0000; + if (ecx_eeprom_waitnotbusyFP(context, configadr, &estat, timeout)) + { + if (estat & EC_ESTAT_NACK) + { + nackcnt++; + osal_usleep(EC_LOCALDELAY * 5); + } + else + { + nackcnt = 0; + if (estat & EC_ESTAT_R64) + { + cnt = 0; + do + { + wkc=ecx_FPRD(context->port, configadr, ECT_REG_EEPDAT, sizeof(edat64), &edat64, EC_TIMEOUTRET); + } + while ((wkc <= 0) && (cnt++ < EC_DEFAULTRETRIES)); + } + else + { + cnt = 0; + do + { + wkc=ecx_FPRD(context->port, configadr, ECT_REG_EEPDAT, sizeof(edat32), &edat32, EC_TIMEOUTRET); + } + while ((wkc <= 0) && (cnt++ < EC_DEFAULTRETRIES)); + edat64=(uint64)edat32; + } + } + } + } + } + while ((nackcnt > 0) && (nackcnt < 3)); + } + + return edat64; +} + +/** Write EEPROM to slave bypassing cache. FPWR method. + * @param[in] context = context struct + * @param[in] configadr = configured address of slave + * @param[in] eeproma = (WORD) Address in the EEPROM + * @param[in] data = 16bit data + * @param[in] timeout = Timeout in us. + * @return >0 if OK + */ +int ecx_writeeepromFP(ecx_contextt *context, uint16 configadr, uint16 eeproma, uint16 data, int timeout) +{ + uint16 estat; + ec_eepromt ed; + int wkc, rval = 0, cnt = 0, nackcnt = 0; + + if (ecx_eeprom_waitnotbusyFP(context, configadr, &estat, timeout)) + { + if (estat & EC_ESTAT_EMASK) /* error bits are set */ + { + estat = htoes(EC_ECMD_NOP); /* clear error bits */ + wkc = ecx_FPWR(context->port, configadr, ECT_REG_EEPCTL, sizeof(estat), &estat, EC_TIMEOUTRET3); + } + do + { + cnt = 0; + do + { + wkc = ecx_FPWR(context->port, configadr, ECT_REG_EEPDAT, sizeof(data), &data, EC_TIMEOUTRET); + } + while ((wkc <= 0) && (cnt++ < EC_DEFAULTRETRIES)); + ed.comm = EC_ECMD_WRITE; + ed.addr = eeproma; + ed.d2 = 0x0000; + cnt = 0; + do + { + wkc = ecx_FPWR(context->port, configadr, ECT_REG_EEPCTL, sizeof(ed), &ed, EC_TIMEOUTRET); + } + while ((wkc <= 0) && (cnt++ < EC_DEFAULTRETRIES)); + if (wkc) + { + osal_usleep(EC_LOCALDELAY * 2); + estat = 0x0000; + if (ecx_eeprom_waitnotbusyFP(context, configadr, &estat, timeout)) + { + if (estat & EC_ESTAT_NACK) + { + nackcnt++; + osal_usleep(EC_LOCALDELAY * 5); + } + else + { + nackcnt = 0; + rval = 1; + } + } + } + } + while ((nackcnt > 0) && (nackcnt < 3)); + } + + return rval; +} + +/** Read EEPROM from slave bypassing cache. + * Parallel read step 1, make request to slave. + * @param[in] context = context struct + * @param[in] slave = Slave number + * @param[in] eeproma = (WORD) Address in the EEPROM + */ +void ecx_readeeprom1(ecx_contextt *context, uint16 slave, uint16 eeproma) +{ + uint16 configadr, estat; + ec_eepromt ed; + int wkc, cnt = 0; + + ecx_eeprom2master(context, slave); /* set eeprom control to master */ + configadr = context->slavelist[slave].configadr; + if (ecx_eeprom_waitnotbusyFP(context, configadr, &estat, EC_TIMEOUTEEP)) + { + if (estat & EC_ESTAT_EMASK) /* error bits are set */ + { + estat = htoes(EC_ECMD_NOP); /* clear error bits */ + wkc = ecx_FPWR(context->port, configadr, ECT_REG_EEPCTL, sizeof(estat), &estat, EC_TIMEOUTRET3); + } + ed.comm = htoes(EC_ECMD_READ); + ed.addr = htoes(eeproma); + ed.d2 = 0x0000; + do + { + wkc = ecx_FPWR(context->port, configadr, ECT_REG_EEPCTL, sizeof(ed), &ed, EC_TIMEOUTRET); + } + while ((wkc <= 0) && (cnt++ < EC_DEFAULTRETRIES)); + } +} + +/** Read EEPROM from slave bypassing cache. + * Parallel read step 2, actual read from slave. + * @param[in] context = context struct + * @param[in] slave = Slave number + * @param[in] timeout = Timeout in us. + * @return EEPROM data 32bit + */ +uint32 ecx_readeeprom2(ecx_contextt *context, uint16 slave, int timeout) +{ + uint16 estat, configadr; + uint32 edat; + int wkc, cnt = 0; + + configadr = context->slavelist[slave].configadr; + edat = 0; + estat = 0x0000; + if (ecx_eeprom_waitnotbusyFP(context, configadr, &estat, timeout)) + { + do + { + wkc = ecx_FPRD(context->port, configadr, ECT_REG_EEPDAT, sizeof(edat), &edat, EC_TIMEOUTRET); + } + while ((wkc <= 0) && (cnt++ < EC_DEFAULTRETRIES)); + } + + return edat; +} + +/** Push index of segmented LRD/LWR/LRW combination. + * @param[in] context = context struct + * @param[in] idx = Used datagram index. + * @param[in] data = Pointer to process data segment. + * @param[in] length = Length of data segment in bytes. + * @param[in] DCO = Offset position of DC frame. + */ +static void ecx_pushindex(ecx_contextt *context, uint8 idx, void *data, uint16 length, uint16 DCO) +{ + if(context->idxstack->pushed < EC_MAXBUF) + { + context->idxstack->idx[context->idxstack->pushed] = idx; + context->idxstack->data[context->idxstack->pushed] = data; + context->idxstack->length[context->idxstack->pushed] = length; + context->idxstack->dcoffset[context->idxstack->pushed] = DCO; + context->idxstack->pushed++; + } +} + +/** Pull index of segmented LRD/LWR/LRW combination. + * @param[in] context = context struct + * @return Stack location, -1 if stack is empty. + */ +static int ecx_pullindex(ecx_contextt *context) +{ + int rval = -1; + if(context->idxstack->pulled < context->idxstack->pushed) + { + rval = context->idxstack->pulled; + context->idxstack->pulled++; + } + + return rval; +} + +/** + * Clear the idx stack. + * + * @param context = context struct + */ +static void ecx_clearindex(ecx_contextt *context) { + + context->idxstack->pushed = 0; + context->idxstack->pulled = 0; + +} + +/** Transmit processdata to slaves. + * Uses LRW, or LRD/LWR if LRW is not allowed (blockLRW). + * Both the input and output processdata are transmitted. + * The outputs with the actual data, the inputs have a placeholder. + * The inputs are gathered with the receive processdata function. + * In contrast to the base LRW function this function is non-blocking. + * If the processdata does not fit in one datagram, multiple are used. + * In order to recombine the slave response, a stack is used. + * @param[in] context = context struct + * @param[in] group = group number + * @param[in] use_overlap_io = flag if overlapped iomap is used + * @return >0 if processdata is transmitted. + */ +static int ecx_main_send_processdata(ecx_contextt *context, uint8 group, boolean use_overlap_io) +{ + uint32 LogAdr; + uint16 w1, w2; + int length; + uint16 sublength; + uint8 idx; + int wkc; + uint8* data; + boolean first=FALSE; + uint16 currentsegment = 0; + uint32 iomapinputoffset; + uint16 DCO; + + wkc = 0; + if(context->grouplist[group].hasdc) + { + first = TRUE; + } + + /* For overlapping IO map use the biggest */ + if(use_overlap_io == TRUE) + { + /* For overlap IOmap make the frame EQ big to biggest part */ + length = (context->grouplist[group].Obytes > context->grouplist[group].Ibytes) ? + context->grouplist[group].Obytes : context->grouplist[group].Ibytes; + /* Save the offset used to compensate where to save inputs when frame returns */ + iomapinputoffset = context->grouplist[group].Obytes; + } + else + { + length = context->grouplist[group].Obytes + context->grouplist[group].Ibytes; + iomapinputoffset = 0; + } + + LogAdr = context->grouplist[group].logstartaddr; + if(length) + { + + wkc = 1; + /* LRW blocked by one or more slaves ? */ + if(context->grouplist[group].blockLRW) + { + /* if inputs available generate LRD */ + if(context->grouplist[group].Ibytes) + { + currentsegment = context->grouplist[group].Isegment; + data = context->grouplist[group].inputs; + length = context->grouplist[group].Ibytes; + LogAdr += context->grouplist[group].Obytes; + /* segment transfer if needed */ + do + { + if(currentsegment == context->grouplist[group].Isegment) + { + sublength = (uint16)(context->grouplist[group].IOsegment[currentsegment++] - context->grouplist[group].Ioffset); + } + else + { + sublength = (uint16)context->grouplist[group].IOsegment[currentsegment++]; + } + /* get new index */ + idx = ecx_getindex(context->port); + w1 = LO_WORD(LogAdr); + w2 = HI_WORD(LogAdr); + DCO = 0; + ecx_setupdatagram(context->port, &(context->port->txbuf[idx]), EC_CMD_LRD, idx, w1, w2, sublength, data); + if(first) + { + /* FPRMW in second datagram */ + DCO = ecx_adddatagram(context->port, &(context->port->txbuf[idx]), EC_CMD_FRMW, idx, FALSE, + context->slavelist[context->grouplist[group].DCnext].configadr, + ECT_REG_DCSYSTIME, sizeof(int64), context->DCtime); + first = FALSE; + } + /* send frame */ + ecx_outframe_red(context->port, idx); + /* push index and data pointer on stack */ + ecx_pushindex(context, idx, data, sublength, DCO); + length -= sublength; + LogAdr += sublength; + data += sublength; + } while (length && (currentsegment < context->grouplist[group].nsegments)); + } + /* if outputs available generate LWR */ + if(context->grouplist[group].Obytes) + { + data = context->grouplist[group].outputs; + length = context->grouplist[group].Obytes; + LogAdr = context->grouplist[group].logstartaddr; + currentsegment = 0; + /* segment transfer if needed */ + do + { + sublength = (uint16)context->grouplist[group].IOsegment[currentsegment++]; + if((length - sublength) < 0) + { + sublength = (uint16)length; + } + /* get new index */ + idx = ecx_getindex(context->port); + w1 = LO_WORD(LogAdr); + w2 = HI_WORD(LogAdr); + DCO = 0; + ecx_setupdatagram(context->port, &(context->port->txbuf[idx]), EC_CMD_LWR, idx, w1, w2, sublength, data); + if(first) + { + /* FPRMW in second datagram */ + DCO = ecx_adddatagram(context->port, &(context->port->txbuf[idx]), EC_CMD_FRMW, idx, FALSE, + context->slavelist[context->grouplist[group].DCnext].configadr, + ECT_REG_DCSYSTIME, sizeof(int64), context->DCtime); + first = FALSE; + } + /* send frame */ + ecx_outframe_red(context->port, idx); + /* push index and data pointer on stack */ + ecx_pushindex(context, idx, data, sublength, DCO); + length -= sublength; + LogAdr += sublength; + data += sublength; + } while (length && (currentsegment < context->grouplist[group].nsegments)); + } + } + /* LRW can be used */ + else + { + if (context->grouplist[group].Obytes) + { + data = context->grouplist[group].outputs; + } + else + { + data = context->grouplist[group].inputs; + /* Clear offset, don't compensate for overlapping IOmap if we only got inputs */ + iomapinputoffset = 0; + } + /* segment transfer if needed */ + do + { + sublength = (uint16)context->grouplist[group].IOsegment[currentsegment++]; + /* get new index */ + idx = ecx_getindex(context->port); + w1 = LO_WORD(LogAdr); + w2 = HI_WORD(LogAdr); + DCO = 0; + ecx_setupdatagram(context->port, &(context->port->txbuf[idx]), EC_CMD_LRW, idx, w1, w2, sublength, data); + if(first) + { + /* FPRMW in second datagram */ + DCO = ecx_adddatagram(context->port, &(context->port->txbuf[idx]), EC_CMD_FRMW, idx, FALSE, + context->slavelist[context->grouplist[group].DCnext].configadr, + ECT_REG_DCSYSTIME, sizeof(int64), context->DCtime); + first = FALSE; + } + /* send frame */ + ecx_outframe_red(context->port, idx); + /* push index and data pointer on stack. + * the iomapinputoffset compensate for where the inputs are stored + * in the IOmap if we use an overlapping IOmap. If a regular IOmap + * is used it should always be 0. + */ + ecx_pushindex(context, idx, (data + iomapinputoffset), sublength, DCO); + length -= sublength; + LogAdr += sublength; + data += sublength; + } while (length && (currentsegment < context->grouplist[group].nsegments)); + } + } + + return wkc; +} + +/** Transmit processdata to slaves. +* Uses LRW, or LRD/LWR if LRW is not allowed (blockLRW). +* Both the input and output processdata are transmitted in the overlapped IOmap. +* The outputs with the actual data, the inputs replace the output data in the +* returning frame. The inputs are gathered with the receive processdata function. +* In contrast to the base LRW function this function is non-blocking. +* If the processdata does not fit in one datagram, multiple are used. +* In order to recombine the slave response, a stack is used. +* @param[in] context = context struct +* @param[in] group = group number +* @return >0 if processdata is transmitted. +*/ +int ecx_send_overlap_processdata_group(ecx_contextt *context, uint8 group) +{ + return ecx_main_send_processdata(context, group, TRUE); +} + +/** Transmit processdata to slaves. +* Uses LRW, or LRD/LWR if LRW is not allowed (blockLRW). +* Both the input and output processdata are transmitted. +* The outputs with the actual data, the inputs have a placeholder. +* The inputs are gathered with the receive processdata function. +* In contrast to the base LRW function this function is non-blocking. +* If the processdata does not fit in one datagram, multiple are used. +* In order to recombine the slave response, a stack is used. +* @param[in] context = context struct +* @param[in] group = group number +* @return >0 if processdata is transmitted. +*/ +int ecx_send_processdata_group(ecx_contextt *context, uint8 group) +{ + return ecx_main_send_processdata(context, group, FALSE); +} + +/** Receive processdata from slaves. + * Second part from ec_send_processdata(). + * Received datagrams are recombined with the processdata with help from the stack. + * If a datagram contains input processdata it copies it to the processdata structure. + * @param[in] context = context struct + * @param[in] group = group number + * @param[in] timeout = Timeout in us. + * @return Work counter. + */ +int ecx_receive_processdata_group(ecx_contextt *context, uint8 group, int timeout) +{ + uint8 idx; + int pos; + int wkc = 0, wkc2; + uint16 le_wkc = 0; + int valid_wkc = 0; + int64 le_DCtime; + ec_idxstackT *idxstack; + ec_bufT *rxbuf; + + /* just to prevent compiler warning for unused group */ + wkc2 = group; + + idxstack = context->idxstack; + rxbuf = context->port->rxbuf; + /* get first index */ + pos = ecx_pullindex(context); + /* read the same number of frames as send */ + while (pos >= 0) + { + idx = idxstack->idx[pos]; + wkc2 = ecx_waitinframe(context->port, idx, timeout); + /* check if there is input data in frame */ + if (wkc2 > EC_NOFRAME) + { + if((rxbuf[idx][EC_CMDOFFSET]==EC_CMD_LRD) || (rxbuf[idx][EC_CMDOFFSET]==EC_CMD_LRW)) + { + if(idxstack->dcoffset[pos] > 0) + { + memcpy(idxstack->data[pos], &(rxbuf[idx][EC_HEADERSIZE]), idxstack->length[pos]); + memcpy(&le_wkc, &(rxbuf[idx][EC_HEADERSIZE + idxstack->length[pos]]), EC_WKCSIZE); + wkc = etohs(le_wkc); + memcpy(&le_DCtime, &(rxbuf[idx][idxstack->dcoffset[pos]]), sizeof(le_DCtime)); + *(context->DCtime) = etohll(le_DCtime); + } + else + { + /* copy input data back to process data buffer */ + memcpy(idxstack->data[pos], &(rxbuf[idx][EC_HEADERSIZE]), idxstack->length[pos]); + wkc += wkc2; + } + valid_wkc = 1; + } + else if(rxbuf[idx][EC_CMDOFFSET]==EC_CMD_LWR) + { + if(idxstack->dcoffset[pos] > 0) + { + memcpy(&le_wkc, &(rxbuf[idx][EC_HEADERSIZE + idxstack->length[pos]]), EC_WKCSIZE); + /* output WKC counts 2 times when using LRW, emulate the same for LWR */ + wkc = etohs(le_wkc) * 2; + memcpy(&le_DCtime, &(rxbuf[idx][idxstack->dcoffset[pos]]), sizeof(le_DCtime)); + *(context->DCtime) = etohll(le_DCtime); + } + else + { + /* output WKC counts 2 times when using LRW, emulate the same for LWR */ + wkc += wkc2 * 2; + } + valid_wkc = 1; + } + } + /* release buffer */ + ecx_setbufstat(context->port, idx, EC_BUF_EMPTY); + /* get next index */ + pos = ecx_pullindex(context); + } + + ecx_clearindex(context); + + /* if no frames has arrived */ + if (valid_wkc == 0) + { + return EC_NOFRAME; + } + return wkc; +} + + +int ecx_send_processdata(ecx_contextt *context) +{ + return ecx_send_processdata_group(context, 0); +} + +int ecx_send_overlap_processdata(ecx_contextt *context) +{ + return ecx_send_overlap_processdata_group(context, 0); +} + +int ecx_receive_processdata(ecx_contextt *context, int timeout) +{ + return ecx_receive_processdata_group(context, 0, timeout); +} + +#ifdef EC_VER1 +void ec_pusherror(const ec_errort *Ec) +{ + ecx_pusherror(&ecx_context, Ec); +} + +boolean ec_poperror(ec_errort *Ec) +{ + return ecx_poperror(&ecx_context, Ec); +} + +boolean ec_iserror(void) +{ + return ecx_iserror(&ecx_context); +} + +void ec_packeterror(uint16 Slave, uint16 Index, uint8 SubIdx, uint16 ErrorCode) +{ + ecx_packeterror(&ecx_context, Slave, Index, SubIdx, ErrorCode); +} + +/** Initialise lib in single NIC mode + * @param[in] ifname = Dev name, f.e. "eth0" + * @return >0 if OK + * @see ecx_init + */ +int ec_init(const char * ifname) +{ + return ecx_init(&ecx_context, ifname); +} + +/** Initialise lib in redundant NIC mode + * @param[in] ifname = Primary Dev name, f.e. "eth0" + * @param[in] if2name = Secondary Dev name, f.e. "eth1" + * @return >0 if OK + * @see ecx_init_redundant + */ +int ec_init_redundant(const char *ifname, char *if2name) +{ + return ecx_init_redundant (&ecx_context, &ecx_redport, ifname, if2name); +} + +/** Close lib. + * @see ecx_close + */ +void ec_close(void) +{ + ecx_close(&ecx_context); +}; + +/** Read one byte from slave EEPROM via cache. + * If the cache location is empty then a read request is made to the slave. + * Depending on the slave capabillities the request is 4 or 8 bytes. + * @param[in] slave = slave number + * @param[in] address = eeprom address in bytes (slave uses words) + * @return requested byte, if not available then 0xff + * @see ecx_siigetbyte + */ +uint8 ec_siigetbyte(uint16 slave, uint16 address) +{ + return ecx_siigetbyte (&ecx_context, slave, address); +} + +/** Find SII section header in slave EEPROM. + * @param[in] slave = slave number + * @param[in] cat = section category + * @return byte address of section at section length entry, if not available then 0 + * @see ecx_siifind + */ +int16 ec_siifind(uint16 slave, uint16 cat) +{ + return ecx_siifind (&ecx_context, slave, cat); +} + +/** Get string from SII string section in slave EEPROM. + * @param[out] str = requested string, 0x00 if not found + * @param[in] slave = slave number + * @param[in] Sn = string number + * @see ecx_siistring + */ +void ec_siistring(char *str, uint16 slave, uint16 Sn) +{ + ecx_siistring(&ecx_context, str, slave, Sn); +} + +/** Get FMMU data from SII FMMU section in slave EEPROM. + * @param[in] slave = slave number + * @param[out] FMMU = FMMU struct from SII, max. 4 FMMU's + * @return number of FMMU's defined in section + * @see ecx_siiFMMU + */ +uint16 ec_siiFMMU(uint16 slave, ec_eepromFMMUt* FMMU) +{ + return ecx_siiFMMU (&ecx_context, slave, FMMU); +} + +/** Get SM data from SII SM section in slave EEPROM. + * @param[in] slave = slave number + * @param[out] SM = first SM struct from SII + * @return number of SM's defined in section + * @see ecx_siiSM + */ +uint16 ec_siiSM(uint16 slave, ec_eepromSMt* SM) +{ + return ecx_siiSM (&ecx_context, slave, SM); +} + +/** Get next SM data from SII SM section in slave EEPROM. + * @param[in] slave = slave number + * @param[out] SM = first SM struct from SII + * @param[in] n = SM number + * @return >0 if OK + * @see ecx_siiSMnext + */ +uint16 ec_siiSMnext(uint16 slave, ec_eepromSMt* SM, uint16 n) +{ + return ecx_siiSMnext (&ecx_context, slave, SM, n); +} + +/** Get PDO data from SII PDO section in slave EEPROM. + * @param[in] slave = slave number + * @param[out] PDO = PDO struct from SII + * @param[in] t = 0=RXPDO 1=TXPDO + * @return mapping size in bits of PDO + * @see ecx_siiPDO + */ +uint32 ec_siiPDO(uint16 slave, ec_eepromPDOt* PDO, uint8 t) +{ + return ecx_siiPDO (&ecx_context, slave, PDO, t); +} + +/** Read all slave states in ec_slave. + * @warning The BOOT state is actually higher than INIT and PRE_OP (see state representation). + * @return lowest state found + * @see ecx_readstate + */ +int ec_readstate(void) +{ + return ecx_readstate (&ecx_context); +} + +/** Write slave state, if slave = 0 then write to all slaves. + * The function does not check if the actual state is changed. + * @param[in] slave = Slave number, 0 = master + * @return 0 + * @see ecx_writestate + */ +int ec_writestate(uint16 slave) +{ + return ecx_writestate(&ecx_context, slave); +} + +/** Check actual slave state. + * This is a blocking function. + * To refresh the state of all slaves ecx_readstate() should be called. + * @warning If this is used for slave 0 (=all slaves), the state of all slaves is read by an bitwise OR operation. + * The returned value is also the bitwise OR state of all slaves. + * This has some implications for the BOOT state. The Boot state representation collides with INIT | PRE_OP so this + * function cannot be used for slave = 0 and reqstate = EC_STATE_BOOT and also, if the returned state is BOOT, some + * slaves might actually be in INIT and PRE_OP and not in BOOT. + * @param[in] slave = Slave number, 0 = all slaves + * @param[in] reqstate = Requested state + * @param[in] timeout = Timeout value in us + * @return Requested state, or found state after timeout. + * @see ecx_statecheck + */ +uint16 ec_statecheck(uint16 slave, uint16 reqstate, int timeout) +{ + return ecx_statecheck (&ecx_context, slave, reqstate, timeout); +} + +/** Check if IN mailbox of slave is empty. + * @param[in] slave = Slave number + * @param[in] timeout = Timeout in us + * @return >0 is success + * @see ecx_mbxempty + */ +int ec_mbxempty(uint16 slave, int timeout) +{ + return ecx_mbxempty (&ecx_context, slave, timeout); +} + +/** Write IN mailbox to slave. + * @param[in] slave = Slave number + * @param[out] mbx = Mailbox data + * @param[in] timeout = Timeout in us + * @return Work counter (>0 is success) + * @see ecx_mbxsend + */ +int ec_mbxsend(uint16 slave,ec_mbxbuft *mbx, int timeout) +{ + return ecx_mbxsend (&ecx_context, slave, mbx, timeout); +} + +/** Read OUT mailbox from slave. + * Supports Mailbox Link Layer with repeat requests. + * @param[in] slave = Slave number + * @param[out] mbx = Mailbox data + * @param[in] timeout = Timeout in us + * @return Work counter (>0 is success) + * @see ecx_mbxreceive + */ +int ec_mbxreceive(uint16 slave, ec_mbxbuft *mbx, int timeout) +{ + return ecx_mbxreceive (&ecx_context, slave, mbx, timeout); +} + +/** Dump complete EEPROM data from slave in buffer. + * @param[in] slave = Slave number + * @param[out] esibuf = EEPROM data buffer, make sure it is big enough. + * @see ecx_esidump + */ +void ec_esidump(uint16 slave, uint8 *esibuf) +{ + ecx_esidump (&ecx_context, slave, esibuf); +} + +/** Read EEPROM from slave bypassing cache. + * @param[in] slave = Slave number + * @param[in] eeproma = (WORD) Address in the EEPROM + * @param[in] timeout = Timeout in us. + * @return EEPROM data 32bit + * @see ecx_readeeprom + */ +uint32 ec_readeeprom(uint16 slave, uint16 eeproma, int timeout) +{ + return ecx_readeeprom (&ecx_context, slave, eeproma, timeout); +} + +/** Write EEPROM to slave bypassing cache. + * @param[in] slave = Slave number + * @param[in] eeproma = (WORD) Address in the EEPROM + * @param[in] data = 16bit data + * @param[in] timeout = Timeout in us. + * @return >0 if OK + * @see ecx_writeeeprom + */ +int ec_writeeeprom(uint16 slave, uint16 eeproma, uint16 data, int timeout) +{ + return ecx_writeeeprom (&ecx_context, slave, eeproma, data, timeout); +} + +/** Set eeprom control to master. Only if set to PDI. + * @param[in] slave = Slave number + * @return >0 if OK + * @see ecx_eeprom2master + */ +int ec_eeprom2master(uint16 slave) +{ + return ecx_eeprom2master(&ecx_context, slave); +} + +int ec_eeprom2pdi(uint16 slave) +{ + return ecx_eeprom2pdi(&ecx_context, slave); +} + +uint16 ec_eeprom_waitnotbusyAP(uint16 aiadr,uint16 *estat, int timeout) +{ + return ecx_eeprom_waitnotbusyAP (&ecx_context, aiadr, estat, timeout); +} + +/** Read EEPROM from slave bypassing cache. APRD method. + * @param[in] aiadr = auto increment address of slave + * @param[in] eeproma = (WORD) Address in the EEPROM + * @param[in] timeout = Timeout in us. + * @return EEPROM data 64bit or 32bit + */ +uint64 ec_readeepromAP(uint16 aiadr, uint16 eeproma, int timeout) +{ + return ecx_readeepromAP (&ecx_context, aiadr, eeproma, timeout); +} + +/** Write EEPROM to slave bypassing cache. APWR method. + * @param[in] aiadr = configured address of slave + * @param[in] eeproma = (WORD) Address in the EEPROM + * @param[in] data = 16bit data + * @param[in] timeout = Timeout in us. + * @return >0 if OK + * @see ecx_writeeepromAP + */ +int ec_writeeepromAP(uint16 aiadr, uint16 eeproma, uint16 data, int timeout) +{ + return ecx_writeeepromAP (&ecx_context, aiadr, eeproma, data, timeout); +} + +uint16 ec_eeprom_waitnotbusyFP(uint16 configadr,uint16 *estat, int timeout) +{ + return ecx_eeprom_waitnotbusyFP (&ecx_context, configadr, estat, timeout); +} + +/** Read EEPROM from slave bypassing cache. FPRD method. + * @param[in] configadr = configured address of slave + * @param[in] eeproma = (WORD) Address in the EEPROM + * @param[in] timeout = Timeout in us. + * @return EEPROM data 64bit or 32bit + * @see ecx_readeepromFP + */ +uint64 ec_readeepromFP(uint16 configadr, uint16 eeproma, int timeout) +{ + return ecx_readeepromFP (&ecx_context, configadr, eeproma, timeout); +} + +/** Write EEPROM to slave bypassing cache. FPWR method. + * @param[in] configadr = configured address of slave + * @param[in] eeproma = (WORD) Address in the EEPROM + * @param[in] data = 16bit data + * @param[in] timeout = Timeout in us. + * @return >0 if OK + * @see ecx_writeeepromFP + */ +int ec_writeeepromFP(uint16 configadr, uint16 eeproma, uint16 data, int timeout) +{ + return ecx_writeeepromFP (&ecx_context, configadr, eeproma, data, timeout); +} + +/** Read EEPROM from slave bypassing cache. + * Parallel read step 1, make request to slave. + * @param[in] slave = Slave number + * @param[in] eeproma = (WORD) Address in the EEPROM + * @see ecx_readeeprom1 + */ +void ec_readeeprom1(uint16 slave, uint16 eeproma) +{ + ecx_readeeprom1 (&ecx_context, slave, eeproma); +} + +/** Read EEPROM from slave bypassing cache. + * Parallel read step 2, actual read from slave. + * @param[in] slave = Slave number + * @param[in] timeout = Timeout in us. + * @return EEPROM data 32bit + * @see ecx_readeeprom2 + */ +uint32 ec_readeeprom2(uint16 slave, int timeout) +{ + return ecx_readeeprom2 (&ecx_context, slave, timeout); +} + +/** Transmit processdata to slaves. + * Uses LRW, or LRD/LWR if LRW is not allowed (blockLRW). + * Both the input and output processdata are transmitted. + * The outputs with the actual data, the inputs have a placeholder. + * The inputs are gathered with the receive processdata function. + * In contrast to the base LRW function this function is non-blocking. + * If the processdata does not fit in one datagram, multiple are used. + * In order to recombine the slave response, a stack is used. + * @param[in] group = group number + * @return >0 if processdata is transmitted. + * @see ecx_send_processdata_group + */ +int ec_send_processdata_group(uint8 group) +{ + return ecx_send_processdata_group (&ecx_context, group); +} + +/** Transmit processdata to slaves. +* Uses LRW, or LRD/LWR if LRW is not allowed (blockLRW). +* Both the input and output processdata are transmitted in the overlapped IOmap. +* The outputs with the actual data, the inputs replace the output data in the +* returning frame. The inputs are gathered with the receive processdata function. +* In contrast to the base LRW function this function is non-blocking. +* If the processdata does not fit in one datagram, multiple are used. +* In order to recombine the slave response, a stack is used. +* @param[in] group = group number +* @return >0 if processdata is transmitted. +* @see ecx_send_overlap_processdata_group +*/ +int ec_send_overlap_processdata_group(uint8 group) +{ + return ecx_send_overlap_processdata_group(&ecx_context, group); +} + +/** Receive processdata from slaves. + * Second part from ec_send_processdata(). + * Received datagrams are recombined with the processdata with help from the stack. + * If a datagram contains input processdata it copies it to the processdata structure. + * @param[in] group = group number + * @param[in] timeout = Timeout in us. + * @return Work counter. + * @see ecx_receive_processdata_group + */ +int ec_receive_processdata_group(uint8 group, int timeout) +{ + return ecx_receive_processdata_group (&ecx_context, group, timeout); +} + +int ec_send_processdata(void) +{ + return ec_send_processdata_group(0); +} + +int ec_send_overlap_processdata(void) +{ + return ec_send_overlap_processdata_group(0); +} + +int ec_receive_processdata(int timeout) +{ + return ec_receive_processdata_group(0, timeout); +} +#endif diff --git a/soem-src/soem/ethercatmain.h b/soem-src/soem/ethercatmain.h new file mode 100644 index 0000000..705402d --- /dev/null +++ b/soem-src/soem/ethercatmain.h @@ -0,0 +1,539 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +/** \file + * \brief + * Headerfile for ethercatmain.c + */ + +#ifndef _ethercatmain_ +#define _ethercatmain_ + + +#ifdef __cplusplus +extern "C" +{ +#endif + +/** max. entries in EtherCAT error list */ +#define EC_MAXELIST 64 +/** max. length of readable name in slavelist and Object Description List */ +#define EC_MAXNAME 40 +/** max. number of slaves in array */ +#define EC_MAXSLAVE 200 +/** max. number of groups */ +#define EC_MAXGROUP 2 +/** max. number of IO segments per group */ +#define EC_MAXIOSEGMENTS 64 +/** max. mailbox size */ +#define EC_MAXMBX 1486 +/** max. eeprom PDO entries */ +#define EC_MAXEEPDO 0x200 +/** max. SM used */ +#define EC_MAXSM 8 +/** max. FMMU used */ +#define EC_MAXFMMU 4 +/** max. Adapter */ +#define EC_MAXLEN_ADAPTERNAME 128 +/** define maximum number of concurrent threads in mapping */ +#define EC_MAX_MAPT 1 + +typedef struct ec_adapter ec_adaptert; +struct ec_adapter +{ + char name[EC_MAXLEN_ADAPTERNAME]; + char desc[EC_MAXLEN_ADAPTERNAME]; + ec_adaptert *next; +}; + +/** record for FMMU */ +PACKED_BEGIN +typedef struct PACKED ec_fmmu +{ + uint32 LogStart; + uint16 LogLength; + uint8 LogStartbit; + uint8 LogEndbit; + uint16 PhysStart; + uint8 PhysStartBit; + uint8 FMMUtype; + uint8 FMMUactive; + uint8 unused1; + uint16 unused2; +} ec_fmmut; +PACKED_END + +/** record for sync manager */ +PACKED_BEGIN +typedef struct PACKED ec_sm +{ + uint16 StartAddr; + uint16 SMlength; + uint32 SMflags; +} ec_smt; +PACKED_END + +PACKED_BEGIN +typedef struct PACKED ec_state_status +{ + uint16 State; + uint16 Unused; + uint16 ALstatuscode; +} ec_state_status; +PACKED_END + +#define ECT_MBXPROT_AOE 0x0001 +#define ECT_MBXPROT_EOE 0x0002 +#define ECT_MBXPROT_COE 0x0004 +#define ECT_MBXPROT_FOE 0x0008 +#define ECT_MBXPROT_SOE 0x0010 +#define ECT_MBXPROT_VOE 0x0020 + +#define ECT_COEDET_SDO 0x01 +#define ECT_COEDET_SDOINFO 0x02 +#define ECT_COEDET_PDOASSIGN 0x04 +#define ECT_COEDET_PDOCONFIG 0x08 +#define ECT_COEDET_UPLOAD 0x10 +#define ECT_COEDET_SDOCA 0x20 + +#define EC_SMENABLEMASK 0xfffeffff + +typedef struct ecx_context ecx_contextt; + +/** for list of ethercat slaves detected */ +typedef struct ec_slave +{ + /** state of slave */ + uint16 state; + /** AL status code */ + uint16 ALstatuscode; + /** Configured address */ + uint16 configadr; + /** Alias address */ + uint16 aliasadr; + /** Manufacturer from EEprom */ + uint32 eep_man; + /** ID from EEprom */ + uint32 eep_id; + /** revision from EEprom */ + uint32 eep_rev; + /** serial number from EEprom */ + uint32 eep_sn; + /** Interface type */ + uint16 Itype; + /** Device type */ + uint16 Dtype; + /** output bits */ + uint16 Obits; + /** output bytes, if Obits < 8 then Obytes = 0 */ + uint32 Obytes; + /** output pointer in IOmap buffer */ + uint8 *outputs; + /** startbit in first output byte */ + uint8 Ostartbit; + /** input bits */ + uint16 Ibits; + /** input bytes, if Ibits < 8 then Ibytes = 0 */ + uint32 Ibytes; + /** input pointer in IOmap buffer */ + uint8 *inputs; + /** startbit in first input byte */ + uint8 Istartbit; + /** SM structure */ + ec_smt SM[EC_MAXSM]; + /** SM type 0=unused 1=MbxWr 2=MbxRd 3=Outputs 4=Inputs */ + uint8 SMtype[EC_MAXSM]; + /** FMMU structure */ + ec_fmmut FMMU[EC_MAXFMMU]; + /** FMMU0 function */ + uint8 FMMU0func; + /** FMMU1 function */ + uint8 FMMU1func; + /** FMMU2 function */ + uint8 FMMU2func; + /** FMMU3 function */ + uint8 FMMU3func; + /** length of write mailbox in bytes, if no mailbox then 0 */ + uint16 mbx_l; + /** mailbox write offset */ + uint16 mbx_wo; + /** length of read mailbox in bytes */ + uint16 mbx_rl; + /** mailbox read offset */ + uint16 mbx_ro; + /** mailbox supported protocols */ + uint16 mbx_proto; + /** Counter value of mailbox link layer protocol 1..7 */ + uint8 mbx_cnt; + /** has DC capability */ + boolean hasdc; + /** Physical type; Ebus, EtherNet combinations */ + uint8 ptype; + /** topology: 1 to 3 links */ + uint8 topology; + /** active ports bitmap : ....3210 , set if respective port is active **/ + uint8 activeports; + /** consumed ports bitmap : ....3210, used for internal delay measurement **/ + uint8 consumedports; + /** slave number for parent, 0=master */ + uint16 parent; + /** port number on parent this slave is connected to **/ + uint8 parentport; + /** port number on this slave the parent is connected to **/ + uint8 entryport; + /** DC receivetimes on port A */ + int32 DCrtA; + /** DC receivetimes on port B */ + int32 DCrtB; + /** DC receivetimes on port C */ + int32 DCrtC; + /** DC receivetimes on port D */ + int32 DCrtD; + /** propagation delay */ + int32 pdelay; + /** next DC slave */ + uint16 DCnext; + /** previous DC slave */ + uint16 DCprevious; + /** DC cycle time in ns */ + int32 DCcycle; + /** DC shift from clock modulus boundary */ + int32 DCshift; + /** DC sync activation, 0=off, 1=on */ + uint8 DCactive; + /** link to config table */ + uint16 configindex; + /** link to SII config */ + uint16 SIIindex; + /** 1 = 8 bytes per read, 0 = 4 bytes per read */ + uint8 eep_8byte; + /** 0 = eeprom to master , 1 = eeprom to PDI */ + uint8 eep_pdi; + /** CoE details */ + uint8 CoEdetails; + /** FoE details */ + uint8 FoEdetails; + /** EoE details */ + uint8 EoEdetails; + /** SoE details */ + uint8 SoEdetails; + /** E-bus current */ + int16 Ebuscurrent; + /** if >0 block use of LRW in processdata */ + uint8 blockLRW; + /** group */ + uint8 group; + /** first unused FMMU */ + uint8 FMMUunused; + /** Boolean for tracking whether the slave is (not) responding, not used/set by the SOEM library */ + boolean islost; + /** registered configuration function PO->SO, (DEPRECATED)*/ + int (*PO2SOconfig)(uint16 slave, void *user); + /** registered configuration function PO->SO */ + int (*PO2SOconfigx)(ecx_contextt * context, uint16 slave); + /** user data for PO2SOconfig */ + void* user; + /** readable name */ + char name[EC_MAXNAME + 1]; +} ec_slavet; + +/** for list of ethercat slave groups */ +typedef struct ec_group +{ + /** logical start address for this group */ + uint32 logstartaddr; + /** output bytes, if Obits < 8 then Obytes = 0 */ + uint32 Obytes; + /** output pointer in IOmap buffer */ + uint8 *outputs; + /** input bytes, if Ibits < 8 then Ibytes = 0 */ + uint32 Ibytes; + /** input pointer in IOmap buffer */ + uint8 *inputs; + /** has DC capabillity */ + boolean hasdc; + /** next DC slave */ + uint16 DCnext; + /** E-bus current */ + int16 Ebuscurrent; + /** if >0 block use of LRW in processdata */ + uint8 blockLRW; + /** IO segments used */ + uint16 nsegments; + /** 1st input segment */ + uint16 Isegment; + /** Offset in input segment */ + uint16 Ioffset; + /** Expected workcounter outputs */ + uint16 outputsWKC; + /** Expected workcounter inputs */ + uint16 inputsWKC; + /** check slave states */ + boolean docheckstate; + /** IO segmentation list. Datagrams must not break SM in two. */ + uint32 IOsegment[EC_MAXIOSEGMENTS]; +} ec_groupt; + +/** SII FMMU structure */ +typedef struct ec_eepromFMMU +{ + uint16 Startpos; + uint8 nFMMU; + uint8 FMMU0; + uint8 FMMU1; + uint8 FMMU2; + uint8 FMMU3; +} ec_eepromFMMUt; + +/** SII SM structure */ +typedef struct ec_eepromSM +{ + uint16 Startpos; + uint8 nSM; + uint16 PhStart; + uint16 Plength; + uint8 Creg; + uint8 Sreg; /* don't care */ + uint8 Activate; + uint8 PDIctrl; /* don't care */ +} ec_eepromSMt; + +/** record to store rxPDO and txPDO table from eeprom */ +typedef struct ec_eepromPDO +{ + uint16 Startpos; + uint16 Length; + uint16 nPDO; + uint16 Index[EC_MAXEEPDO]; + uint16 SyncM[EC_MAXEEPDO]; + uint16 BitSize[EC_MAXEEPDO]; + uint16 SMbitsize[EC_MAXSM]; +} ec_eepromPDOt; + +/** mailbox buffer array */ +typedef uint8 ec_mbxbuft[EC_MAXMBX + 1]; + +/** standard ethercat mailbox header */ +PACKED_BEGIN +typedef struct PACKED ec_mbxheader +{ + uint16 length; + uint16 address; + uint8 priority; + uint8 mbxtype; +} ec_mbxheadert; +PACKED_END + +/** ALstatus and ALstatus code */ +PACKED_BEGIN +typedef struct PACKED ec_alstatus +{ + uint16 alstatus; + uint16 unused; + uint16 alstatuscode; +} ec_alstatust; +PACKED_END + +/** stack structure to store segmented LRD/LWR/LRW constructs */ +typedef struct ec_idxstack +{ + uint8 pushed; + uint8 pulled; + uint8 idx[EC_MAXBUF]; + void *data[EC_MAXBUF]; + uint16 length[EC_MAXBUF]; + uint16 dcoffset[EC_MAXBUF]; +} ec_idxstackT; + +/** ringbuf for error storage */ +typedef struct ec_ering +{ + int16 head; + int16 tail; + ec_errort Error[EC_MAXELIST + 1]; +} ec_eringt; + +/** SyncManager Communication Type structure for CA */ +PACKED_BEGIN +typedef struct PACKED ec_SMcommtype +{ + uint8 n; + uint8 nu1; + uint8 SMtype[EC_MAXSM]; +} ec_SMcommtypet; +PACKED_END + +/** SDO assign structure for CA */ +PACKED_BEGIN +typedef struct PACKED ec_PDOassign +{ + uint8 n; + uint8 nu1; + uint16 index[256]; +} ec_PDOassignt; +PACKED_END + +/** SDO description structure for CA */ +PACKED_BEGIN +typedef struct PACKED ec_PDOdesc +{ + uint8 n; + uint8 nu1; + uint32 PDO[256]; +} ec_PDOdesct; +PACKED_END + +/** Context structure , referenced by all ecx functions*/ +struct ecx_context +{ + /** port reference, may include red_port */ + ecx_portt *port; + /** slavelist reference */ + ec_slavet *slavelist; + /** number of slaves found in configuration */ + int *slavecount; + /** maximum number of slaves allowed in slavelist */ + int maxslave; + /** grouplist reference */ + ec_groupt *grouplist; + /** maximum number of groups allowed in grouplist */ + int maxgroup; + /** internal, reference to eeprom cache buffer */ + uint8 *esibuf; + /** internal, reference to eeprom cache map */ + uint32 *esimap; + /** internal, current slave for eeprom cache */ + uint16 esislave; + /** internal, reference to error list */ + ec_eringt *elist; + /** internal, reference to processdata stack buffer info */ + ec_idxstackT *idxstack; + /** reference to ecaterror state */ + boolean *ecaterror; + /** internal, position of DC datagram in process data packet */ + uint16 DCtO; + /** internal, length of DC datagram */ + uint16 DCl; + /** reference to last DC time from slaves */ + int64 *DCtime; + /** internal, SM buffer */ + ec_SMcommtypet *SMcommtype; + /** internal, PDO assign list */ + ec_PDOassignt *PDOassign; + /** internal, PDO description list */ + ec_PDOdesct *PDOdesc; + /** internal, SM list from eeprom */ + ec_eepromSMt *eepSM; + /** internal, FMMU list from eeprom */ + ec_eepromFMMUt *eepFMMU; + /** registered FoE hook */ + int (*FOEhook)(uint16 slave, int packetnumber, int datasize); + /** registered EoE hook */ + int (*EOEhook)(ecx_contextt * context, uint16 slave, void * eoembx); + /** flag to control legacy automatic state change or manual state change */ + int manualstatechange; + /** userdata, promotes application configuration esp. in EC_VER2 with multiple + * ec_context instances. Note: userdata memory is managed by application, not SOEM */ + void *userdata; +}; + +#ifdef EC_VER1 +/** global struct to hold default master context */ +extern ecx_contextt ecx_context; +/** main slave data structure array */ +extern ec_slavet ec_slave[EC_MAXSLAVE]; +/** number of slaves found by configuration function */ +extern int ec_slavecount; +/** slave group structure */ +extern ec_groupt ec_group[EC_MAXGROUP]; +extern boolean EcatError; +extern int64 ec_DCtime; + +void ec_pusherror(const ec_errort *Ec); +boolean ec_poperror(ec_errort *Ec); +boolean ec_iserror(void); +void ec_packeterror(uint16 Slave, uint16 Index, uint8 SubIdx, uint16 ErrorCode); +int ec_init(const char * ifname); +int ec_init_redundant(const char *ifname, char *if2name); +void ec_close(void); +uint8 ec_siigetbyte(uint16 slave, uint16 address); +int16 ec_siifind(uint16 slave, uint16 cat); +void ec_siistring(char *str, uint16 slave, uint16 Sn); +uint16 ec_siiFMMU(uint16 slave, ec_eepromFMMUt* FMMU); +uint16 ec_siiSM(uint16 slave, ec_eepromSMt* SM); +uint16 ec_siiSMnext(uint16 slave, ec_eepromSMt* SM, uint16 n); +uint32 ec_siiPDO(uint16 slave, ec_eepromPDOt* PDO, uint8 t); +int ec_readstate(void); +int ec_writestate(uint16 slave); +uint16 ec_statecheck(uint16 slave, uint16 reqstate, int timeout); +int ec_mbxempty(uint16 slave, int timeout); +int ec_mbxsend(uint16 slave,ec_mbxbuft *mbx, int timeout); +int ec_mbxreceive(uint16 slave, ec_mbxbuft *mbx, int timeout); +void ec_esidump(uint16 slave, uint8 *esibuf); +uint32 ec_readeeprom(uint16 slave, uint16 eeproma, int timeout); +int ec_writeeeprom(uint16 slave, uint16 eeproma, uint16 data, int timeout); +int ec_eeprom2master(uint16 slave); +int ec_eeprom2pdi(uint16 slave); +uint64 ec_readeepromAP(uint16 aiadr, uint16 eeproma, int timeout); +int ec_writeeepromAP(uint16 aiadr, uint16 eeproma, uint16 data, int timeout); +uint64 ec_readeepromFP(uint16 configadr, uint16 eeproma, int timeout); +int ec_writeeepromFP(uint16 configadr, uint16 eeproma, uint16 data, int timeout); +void ec_readeeprom1(uint16 slave, uint16 eeproma); +uint32 ec_readeeprom2(uint16 slave, int timeout); +int ec_send_processdata_group(uint8 group); +int ec_send_overlap_processdata_group(uint8 group); +int ec_receive_processdata_group(uint8 group, int timeout); +int ec_send_processdata(void); +int ec_send_overlap_processdata(void); +int ec_receive_processdata(int timeout); +#endif + +ec_adaptert * ec_find_adapters(void); +void ec_free_adapters(ec_adaptert * adapter); +uint8 ec_nextmbxcnt(uint8 cnt); +void ec_clearmbx(ec_mbxbuft *Mbx); +void ecx_pusherror(ecx_contextt *context, const ec_errort *Ec); +boolean ecx_poperror(ecx_contextt *context, ec_errort *Ec); +boolean ecx_iserror(ecx_contextt *context); +void ecx_packeterror(ecx_contextt *context, uint16 Slave, uint16 Index, uint8 SubIdx, uint16 ErrorCode); +int ecx_init(ecx_contextt *context, const char * ifname); +int ecx_init_redundant(ecx_contextt *context, ecx_redportt *redport, const char *ifname, char *if2name); +void ecx_close(ecx_contextt *context); +uint8 ecx_siigetbyte(ecx_contextt *context, uint16 slave, uint16 address); +int16 ecx_siifind(ecx_contextt *context, uint16 slave, uint16 cat); +void ecx_siistring(ecx_contextt *context, char *str, uint16 slave, uint16 Sn); +uint16 ecx_siiFMMU(ecx_contextt *context, uint16 slave, ec_eepromFMMUt* FMMU); +uint16 ecx_siiSM(ecx_contextt *context, uint16 slave, ec_eepromSMt* SM); +uint16 ecx_siiSMnext(ecx_contextt *context, uint16 slave, ec_eepromSMt* SM, uint16 n); +uint32 ecx_siiPDO(ecx_contextt *context, uint16 slave, ec_eepromPDOt* PDO, uint8 t); +int ecx_readstate(ecx_contextt *context); +int ecx_writestate(ecx_contextt *context, uint16 slave); +uint16 ecx_statecheck(ecx_contextt *context, uint16 slave, uint16 reqstate, int timeout); +int ecx_mbxempty(ecx_contextt *context, uint16 slave, int timeout); +int ecx_mbxsend(ecx_contextt *context, uint16 slave,ec_mbxbuft *mbx, int timeout); +int ecx_mbxreceive(ecx_contextt *context, uint16 slave, ec_mbxbuft *mbx, int timeout); +void ecx_esidump(ecx_contextt *context, uint16 slave, uint8 *esibuf); +uint32 ecx_readeeprom(ecx_contextt *context, uint16 slave, uint16 eeproma, int timeout); +int ecx_writeeeprom(ecx_contextt *context, uint16 slave, uint16 eeproma, uint16 data, int timeout); +int ecx_eeprom2master(ecx_contextt *context, uint16 slave); +int ecx_eeprom2pdi(ecx_contextt *context, uint16 slave); +uint64 ecx_readeepromAP(ecx_contextt *context, uint16 aiadr, uint16 eeproma, int timeout); +int ecx_writeeepromAP(ecx_contextt *context, uint16 aiadr, uint16 eeproma, uint16 data, int timeout); +uint64 ecx_readeepromFP(ecx_contextt *context, uint16 configadr, uint16 eeproma, int timeout); +int ecx_writeeepromFP(ecx_contextt *context, uint16 configadr, uint16 eeproma, uint16 data, int timeout); +void ecx_readeeprom1(ecx_contextt *context, uint16 slave, uint16 eeproma); +uint32 ecx_readeeprom2(ecx_contextt *context, uint16 slave, int timeout); +int ecx_send_overlap_processdata_group(ecx_contextt *context, uint8 group); +int ecx_receive_processdata_group(ecx_contextt *context, uint8 group, int timeout); +int ecx_send_processdata(ecx_contextt *context); +int ecx_send_overlap_processdata(ecx_contextt *context); +int ecx_receive_processdata(ecx_contextt *context, int timeout); +int ecx_send_processdata_group(ecx_contextt *context, uint8 group); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/soem-src/soem/ethercatprint.c b/soem-src/soem/ethercatprint.c new file mode 100644 index 0000000..6e4f801 --- /dev/null +++ b/soem-src/soem/ethercatprint.c @@ -0,0 +1,375 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +/** \file + * \brief + * Module to convert EtherCAT errors to readable messages. + * + * SDO abort messages and AL status codes are used to relay slave errors to + * the user application. This module converts the binary codes to readable text. + * For the defined error codes see the EtherCAT protocol documentation. + */ + +#include +#include "oshw.h" +#include "ethercattype.h" +#include "ethercatmain.h" + +#define EC_MAXERRORNAME 127 + +/** SDO error list type definition */ +typedef struct +{ + /** Error code returned from SDO */ + uint32 errorcode; + /** Readable error description */ + char errordescription[EC_MAXERRORNAME + 1]; +} ec_sdoerrorlist_t; + +/** AL status code list type definition */ +typedef struct +{ + /** AL status code */ + uint16 ALstatuscode; + /** Readable description */ + char ALstatuscodedescription[EC_MAXERRORNAME + 1]; +} ec_ALstatuscodelist_t; + +/** SoE error list type definition */ +typedef struct +{ + /** SoE error code */ + uint16 errorcode; + /** Readable description */ + char errordescription[EC_MAXERRORNAME + 1]; +} ec_soeerrorlist_t; + +/** MBX error list type definition */ +typedef struct +{ + /** MBX error code */ + uint16 errorcode; + /** Readable description */ + char errordescription[EC_MAXERRORNAME + 1]; +} ec_mbxerrorlist_t; + +char estring[EC_MAXERRORNAME]; + +/** SDO error list definition */ +const ec_sdoerrorlist_t ec_sdoerrorlist[] = { + {0x00000000, "No error" }, + {0x05030000, "Toggle bit not changed" }, + {0x05040000, "SDO protocol timeout" }, + {0x05040001, "Client/Server command specifier not valid or unknown" }, + {0x05040005, "Out of memory" }, + {0x06010000, "Unsupported access to an object" }, + {0x06010001, "Attempt to read to a write only object" }, + {0x06010002, "Attempt to write to a read only object" }, + {0x06010003, "Subindex can not be written, SI0 must be 0 for write access" }, + {0x06010004, "SDO Complete access not supported for variable length objects" }, + {0x06010005, "Object length exceeds mailbox size" }, + {0x06010006, "Object mapped to RxPDO, SDO download blocked" }, + {0x06020000, "The object does not exist in the object directory" }, + {0x06040041, "The object can not be mapped into the PDO" }, + {0x06040042, "The number and length of the objects to be mapped would exceed the PDO length" }, + {0x06040043, "General parameter incompatibility reason" }, + {0x06040047, "General internal incompatibility in the device" }, + {0x06060000, "Access failed due to a hardware error" }, + {0x06070010, "Data type does not match, length of service parameter does not match" }, + {0x06070012, "Data type does not match, length of service parameter too high" }, + {0x06070013, "Data type does not match, length of service parameter too low" }, + {0x06090011, "Subindex does not exist" }, + {0x06090030, "Value range of parameter exceeded (only for write access)" }, + {0x06090031, "Value of parameter written too high" }, + {0x06090032, "Value of parameter written too low" }, + {0x06090036, "Maximum value is less than minimum value" }, + {0x08000000, "General error" }, + {0x08000020, "Data cannot be transferred or stored to the application" }, + {0x08000021, "Data cannot be transferred or stored to the application because of local control" }, + {0x08000022, "Data cannot be transferred or stored to the application because of the present device state" }, + {0x08000023, "Object dictionary dynamic generation fails or no object dictionary is present" }, + {0xffffffff, "Unknown" } +}; + +/** AL status code list definition */ +const ec_ALstatuscodelist_t ec_ALstatuscodelist[] = { + {0x0000 , "No error" }, + {0x0001 , "Unspecified error" }, + {0x0002 , "No memory" }, + {0x0011 , "Invalid requested state change" }, + {0x0012 , "Unknown requested state" }, + {0x0013 , "Bootstrap not supported" }, + {0x0014 , "No valid firmware" }, + {0x0015 , "Invalid mailbox configuration" }, + {0x0016 , "Invalid mailbox configuration" }, + {0x0017 , "Invalid sync manager configuration" }, + {0x0018 , "No valid inputs available" }, + {0x0019 , "No valid outputs" }, + {0x001A , "Synchronization error" }, + {0x001B , "Sync manager watchdog" }, + {0x001C , "Invalid sync Manager types" }, + {0x001D , "Invalid output configuration" }, + {0x001E , "Invalid input configuration" }, + {0x001F , "Invalid watchdog configuration" }, + {0x0020 , "Slave needs cold start" }, + {0x0021 , "Slave needs INIT" }, + {0x0022 , "Slave needs PREOP" }, + {0x0023 , "Slave needs SAFEOP" }, + {0x0024 , "Invalid input mapping" }, + {0x0025 , "Invalid output mapping" }, + {0x0026 , "Inconsistent settings" }, + {0x0027 , "Freerun not supported" }, + {0x0028 , "Synchronisation not supported" }, + {0x0029 , "Freerun needs 3buffer mode" }, + {0x002A , "Background watchdog" }, + {0x002B , "No valid Inputs and Outputs" }, + {0x002C , "Fatal sync error" }, + {0x002D , "No sync error" }, // was "Invalid Output FMMU Configuration" + {0x002E , "Invalid input FMMU configuration" }, + {0x0030 , "Invalid DC SYNC configuration" }, + {0x0031 , "Invalid DC latch configuration" }, + {0x0032 , "PLL error" }, + {0x0033 , "DC sync IO error" }, + {0x0034 , "DC sync timeout error" }, + {0x0035 , "DC invalid sync cycle time" }, + {0x0036 , "DC invalid sync0 cycle time" }, + {0x0037 , "DC invalid sync1 cycle time" }, + {0x0041 , "MBX_AOE" }, + {0x0042 , "MBX_EOE" }, + {0x0043 , "MBX_COE" }, + {0x0044 , "MBX_FOE" }, + {0x0045 , "MBX_SOE" }, + {0x004F , "MBX_VOE" }, + {0x0050 , "EEPROM no access" }, + {0x0051 , "EEPROM error" }, + {0x0060 , "Slave restarted locally" }, + {0x0061 , "Device identification value updated" }, + {0x00f0 , "Application controller available" }, + {0xffff , "Unknown" } +}; + +/** SoE error list definition */ +const ec_soeerrorlist_t ec_soeerrorlist[] = { + {0x0000, "No error" }, + {0x1001, "No IDN" }, + {0x1009, "Invalid access to element 1" }, + {0x2001, "No Name" }, + {0x2002, "Name transmission too short" }, + {0x2003, "Name transmission too long" }, + {0x2004, "Name cannot be changed (read only)" }, + {0x2005, "Name is write-protected at this time" }, + {0x3002, "Attribute transmission too short" }, + {0x3003, "Attribute transmission too long" }, + {0x3004, "Attribute cannot be changed (read only)" }, + {0x3005, "Attribute is write-protected at this time" }, + {0x4001, "No units" }, + {0x4002, "Unit transmission too short" }, + {0x4003, "Unit transmission too long" }, + {0x4004, "Unit cannot be changed (read only)" }, + {0x4005, "Unit is write-protected at this time" }, + {0x5001, "No minimum input value" }, + {0x5002, "Minimum input value transmission too short" }, + {0x5003, "Minimum input value transmission too long" }, + {0x5004, "Minimum input value cannot be changed (read only)" }, + {0x5005, "Minimum input value is write-protected at this time" }, + {0x6001, "No maximum input value" }, + {0x6002, "Maximum input value transmission too short" }, + {0x6003, "Maximum input value transmission too long" }, + {0x6004, "Maximum input value cannot be changed (read only)" }, + {0x6005, "Maximum input value is write-protected at this time" }, + {0x7002, "Operation data transmission too short" }, + {0x7003, "Operation data transmission too long" }, + {0x7004, "Operation data cannot be changed (read only)" }, + {0x7005, "Operation data is write-protected at this time (state)" }, + {0x7006, "Operation data is smaller than the minimum input value" }, + {0x7007, "Operation data is smaller than the maximum input value" }, + {0x7008, "Invalid operation data:Configured IDN will not be supported" }, + {0x7009, "Operation data write protected by a password" }, + {0x700A, "Operation data is write protected, it is configured cyclically" }, + {0x700B, "Invalid indirect addressing: (e.g., data container, list handling)" }, + {0x700C, "Operation data is write protected, due to other settings" }, + {0x700D, "Reserved" }, + {0x7010, "Procedure command already active" }, + {0x7011, "Procedure command not interruptible" }, + {0x7012, "Procedure command at this time not executable (state)" }, + {0x7013, "Procedure command not executable (invalid or false parameters)" }, + {0x7014, "No data state" }, + {0x8001, "No default value" }, + {0x8002, "Default value transmission too long" }, + {0x8004, "Default value cannot be changed, read only" }, + {0x800A, "Invalid drive number" }, + {0x800A, "General error" }, + {0x800A, "No element addressed" }, + {0xffff, "Unknown" } +}; + +/** MBX error list definition */ +const ec_mbxerrorlist_t ec_mbxerrorlist[] = { + {0x0000, "No error" }, + {0x0001, "Syntax of 6 octet Mailbox Header is wrong" }, + {0x0002, "The mailbox protocol is not supported" }, + {0x0003, "Channel Field contains wrong value"}, + {0x0004, "The service is no supported"}, + {0x0005, "Invalid mailbox header"}, + {0x0006, "Length of received mailbox data is too short"}, + {0x0007, "No more memory in slave"}, + {0x0008, "The length of data is inconsistent"}, + {0xffff, "Unknown"} +}; + +/** Look up text string that belongs to SDO error code. + * + * @param[in] sdoerrorcode = SDO error code as defined in EtherCAT protocol + * @return readable string + */ +const char* ec_sdoerror2string( uint32 sdoerrorcode) +{ + int i = 0; + + while ( (ec_sdoerrorlist[i].errorcode != 0xffffffffUL) && + (ec_sdoerrorlist[i].errorcode != sdoerrorcode) ) + { + i++; + } + + return ec_sdoerrorlist[i].errordescription; +} + +/** Look up text string that belongs to AL status code. + * + * @param[in] ALstatuscode = AL status code as defined in EtherCAT protocol + * @return readable string + */ +char* ec_ALstatuscode2string( uint16 ALstatuscode) +{ + int i = 0; + + while ( (ec_ALstatuscodelist[i].ALstatuscode != 0xffff) && + (ec_ALstatuscodelist[i].ALstatuscode != ALstatuscode) ) + { + i++; + } + + return (char *) ec_ALstatuscodelist[i].ALstatuscodedescription; +} + +/** Look up text string that belongs to SoE error code. + * + * @param[in] errorcode = SoE error code as defined in EtherCAT protocol + * @return readable string + */ +char* ec_soeerror2string( uint16 errorcode) +{ + int i = 0; + + while ( (ec_soeerrorlist[i].errorcode != 0xffff) && + (ec_soeerrorlist[i].errorcode != errorcode) ) + { + i++; + } + + return (char *) ec_soeerrorlist[i].errordescription; +} + +/** Look up text string that belongs to MBX error code. + * + * @param[in] errorcode = MBX error code as defined in EtherCAT protocol + * @return readable string + */ +char* ec_mbxerror2string( uint16 errorcode) +{ + int i = 0; + + while ( (ec_mbxerrorlist[i].errorcode != 0xffff) && + (ec_mbxerrorlist[i].errorcode != errorcode) ) + { + i++; + } + + return (char *) ec_mbxerrorlist[i].errordescription; +} + +/** Convert an error to text string. + * + * @param[in] Ec = Struct describing the error. + * @return readable string + */ +char* ecx_err2string(const ec_errort Ec) +{ + char timestr[20]; + sprintf(timestr, "Time:%12.3f", Ec.Time.sec + (Ec.Time.usec / 1000000.0) ); + switch (Ec.Etype) + { + case EC_ERR_TYPE_SDO_ERROR: + { + sprintf(estring, "%s SDO slave:%d index:%4.4x.%2.2x error:%8.8x %s\n", + timestr, Ec.Slave, Ec.Index, Ec.SubIdx, (unsigned)Ec.AbortCode, ec_sdoerror2string(Ec.AbortCode)); + break; + } + case EC_ERR_TYPE_EMERGENCY: + { + sprintf(estring, "%s EMERGENCY slave:%d error:%4.4x\n", + timestr, Ec.Slave, Ec.ErrorCode); + break; + } + case EC_ERR_TYPE_PACKET_ERROR: + { + sprintf(estring, "%s PACKET slave:%d index:%4.4x.%2.2x error:%d\n", + timestr, Ec.Slave, Ec.Index, Ec.SubIdx, Ec.ErrorCode); + break; + } + case EC_ERR_TYPE_SDOINFO_ERROR: + { + sprintf(estring, "%s SDO slave:%d index:%4.4x.%2.2x error:%8.8x %s\n", + timestr, Ec.Slave, Ec.Index, Ec.SubIdx, (unsigned)Ec.AbortCode, ec_sdoerror2string(Ec.AbortCode)); + break; + } + case EC_ERR_TYPE_SOE_ERROR: + { + sprintf(estring, "%s SoE slave:%d IDN:%4.4x error:%4.4x %s\n", + timestr, Ec.Slave, Ec.Index, (unsigned)Ec.AbortCode, ec_soeerror2string(Ec.ErrorCode)); + break; + } + case EC_ERR_TYPE_MBX_ERROR: + { + sprintf(estring, "%s MBX slave:%d error:%4.4x %s\n", + timestr, Ec.Slave, Ec.ErrorCode, ec_mbxerror2string(Ec.ErrorCode)); + break; + } + default: + { + sprintf(estring, "%s error:%8.8x\n", + timestr, (unsigned)Ec.AbortCode); + break; + } + } + return (char*) estring; +} + +/** Look up error in ec_errorlist and convert to text string. + * + * @param[in] context = context struct + * @return readable string + */ +char* ecx_elist2string(ecx_contextt *context) +{ + ec_errort Ec; + + if (ecx_poperror(context, &Ec)) + { + return ecx_err2string(Ec); + } + else + { + return ""; + } +} + +#ifdef EC_VER1 +char* ec_elist2string(void) +{ + return ecx_elist2string(&ecx_context); +} +#endif diff --git a/soem-src/soem/ethercatprint.h b/soem-src/soem/ethercatprint.h new file mode 100644 index 0000000..43acc2e --- /dev/null +++ b/soem-src/soem/ethercatprint.h @@ -0,0 +1,34 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +/** \file + * \brief + * Headerfile for ethercatprint.c + */ + +#ifndef _ethercatprint_ +#define _ethercatprint_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +char* ec_sdoerror2string( uint32 sdoerrorcode); +char* ec_ALstatuscode2string( uint16 ALstatuscode); +char* ec_soeerror2string( uint16 errorcode); +char* ec_mbxerror2string( uint16 errorcode); +char* ecx_err2string(const ec_errort Ec); +char* ecx_elist2string(ecx_contextt *context); + +#ifdef EC_VER1 +char* ec_elist2string(void); +#endif + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/soem-src/soem/ethercatsoe.c b/soem-src/soem/ethercatsoe.c new file mode 100644 index 0000000..eb83a64 --- /dev/null +++ b/soem-src/soem/ethercatsoe.c @@ -0,0 +1,389 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +/** \file + * \brief + * Servo over EtherCAT (SoE) Module. + */ + +#include +#include +#include "osal.h" +#include "oshw.h" +#include "ethercattype.h" +#include "ethercatbase.h" +#include "ethercatmain.h" +#include "ethercatsoe.h" + +#define EC_SOE_MAX_DRIVES 8 + +/** SoE (Servo over EtherCAT) mailbox structure */ +PACKED_BEGIN +typedef struct PACKED +{ + ec_mbxheadert MbxHeader; + uint8 opCode :3; + uint8 incomplete :1; + uint8 error :1; + uint8 driveNo :3; + uint8 elementflags; + union + { + uint16 idn; + uint16 fragmentsleft; + }; +} ec_SoEt; +PACKED_END + +/** Report SoE error. + * + * @param[in] context = context struct + * @param[in] Slave = Slave number + * @param[in] idn = IDN that generated error + * @param[in] Error = Error code, see EtherCAT documentation for list + */ +void ecx_SoEerror(ecx_contextt *context, uint16 Slave, uint16 idn, uint16 Error) +{ + ec_errort Ec; + + memset(&Ec, 0, sizeof(Ec)); + Ec.Time = osal_current_time(); + Ec.Slave = Slave; + Ec.Index = idn; + Ec.SubIdx = 0; + *(context->ecaterror) = TRUE; + Ec.Etype = EC_ERR_TYPE_SOE_ERROR; + Ec.ErrorCode = Error; + ecx_pusherror(context, &Ec); +} + +/** SoE read, blocking. + * + * The IDN object of the selected slave and DriveNo is read. If a response + * is larger than the mailbox size then the response is segmented. The function + * will combine all segments and copy them to the parameter buffer. + * + * @param[in] context = context struct + * @param[in] slave = Slave number + * @param[in] driveNo = Drive number in slave + * @param[in] elementflags = Flags to select what properties of IDN are to be transferred. + * @param[in] idn = IDN. + * @param[in,out] psize = Size in bytes of parameter buffer, returns bytes read from SoE. + * @param[out] p = Pointer to parameter buffer + * @param[in] timeout = Timeout in us, standard is EC_TIMEOUTRXM + * @return Workcounter from last slave response + */ +int ecx_SoEread(ecx_contextt *context, uint16 slave, uint8 driveNo, uint8 elementflags, uint16 idn, int *psize, void *p, int timeout) +{ + ec_SoEt *SoEp, *aSoEp; + int totalsize, framedatasize; + int wkc; + uint8 *bp; + uint8 *mp; + uint16 *errorcode; + ec_mbxbuft MbxIn, MbxOut; + uint8 cnt; + boolean NotLast; + + ec_clearmbx(&MbxIn); + /* Empty slave out mailbox if something is in. Timeout set to 0 */ + wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, 0); + ec_clearmbx(&MbxOut); + aSoEp = (ec_SoEt *)&MbxIn; + SoEp = (ec_SoEt *)&MbxOut; + SoEp->MbxHeader.length = htoes(sizeof(ec_SoEt) - sizeof(ec_mbxheadert)); + SoEp->MbxHeader.address = htoes(0x0000); + SoEp->MbxHeader.priority = 0x00; + /* get new mailbox count value, used as session handle */ + cnt = ec_nextmbxcnt(context->slavelist[slave].mbx_cnt); + context->slavelist[slave].mbx_cnt = cnt; + SoEp->MbxHeader.mbxtype = ECT_MBXT_SOE + MBX_HDR_SET_CNT(cnt); /* SoE */ + SoEp->opCode = ECT_SOE_READREQ; + SoEp->incomplete = 0; + SoEp->error = 0; + SoEp->driveNo = driveNo; + SoEp->elementflags = elementflags; + SoEp->idn = htoes(idn); + totalsize = 0; + bp = p; + mp = (uint8 *)&MbxIn + sizeof(ec_SoEt); + NotLast = TRUE; + /* send SoE request to slave */ + wkc = ecx_mbxsend(context, slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM); + if (wkc > 0) /* succeeded to place mailbox in slave ? */ + { + while (NotLast) + { + /* clean mailboxbuffer */ + ec_clearmbx(&MbxIn); + /* read slave response */ + wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, timeout); + if (wkc > 0) /* succeeded to read slave response ? */ + { + /* slave response should be SoE, ReadRes */ + if (((aSoEp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_SOE) && + (aSoEp->opCode == ECT_SOE_READRES) && + (aSoEp->error == 0) && + (aSoEp->driveNo == driveNo) && + (aSoEp->elementflags == elementflags)) + { + framedatasize = etohs(aSoEp->MbxHeader.length) - sizeof(ec_SoEt) + sizeof(ec_mbxheadert); + totalsize += framedatasize; + /* Does parameter fit in parameter buffer ? */ + if (totalsize <= *psize) + { + /* copy parameter data in parameter buffer */ + memcpy(bp, mp, framedatasize); + /* increment buffer pointer */ + bp += framedatasize; + } + else + { + framedatasize -= totalsize - *psize; + totalsize = *psize; + /* copy parameter data in parameter buffer */ + if (framedatasize > 0) memcpy(bp, mp, framedatasize); + } + + if (!aSoEp->incomplete) + { + NotLast = FALSE; + *psize = totalsize; + } + } + /* other slave response */ + else + { + NotLast = FALSE; + if (((aSoEp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_SOE) && + (aSoEp->opCode == ECT_SOE_READRES) && + (aSoEp->error == 1)) + { + mp = (uint8 *)&MbxIn + (etohs(aSoEp->MbxHeader.length) + sizeof(ec_mbxheadert) - sizeof(uint16)); + errorcode = (uint16 *)mp; + ecx_SoEerror(context, slave, idn, *errorcode); + } + else + { + ecx_packeterror(context, slave, idn, 0, 1); /* Unexpected frame returned */ + } + wkc = 0; + } + } + else + { + NotLast = FALSE; + ecx_packeterror(context, slave, idn, 0, 4); /* no response */ + } + } + } + return wkc; +} + +/** SoE write, blocking. + * + * The IDN object of the selected slave and DriveNo is written. If a response + * is larger than the mailbox size then the response is segmented. + * + * @param[in] context = context struct + * @param[in] slave = Slave number + * @param[in] driveNo = Drive number in slave + * @param[in] elementflags = Flags to select what properties of IDN are to be transferred. + * @param[in] idn = IDN. + * @param[in] psize = Size in bytes of parameter buffer. + * @param[out] p = Pointer to parameter buffer + * @param[in] timeout = Timeout in us, standard is EC_TIMEOUTRXM + * @return Workcounter from last slave response + */ +int ecx_SoEwrite(ecx_contextt *context, uint16 slave, uint8 driveNo, uint8 elementflags, uint16 idn, int psize, void *p, int timeout) +{ + ec_SoEt *SoEp, *aSoEp; + int framedatasize, maxdata; + int wkc; + uint8 *mp; + uint8 *hp; + uint16 *errorcode; + ec_mbxbuft MbxIn, MbxOut; + uint8 cnt; + boolean NotLast; + + ec_clearmbx(&MbxIn); + /* Empty slave out mailbox if something is in. Timeout set to 0 */ + wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, 0); + ec_clearmbx(&MbxOut); + aSoEp = (ec_SoEt *)&MbxIn; + SoEp = (ec_SoEt *)&MbxOut; + SoEp->MbxHeader.address = htoes(0x0000); + SoEp->MbxHeader.priority = 0x00; + SoEp->opCode = ECT_SOE_WRITEREQ; + SoEp->error = 0; + SoEp->driveNo = driveNo; + SoEp->elementflags = elementflags; + hp = p; + mp = (uint8 *)&MbxOut + sizeof(ec_SoEt); + maxdata = context->slavelist[slave].mbx_l - sizeof(ec_SoEt); + NotLast = TRUE; + while (NotLast) + { + framedatasize = psize; + NotLast = FALSE; + SoEp->idn = htoes(idn); + SoEp->incomplete = 0; + if (framedatasize > maxdata) + { + framedatasize = maxdata; /* segmented transfer needed */ + NotLast = TRUE; + SoEp->incomplete = 1; + SoEp->fragmentsleft = (uint16)(psize / maxdata); + } + SoEp->MbxHeader.length = htoes((uint16)(sizeof(ec_SoEt) - sizeof(ec_mbxheadert) + framedatasize)); + /* get new mailbox counter, used for session handle */ + cnt = ec_nextmbxcnt(context->slavelist[slave].mbx_cnt); + context->slavelist[slave].mbx_cnt = cnt; + SoEp->MbxHeader.mbxtype = ECT_MBXT_SOE + MBX_HDR_SET_CNT(cnt); /* SoE */ + /* copy parameter data to mailbox */ + memcpy(mp, hp, framedatasize); + hp += framedatasize; + psize -= framedatasize; + /* send SoE request to slave */ + wkc = ecx_mbxsend(context, slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM); + if (wkc > 0) /* succeeded to place mailbox in slave ? */ + { + if (!NotLast || !ecx_mbxempty(context, slave, timeout)) + { + /* clean mailboxbuffer */ + ec_clearmbx(&MbxIn); + /* read slave response */ + wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, timeout); + if (wkc > 0) /* succeeded to read slave response ? */ + { + NotLast = FALSE; + /* slave response should be SoE, WriteRes */ + if (((aSoEp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_SOE) && + (aSoEp->opCode == ECT_SOE_WRITERES) && + (aSoEp->error == 0) && + (aSoEp->driveNo == driveNo) && + (aSoEp->elementflags == elementflags)) + { + /* SoE write succeeded */ + } + /* other slave response */ + else + { + if (((aSoEp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_SOE) && + (aSoEp->opCode == ECT_SOE_READRES) && + (aSoEp->error == 1)) + { + mp = (uint8 *)&MbxIn + (etohs(aSoEp->MbxHeader.length) + sizeof(ec_mbxheadert) - sizeof(uint16)); + errorcode = (uint16 *)mp; + ecx_SoEerror(context, slave, idn, *errorcode); + } + else + { + ecx_packeterror(context, slave, idn, 0, 1); /* Unexpected frame returned */ + } + wkc = 0; + } + } + else + { + ecx_packeterror(context, slave, idn, 0, 4); /* no response */ + } + } + } + } + return wkc; +} + +/** SoE read AT and MTD mapping. + * + * SoE has standard indexes defined for mapping. This function + * tries to read them and collect a full input and output mapping size + * of designated slave. + * + * @param[in] context = context struct + * @param[in] slave = Slave number + * @param[out] Osize = Size in bits of output mapping (MTD) found + * @param[out] Isize = Size in bits of input mapping (AT) found + * @return >0 if mapping successful. + */ +int ecx_readIDNmap(ecx_contextt *context, uint16 slave, uint32 *Osize, uint32 *Isize) +{ + int retVal = 0; + int wkc; + int psize; + uint8 driveNr; + uint16 entries, itemcount; + ec_SoEmappingt SoEmapping; + ec_SoEattributet SoEattribute; + + *Isize = 0; + *Osize = 0; + for(driveNr = 0; driveNr < EC_SOE_MAX_DRIVES; driveNr++) + { + psize = sizeof(SoEmapping); + /* read output mapping via SoE */ + wkc = ecx_SoEread(context, slave, driveNr, EC_SOE_VALUE_B, EC_IDN_MDTCONFIG, &psize, &SoEmapping, EC_TIMEOUTRXM); + if ((wkc > 0) && (psize >= 4) && ((entries = etohs(SoEmapping.currentlength) / 2) > 0) && (entries <= EC_SOE_MAXMAPPING)) + { + /* command word (uint16) is always mapped but not in list */ + *Osize += 16; + for (itemcount = 0 ; itemcount < entries ; itemcount++) + { + psize = sizeof(SoEattribute); + /* read attribute of each IDN in mapping list */ + wkc = ecx_SoEread(context, slave, driveNr, EC_SOE_ATTRIBUTE_B, SoEmapping.idn[itemcount], &psize, &SoEattribute, EC_TIMEOUTRXM); + if ((wkc > 0) && (!SoEattribute.list)) + { + /* length : 0 = 8bit, 1 = 16bit .... */ + *Osize += (int)8 << SoEattribute.length; + } + } + } + psize = sizeof(SoEmapping); + /* read input mapping via SoE */ + wkc = ecx_SoEread(context, slave, driveNr, EC_SOE_VALUE_B, EC_IDN_ATCONFIG, &psize, &SoEmapping, EC_TIMEOUTRXM); + if ((wkc > 0) && (psize >= 4) && ((entries = etohs(SoEmapping.currentlength) / 2) > 0) && (entries <= EC_SOE_MAXMAPPING)) + { + /* status word (uint16) is always mapped but not in list */ + *Isize += 16; + for (itemcount = 0 ; itemcount < entries ; itemcount++) + { + psize = sizeof(SoEattribute); + /* read attribute of each IDN in mapping list */ + wkc = ecx_SoEread(context, slave, driveNr, EC_SOE_ATTRIBUTE_B, SoEmapping.idn[itemcount], &psize, &SoEattribute, EC_TIMEOUTRXM); + if ((wkc > 0) && (!SoEattribute.list)) + { + /* length : 0 = 8bit, 1 = 16bit .... */ + *Isize += (int)8 << SoEattribute.length; + } + } + } + } + + /* found some I/O bits ? */ + if ((*Isize > 0) || (*Osize > 0)) + { + retVal = 1; + } + return retVal; +} + +#ifdef EC_VER1 +int ec_SoEread(uint16 slave, uint8 driveNo, uint8 elementflags, uint16 idn, int *psize, void *p, int timeout) +{ + return ecx_SoEread(&ecx_context, slave, driveNo, elementflags, idn, psize, p, timeout); +} + +int ec_SoEwrite(uint16 slave, uint8 driveNo, uint8 elementflags, uint16 idn, int psize, void *p, int timeout) +{ + return ecx_SoEwrite(&ecx_context, slave, driveNo, elementflags, idn, psize, p, timeout); +} + +int ec_readIDNmap(uint16 slave, uint32 *Osize, uint32 *Isize) +{ + return ecx_readIDNmap(&ecx_context, slave, Osize, Isize); +} +#endif diff --git a/soem-src/soem/ethercatsoe.h b/soem-src/soem/ethercatsoe.h new file mode 100644 index 0000000..35d76d1 --- /dev/null +++ b/soem-src/soem/ethercatsoe.h @@ -0,0 +1,130 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +/** \file + * \brief + * Headerfile for ethercatsoe.c + */ + +#ifndef _ethercatsoe_ +#define _ethercatsoe_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#define EC_SOE_DATASTATE_B 0x01 +#define EC_SOE_NAME_B 0x02 +#define EC_SOE_ATTRIBUTE_B 0x04 +#define EC_SOE_UNIT_B 0x08 +#define EC_SOE_MIN_B 0x10 +#define EC_SOE_MAX_B 0x20 +#define EC_SOE_VALUE_B 0x40 +#define EC_SOE_DEFAULT_B 0x80 + +#define EC_SOE_MAXNAME 60 +#define EC_SOE_MAXMAPPING 64 + +#define EC_IDN_MDTCONFIG 24 +#define EC_IDN_ATCONFIG 16 + +/** SoE name structure */ +PACKED_BEGIN +typedef struct PACKED +{ + /** current length in bytes of list */ + uint16 currentlength; + /** maximum length in bytes of list */ + uint16 maxlength; + char name[EC_SOE_MAXNAME]; +} ec_SoEnamet; +PACKED_END + +/** SoE list structure */ +PACKED_BEGIN +typedef struct PACKED +{ + /** current length in bytes of list */ + uint16 currentlength; + /** maximum length in bytes of list */ + uint16 maxlength; + union + { + uint8 byte[8]; + uint16 word[4]; + uint32 dword[2]; + uint64 lword[1]; + }; +} ec_SoElistt; +PACKED_END + +/** SoE IDN mapping structure */ +PACKED_BEGIN +typedef struct PACKED +{ + /** current length in bytes of list */ + uint16 currentlength; + /** maximum length in bytes of list */ + uint16 maxlength; + uint16 idn[EC_SOE_MAXMAPPING]; +} ec_SoEmappingt; +PACKED_END + +#define EC_SOE_LENGTH_1 0x00 +#define EC_SOE_LENGTH_2 0x01 +#define EC_SOE_LENGTH_4 0x02 +#define EC_SOE_LENGTH_8 0x03 +#define EC_SOE_TYPE_BINARY 0x00 +#define EC_SOE_TYPE_UINT 0x01 +#define EC_SOE_TYPE_INT 0x02 +#define EC_SOE_TYPE_HEX 0x03 +#define EC_SOE_TYPE_STRING 0x04 +#define EC_SOE_TYPE_IDN 0x05 +#define EC_SOE_TYPE_FLOAT 0x06 +#define EC_SOE_TYPE_PARAMETER 0x07 + +/** SoE attribute structure */ +PACKED_BEGIN +typedef struct PACKED +{ + /** evaluation factor for display purposes */ + uint32 evafactor :16; + /** length of IDN element(s) */ + uint32 length :2; + /** IDN is list */ + uint32 list :1; + /** IDN is command */ + uint32 command :1; + /** datatype */ + uint32 datatype :3; + uint32 reserved1 :1; + /** decimals to display if float datatype */ + uint32 decimals :4; + /** write protected in pre-op */ + uint32 wppreop :1; + /** write protected in safe-op */ + uint32 wpsafeop :1; + /** write protected in op */ + uint32 wpop :1; + uint32 reserved2 :1; +} ec_SoEattributet; +PACKED_END + +#ifdef EC_VER1 +int ec_SoEread(uint16 slave, uint8 driveNo, uint8 elementflags, uint16 idn, int *psize, void *p, int timeout); +int ec_SoEwrite(uint16 slave, uint8 driveNo, uint8 elementflags, uint16 idn, int psize, void *p, int timeout); +int ec_readIDNmap(uint16 slave, uint32 *Osize, uint32 *Isize); +#endif + +int ecx_SoEread(ecx_contextt *context, uint16 slave, uint8 driveNo, uint8 elementflags, uint16 idn, int *psize, void *p, int timeout); +int ecx_SoEwrite(ecx_contextt *context, uint16 slave, uint8 driveNo, uint8 elementflags, uint16 idn, int psize, void *p, int timeout); +int ecx_readIDNmap(ecx_contextt *context, uint16 slave, uint32 *Osize, uint32 *Isize); + +#ifdef __cplusplus +} +#endif + +#endif \ No newline at end of file diff --git a/soem-src/soem/ethercattype.h b/soem-src/soem/ethercattype.h new file mode 100644 index 0000000..b6d2a6a --- /dev/null +++ b/soem-src/soem/ethercattype.h @@ -0,0 +1,581 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +/** \file + * \brief + * General typedefs and defines for EtherCAT. + * + * Defines that could need optimisation for specific applications + * are the EC_TIMEOUTxxx. Assumptions for the standard settings are a + * standard linux PC or laptop and a wired connection to maximal 100 slaves. + * For use with wireless connections or lots of slaves the timeouts need + * increasing. For fast systems running Xenomai and RT-net or alike the + * timeouts need to be shorter. + */ + +#ifndef _EC_TYPE_H +#define _EC_TYPE_H + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include "osal.h" +#ifdef USE_SOEM_CONFIG_H +#include "soem_config.h" +#endif + +/** define EC_VER1 if version 1 default context and functions are needed + * define EC_VER2 if application uses only ecx_ functions and own context */ +#if !defined(EC_VER1) && !defined(EC_VER2) +# define EC_VER1 +#endif + + +/** Define little endian target by default if no endian is set */ +#if !defined(EC_LITTLE_ENDIAN) && !defined(EC_BIG_ENDIAN) +# define EC_LITTLE_ENDIAN +#endif + +/** return value no frame returned */ +#define EC_NOFRAME -1 +/** return value unknown frame received */ +#define EC_OTHERFRAME -2 +/** return value general error */ +#define EC_ERROR -3 +/** return value too many slaves */ +#define EC_SLAVECOUNTEXCEEDED -4 +/** return value request timeout */ +#define EC_TIMEOUT -5 +/** maximum EtherCAT frame length in bytes */ +#define EC_MAXECATFRAME 1518 +/** maximum EtherCAT LRW frame length in bytes */ +/* MTU - Ethernet header - length - datagram header - WCK - FCS */ +#define EC_MAXLRWDATA (EC_MAXECATFRAME - 14 - 2 - 10 - 2 - 4) +/** size of DC datagram used in first LRW frame */ +#define EC_FIRSTDCDATAGRAM 20 +/** standard frame buffer size in bytes */ +#define EC_BUFSIZE EC_MAXECATFRAME +/** datagram type EtherCAT */ +#define EC_ECATTYPE 0x1000 +/** number of frame buffers per channel (tx, rx1 rx2) */ +#define EC_MAXBUF 16 +#ifndef USE_SOEM_CONFIG_H +/** timeout value in us for tx frame to return to rx */ +#define EC_TIMEOUTRET 2000 +/** timeout value in us for safe data transfer, max. triple retry */ +#define EC_TIMEOUTRET3 (EC_TIMEOUTRET * 3) +/** timeout value in us for return "safe" variant (f.e. wireless) */ +#define EC_TIMEOUTSAFE 20000 +/** timeout value in us for EEPROM access */ +#define EC_TIMEOUTEEP 20000 +/** timeout value in us for tx mailbox cycle */ +#define EC_TIMEOUTTXM 20000 +/** timeout value in us for rx mailbox cycle */ +#define EC_TIMEOUTRXM 700000 +/** timeout value in us for check statechange */ +#define EC_TIMEOUTSTATE 2000000 +#endif +/** size of EEPROM bitmap cache */ +#define EC_MAXEEPBITMAP 128 +/** size of EEPROM cache buffer */ +#define EC_MAXEEPBUF EC_MAXEEPBITMAP << 5 +/** default number of retries if wkc <= 0 */ +#define EC_DEFAULTRETRIES 3 +/** default group size in 2^x */ +#define EC_LOGGROUPOFFSET 16 + +/** definition for frame buffers */ +typedef uint8 ec_bufT[EC_BUFSIZE]; + +/** ethernet header definition */ +PACKED_BEGIN +typedef struct PACKED +{ + /** destination MAC */ + uint16 da0,da1,da2; + /** source MAC */ + uint16 sa0,sa1,sa2; + /** ethernet type */ + uint16 etype; +} ec_etherheadert; +PACKED_END + +/** ethernet header size */ +#define ETH_HEADERSIZE sizeof(ec_etherheadert) + +/** EtherCAT datagram header definition */ +PACKED_BEGIN +typedef struct PACKED +{ + /** length of EtherCAT datagram */ + uint16 elength; + /** EtherCAT command, see ec_cmdtype */ + uint8 command; + /** index, used in SOEM for Tx to Rx recombination */ + uint8 index; + /** ADP */ + uint16 ADP; + /** ADO */ + uint16 ADO; + /** length of data portion in datagram */ + uint16 dlength; + /** interrupt, currently unused */ + uint16 irpt; +} ec_comt; +PACKED_END + +/** EtherCAT header size */ +#define EC_HEADERSIZE sizeof(ec_comt) +/** size of ec_comt.elength item in EtherCAT header */ +#define EC_ELENGTHSIZE sizeof(uint16) +/** offset position of command in EtherCAT header */ +#define EC_CMDOFFSET EC_ELENGTHSIZE +/** size of workcounter item in EtherCAT datagram */ +#define EC_WKCSIZE sizeof(uint16) +/** definition of datagram follows bit in ec_comt.dlength */ +#define EC_DATAGRAMFOLLOWS (1 << 15) + +/** Possible error codes returned. */ +typedef enum +{ + /** No error */ + EC_ERR_OK = 0, + /** Library already initialized. */ + EC_ERR_ALREADY_INITIALIZED, + /** Library not initialized. */ + EC_ERR_NOT_INITIALIZED, + /** Timeout occurred during execution of the function. */ + EC_ERR_TIMEOUT, + /** No slaves were found. */ + EC_ERR_NO_SLAVES, + /** Function failed. */ + EC_ERR_NOK +} ec_err; + +/** Possible EtherCAT slave states */ +typedef enum +{ + /** No valid state. */ + EC_STATE_NONE = 0x00, + /** Init state*/ + EC_STATE_INIT = 0x01, + /** Pre-operational. */ + EC_STATE_PRE_OP = 0x02, + /** Boot state*/ + EC_STATE_BOOT = 0x03, + /** Safe-operational. */ + EC_STATE_SAFE_OP = 0x04, + /** Operational */ + EC_STATE_OPERATIONAL = 0x08, + /** Error or ACK error */ + EC_STATE_ACK = 0x10, + EC_STATE_ERROR = 0x10 +} ec_state; + +/** Possible buffer states */ +typedef enum +{ + /** Empty */ + EC_BUF_EMPTY = 0x00, + /** Allocated, but not filled */ + EC_BUF_ALLOC = 0x01, + /** Transmitted */ + EC_BUF_TX = 0x02, + /** Received, but not consumed */ + EC_BUF_RCVD = 0x03, + /** Cycle completed */ + EC_BUF_COMPLETE = 0x04 +} ec_bufstate; + +/** Ethercat data types */ +typedef enum +{ + ECT_BOOLEAN = 0x0001, + ECT_INTEGER8 = 0x0002, + ECT_INTEGER16 = 0x0003, + ECT_INTEGER32 = 0x0004, + ECT_UNSIGNED8 = 0x0005, + ECT_UNSIGNED16 = 0x0006, + ECT_UNSIGNED32 = 0x0007, + ECT_REAL32 = 0x0008, + ECT_VISIBLE_STRING = 0x0009, + ECT_OCTET_STRING = 0x000A, + ECT_UNICODE_STRING = 0x000B, + ECT_TIME_OF_DAY = 0x000C, + ECT_TIME_DIFFERENCE = 0x000D, + ECT_DOMAIN = 0x000F, + ECT_INTEGER24 = 0x0010, + ECT_REAL64 = 0x0011, + ECT_INTEGER64 = 0x0015, + ECT_UNSIGNED24 = 0x0016, + ECT_UNSIGNED64 = 0x001B, + ECT_BIT1 = 0x0030, + ECT_BIT2 = 0x0031, + ECT_BIT3 = 0x0032, + ECT_BIT4 = 0x0033, + ECT_BIT5 = 0x0034, + ECT_BIT6 = 0x0035, + ECT_BIT7 = 0x0036, + ECT_BIT8 = 0x0037 +} ec_datatype; + +/** Ethercat command types */ +typedef enum +{ + /** No operation */ + EC_CMD_NOP = 0x00, + /** Auto Increment Read */ + EC_CMD_APRD, + /** Auto Increment Write */ + EC_CMD_APWR, + /** Auto Increment Read Write */ + EC_CMD_APRW, + /** Configured Address Read */ + EC_CMD_FPRD, + /** Configured Address Write */ + EC_CMD_FPWR, + /** Configured Address Read Write */ + EC_CMD_FPRW, + /** Broadcast Read */ + EC_CMD_BRD, + /** Broadcast Write */ + EC_CMD_BWR, + /** Broadcast Read Write */ + EC_CMD_BRW, + /** Logical Memory Read */ + EC_CMD_LRD, + /** Logical Memory Write */ + EC_CMD_LWR, + /** Logical Memory Read Write */ + EC_CMD_LRW, + /** Auto Increment Read Multiple Write */ + EC_CMD_ARMW, + /** Configured Read Multiple Write */ + EC_CMD_FRMW + /** Reserved */ +} ec_cmdtype; + +/** Ethercat EEprom command types */ +typedef enum +{ + /** No operation */ + EC_ECMD_NOP = 0x0000, + /** Read */ + EC_ECMD_READ = 0x0100, + /** Write */ + EC_ECMD_WRITE = 0x0201, + /** Reload */ + EC_ECMD_RELOAD = 0x0300 +} ec_ecmdtype; + +/** EEprom state machine read size */ +#define EC_ESTAT_R64 0x0040 +/** EEprom state machine busy flag */ +#define EC_ESTAT_BUSY 0x8000 +/** EEprom state machine error flag mask */ +#define EC_ESTAT_EMASK 0x7800 +/** EEprom state machine error acknowledge */ +#define EC_ESTAT_NACK 0x2000 + +/* Ethercat SSI (Slave Information Interface) */ + +/** Start address SII sections in Eeprom */ +#define ECT_SII_START 0x0040 + +enum +{ + /** SII category strings */ + ECT_SII_STRING = 10, + /** SII category general */ + ECT_SII_GENERAL = 30, + /** SII category FMMU */ + ECT_SII_FMMU = 40, + /** SII category SM */ + ECT_SII_SM = 41, + /** SII category PDO */ + ECT_SII_PDO = 50 +}; + +/** Item offsets in SII general section */ +enum +{ + ECT_SII_MANUF = 0x0008, + ECT_SII_ID = 0x000a, + ECT_SII_REV = 0x000c, + ECT_SII_SN = 0x000e, + ECT_SII_BOOTRXMBX = 0x0014, + ECT_SII_BOOTTXMBX = 0x0016, + ECT_SII_MBXSIZE = 0x0019, + ECT_SII_TXMBXADR = 0x001a, + ECT_SII_RXMBXADR = 0x0018, + ECT_SII_MBXPROTO = 0x001c +}; + +/** Mailbox types definitions */ +enum +{ + /** Error mailbox type */ + ECT_MBXT_ERR = 0x00, + /** ADS over EtherCAT mailbox type */ + ECT_MBXT_AOE, + /** Ethernet over EtherCAT mailbox type */ + ECT_MBXT_EOE, + /** CANopen over EtherCAT mailbox type */ + ECT_MBXT_COE, + /** File over EtherCAT mailbox type */ + ECT_MBXT_FOE, + /** Servo over EtherCAT mailbox type */ + ECT_MBXT_SOE, + /** Vendor over EtherCAT mailbox type */ + ECT_MBXT_VOE = 0x0f +}; + +/** CoE mailbox types */ +enum +{ + ECT_COES_EMERGENCY = 0x01, + ECT_COES_SDOREQ, + ECT_COES_SDORES, + ECT_COES_TXPDO, + ECT_COES_RXPDO, + ECT_COES_TXPDO_RR, + ECT_COES_RXPDO_RR, + ECT_COES_SDOINFO +}; + +/** CoE SDO commands */ +enum +{ + ECT_SDO_DOWN_INIT = 0x21, + ECT_SDO_DOWN_EXP = 0x23, + ECT_SDO_DOWN_INIT_CA = 0x31, + ECT_SDO_UP_REQ = 0x40, + ECT_SDO_UP_REQ_CA = 0x50, + ECT_SDO_SEG_UP_REQ = 0x60, + ECT_SDO_ABORT = 0x80 +}; + +/** CoE Object Description commands */ +enum +{ + ECT_GET_ODLIST_REQ = 0x01, + ECT_GET_ODLIST_RES = 0x02, + ECT_GET_OD_REQ = 0x03, + ECT_GET_OD_RES = 0x04, + ECT_GET_OE_REQ = 0x05, + ECT_GET_OE_RES = 0x06, + ECT_SDOINFO_ERROR = 0x07 +}; + +/** FoE opcodes */ +enum +{ + ECT_FOE_READ = 0x01, + ECT_FOE_WRITE, + ECT_FOE_DATA, + ECT_FOE_ACK, + ECT_FOE_ERROR, + ECT_FOE_BUSY +}; + +/** SoE opcodes */ +enum +{ + ECT_SOE_READREQ = 0x01, + ECT_SOE_READRES, + ECT_SOE_WRITEREQ, + ECT_SOE_WRITERES, + ECT_SOE_NOTIFICATION, + ECT_SOE_EMERGENCY +}; + +/** Ethercat registers */ +enum +{ + ECT_REG_TYPE = 0x0000, + ECT_REG_PORTDES = 0x0007, + ECT_REG_ESCSUP = 0x0008, + ECT_REG_STADR = 0x0010, + ECT_REG_ALIAS = 0x0012, + ECT_REG_DLCTL = 0x0100, + ECT_REG_DLPORT = 0x0101, + ECT_REG_DLALIAS = 0x0103, + ECT_REG_DLSTAT = 0x0110, + ECT_REG_ALCTL = 0x0120, + ECT_REG_ALSTAT = 0x0130, + ECT_REG_ALSTATCODE = 0x0134, + ECT_REG_PDICTL = 0x0140, + ECT_REG_IRQMASK = 0x0200, + ECT_REG_RXERR = 0x0300, + ECT_REG_FRXERR = 0x0308, + ECT_REG_EPUECNT = 0x030C, + ECT_REG_PECNT = 0x030D, + ECT_REG_PECODE = 0x030E, + ECT_REG_LLCNT = 0x0310, + ECT_REG_WDCNT = 0x0442, + ECT_REG_EEPCFG = 0x0500, + ECT_REG_EEPCTL = 0x0502, + ECT_REG_EEPSTAT = 0x0502, + ECT_REG_EEPADR = 0x0504, + ECT_REG_EEPDAT = 0x0508, + ECT_REG_FMMU0 = 0x0600, + ECT_REG_FMMU1 = ECT_REG_FMMU0 + 0x10, + ECT_REG_FMMU2 = ECT_REG_FMMU1 + 0x10, + ECT_REG_FMMU3 = ECT_REG_FMMU2 + 0x10, + ECT_REG_SM0 = 0x0800, + ECT_REG_SM1 = ECT_REG_SM0 + 0x08, + ECT_REG_SM2 = ECT_REG_SM1 + 0x08, + ECT_REG_SM3 = ECT_REG_SM2 + 0x08, + ECT_REG_SM0STAT = ECT_REG_SM0 + 0x05, + ECT_REG_SM1STAT = ECT_REG_SM1 + 0x05, + ECT_REG_SM1ACT = ECT_REG_SM1 + 0x06, + ECT_REG_SM1CONTR = ECT_REG_SM1 + 0x07, + ECT_REG_DCTIME0 = 0x0900, + ECT_REG_DCTIME1 = 0x0904, + ECT_REG_DCTIME2 = 0x0908, + ECT_REG_DCTIME3 = 0x090C, + ECT_REG_DCSYSTIME = 0x0910, + ECT_REG_DCSOF = 0x0918, + ECT_REG_DCSYSOFFSET = 0x0920, + ECT_REG_DCSYSDELAY = 0x0928, + ECT_REG_DCSYSDIFF = 0x092C, + ECT_REG_DCSPEEDCNT = 0x0930, + ECT_REG_DCTIMEFILT = 0x0934, + ECT_REG_DCCUC = 0x0980, + ECT_REG_DCSYNCACT = 0x0981, + ECT_REG_DCSTART0 = 0x0990, + ECT_REG_DCCYCLE0 = 0x09A0, + ECT_REG_DCCYCLE1 = 0x09A4 +}; + +/** standard SDO Sync Manager Communication Type */ +#define ECT_SDO_SMCOMMTYPE 0x1c00 +/** standard SDO PDO assignment */ +#define ECT_SDO_PDOASSIGN 0x1c10 +/** standard SDO RxPDO assignment */ +#define ECT_SDO_RXPDOASSIGN 0x1c12 +/** standard SDO TxPDO assignment */ +#define ECT_SDO_TXPDOASSIGN 0x1c13 + +/** Ethercat packet type */ +#define ETH_P_ECAT 0x88A4 + +/** Error types */ +typedef enum +{ + EC_ERR_TYPE_SDO_ERROR = 0, + EC_ERR_TYPE_EMERGENCY = 1, + EC_ERR_TYPE_PACKET_ERROR = 3, + EC_ERR_TYPE_SDOINFO_ERROR = 4, + EC_ERR_TYPE_FOE_ERROR = 5, + EC_ERR_TYPE_FOE_BUF2SMALL = 6, + EC_ERR_TYPE_FOE_PACKETNUMBER = 7, + EC_ERR_TYPE_SOE_ERROR = 8, + EC_ERR_TYPE_MBX_ERROR = 9, + EC_ERR_TYPE_FOE_FILE_NOTFOUND = 10, + EC_ERR_TYPE_EOE_INVALID_RX_DATA = 11 +} ec_err_type; + +/** Struct to retrieve errors. */ +typedef struct +{ + /** Time at which the error was generated. */ + ec_timet Time; + /** Signal bit, error set but not read */ + boolean Signal; + /** Slave number that generated the error */ + uint16 Slave; + /** CoE SDO index that generated the error */ + uint16 Index; + /** CoE SDO subindex that generated the error */ + uint8 SubIdx; + /** Type of error */ + ec_err_type Etype; + union + { + /** General abortcode */ + int32 AbortCode; + /** Specific error for Emergency mailbox */ + struct + { + uint16 ErrorCode; + uint8 ErrorReg; + uint8 b1; + uint16 w1; + uint16 w2; + }; + }; +} ec_errort; + +/** Helper macros */ + +/** Set the count value in the Mailbox header */ +#define MBX_HDR_SET_CNT(cnt) ((uint8)((cnt) << 4)) + +/** Macro to make a word from 2 bytes */ +#define MK_WORD(msb, lsb) ((((uint16)(msb))<<8) | (lsb)) +/** Macro to get hi byte of a word */ +#define HI_BYTE(w) ((w) >> 8) +/** Macro to get low byte of a word */ +#define LO_BYTE(w) ((w) & 0x00ff) +/** Macro to swap hi and low byte of a word */ +#define SWAP(w) ((((w)& 0xff00) >> 8) | (((w) & 0x00ff) << 8)) +/** Macro to get hi word of a dword */ +#define LO_WORD(l) ((l) & 0xffff) +/** Macro to get hi word of a dword */ +#define HI_WORD(l) ((l) >> 16) + +#define get_unaligned(ptr) \ + ({ __typeof__(*(ptr)) __tmp; memcpy(&__tmp, (ptr), sizeof(*(ptr))); __tmp; }) + +#define put_unaligned32(val, ptr) \ + (memcpy((ptr), &(val), 4)) + +#define put_unaligned64(val, ptr) \ + (memcpy((ptr), &(val), 8)) + +#if !defined(EC_BIG_ENDIAN) && defined(EC_LITTLE_ENDIAN) + + #define htoes(A) (A) + #define htoel(A) (A) + #define htoell(A) (A) + #define etohs(A) (A) + #define etohl(A) (A) + #define etohll(A) (A) + +#elif !defined(EC_LITTLE_ENDIAN) && defined(EC_BIG_ENDIAN) + + #define htoes(A) ((((uint16)(A) & 0xff00) >> 8) | \ + (((uint16)(A) & 0x00ff) << 8)) + #define htoel(A) ((((uint32)(A) & 0xff000000) >> 24) | \ + (((uint32)(A) & 0x00ff0000) >> 8) | \ + (((uint32)(A) & 0x0000ff00) << 8) | \ + (((uint32)(A) & 0x000000ff) << 24)) + #define htoell(A) ((((uint64)(A) & (uint64)0xff00000000000000ULL) >> 56) | \ + (((uint64)(A) & (uint64)0x00ff000000000000ULL) >> 40) | \ + (((uint64)(A) & (uint64)0x0000ff0000000000ULL) >> 24) | \ + (((uint64)(A) & (uint64)0x000000ff00000000ULL) >> 8) | \ + (((uint64)(A) & (uint64)0x00000000ff000000ULL) << 8) | \ + (((uint64)(A) & (uint64)0x0000000000ff0000ULL) << 24) | \ + (((uint64)(A) & (uint64)0x000000000000ff00ULL) << 40) | \ + (((uint64)(A) & (uint64)0x00000000000000ffULL) << 56)) + + #define etohs htoes + #define etohl htoel + #define etohll htoell + +#else + + #error "Must define one of EC_BIG_ENDIAN or EC_LITTLE_ENDIAN" + +#endif + +#ifdef __cplusplus +} +#endif + +#endif /* _EC_TYPE_H */ diff --git a/soem-src/test/intime/ec_master/ec_master.c b/soem-src/test/intime/ec_master/ec_master.c new file mode 100644 index 0000000..035fdd2 --- /dev/null +++ b/soem-src/test/intime/ec_master/ec_master.c @@ -0,0 +1,362 @@ +/***************************************************************************** +* +* FILE NAME: ec_master.c +* +* DESCRIPTION: This is the main program module. +* +\*****************************************************************************/ + +#include + + +/***************************************************************************** +* +* FUNCTION: main +* +* DESCRIPTION: +* This is the main program module. +* +\*****************************************************************************/ + +/** \file + * \brief Example code for Simple Open EtherCAT master + * + * Usage : slaveinfo [ifname] [-sdo] [-map] + * Ifname is NIC interface, f.e. eth0. + * Optional -sdo to display CoE object dictionary. + * Optional -map to display slave PDO mapping + * + * This shows the configured slave data. + * + * (c)Arthur Ketels 2010 - 2011 + */ + +#include +#include +#include +#include + +#include "ethercat.h" + + +char IOmap[4096]; +char IOmap2[4096]; + +/** Main slave data array. + * Each slave found on the network gets its own record. + * ec_slave[0] is reserved for the master. Structure gets filled + * in by the configuration function ec_config(). + */ +ec_slavet ec_slave[EC_MAXSLAVE]; +ec_slavet ec_slave2[EC_MAXSLAVE]; +/** number of slaves found on the network */ +int ec_slavecount; +int ec_slavecount2; +/** slave group structure */ +ec_groupt ec_groups[EC_MAXGROUP]; +ec_groupt ec_groups2[EC_MAXGROUP]; + +/** cache for EEPROM read functions */ +static uint8 esibuf[EC_MAXEEPBUF]; +static uint8 esibuf2[EC_MAXEEPBUF]; +/** bitmap for filled cache buffer bytes */ +static uint32 esimap[EC_MAXEEPBITMAP]; +static uint32 esimap2[EC_MAXEEPBITMAP]; +/** current slave for EEPROM cache buffer */ +static ec_eringt ec_elist; +static ec_eringt ec_elist2; +static ec_idxstackT ec_idxstack; +static ec_idxstackT ec_idxstack2; + +/** SyncManager Communication Type struct to store data of one slave */ +static ec_SMcommtypet ec_SMcommtype; +static ec_SMcommtypet ec_SMcommtype2; +/** PDO assign struct to store data of one slave */ +static ec_PDOassignt ec_PDOassign; +static ec_PDOassignt ec_PDOassign2; +/** PDO description struct to store data of one slave */ +static ec_PDOdesct ec_PDOdesc; +static ec_PDOdesct ec_PDOdesc2; + +/** buffer for EEPROM SM data */ +static ec_eepromSMt ec_SM; +static ec_eepromSMt ec_SM2; +/** buffer for EEPROM FMMU data */ +static ec_eepromFMMUt ec_FMMU; +static ec_eepromFMMUt ec_FMMU2; + +/** Global variable TRUE if error available in error stack */ +static boolean EcatError = FALSE; +static boolean EcatError2 = FALSE; + +int64 ec_DCtime; +int64 ec_DCtime2; + +static ecx_portt ecx_port; +static ecx_portt ecx_port2; + + +static ecx_contextt ctx [] = { + { + &ecx_port, + &ec_slave[0], + &ec_slavecount, + EC_MAXSLAVE, + &ec_groups[0], + EC_MAXGROUP, + &esibuf[0], + &esimap[0], + 0, + &ec_elist, + &ec_idxstack, + &EcatError, + 0, + 0, + &ec_DCtime, + &ec_SMcommtype, + &ec_PDOassign, + &ec_PDOdesc, + &ec_SM, + &ec_FMMU + }, + { + &ecx_port2, + &ec_slave2[0], + &ec_slavecount2, + EC_MAXSLAVE, + &ec_groups2[0], + EC_MAXGROUP, + &esibuf2[0], + &esimap2[0], + 0, + &ec_elist2, + &ec_idxstack2, + &EcatError2, + 0, + 0, + &ec_DCtime2, + &ec_SMcommtype2, + &ec_PDOassign2, + &ec_PDOdesc2, + &ec_SM2, + &ec_FMMU2 + } +}; + +void slaveinfo(char *ifname) +{ + int cnt, i, j, nSM; + int ctx_count; + uint16 ssigen; + int expectedWKC[2]; + int wkc_count; + int chk; + volatile int wkc[2]; + boolean inOP; + + printf("Starting slaveinfo\n"); + + /* initialise SOEM, bind socket to ifname */ + if (ecx_init(&ctx[0],"ie1g1") && ecx_init(&ctx[1],"ie1g0")) + { + printf("ec_init on %s succeeded.\n",ifname); + /* find and auto-config slaves */ + if ( ecx_config_init(&ctx[0],FALSE) > 0 && ecx_config_init(&ctx[1],FALSE) > 0 ) + { + for (ctx_count = 0; ctx_count < 2; ctx_count++) + { + ecx_config_map_group(&ctx[ctx_count], IOmap, 0); + ecx_configdc(&ctx[ctx_count]); + while(*(ctx[ctx_count].ecaterror)) printf("%s", ecx_elist2string(&ctx[ctx_count])); + printf("%d slaves found and configured.\n",*(ctx[ctx_count].slavecount)); + expectedWKC[ctx_count] = ( ctx[ctx_count].grouplist[0].outputsWKC * 2) + ctx[ctx_count].grouplist[0].inputsWKC; + printf("Calculated workcounter %d\n", expectedWKC[ctx_count]); + /* wait for all slaves to reach SAFE_OP state */ + ecx_statecheck(&ctx[ctx_count],0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE * 3); + if (ctx[ctx_count].slavelist[ctx_count].state != EC_STATE_SAFE_OP ) + { + printf("Not all slaves reached safe operational state.\n"); + ecx_readstate(&ctx[ctx_count]); + for(i = 1; i <= *(ctx[ctx_count].slavecount) ; i++) + { + if(ctx[ctx_count].slavelist[i].state != EC_STATE_SAFE_OP) + { + printf("Slave %d State=%2x StatusCode=%4x : %s\n", + i, ctx[ctx_count].slavelist[i].state, ctx[ctx_count].slavelist[i].ALstatuscode, ec_ALstatuscode2string(ctx[ctx_count].slavelist[i].ALstatuscode)); + } + } + } + ecx_readstate(&ctx[ctx_count]); + + for( cnt = 1 ; cnt <= *(ctx[ctx_count].slavecount) ; cnt++) + { + printf("\nSlave:%d\n Name:%s\n Output size: %dbits\n Input size: %dbits\n State: %d\n Delay: %d[ns]\n Has DC: %d\n", + cnt, ctx[ctx_count].slavelist[cnt].name, ctx[ctx_count].slavelist[cnt].Obits, ctx[ctx_count].slavelist[cnt].Ibits, + ctx[ctx_count].slavelist[cnt].state, ctx[ctx_count].slavelist[cnt].pdelay, ctx[ctx_count].slavelist[cnt].hasdc); + if (ctx[ctx_count].slavelist[cnt].hasdc) + { + printf(" DCParentport:%d\n", ctx[ctx_count].slavelist[cnt].parentport); + } + printf(" Activeports:%d.%d.%d.%d\n", (ctx[ctx_count].slavelist[cnt].activeports & 0x01) > 0 , + (ctx[ctx_count].slavelist[cnt].activeports & 0x02) > 0 , + (ctx[ctx_count].slavelist[cnt].activeports & 0x04) > 0 , + (ctx[ctx_count].slavelist[cnt].activeports & 0x08) > 0 ); + printf(" Configured address: %4.4x\n", ctx[ctx_count].slavelist[cnt].configadr); + printf(" Man: %8.8x ID: %8.8x Rev: %8.8x\n", (int)ctx[ctx_count].slavelist[cnt].eep_man, + (int)ctx[ctx_count].slavelist[cnt].eep_id, (int)ctx[ctx_count].slavelist[cnt].eep_rev); + for(nSM = 0 ; nSM < EC_MAXSM ; nSM++) + { + if(ctx[ctx_count].slavelist[cnt].SM[nSM].StartAddr > 0) + printf(" SM%1d A:%4.4x L:%4d F:%8.8x Type:%d\n",nSM, ctx[ctx_count].slavelist[cnt].SM[nSM].StartAddr, + ctx[ctx_count].slavelist[cnt].SM[nSM].SMlength,(int)ctx[ctx_count].slavelist[cnt].SM[nSM].SMflags, + ctx[ctx_count].slavelist[cnt].SMtype[nSM]); + } + for(j = 0 ; j < ctx[ctx_count].slavelist[cnt].FMMUunused ; j++) + { + printf(" FMMU%1d Ls:%8.8x Ll:%4d Lsb:%d Leb:%d Ps:%4.4x Psb:%d Ty:%2.2x Act:%2.2x\n", j, + (int)ctx[ctx_count].slavelist[cnt].FMMU[j].LogStart, ctx[ctx_count].slavelist[cnt].FMMU[j].LogLength, + ctx[ctx_count].slavelist[cnt].FMMU[j].LogStartbit, ctx[ctx_count].slavelist[cnt].FMMU[j].LogEndbit, + ctx[ctx_count].slavelist[cnt].FMMU[j].PhysStart, ctx[ctx_count].slavelist[cnt].FMMU[j].PhysStartBit, + ctx[ctx_count].slavelist[cnt].FMMU[j].FMMUtype, ctx[ctx_count].slavelist[cnt].FMMU[j].FMMUactive); + } + printf(" FMMUfunc 0:%d 1:%d 2:%d 3:%d\n", + ctx[ctx_count].slavelist[cnt].FMMU0func, ctx[ctx_count].slavelist[cnt].FMMU1func, ctx[ctx_count].slavelist[cnt].FMMU2func, + ctx[ctx_count].slavelist[cnt].FMMU3func); + printf(" MBX length wr: %d rd: %d MBX protocols : %2.2x\n", ctx[ctx_count].slavelist[cnt].mbx_l, + ctx[ctx_count].slavelist[cnt].mbx_rl, ctx[ctx_count].slavelist[cnt].mbx_proto); + ssigen = ecx_siifind(&ctx[ctx_count], cnt, ECT_SII_GENERAL); + /* SII general section */ + if (ssigen) + { + ctx[ctx_count].slavelist[cnt].CoEdetails = ecx_siigetbyte(&ctx[ctx_count], cnt, ssigen + 0x07); + ctx[ctx_count].slavelist[cnt].FoEdetails = ecx_siigetbyte(&ctx[ctx_count], cnt, ssigen + 0x08); + ctx[ctx_count].slavelist[cnt].EoEdetails = ecx_siigetbyte(&ctx[ctx_count], cnt, ssigen + 0x09); + ctx[ctx_count].slavelist[cnt].SoEdetails = ecx_siigetbyte(&ctx[ctx_count], cnt, ssigen + 0x0a); + if((ecx_siigetbyte(&ctx[ctx_count], cnt, ssigen + 0x0d) & 0x02) > 0) + { + ctx[ctx_count].slavelist[cnt].blockLRW = 1; + ctx[ctx_count].slavelist[0].blockLRW++; + } + ctx[ctx_count].slavelist[cnt].Ebuscurrent = ecx_siigetbyte(&ctx[ctx_count], cnt, ssigen + 0x0e); + ctx[ctx_count].slavelist[cnt].Ebuscurrent += ecx_siigetbyte(&ctx[ctx_count], cnt, ssigen + 0x0f) << 8; + ctx[ctx_count].slavelist[0].Ebuscurrent += ctx[ctx_count].slavelist[cnt].Ebuscurrent; + } + printf(" CoE details: %2.2x FoE details: %2.2x EoE details: %2.2x SoE details: %2.2x\n", + ctx[ctx_count].slavelist[cnt].CoEdetails, ctx[ctx_count].slavelist[cnt].FoEdetails, ctx[ctx_count].slavelist[cnt].EoEdetails, ctx[ctx_count].slavelist[cnt].SoEdetails); + printf(" Ebus current: %d[mA]\n only LRD/LWR:%d\n", + ctx[ctx_count].slavelist[cnt].Ebuscurrent, ctx[ctx_count].slavelist[cnt].blockLRW); + } + } + + inOP = FALSE; + + printf("Request operational state for all slaves\n"); + expectedWKC[0] = (ctx[0].grouplist[0].outputsWKC * 2) + ctx[0].grouplist[0].inputsWKC; + expectedWKC[1] = (ctx[1].grouplist[0].outputsWKC * 2) + ctx[1].grouplist[0].inputsWKC; + printf("Calculated workcounter master 1 %d\n", expectedWKC[0]); + printf("Calculated workcounter master 2 %d\n", expectedWKC[1]); + ctx[0].slavelist[0].state = EC_STATE_OPERATIONAL; + ctx[1].slavelist[0].state = EC_STATE_OPERATIONAL; + /* send one valid process data to make outputs in slaves happy*/ + ecx_send_processdata(&ctx[0]); + ecx_send_processdata(&ctx[1]); + ecx_receive_processdata(&ctx[0],EC_TIMEOUTRET); + ecx_receive_processdata(&ctx[1],EC_TIMEOUTRET); + /* request OP state for all slaves */ + ecx_writestate(&ctx[0], 0); + ecx_writestate(&ctx[1], 0); + chk = 40; + /* wait for all slaves to reach OP state */ + do + { + ecx_send_processdata(&ctx[0]); + ecx_send_processdata(&ctx[1]); + ecx_receive_processdata(&ctx[0],EC_TIMEOUTRET); + ecx_receive_processdata(&ctx[1],EC_TIMEOUTRET); + ecx_statecheck(&ctx[0],0, EC_STATE_OPERATIONAL, 50000); + ecx_statecheck(&ctx[1],0, EC_STATE_OPERATIONAL, 50000); + } + while (chk-- && (ctx[0].slavelist[0].state != EC_STATE_OPERATIONAL) && (ctx[1].slavelist[0].state != EC_STATE_OPERATIONAL)); + + + *(ctx[0].slavelist[6].outputs) = 0xFF; + *(ctx[1].slavelist[1].outputs) = 0x0F; + + if (ctx[0].slavelist[0].state == EC_STATE_OPERATIONAL && ctx[1].slavelist[0].state == EC_STATE_OPERATIONAL ) + { + printf("Operational state reached for all slaves.\n"); + /* cyclic loop */ + start_RT_trace (1); + wkc_count = 0; + inOP = TRUE; + for(i = 1; i <= 10000; i++) + { + for (ctx_count = 0; ctx_count < 2; ctx_count++) + { + /* Receive processdata */ + log_RT_event('R',(WORD)(1 + ctx_count * 10)); + wkc[ctx_count] = ecx_receive_processdata(&ctx[ctx_count], 0); + log_RT_event('R',(WORD)(99 + ctx_count * 100)); + } + for (ctx_count = 0; ctx_count < 2; ctx_count++) + { + /* Send processdata */ + log_RT_event('S',(WORD)(1 + ctx_count * 10)); + if (!ecx_send_processdata(&ctx[ctx_count])) + { + printf (" Frame no: %d on master %d,Not sentOK\n", i,ctx_count); + } + log_RT_event('S',(WORD)(99 + ctx_count * 100)); + } + knRtSleep(1); + + for (ctx_count = 0; ctx_count < 2; ctx_count++) + { + if(wkc[ctx_count] >= expectedWKC[ctx_count]) + { + } + else + { + printf (" Frame no: %d on master %d , wc not wkc >= expectedWKC, %d\n",i, ctx_count, wkc[ctx_count]); + } + } + } + stop_RT_trace (); + inOP = FALSE; + + } + } + else + { + printf("No slaves found!\n"); + } + printf("End slaveinfo, close socket\n"); + /* stop SOEM, close socket */ + ecx_close(&ctx[0]); + ecx_close(&ctx[1]); + } + else + { + printf("No socket connection on %s\nExcecute as root\n",ifname); + } +} + + +void main(int argc, char* argv[]) +{ + printf("SOEM (Simple Open EtherCAT Master)\nSlaveinfo\n"); + + if (argc > 1) + { + /* start slaveinfo */ + slaveinfo(argv[1]); + } + else + { + printf("Usage: slaveinfo ifname [options]\nifname = eth0 for example\nOptions :\n -sdo : print SDO info\n -map : print mapping\n"); + } + + printf("End program\n"); + +} + diff --git a/soem-src/test/linux/aliastool.c b/soem-src/test/linux/aliastool.c new file mode 100644 index 0000000..c9c649c --- /dev/null +++ b/soem-src/test/linux/aliastool.c @@ -0,0 +1,429 @@ +/** \file + * \brief EEprom tool for Simple Open EtherCAT master + * + * Usage : eepromtool ifname slave OPTION fname|alias + * ifname is NIC interface, f.e. eth0 + * slave = slave number in EtherCAT order 1..n + * -r read EEPROM, output binary format + * -ri read EEPROM, output Intel Hex format + * -w write EEPROM, input binary format + * -wi write EEPROM, input Intel Hex format + * -i display EEPROM information + * -walias write slave alias in EEPROM + * + * (c)Arthur Ketels 2010-2012 + */ + +#include +#include +#include +#include +#include +#include + +#include "ethercat.h" + +#define MAXBUF 32768 +#define STDBUF 2048 +#define MINBUF 128 + +#define MODE_NONE 0 +#define MODE_READBIN 1 +#define MODE_READINTEL 2 +#define MODE_WRITEBIN 3 +#define MODE_WRITEINTEL 4 +#define MODE_WRITEALIAS 5 +#define MODE_INFO 6 + +#define MAXSLENGTH 256 + +uint8 ebuf[MAXBUF]; +uint8 ob; +uint16 ow; +int os; +int slave; +int alias; +struct timeval tstart,tend, tdif; +int wkc; +int mode; +char sline[MAXSLENGTH]; + +#define IHEXLENGTH 0x20 + +int input_bin(char *fname, int *length) +{ + FILE *fp; + + int cc = 0, c; + + fp = fopen(fname, "rb"); + if(fp == NULL) + return 0; + while (((c = fgetc(fp)) != EOF) && (cc < MAXBUF)) + ebuf[cc++] = (uint8)c; + *length = cc; + fclose(fp); + + return 1; +} + +int input_intelhex(char *fname, int *start, int *length) +{ + FILE *fp; + + int c, sc, retval = 1; + int ll, ladr, lt, sn, i, lval; + int hstart, hlength, sum; + + fp = fopen(fname, "r"); + if(fp == NULL) + return 0; + hstart = MAXBUF; + hlength = 0; + sum = 0; + do + { + memset(sline, 0x00, MAXSLENGTH); + sc = 0; + while (((c = fgetc(fp)) != EOF) && (c != 0x0A) && (sc < (MAXSLENGTH -1))) + sline[sc++] = (uint8)c; + if ((c != EOF) && ((sc < 11) || (sline[0] != ':'))) + { + c = EOF; + retval = 0; + printf("Invalid Intel Hex format.\n"); + } + if (c != EOF) + { + sn = sscanf(sline , ":%2x%4x%2x", &ll, &ladr, <); + if ((sn == 3) && ((ladr + ll) <= MAXBUF) && (lt == 0)) + { + sum = ll + (ladr >> 8) + (ladr & 0xff) + lt; + if(ladr < hstart) hstart = ladr; + for(i = 0; i < ll ; i++) + { + sn = sscanf(&sline[9 + (i << 1)], "%2x", &lval); + ebuf[ladr + i] = (uint8)lval; + sum += (uint8)lval; + } + if(((ladr + ll) - hstart) > hlength) + hlength = (ladr + ll) - hstart; + sum = (0x100 - sum) & 0xff; + sn = sscanf(&sline[9 + (i << 1)], "%2x", &lval); + if (!sn || ((sum - lval) != 0)) + { + c = EOF; + retval = 0; + printf("Invalid checksum.\n"); + } + } + } + } + while (c != EOF); + if (retval) + { + *length = hlength; + *start = hstart; + } + fclose(fp); + + return retval; +} + +int output_bin(char *fname, int length) +{ + FILE *fp; + + int cc; + + fp = fopen(fname, "wb"); + if(fp == NULL) + return 0; + for (cc = 0 ; cc < length ; cc++) + fputc( ebuf[cc], fp); + fclose(fp); + + return 1; +} + +int output_intelhex(char *fname, int length) +{ + FILE *fp; + + int cc = 0, ll, sum, i; + + fp = fopen(fname, "w"); + if(fp == NULL) + return 0; + while (cc < length) + { + ll = length - cc; + if (ll > IHEXLENGTH) ll = IHEXLENGTH; + sum = ll + (cc >> 8) + (cc & 0xff); + fprintf(fp, ":%2.2X%4.4X00", ll, cc); + for (i = 0; i < ll; i++) + { + fprintf(fp, "%2.2X", ebuf[cc + i]); + sum += ebuf[cc + i]; + } + fprintf(fp, "%2.2X\n", (0x100 - sum) & 0xff); + cc += ll; + } + fprintf(fp, ":00000001FF\n"); + fclose(fp); + + return 1; +} + +int eeprom_read(int slave, int start, int length) +{ + int i, wkc, ainc = 4; + uint16 estat, aiadr; + uint32 b4; + uint64 b8; + uint8 eepctl; + + if((ec_slavecount >= slave) && (slave > 0) && ((start + length) <= MAXBUF)) + { + aiadr = 1 - slave; + eepctl = 2; + wkc = ec_APWR(aiadr, ECT_REG_EEPCFG, sizeof(eepctl), &eepctl , EC_TIMEOUTRET); /* force Eeprom from PDI */ + eepctl = 0; + wkc = ec_APWR(aiadr, ECT_REG_EEPCFG, sizeof(eepctl), &eepctl , EC_TIMEOUTRET); /* set Eeprom to master */ + + estat = 0x0000; + aiadr = 1 - slave; + wkc=ec_APRD(aiadr, ECT_REG_EEPSTAT, sizeof(estat), &estat, EC_TIMEOUTRET); /* read eeprom status */ + estat = etohs(estat); + if (estat & EC_ESTAT_R64) + { + ainc = 8; + for (i = start ; i < (start + length) ; i+=ainc) + { + b8 = ec_readeepromAP(aiadr, i >> 1 , EC_TIMEOUTEEP); + ebuf[i] = b8; + ebuf[i+1] = b8 >> 8; + ebuf[i+2] = b8 >> 16; + ebuf[i+3] = b8 >> 24; + ebuf[i+4] = b8 >> 32; + ebuf[i+5] = b8 >> 40; + ebuf[i+6] = b8 >> 48; + ebuf[i+7] = b8 >> 56; + } + } + else + { + for (i = start ; i < (start + length) ; i+=ainc) + { + b4 = ec_readeepromAP(aiadr, i >> 1 , EC_TIMEOUTEEP); + ebuf[i] = b4; + ebuf[i+1] = b4 >> 8; + ebuf[i+2] = b4 >> 16; + ebuf[i+3] = b4 >> 24; + } + } + + return 1; + } + + return 0; +} + +int eeprom_write(int slave, int start, int length) +{ + int i, wkc, dc = 0; + uint16 aiadr, *wbuf; + uint8 eepctl; + int ret; + + if((ec_slavecount >= slave) && (slave > 0) && ((start + length) <= MAXBUF)) + { + aiadr = 1 - slave; + eepctl = 2; + wkc = ec_APWR(aiadr, ECT_REG_EEPCFG, sizeof(eepctl), &eepctl , EC_TIMEOUTRET); /* force Eeprom from PDI */ + eepctl = 0; + wkc = ec_APWR(aiadr, ECT_REG_EEPCFG, sizeof(eepctl), &eepctl , EC_TIMEOUTRET); /* set Eeprom to master */ + + aiadr = 1 - slave; + wbuf = (uint16 *)&ebuf[0]; + for (i = start ; i < (start + length) ; i+=2) + { + ret = ec_writeeepromAP(aiadr, i >> 1 , *(wbuf + (i >> 1)), EC_TIMEOUTEEP); + if (++dc >= 100) + { + dc = 0; + printf("."); + fflush(stdout); + } + } + + return 1; + } + + return 0; +} + +int eeprom_writealias(int slave, int alias) +{ + int i, wkc, dc = 0; + uint16 aiadr, *wbuf; + uint8 eepctl; + int ret; + + if((ec_slavecount >= slave) && (slave > 0) && (alias <= 0xffff)) + { + aiadr = 1 - slave; + eepctl = 2; + wkc = ec_APWR(aiadr, ECT_REG_EEPCFG, sizeof(eepctl), &eepctl , EC_TIMEOUTRET); /* force Eeprom from PDI */ + eepctl = 0; + wkc = ec_APWR(aiadr, ECT_REG_EEPCFG, sizeof(eepctl), &eepctl , EC_TIMEOUTRET); /* set Eeprom to master */ + + ret = ec_writeeepromAP(aiadr, 0x04 , alias, EC_TIMEOUTEEP); + + return ret; + } + + return 0; +} + +void eepromtool(char *ifname, int slave, int mode, char *fname) +{ + int w, rc = 0, estart, esize; + uint16 *wbuf; + + /* initialise SOEM, bind socket to ifname */ + if (ec_init(ifname)) + { + printf("ec_init on %s succeeded.\n",ifname); + + w = 0x0000; + wkc = ec_BRD(0x0000, ECT_REG_TYPE, sizeof(w), &w, EC_TIMEOUTSAFE); /* detect number of slaves */ + if (wkc > 0) + { + ec_slavecount = wkc; + + printf("%d slaves found.\n",ec_slavecount); + if((ec_slavecount >= slave) && (slave > 0)) + { + if ((mode == MODE_INFO) || (mode == MODE_READBIN) || (mode == MODE_READINTEL)) + { + rc = gettimeofday(&tstart, NULL); + eeprom_read(slave, 0x0000, MINBUF); // read first 128 bytes + + wbuf = (uint16 *)&ebuf[0]; + printf("Slave %d data\n", slave); + printf(" PDI Control : %4.4X\n",*(wbuf + 0x00)); + printf(" PDI Config : %4.4X\n",*(wbuf + 0x01)); + printf(" Config Alias : %4.4X\n",*(wbuf + 0x04)); + printf(" Checksum : %4.4X\n",*(wbuf + 0x07)); + printf(" Vendor ID : %8.8X\n",*(uint32 *)(wbuf + 0x08)); + printf(" Product Code : %8.8X\n",*(uint32 *)(wbuf + 0x0A)); + printf(" Revision Number : %8.8X\n",*(uint32 *)(wbuf + 0x0C)); + printf(" Serial Number : %8.8X\n",*(uint32 *)(wbuf + 0x0E)); + printf(" Mailbox Protocol : %4.4X\n",*(wbuf + 0x1C)); + esize = (*(wbuf + 0x3E) + 1) * 128; + if (esize > MAXBUF) esize = MAXBUF; + printf(" Size : %4.4X = %d bytes\n",*(wbuf + 0x3E), esize); + printf(" Version : %4.4X\n",*(wbuf + 0x3F)); + } + if ((mode == MODE_READBIN) || (mode == MODE_READINTEL)) + { + if (esize > MINBUF) + eeprom_read(slave, MINBUF, esize - MINBUF); // read reminder + + rc = gettimeofday(&tend, NULL); + timersub(&tend, &tstart, &tdif); + if (mode == MODE_READINTEL) output_intelhex(fname, esize); + if (mode == MODE_READBIN) output_bin(fname, esize); + + printf("\nTotal EEPROM read time :%ldms\n", (tdif.tv_usec+(tdif.tv_sec*1000000L)) / 1000); + } + if ((mode == MODE_WRITEBIN) || (mode == MODE_WRITEINTEL)) + { + estart = 0; + if (mode == MODE_WRITEINTEL) rc = input_intelhex(fname, &estart, &esize); + if (mode == MODE_WRITEBIN) rc = input_bin(fname, &esize); + + if (rc > 0) + { + wbuf = (uint16 *)&ebuf[0]; + printf("Slave %d\n", slave); + printf(" Vendor ID : %8.8X\n",*(uint32 *)(wbuf + 0x08)); + printf(" Product Code : %8.8X\n",*(uint32 *)(wbuf + 0x0A)); + printf(" Revision Number : %8.8X\n",*(uint32 *)(wbuf + 0x0C)); + printf(" Serial Number : %8.8X\n",*(uint32 *)(wbuf + 0x0E)); + + printf("Busy"); + fflush(stdout); + rc = gettimeofday(&tstart, NULL); + eeprom_write(slave, estart, esize); + rc = gettimeofday(&tend, NULL); + timersub(&tend, &tstart, &tdif); + + printf("\nTotal EEPROM write time :%ldms\n", (tdif.tv_usec+(tdif.tv_sec*1000000L)) / 1000); + } + else + printf("Error reading file, abort.\n"); + } + if (mode == MODE_WRITEALIAS) + { + if(eeprom_writealias(slave, alias)) + printf("Alias %4.4X written successfully to slave %d\n"); + else + printf("Alias not written\n"); + } + } + else + printf("Slave number outside range.\n"); + } + else + printf("No slaves found!\n"); + printf("End, close socket\n"); + /* stop SOEM, close socket */ + ec_close(); + } + else + printf("No socket connection on %s\nExcecute as root\n",ifname); +} + +int main(int argc, char *argv[]) +{ + printf("SOEM (Simple Open EtherCAT Master)\nEEPROM tool\n"); + + mode = MODE_NONE; + if (argc > 3) + { + slave = atoi(argv[2]); + if ((strncmp(argv[3], "-i", sizeof("-i")) == 0)) mode = MODE_INFO; + if (argc > 4) + { + if ((strncmp(argv[3], "-r", sizeof("-r")) == 0)) mode = MODE_READBIN; + if ((strncmp(argv[3], "-ri", sizeof("-ri")) == 0)) mode = MODE_READINTEL; + if ((strncmp(argv[3], "-w", sizeof("-w")) == 0)) mode = MODE_WRITEBIN; + if ((strncmp(argv[3], "-wi", sizeof("-wi")) == 0)) mode = MODE_WRITEINTEL; + if ((strncmp(argv[3], "-walias", sizeof("-walias")) == 0)) + { + mode = MODE_WRITEALIAS; + alias = atoi(argv[4]); + } + } + /* start tool */ + eepromtool(argv[1],slave,mode,argv[4]); + } + else + { + printf("Usage: eepromtool ifname slave OPTION fname|alias\n"); + printf("ifname = eth0 for example\n"); + printf("slave = slave number in EtherCAT order 1..n\n"); + printf(" -i display EEPROM information\n"); + printf(" -walias write slave alias\n"); + printf(" -r read EEPROM, output binary format\n"); + printf(" -ri read EEPROM, output Intel Hex format\n"); + printf(" -w write EEPROM, input binary format\n"); + printf(" -wi write EEPROM, input Intel Hex format\n"); + } + + printf("End program\n"); + + return (0); +} diff --git a/soem-src/test/linux/ebox/ebox.c b/soem-src/test/linux/ebox/ebox.c new file mode 100644 index 0000000..2628734 --- /dev/null +++ b/soem-src/test/linux/ebox/ebox.c @@ -0,0 +1,386 @@ +/** \file + * \brief Example code for Simple Open EtherCAT master + * + * Usage : ebox [ifname] [cycletime] + * ifname is NIC interface, f.e. eth0 + * cycletime in us, f.e. 500 + * + * This test is specifically build for the E/BOX. + * + * (c)Arthur Ketels 2011 + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "ethercat.h" + +#define NSEC_PER_SEC 1000000000 + +typedef struct PACKED +{ + uint8 status; + uint8 counter; + uint8 din; + int32 ain[2]; + uint32 tsain; + int32 enc[2]; +} in_EBOXt; + +typedef struct PACKED +{ + uint8 counter; + int16 stream[100]; +} in_EBOX_streamt; + +typedef struct PACKED +{ + uint8 control; + uint8 dout; + int16 aout[2]; + uint16 pwmout[2]; +} out_EBOXt; + +typedef struct PACKED +{ + uint8 control; +} out_EBOX_streamt; + +// total samples to capture +#define MAXSTREAM 200000 +// sample interval in ns, here 8us -> 125kHz +// maximum data rate for E/BOX v1.0.1 is around 150kHz +#define SYNC0TIME 8000 + +struct sched_param schedp; +char IOmap[4096]; +pthread_t thread1; +struct timeval tv,t1,t2; +int dorun = 0; +int deltat, tmax=0; +int64 toff; +int DCdiff; +int os; +uint32 ob; +int16 ob2; +uint8 ob3; +pthread_cond_t cond = PTHREAD_COND_INITIALIZER; +pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER; +int64 integral=0; +uint32 cyclecount; +in_EBOX_streamt *in_EBOX; +out_EBOX_streamt *out_EBOX; +double ain[2]; +int ainc; +int streampos; +int16 stream1[MAXSTREAM]; +int16 stream2[MAXSTREAM]; + +int output_cvs(char *fname, int length) +{ + FILE *fp; + + int i; + + fp = fopen(fname, "w"); + if(fp == NULL) + return 0; + for (i = 0; i < length; i++) + { + fprintf(fp, "%d %d %d\n", i, stream1[i], stream2[i]); + } + fclose(fp); + + return 1; +} + +void eboxtest(char *ifname) +{ + int cnt, i; + + printf("Starting E/BOX test\n"); + + /* initialise SOEM, bind socket to ifname */ + if (ec_init(ifname)) + { + printf("ec_init on %s succeeded.\n",ifname); + /* find and auto-config slaves */ + if ( ec_config_init(FALSE) > 0 ) + { + printf("%d slaves found and configured.\n",ec_slavecount); + + // check if first slave is an E/BOX + if (( ec_slavecount >= 1 ) && + (strcmp(ec_slave[1].name,"E/BOX") == 0)) + { + // reprogram PDO mapping to set slave in stream mode + // this can only be done in pre-OP state + os=sizeof(ob2); ob2 = 0x1601; + ec_SDOwrite(1,0x1c12,01,FALSE,os,&ob2,EC_TIMEOUTRXM); + os=sizeof(ob2); ob2 = 0x1a01; + ec_SDOwrite(1,0x1c13,01,FALSE,os,&ob2,EC_TIMEOUTRXM); + } + + ec_config_map(&IOmap); + + ec_configdc(); + + /* wait for all slaves to reach SAFE_OP state */ + ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE); + + /* configure DC options for every DC capable slave found in the list */ + printf("DC capable : %d\n",ec_configdc()); + + /* check configuration */ + if (( ec_slavecount >= 1 ) && + (strcmp(ec_slave[1].name,"E/BOX") == 0) + ) + { + printf("E/BOX found.\n"); + + /* connect struct pointers to slave I/O pointers */ + in_EBOX = (in_EBOX_streamt*) ec_slave[1].inputs; + out_EBOX = (out_EBOX_streamt*) ec_slave[1].outputs; + + /* read indevidual slave state and store in ec_slave[] */ + ec_readstate(); + for(cnt = 1; cnt <= ec_slavecount ; cnt++) + { + printf("Slave:%d Name:%s Output size:%3dbits Input size:%3dbits State:%2d delay:%d.%d\n", + cnt, ec_slave[cnt].name, ec_slave[cnt].Obits, ec_slave[cnt].Ibits, + ec_slave[cnt].state, (int)ec_slave[cnt].pdelay, ec_slave[cnt].hasdc); + } + printf("Request operational state for all slaves\n"); + + /* send one processdata cycle to init SM in slaves */ + ec_send_processdata(); + ec_receive_processdata(EC_TIMEOUTRET); + + ec_slave[0].state = EC_STATE_OPERATIONAL; + /* request OP state for all slaves */ + ec_writestate(0); + /* wait for all slaves to reach OP state */ + ec_statecheck(0, EC_STATE_OPERATIONAL, EC_TIMEOUTSTATE); + if (ec_slave[0].state == EC_STATE_OPERATIONAL ) + { + printf("Operational state reached for all slaves.\n"); + ain[0] = 0; + ain[1] = 0; + ainc = 0; + dorun = 1; + usleep(100000); // wait for linux to sync on DC + ec_dcsync0(1, TRUE, SYNC0TIME, 0); // SYNC0 on slave 1 + /* acyclic loop 20ms */ + for(i = 1; i <= 200; i++) + { + /* read DC difference register for slave 2 */ + // ec_FPRD(ec_slave[1].configadr, ECT_REG_DCSYSDIFF, sizeof(DCdiff), &DCdiff, EC_TIMEOUTRET); + // if(DCdiff<0) { DCdiff = - (int32)((uint32)DCdiff & 0x7ffffff); } + printf("PD cycle %5d DCtime %12lld Cnt:%3d Data: %6d %6d %6d %6d %6d %6d %6d %6d \n", + cyclecount, ec_DCtime, in_EBOX->counter, in_EBOX->stream[0], in_EBOX->stream[1], + in_EBOX->stream[2], in_EBOX->stream[3], in_EBOX->stream[4], in_EBOX->stream[5], + in_EBOX->stream[98], in_EBOX->stream[99]); + usleep(20000); + } + dorun = 0; + // printf("\nCnt %d : Ain0 = %f Ain2 = %f\n", ainc, ain[0] / ainc, ain[1] / ainc); + } + else + { + printf("Not all slaves reached operational state.\n"); + } + } + else + { + printf("E/BOX not found in slave configuration.\n"); + } + ec_dcsync0(1, FALSE, 8000, 0); // SYNC0 off + printf("Request safe operational state for all slaves\n"); + ec_slave[0].state = EC_STATE_SAFE_OP; + /* request SAFE_OP state for all slaves */ + ec_writestate(0); + /* wait for all slaves to reach state */ + ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE); + ec_slave[0].state = EC_STATE_PRE_OP; + /* request SAFE_OP state for all slaves */ + ec_writestate(0); + /* wait for all slaves to reach state */ + ec_statecheck(0, EC_STATE_PRE_OP, EC_TIMEOUTSTATE); + if (( ec_slavecount >= 1 ) && + (strcmp(ec_slave[1].name,"E/BOX") == 0)) + { + // restore PDO to standard mode + // this can only be done is pre-op state + os=sizeof(ob2); ob2 = 0x1600; + ec_SDOwrite(1,0x1c12,01,FALSE,os,&ob2,EC_TIMEOUTRXM); + os=sizeof(ob2); ob2 = 0x1a00; + ec_SDOwrite(1,0x1c13,01,FALSE,os,&ob2,EC_TIMEOUTRXM); + } + printf("Streampos %d\n", streampos); + output_cvs("stream.txt", streampos); + } + else + { + printf("No slaves found!\n"); + } + printf("End E/BOX, close socket\n"); + /* stop SOEM, close socket */ + ec_close(); + } + else + { + printf("No socket connection on %s\nExcecute as root\n",ifname); + } +} + +/* add ns to timespec */ +void add_timespec(struct timespec *ts, int64 addtime) +{ + int64 sec, nsec; + + nsec = addtime % NSEC_PER_SEC; + sec = (addtime - nsec) / NSEC_PER_SEC; + ts->tv_sec += sec; + ts->tv_nsec += nsec; + if ( ts->tv_nsec >= NSEC_PER_SEC ) + { + nsec = ts->tv_nsec % NSEC_PER_SEC; + ts->tv_sec += (ts->tv_nsec - nsec) / NSEC_PER_SEC; + ts->tv_nsec = nsec; + } +} + +/* PI calculation to get linux time synced to DC time */ +void ec_sync(int64 reftime, int64 cycletime , int64 *offsettime) +{ + int64 delta; + /* set linux sync point 50us later than DC sync, just as example */ + delta = (reftime - 50000) % cycletime; + if(delta> (cycletime /2)) { delta= delta - cycletime; } + if(delta>0){ integral++; } + if(delta<0){ integral--; } + *offsettime = -(delta / 100) - (integral /20); +} + +/* RT EtherCAT thread */ +void ecatthread( void *ptr ) +{ + struct timespec ts; + struct timeval tp; + int ht; + int i; + int pcounter = 0; + int64 cycletime; + + pthread_mutex_lock(&mutex); + gettimeofday(&tp, NULL); + + /* Convert from timeval to timespec */ + ts.tv_sec = tp.tv_sec; + ht = (tp.tv_usec / 1000) + 1; /* round to nearest ms */ + ts.tv_nsec = ht * 1000000; + cycletime = *(int*)ptr * 1000; /* cycletime in ns */ + toff = 0; + dorun = 0; + while(1) + { + /* calculate next cycle start */ + add_timespec(&ts, cycletime + toff); + /* wait to cycle start */ + pthread_cond_timedwait(&cond, &mutex, &ts); + if (dorun>0) + { + gettimeofday(&tp, NULL); + + ec_send_processdata(); + + ec_receive_processdata(EC_TIMEOUTRET); + + cyclecount++; + + + if((in_EBOX->counter != pcounter) && (streampos < (MAXSTREAM - 1))) + { + // check if we have timing problems in master + // if so, overwrite stream data so it shows up clearly in plots. + if(in_EBOX->counter > (pcounter + 1)) + { + for(i = 0 ; i < 50 ; i++) + { + stream1[streampos] = 20000; + stream2[streampos++] = -20000; + } + } + else + { + for(i = 0 ; i < 50 ; i++) + { + stream1[streampos] = in_EBOX->stream[i * 2]; + stream2[streampos++] = in_EBOX->stream[(i * 2) + 1]; + } + } + pcounter = in_EBOX->counter; + } + + /* calulate toff to get linux time and DC synced */ + ec_sync(ec_DCtime, cycletime, &toff); + } + } +} + +int main(int argc, char *argv[]) +{ + int ctime; + struct sched_param param; + int policy = SCHED_OTHER; + + printf("SOEM (Simple Open EtherCAT Master)\nE/BOX test\n"); + + memset(&schedp, 0, sizeof(schedp)); + /* do not set priority above 49, otherwise sockets are starved */ + schedp.sched_priority = 30; + sched_setscheduler(0, SCHED_FIFO, &schedp); + + do + { + usleep(1000); + } + while (dorun); + + if (argc > 1) + { + dorun = 1; + if( argc > 2) + ctime = atoi(argv[2]); + else + ctime = 1000; // 1ms cycle time + /* create RT thread */ + pthread_create( &thread1, NULL, (void *) &ecatthread, (void*) &ctime); + memset(¶m, 0, sizeof(param)); + /* give it higher priority */ + param.sched_priority = 40; + pthread_setschedparam(thread1, policy, ¶m); + + /* start acyclic part */ + eboxtest(argv[1]); + } + else + { + printf("Usage: ebox ifname [cycletime]\nifname = eth0 for example\ncycletime in us\n"); + } + + schedp.sched_priority = 0; + sched_setscheduler(0, SCHED_OTHER, &schedp); + + printf("End program\n"); + + return (0); +} diff --git a/soem-src/test/linux/eepromtool/eepromtool.c b/soem-src/test/linux/eepromtool/eepromtool.c new file mode 100644 index 0000000..d9323dd --- /dev/null +++ b/soem-src/test/linux/eepromtool/eepromtool.c @@ -0,0 +1,485 @@ +/** \file + * \brief EEprom tool for Simple Open EtherCAT master + * + * Usage : eepromtool ifname slave OPTION fname|alias + * ifname is NIC interface, f.e. eth0 + * slave = slave number in EtherCAT order 1..n + * -r read EEPROM, output binary format + * -ri read EEPROM, output Intel Hex format + * -w write EEPROM, input binary format + * -wi write EEPROM, input Intel Hex format + * -i display EEPROM information + * -walias write slave alias in EEPROM + * + * (c)Arthur Ketels 2010-2012 + */ + +#include +#include +#include + +#include "ethercat.h" + +#define MAXBUF 524288 +#define STDBUF 2048 +#define MINBUF 128 +#define CRCBUF 14 + +#define MODE_NONE 0 +#define MODE_READBIN 1 +#define MODE_READINTEL 2 +#define MODE_WRITEBIN 3 +#define MODE_WRITEINTEL 4 +#define MODE_WRITEALIAS 5 +#define MODE_INFO 6 + +#define MAXSLENGTH 256 + +uint8 ebuf[MAXBUF]; +uint8 ob; +uint16 ow; +int os; +int slave; +int alias; +ec_timet tstart,tend, tdif; +int wkc; +int mode; +char sline[MAXSLENGTH]; + +#define IHEXLENGTH 0x20 + +void calc_crc(uint8 *crc, uint8 b) +{ + int j; + *crc ^= b; + for(j = 0; j <= 7 ; j++ ) + { + if(*crc & 0x80) + *crc = (*crc << 1) ^ 0x07; + else + *crc = (*crc << 1); + } +} + +uint16 SIIcrc(uint8 *buf) +{ + int i; + uint8 crc; + + crc = 0xff; + for( i = 0 ; i <= 13 ; i++ ) + { + calc_crc(&crc , *(buf++)); + } + return (uint16)crc; +} + +int input_bin(char *fname, int *length) +{ + FILE *fp; + + int cc = 0, c; + + fp = fopen(fname, "rb"); + if(fp == NULL) + return 0; + while (((c = fgetc(fp)) != EOF) && (cc < MAXBUF)) + ebuf[cc++] = (uint8)c; + *length = cc; + fclose(fp); + + return 1; +} + +int input_intelhex(char *fname, int *start, int *length) +{ + FILE *fp; + + int c, sc, retval = 1; + int ll, ladr, lt, sn, i, lval; + int hstart, hlength, sum; + + fp = fopen(fname, "r"); + if(fp == NULL) + return 0; + hstart = MAXBUF; + hlength = 0; + sum = 0; + do + { + memset(sline, 0x00, MAXSLENGTH); + sc = 0; + while (((c = fgetc(fp)) != EOF) && (c != 0x0A) && (sc < (MAXSLENGTH -1))) + sline[sc++] = (uint8)c; + if ((c != EOF) && ((sc < 11) || (sline[0] != ':'))) + { + c = EOF; + retval = 0; + printf("Invalid Intel Hex format.\n"); + } + if (c != EOF) + { + sn = sscanf(sline , ":%2x%4x%2x", &ll, &ladr, <); + if ((sn == 3) && ((ladr + ll) <= MAXBUF) && (lt == 0)) + { + sum = ll + (ladr >> 8) + (ladr & 0xff) + lt; + if(ladr < hstart) hstart = ladr; + for(i = 0; i < ll ; i++) + { + sn = sscanf(&sline[9 + (i << 1)], "%2x", &lval); + ebuf[ladr + i] = (uint8)lval; + sum += (uint8)lval; + } + if(((ladr + ll) - hstart) > hlength) + hlength = (ladr + ll) - hstart; + sum = (0x100 - sum) & 0xff; + sn = sscanf(&sline[9 + (i << 1)], "%2x", &lval); + if (!sn || ((sum - lval) != 0)) + { + c = EOF; + retval = 0; + printf("Invalid checksum.\n"); + } + } + } + } + while (c != EOF); + if (retval) + { + *length = hlength; + *start = hstart; + } + fclose(fp); + + return retval; +} + +int output_bin(char *fname, int length) +{ + FILE *fp; + + int cc; + + fp = fopen(fname, "wb"); + if(fp == NULL) + return 0; + for (cc = 0 ; cc < length ; cc++) + fputc( ebuf[cc], fp); + fclose(fp); + + return 1; +} + +int output_intelhex(char *fname, int length) +{ + FILE *fp; + + int cc = 0, ll, sum, i; + + fp = fopen(fname, "w"); + if(fp == NULL) + return 0; + while (cc < length) + { + ll = length - cc; + if (ll > IHEXLENGTH) ll = IHEXLENGTH; + sum = ll + (cc >> 8) + (cc & 0xff); + fprintf(fp, ":%2.2X%4.4X00", ll, cc); + for (i = 0; i < ll; i++) + { + fprintf(fp, "%2.2X", ebuf[cc + i]); + sum += ebuf[cc + i]; + } + fprintf(fp, "%2.2X\n", (0x100 - sum) & 0xff); + cc += ll; + } + fprintf(fp, ":00000001FF\n"); + fclose(fp); + + return 1; +} + +int eeprom_read(int slave, int start, int length) +{ + int i, ainc = 4; + uint16 estat, aiadr; + uint32 b4; + uint64 b8; + uint8 eepctl; + + if((ec_slavecount >= slave) && (slave > 0) && ((start + length) <= MAXBUF)) + { + aiadr = 1 - slave; + eepctl = 2; + ec_APWR(aiadr, ECT_REG_EEPCFG, sizeof(eepctl), &eepctl , EC_TIMEOUTRET); /* force Eeprom from PDI */ + eepctl = 0; + ec_APWR(aiadr, ECT_REG_EEPCFG, sizeof(eepctl), &eepctl , EC_TIMEOUTRET); /* set Eeprom to master */ + + estat = 0x0000; + aiadr = 1 - slave; + ec_APRD(aiadr, ECT_REG_EEPSTAT, sizeof(estat), &estat, EC_TIMEOUTRET); /* read eeprom status */ + estat = etohs(estat); + if (estat & EC_ESTAT_R64) + { + ainc = 8; + for (i = start ; i < (start + length) ; i+=ainc) + { + b8 = ec_readeepromAP(aiadr, i >> 1 , EC_TIMEOUTEEP); + ebuf[i] = b8 & 0xFF; + ebuf[i+1] = (b8 >> 8) & 0xFF; + ebuf[i+2] = (b8 >> 16) & 0xFF; + ebuf[i+3] = (b8 >> 24) & 0xFF; + ebuf[i+4] = (b8 >> 32) & 0xFF; + ebuf[i+5] = (b8 >> 40) & 0xFF; + ebuf[i+6] = (b8 >> 48) & 0xFF; + ebuf[i+7] = (b8 >> 56) & 0xFF; + } + } + else + { + for (i = start ; i < (start + length) ; i+=ainc) + { + b4 = ec_readeepromAP(aiadr, i >> 1 , EC_TIMEOUTEEP) & 0xFFFFFFFF; + ebuf[i] = b4 & 0xFF; + ebuf[i+1] = (b4 >> 8) & 0xFF; + ebuf[i+2] = (b4 >> 16) & 0xFF; + ebuf[i+3] = (b4 >> 24) & 0xFF; + } + } + + return 1; + } + + return 0; +} + +int eeprom_write(int slave, int start, int length) +{ + int i, dc = 0; + uint16 aiadr, *wbuf; + uint8 eepctl; + + if((ec_slavecount >= slave) && (slave > 0) && ((start + length) <= MAXBUF)) + { + aiadr = 1 - slave; + eepctl = 2; + ec_APWR(aiadr, ECT_REG_EEPCFG, sizeof(eepctl), &eepctl , EC_TIMEOUTRET); /* force Eeprom from PDI */ + eepctl = 0; + ec_APWR(aiadr, ECT_REG_EEPCFG, sizeof(eepctl), &eepctl , EC_TIMEOUTRET); /* set Eeprom to master */ + + aiadr = 1 - slave; + wbuf = (uint16 *)&ebuf[0]; + for (i = start ; i < (start + length) ; i+=2) + { + ec_writeeepromAP(aiadr, i >> 1 , *(wbuf + (i >> 1)), EC_TIMEOUTEEP); + if (++dc >= 100) + { + dc = 0; + printf("."); + fflush(stdout); + } + } + + return 1; + } + + return 0; +} + +int eeprom_writealias(int slave, int alias, uint16 crc) +{ + uint16 aiadr; + uint8 eepctl; + int ret; + + if((ec_slavecount >= slave) && (slave > 0) && (alias <= 0xffff)) + { + aiadr = 1 - slave; + eepctl = 2; + ec_APWR(aiadr, ECT_REG_EEPCFG, sizeof(eepctl), &eepctl , EC_TIMEOUTRET); /* force Eeprom from PDI */ + eepctl = 0; + ec_APWR(aiadr, ECT_REG_EEPCFG, sizeof(eepctl), &eepctl , EC_TIMEOUTRET); /* set Eeprom to master */ + + ret = ec_writeeepromAP(aiadr, 0x04 , alias, EC_TIMEOUTEEP); + if (ret) + ret = ec_writeeepromAP(aiadr, 0x07 , crc, EC_TIMEOUTEEP); + + return ret; + } + + return 0; +} + +void eepromtool(char *ifname, int slave, int mode, char *fname) +{ + int w, rc = 0, estart, esize; + uint16 *wbuf; + + /* initialise SOEM, bind socket to ifname */ + if (ec_init(ifname)) + { + printf("ec_init on %s succeeded.\n",ifname); + + w = 0x0000; + wkc = ec_BRD(0x0000, ECT_REG_TYPE, sizeof(w), &w, EC_TIMEOUTSAFE); /* detect number of slaves */ + if (wkc > 0) + { + ec_slavecount = wkc; + + printf("%d slaves found.\n",ec_slavecount); + if((ec_slavecount >= slave) && (slave > 0)) + { + if ((mode == MODE_INFO) || (mode == MODE_READBIN) || (mode == MODE_READINTEL)) + { + tstart = osal_current_time(); + eeprom_read(slave, 0x0000, MINBUF); // read first 128 bytes + + wbuf = (uint16 *)&ebuf[0]; + printf("Slave %d data\n", slave); + printf(" PDI Control : %4.4X\n",*(wbuf + 0x00)); + printf(" PDI Config : %4.4X\n",*(wbuf + 0x01)); + printf(" Config Alias : %4.4X\n",*(wbuf + 0x04)); + printf(" Checksum : %4.4X\n",*(wbuf + 0x07)); + printf(" calculated : %4.4X\n",SIIcrc(&ebuf[0])); + printf(" Vendor ID : %8.8X\n",*(uint32 *)(wbuf + 0x08)); + printf(" Product Code : %8.8X\n",*(uint32 *)(wbuf + 0x0A)); + printf(" Revision Number : %8.8X\n",*(uint32 *)(wbuf + 0x0C)); + printf(" Serial Number : %8.8X\n",*(uint32 *)(wbuf + 0x0E)); + printf(" Mailbox Protocol : %4.4X\n",*(wbuf + 0x1C)); + esize = (*(wbuf + 0x3E) + 1) * 128; + if (esize > MAXBUF) esize = MAXBUF; + printf(" Size : %4.4X = %d bytes\n",*(wbuf + 0x3E), esize); + printf(" Version : %4.4X\n",*(wbuf + 0x3F)); + } + if ((mode == MODE_READBIN) || (mode == MODE_READINTEL)) + { + if (esize > MINBUF) + eeprom_read(slave, MINBUF, esize - MINBUF); // read reminder + + tend = osal_current_time(); + osal_time_diff(&tstart, &tend, &tdif); + if (mode == MODE_READINTEL) output_intelhex(fname, esize); + if (mode == MODE_READBIN) output_bin(fname, esize); + + printf("\nTotal EEPROM read time :%ldms\n", (tdif.usec+(tdif.sec*1000000L)) / 1000); + } + if ((mode == MODE_WRITEBIN) || (mode == MODE_WRITEINTEL)) + { + estart = 0; + if (mode == MODE_WRITEINTEL) rc = input_intelhex(fname, &estart, &esize); + if (mode == MODE_WRITEBIN) rc = input_bin(fname, &esize); + + if (rc > 0) + { + wbuf = (uint16 *)&ebuf[0]; + printf("Slave %d\n", slave); + printf(" Vendor ID : %8.8X\n",*(uint32 *)(wbuf + 0x08)); + printf(" Product Code : %8.8X\n",*(uint32 *)(wbuf + 0x0A)); + printf(" Revision Number : %8.8X\n",*(uint32 *)(wbuf + 0x0C)); + printf(" Serial Number : %8.8X\n",*(uint32 *)(wbuf + 0x0E)); + + printf("Busy"); + fflush(stdout); + tstart = osal_current_time(); + eeprom_write(slave, estart, esize); + tend = osal_current_time(); + osal_time_diff(&tstart, &tend, &tdif); + + printf("\nTotal EEPROM write time :%ldms\n", (tdif.usec+(tdif.sec*1000000L)) / 1000); + } + else + printf("Error reading file, abort.\n"); + } + if (mode == MODE_WRITEALIAS) + { + if( eeprom_read(slave, 0x0000, CRCBUF) ) // read first 14 bytes + { + wbuf = (uint16 *)&ebuf[0]; + *(wbuf + 0x04) = alias; + if(eeprom_writealias(slave, alias, SIIcrc(&ebuf[0]))) + { + printf("Alias %4.4X written successfully to slave %d\n", alias, slave); + } + else + { + printf("Alias not written\n"); + } + } + else + { + printf("Could not read slave EEPROM"); + } + } + } + else + { + printf("Slave number outside range.\n"); + } + } + else + { + printf("No slaves found!\n"); + } + printf("End, close socket\n"); + /* stop SOEM, close socket */ + ec_close(); + } + else + { + printf("No socket connection on %s\nExcecute as root\n",ifname); + } +} + +int main(int argc, char *argv[]) +{ + printf("SOEM (Simple Open EtherCAT Master)\nEEPROM tool\n"); + + mode = MODE_NONE; + if (argc > 3) + { + slave = atoi(argv[2]); + if ((strncmp(argv[3], "-i", sizeof("-i")) == 0)) mode = MODE_INFO; + if (argc > 4) + { + if ((strncmp(argv[3], "-r", sizeof("-r")) == 0)) mode = MODE_READBIN; + if ((strncmp(argv[3], "-ri", sizeof("-ri")) == 0)) mode = MODE_READINTEL; + if ((strncmp(argv[3], "-w", sizeof("-w")) == 0)) mode = MODE_WRITEBIN; + if ((strncmp(argv[3], "-wi", sizeof("-wi")) == 0)) mode = MODE_WRITEINTEL; + if ((strncmp(argv[3], "-walias", sizeof("-walias")) == 0)) + { + mode = MODE_WRITEALIAS; + alias = atoi(argv[4]); + } + } + /* start tool */ + eepromtool(argv[1],slave,mode,argv[4]); + } + else + { + ec_adaptert * adapter = NULL; + ec_adaptert * head = NULL; + + printf("Usage: eepromtool ifname slave OPTION fname|alias\n"); + printf("ifname = eth0 for example\n"); + printf("slave = slave number in EtherCAT order 1..n\n"); + printf(" -i display EEPROM information\n"); + printf(" -walias write slave alias\n"); + printf(" -r read EEPROM, output binary format\n"); + printf(" -ri read EEPROM, output Intel Hex format\n"); + printf(" -w write EEPROM, input binary format\n"); + printf(" -wi write EEPROM, input Intel Hex format\n"); + + printf ("\nAvailable adapters:\n"); + head = adapter = ec_find_adapters (); + while (adapter != NULL) + { + printf (" - %s (%s)\n", adapter->name, adapter->desc); + adapter = adapter->next; + } + ec_free_adapters(head); + } + + printf("End program\n"); + + return (0); +} diff --git a/soem-src/test/linux/eoe_test/eoe_test.c b/soem-src/test/linux/eoe_test/eoe_test.c new file mode 100644 index 0000000..58c8048 --- /dev/null +++ b/soem-src/test/linux/eoe_test/eoe_test.c @@ -0,0 +1,419 @@ +/** \file + * \brief Example code for Simple Open EtherCAT master EoE + * + * This example will run the follwing EoE functions + * SetIP + * GetIP + * + * Loop + * Send fragment data (Layer 2 0x88A4) + * Receive fragment data (Layer 2 0x88A4) + * + * For this to work, a special slave test code is running that + * will bounce the sent 0x88A4 back to receive. + * + * Usage : eoe_test [ifname1] + * ifname is NIC interface, f.e. eth0 + * + * This is a minimal test. + * + * (c)Arthur Ketels 2010 - 2011 + */ + +#include +#include +#include + +#include "ethercat.h" + +#define EC_TIMEOUTMON 500 + +char IOmap[4096]; + +int expectedWKC; +boolean needlf; +volatile int globalwkc; +boolean inOP; +uint8 currentgroup = 0; +OSAL_THREAD_HANDLE thread1; +OSAL_THREAD_HANDLE thread2; +uint8 txbuf[1024]; + +/** Current RX fragment number */ +uint8_t rxfragmentno = 0; +/** Complete RX frame size of current frame */ +uint16_t rxframesize = 0; +/** Current RX data offset in frame */ +uint16_t rxframeoffset = 0; +/** Current RX frame number */ +uint16_t rxframeno = 0; +uint8 rxbuf[1024]; +int size_of_rx = sizeof(rxbuf); + +/** registered EoE hook */ +int eoe_hook(ecx_contextt * context, uint16 slave, void * eoembx) +{ + int wkc; + /* Pass received Mbx data to EoE recevive fragment function that + * that will start/continue fill an Ethernet frame buffer + */ + size_of_rx = sizeof(rxbuf); + wkc = ecx_EOEreadfragment(eoembx, + &rxfragmentno, + &rxframesize, + &rxframeoffset, + &rxframeno, + &size_of_rx, + rxbuf); + + printf("Read frameno %d, fragmentno %d\n", rxframeno, rxfragmentno); + + /* wkc == 1 would mean a frame is complete , last fragment flag have been set and all + * other checks must have past + */ + if (wkc > 0) + { + ec_etherheadert *bp = (ec_etherheadert *)rxbuf; + uint16 type = ntohs(bp->etype); + printf("Frameno %d, type 0x%x complete\n", rxframeno, type); + if (type == ETH_P_ECAT) + { + /* Sanity check that received buffer still is OK */ + if (sizeof(txbuf) != size_of_rx) + { + printf("Size differs, expected %d , received %d\n", sizeof(txbuf), size_of_rx); + } + else + { + printf("Size OK, expected %d , received %d\n", sizeof(txbuf), size_of_rx); + } + /* Check that the TX and RX frames are EQ */ + if (memcmp(rxbuf, txbuf, size_of_rx)) + { + printf("memcmp result != 0\n"); + } + else + { + printf("memcmp result == 0\n"); + } + /* Send a new frame */ + int ixme; + for (ixme = ETH_HEADERSIZE; ixme < sizeof(txbuf); ixme++) + { + txbuf[ixme] = (uint8)rand(); + } + printf("Send a new frame\n"); + ecx_EOEsend(context, 1, 0, sizeof(txbuf), txbuf, EC_TIMEOUTRXM); + } + else + { + printf("Skip type 0x%x\n", type); + } + } + /* No point in returning as unhandled */ + return 1; +} + +OSAL_THREAD_FUNC mailbox_reader(void *lpParam) +{ + ecx_contextt *context = (ecx_contextt *)lpParam; + int wkc; + ec_mbxbuft MbxIn; + ec_mbxheadert * MbxHdr = (ec_mbxheadert *)MbxIn; + + int ixme; + ec_setupheader(&txbuf); + for (ixme = ETH_HEADERSIZE; ixme < sizeof(txbuf); ixme++) + { + txbuf[ixme] = (uint8)rand(); + } + /* Send a made up frame to trigger a fragmented transfer + * Used with a special bound impelmentaion of SOES. Will + * trigger a fragmented transfer back of the same frame. + */ + ecx_EOEsend(context, 1, 0, sizeof(txbuf), txbuf, EC_TIMEOUTRXM); + + for (;;) + { + /* Read mailbox if no other mailbox conversation is ongoing eg. SDOwrite/SDOwrite etc.*/ + wkc = ecx_mbxreceive(context, 1, (ec_mbxbuft *)&MbxIn, 0); + if (wkc > 0) + { + printf("Unhandled mailbox response 0x%x\n", MbxHdr->mbxtype); + } + osal_usleep(1 * 1000 * 1000); + } +} + +void test_eoe(ecx_contextt * context) +{ + /* Set the HOOK */ + ecx_EOEdefinehook(context, eoe_hook); + + eoe_param_t ipsettings, re_ipsettings; + memset(&ipsettings, 0, sizeof(ipsettings)); + memset(&re_ipsettings, 0, sizeof(re_ipsettings)); + + ipsettings.ip_set = 1; + ipsettings.subnet_set = 1; + ipsettings.default_gateway_set = 1; + + EOE_IP4_ADDR_TO_U32(&ipsettings.ip, 192, 168, 9, 200); + EOE_IP4_ADDR_TO_U32(&ipsettings.subnet, 255, 255, 255, 0); + EOE_IP4_ADDR_TO_U32(&ipsettings.default_gateway, 0, 0, 0, 0); + + /* Send a set IP request */ + ecx_EOEsetIp(context, 1, 0, &ipsettings, EC_TIMEOUTRXM); + + /* Send a get IP request, should return the expected IP back */ + ecx_EOEgetIp(context, 1, 0, &re_ipsettings, EC_TIMEOUTRXM); + + /* Trigger an MBX read request, to be replaced by slave Mbx + * full notification via polling of FMMU status process data + */ + printf("recieved IP (%d.%d.%d.%d)\n", + eoe_ip4_addr1(&re_ipsettings.ip), + eoe_ip4_addr2(&re_ipsettings.ip), + eoe_ip4_addr3(&re_ipsettings.ip), + eoe_ip4_addr4(&re_ipsettings.ip)); + printf("recieved subnet (%d.%d.%d.%d)\n", + eoe_ip4_addr1(&re_ipsettings.subnet), + eoe_ip4_addr2(&re_ipsettings.subnet), + eoe_ip4_addr3(&re_ipsettings.subnet), + eoe_ip4_addr4(&re_ipsettings.subnet)); + printf("recieved gateway (%d.%d.%d.%d)\n", + eoe_ip4_addr1(&re_ipsettings.default_gateway), + eoe_ip4_addr2(&re_ipsettings.default_gateway), + eoe_ip4_addr3(&re_ipsettings.default_gateway), + eoe_ip4_addr4(&re_ipsettings.default_gateway)); + + /* Create a asyncronous EoE reader */ + osal_thread_create(&thread2, 128000, &mailbox_reader, &ecx_context); +} + +void teststarter(char *ifname) +{ + int i, oloop, iloop, chk; + needlf = FALSE; + inOP = FALSE; + + printf("Starting eoe test\n"); + + /* initialise SOEM, bind socket to ifname */ + if (ec_init(ifname)) + { + printf("ec_init on %s succeeded.\n",ifname); + /* find and auto-config slaves */ + if ( ec_config_init(FALSE) > 0 ) + { + printf("%d slaves found and configured.\n",ec_slavecount); + + ec_config_map(&IOmap); + + ec_configdc(); + + printf("Slaves mapped, state to SAFE_OP.\n"); + /* wait for all slaves to reach SAFE_OP state */ + ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE * 4); + + oloop = ec_slave[0].Obytes; + if ((oloop == 0) && (ec_slave[0].Obits > 0)) oloop = 1; + if (oloop > 8) oloop = 8; + iloop = ec_slave[0].Ibytes; + if ((iloop == 0) && (ec_slave[0].Ibits > 0)) iloop = 1; + if (iloop > 8) iloop = 8; + + printf("segments : %d : %d %d %d %d\n",ec_group[0].nsegments ,ec_group[0].IOsegment[0],ec_group[0].IOsegment[1],ec_group[0].IOsegment[2],ec_group[0].IOsegment[3]); + + printf("Request operational state for all slaves\n"); + expectedWKC = (ec_group[0].outputsWKC * 2) + ec_group[0].inputsWKC; + printf("Calculated workcounter %d\n", expectedWKC); + ec_slave[0].state = EC_STATE_OPERATIONAL; + /* send one valid process data to make outputs in slaves happy*/ + ec_send_processdata(); + ec_receive_processdata(EC_TIMEOUTRET); + + /* Simple EoE test */ + test_eoe(&ecx_context); + + /* request OP state for all slaves */ + ec_writestate(0); + chk = 200; + /* wait for all slaves to reach OP state */ + do + { + ec_send_processdata(); + ec_receive_processdata(EC_TIMEOUTRET); + ec_statecheck(0, EC_STATE_OPERATIONAL, 50000); + } + while (chk-- && (ec_slave[0].state != EC_STATE_OPERATIONAL)); + if (ec_slave[0].state == EC_STATE_OPERATIONAL ) + { + printf("Operational state reached for all slaves.\n"); + globalwkc = expectedWKC; + inOP = TRUE; + /* cyclic loop */ + for(i = 1; i <= 10000; i++) + { + ec_send_processdata(); + globalwkc = ec_receive_processdata(EC_TIMEOUTRET * 5); +#if PRINT_EOE_INFO_INSTEAD + int j; + if(globalwkc >= expectedWKC) + { + printf("Processdata cycle %4d, WKC %d , O:", i, globalwkc); + for(j = 0 ; j < oloop; j++) + { + printf(" %2.2x", *(ec_slave[0].outputs + j)); + } + printf(" I:"); + for(j = 0 ; j < iloop; j++) + { + printf(" %2.2x", *(ec_slave[0].inputs + j)); + } + printf(" T:%"PRId64"\r",ec_DCtime); + needlf = TRUE; + } +#endif + osal_usleep(1000); + } + inOP = FALSE; + } + else + { + printf("Not all slaves reached operational state.\n"); + ec_readstate(); + for(i = 1; i<=ec_slavecount ; i++) + { + if(ec_slave[i].state != EC_STATE_OPERATIONAL) + { + printf("Slave %d State=0x%2.2x StatusCode=0x%4.4x : %s\n", + i, ec_slave[i].state, ec_slave[i].ALstatuscode, ec_ALstatuscode2string(ec_slave[i].ALstatuscode)); + } + } + } + printf("\nRequest init state for all slaves\n"); + ec_slave[0].state = EC_STATE_INIT; + /* request INIT state for all slaves */ + ec_writestate(0); + } + else + { + printf("No slaves found!\n"); + } + printf("End eoe test, close socket\n"); + /* stop SOEM, close socket */ + ec_close(); + } + else + { + printf("No socket connection on %s\nExcecute as root\n",ifname); + } +} + +OSAL_THREAD_FUNC ecatcheck( void *ptr ) +{ + int slave; + (void)ptr; /* Not used */ + + while(1) + { + if( inOP && ((globalwkc < expectedWKC) || ec_group[currentgroup].docheckstate)) + { + if (globalwkc < expectedWKC) + { + printf("(wkc < expectedWKC) , globalwkc %d\n", globalwkc); + } + if (ec_group[currentgroup].docheckstate) + { + printf("(ec_group[currentgroup].docheckstate)\n"); + } + + if (needlf) + { + needlf = FALSE; + printf("\n"); + } + /* one ore more slaves are not responding */ + ec_group[currentgroup].docheckstate = FALSE; + ec_readstate(); + for (slave = 1; slave <= ec_slavecount; slave++) + { + if ((ec_slave[slave].group == currentgroup) && (ec_slave[slave].state != EC_STATE_OPERATIONAL)) + { + ec_group[currentgroup].docheckstate = TRUE; + if (ec_slave[slave].state == (EC_STATE_SAFE_OP + EC_STATE_ERROR)) + { + printf("ERROR : slave %d is in SAFE_OP + ERROR, attempting ack.\n", slave); + ec_slave[slave].state = (EC_STATE_SAFE_OP + EC_STATE_ACK); + ec_writestate(slave); + } + else if(ec_slave[slave].state == EC_STATE_SAFE_OP) + { + printf("WARNING : slave %d is in SAFE_OP, change to OPERATIONAL.\n", slave); + ec_slave[slave].state = EC_STATE_OPERATIONAL; + ec_writestate(slave); + } + else if(ec_slave[slave].state > EC_STATE_NONE) + { + if (ec_reconfig_slave(slave, EC_TIMEOUTMON)) + { + ec_slave[slave].islost = FALSE; + printf("MESSAGE : slave %d reconfigured\n",slave); + } + } + else if(!ec_slave[slave].islost) + { + /* re-check state */ + ec_statecheck(slave, EC_STATE_OPERATIONAL, EC_TIMEOUTRET); + if (ec_slave[slave].state == EC_STATE_NONE) + { + ec_slave[slave].islost = TRUE; + printf("ERROR : slave %d lost\n",slave); + } + } + } + if (ec_slave[slave].islost) + { + if(ec_slave[slave].state == EC_STATE_NONE) + { + if (ec_recover_slave(slave, EC_TIMEOUTMON)) + { + ec_slave[slave].islost = FALSE; + printf("MESSAGE : slave %d recovered\n",slave); + } + } + else + { + ec_slave[slave].islost = FALSE; + printf("MESSAGE : slave %d found\n",slave); + } + } + } + if(!ec_group[currentgroup].docheckstate) + printf("OK : all slaves resumed OPERATIONAL.\n"); + } + osal_usleep(10000); + } +} + +int main(int argc, char *argv[]) +{ + printf("SOEM (Simple Open EtherCAT Master)\nEoE test\n"); + + if (argc > 1) + { + /* create thread to handle slave error handling in OP */ + osal_thread_create(&thread1, 128000, &ecatcheck, NULL); + + /* start cyclic part */ + teststarter(argv[1]); + } + else + { + printf("Usage: eoe_test ifname1\nifname = eth0 for example\n"); + } + + printf("End program\n"); + return (0); +} diff --git a/soem-src/test/linux/firm_update/firm_update.c b/soem-src/test/linux/firm_update/firm_update.c new file mode 100644 index 0000000..b59374f --- /dev/null +++ b/soem-src/test/linux/firm_update/firm_update.c @@ -0,0 +1,162 @@ +/** \file + * \brief Example code for Simple Open EtherCAT master + * + * Usage: firm_update ifname1 slave fname + * ifname is NIC interface, f.e. eth0 + * slave = slave number in EtherCAT order 1..n + * fname = binary file to store in slave + * CAUTION! Using the wrong file can result in a bricked slave! + * + * This is a slave firmware update test. + * + * (c)Arthur Ketels 2011 + */ + +#include +#include +#include +#include +#include + +#include "ethercat.h" + +#define FWBUFSIZE (8 * 1024 * 1024) + +uint8 ob; +uint16 ow; +uint32 data; +char filename[256]; +char filebuffer[FWBUFSIZE]; // 8MB buffer +int filesize; +int j; +uint16 argslave; + +int input_bin(char *fname, int *length) +{ + FILE *fp; + + int cc = 0, c; + + fp = fopen(fname, "rb"); + if(fp == NULL) + return 0; + while (((c = fgetc(fp)) != EOF) && (cc < FWBUFSIZE)) + filebuffer[cc++] = (uint8)c; + *length = cc; + fclose(fp); + return 1; +} + + +void boottest(char *ifname, uint16 slave, char *filename) +{ + printf("Starting firmware update example\n"); + + /* initialise SOEM, bind socket to ifname */ + if (ec_init(ifname)) + { + printf("ec_init on %s succeeded.\n",ifname); + /* find and auto-config slaves */ + + + if ( ec_config_init(FALSE) > 0 ) + { + printf("%d slaves found and configured.\n",ec_slavecount); + + /* wait for all slaves to reach PRE_OP state */ + ec_statecheck(0, EC_STATE_PRE_OP, EC_TIMEOUTSTATE * 4); + + printf("Request init state for slave %d\n", slave); + ec_slave[slave].state = EC_STATE_INIT; + ec_writestate(slave); + + /* wait for slave to reach INIT state */ + ec_statecheck(slave, EC_STATE_INIT, EC_TIMEOUTSTATE * 4); + printf("Slave %d state to INIT.\n", slave); + + /* read BOOT mailbox data, master -> slave */ + data = ec_readeeprom(slave, ECT_SII_BOOTRXMBX, EC_TIMEOUTEEP); + ec_slave[slave].SM[0].StartAddr = (uint16)LO_WORD(data); + ec_slave[slave].SM[0].SMlength = (uint16)HI_WORD(data); + /* store boot write mailbox address */ + ec_slave[slave].mbx_wo = (uint16)LO_WORD(data); + /* store boot write mailbox size */ + ec_slave[slave].mbx_l = (uint16)HI_WORD(data); + + /* read BOOT mailbox data, slave -> master */ + data = ec_readeeprom(slave, ECT_SII_BOOTTXMBX, EC_TIMEOUTEEP); + ec_slave[slave].SM[1].StartAddr = (uint16)LO_WORD(data); + ec_slave[slave].SM[1].SMlength = (uint16)HI_WORD(data); + /* store boot read mailbox address */ + ec_slave[slave].mbx_ro = (uint16)LO_WORD(data); + /* store boot read mailbox size */ + ec_slave[slave].mbx_rl = (uint16)HI_WORD(data); + + printf(" SM0 A:%4.4x L:%4d F:%8.8x\n", ec_slave[slave].SM[0].StartAddr, ec_slave[slave].SM[0].SMlength, + (int)ec_slave[slave].SM[0].SMflags); + printf(" SM1 A:%4.4x L:%4d F:%8.8x\n", ec_slave[slave].SM[1].StartAddr, ec_slave[slave].SM[1].SMlength, + (int)ec_slave[slave].SM[1].SMflags); + /* program SM0 mailbox in for slave */ + ec_FPWR (ec_slave[slave].configadr, ECT_REG_SM0, sizeof(ec_smt), &ec_slave[slave].SM[0], EC_TIMEOUTRET); + /* program SM1 mailbox out for slave */ + ec_FPWR (ec_slave[slave].configadr, ECT_REG_SM1, sizeof(ec_smt), &ec_slave[slave].SM[1], EC_TIMEOUTRET); + + printf("Request BOOT state for slave %d\n", slave); + ec_slave[slave].state = EC_STATE_BOOT; + ec_writestate(slave); + + /* wait for slave to reach BOOT state */ + if (ec_statecheck(slave, EC_STATE_BOOT, EC_TIMEOUTSTATE * 10) == EC_STATE_BOOT) + { + printf("Slave %d state to BOOT.\n", slave); + + if (input_bin(filename, &filesize)) + { + printf("File read OK, %d bytes.\n",filesize); + printf("FoE write...."); + j = ec_FOEwrite(slave, filename, 0, filesize , &filebuffer, EC_TIMEOUTSTATE); + printf("result %d.\n",j); + printf("Request init state for slave %d\n", slave); + ec_slave[slave].state = EC_STATE_INIT; + ec_writestate(slave); + } + else + printf("File not read OK.\n"); + } + + } + else + { + printf("No slaves found!\n"); + } + printf("End firmware update example, close socket\n"); + /* stop SOEM, close socket */ + ec_close(); + } + else + { + printf("No socket connection on %s\nExcecute as root\n",ifname); + } +} + +int main(int argc, char *argv[]) +{ + printf("SOEM (Simple Open EtherCAT Master)\nFirmware update example\n"); + + if (argc > 3) + { + argslave = atoi(argv[2]); + boottest(argv[1], argslave, argv[3]); + } + else + { + printf("Usage: firm_update ifname1 slave fname\n"); + printf("ifname = eth0 for example\n"); + printf("slave = slave number in EtherCAT order 1..n\n"); + printf("fname = binary file to store in slave\n"); + printf("CAUTION! Using the wrong file can result in a bricked slave!\n"); + } + + printf("End program\n"); + return (0); +} diff --git a/soem-src/test/linux/red_test/red_test.c b/soem-src/test/linux/red_test/red_test.c new file mode 100644 index 0000000..368daac --- /dev/null +++ b/soem-src/test/linux/red_test/red_test.c @@ -0,0 +1,341 @@ +/** \file + * \brief Example code for Simple Open EtherCAT master + * + * Usage : red_test [ifname1] [ifname2] [cycletime] + * ifname is NIC interface, f.e. eth0 + * cycletime in us, f.e. 500 + * + * This is a redundancy test. + * + * (c)Arthur Ketels 2008 + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "ethercat.h" + +#define NSEC_PER_SEC 1000000000 +#define EC_TIMEOUTMON 500 + +struct sched_param schedp; +char IOmap[4096]; +pthread_t thread1, thread2; +struct timeval tv, t1, t2; +int dorun = 0; +int deltat, tmax = 0; +int64 toff, gl_delta; +int DCdiff; +int os; +uint8 ob; +uint16 ob2; +uint8 *digout = 0; +int expectedWKC; +boolean needlf; +volatile int wkc; +boolean inOP; +uint8 currentgroup = 0; + + +void redtest(char *ifname, char *ifname2) +{ + int cnt, i, j, oloop, iloop; + + printf("Starting Redundant test\n"); + + /* initialise SOEM, bind socket to ifname */ + (void) ifname2; +// if (ec_init_redundant(ifname, ifname2)) + if (ec_init(ifname)) + { + printf("ec_init on %s succeeded.\n",ifname); + /* find and auto-config slaves */ + if ( ec_config(FALSE, &IOmap) > 0 ) + { + printf("%d slaves found and configured.\n",ec_slavecount); + /* wait for all slaves to reach SAFE_OP state */ + ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE); + + /* configure DC options for every DC capable slave found in the list */ + ec_configdc(); + + /* read indevidual slave state and store in ec_slave[] */ + ec_readstate(); + for(cnt = 1; cnt <= ec_slavecount ; cnt++) + { + printf("Slave:%d Name:%s Output size:%3dbits Input size:%3dbits State:%2d delay:%d.%d\n", + cnt, ec_slave[cnt].name, ec_slave[cnt].Obits, ec_slave[cnt].Ibits, + ec_slave[cnt].state, (int)ec_slave[cnt].pdelay, ec_slave[cnt].hasdc); + printf(" Out:%p,%4d In:%p,%4d\n", + ec_slave[cnt].outputs, ec_slave[cnt].Obytes, ec_slave[cnt].inputs, ec_slave[cnt].Ibytes); + /* check for EL2004 or EL2008 */ + if( !digout && ((ec_slave[cnt].eep_id == 0x0af83052) || (ec_slave[cnt].eep_id == 0x07d83052))) + { + digout = ec_slave[cnt].outputs; + } + } + expectedWKC = (ec_group[0].outputsWKC * 2) + ec_group[0].inputsWKC; + printf("Calculated workcounter %d\n", expectedWKC); + + printf("Request operational state for all slaves\n"); + ec_slave[0].state = EC_STATE_OPERATIONAL; + /* request OP state for all slaves */ + ec_writestate(0); + /* activate cyclic process data */ + dorun = 1; + /* wait for all slaves to reach OP state */ + ec_statecheck(0, EC_STATE_OPERATIONAL, 5 * EC_TIMEOUTSTATE); + oloop = ec_slave[0].Obytes; + if ((oloop == 0) && (ec_slave[0].Obits > 0)) oloop = 1; + if (oloop > 8) oloop = 8; + iloop = ec_slave[0].Ibytes; + if ((iloop == 0) && (ec_slave[0].Ibits > 0)) iloop = 1; + if (iloop > 8) iloop = 8; + if (ec_slave[0].state == EC_STATE_OPERATIONAL ) + { + printf("Operational state reached for all slaves.\n"); + inOP = TRUE; + /* acyclic loop 5000 x 20ms = 10s */ + for(i = 1; i <= 5000; i++) + { + printf("Processdata cycle %5d , Wck %3d, DCtime %12"PRId64", dt %12"PRId64", O:", + dorun, wkc , ec_DCtime, gl_delta); + for(j = 0 ; j < oloop; j++) + { + printf(" %2.2x", *(ec_slave[0].outputs + j)); + } + printf(" I:"); + for(j = 0 ; j < iloop; j++) + { + printf(" %2.2x", *(ec_slave[0].inputs + j)); + } + printf("\r"); + fflush(stdout); + osal_usleep(20000); + } + dorun = 0; + inOP = FALSE; + } + else + { + printf("Not all slaves reached operational state.\n"); + ec_readstate(); + for(i = 1; i<=ec_slavecount ; i++) + { + if(ec_slave[i].state != EC_STATE_OPERATIONAL) + { + printf("Slave %d State=0x%2.2x StatusCode=0x%4.4x : %s\n", + i, ec_slave[i].state, ec_slave[i].ALstatuscode, ec_ALstatuscode2string(ec_slave[i].ALstatuscode)); + } + } + } + printf("Request safe operational state for all slaves\n"); + ec_slave[0].state = EC_STATE_SAFE_OP; + /* request SAFE_OP state for all slaves */ + ec_writestate(0); + } + else + { + printf("No slaves found!\n"); + } + printf("End redundant test, close socket\n"); + /* stop SOEM, close socket */ + ec_close(); + } + else + { + printf("No socket connection on %s\nExcecute as root\n",ifname); + } +} + +/* add ns to timespec */ +void add_timespec(struct timespec *ts, int64 addtime) +{ + int64 sec, nsec; + + nsec = addtime % NSEC_PER_SEC; + sec = (addtime - nsec) / NSEC_PER_SEC; + ts->tv_sec += sec; + ts->tv_nsec += nsec; + if ( ts->tv_nsec >= NSEC_PER_SEC ) + { + nsec = ts->tv_nsec % NSEC_PER_SEC; + ts->tv_sec += (ts->tv_nsec - nsec) / NSEC_PER_SEC; + ts->tv_nsec = nsec; + } +} + +/* PI calculation to get linux time synced to DC time */ +void ec_sync(int64 reftime, int64 cycletime , int64 *offsettime) +{ + static int64 integral = 0; + int64 delta; + /* set linux sync point 50us later than DC sync, just as example */ + delta = (reftime - 50000) % cycletime; + if(delta> (cycletime / 2)) { delta= delta - cycletime; } + if(delta>0){ integral++; } + if(delta<0){ integral--; } + *offsettime = -(delta / 100) - (integral / 20); + gl_delta = delta; +} + +/* RT EtherCAT thread */ +OSAL_THREAD_FUNC_RT ecatthread(void *ptr) +{ + struct timespec ts, tleft; + int ht; + int64 cycletime; + + clock_gettime(CLOCK_MONOTONIC, &ts); + ht = (ts.tv_nsec / 1000000) + 1; /* round to nearest ms */ + ts.tv_nsec = ht * 1000000; + if (ts.tv_nsec >= NSEC_PER_SEC) { + ts.tv_sec++; + ts.tv_nsec -= NSEC_PER_SEC; + } + cycletime = *(int*)ptr * 1000; /* cycletime in ns */ + toff = 0; + dorun = 0; + ec_send_processdata(); + while(1) + { + /* calculate next cycle start */ + add_timespec(&ts, cycletime + toff); + /* wait to cycle start */ + clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &ts, &tleft); + if (dorun>0) + { + wkc = ec_receive_processdata(EC_TIMEOUTRET); + + dorun++; + /* if we have some digital output, cycle */ + if( digout ) *digout = (uint8) ((dorun / 16) & 0xff); + + if (ec_slave[0].hasdc) + { + /* calulate toff to get linux time and DC synced */ + ec_sync(ec_DCtime, cycletime, &toff); + } + ec_send_processdata(); + } + } +} + +OSAL_THREAD_FUNC ecatcheck( void *ptr ) +{ + int slave; + + (void) ptr; + + while(1) + { + if( inOP && ((wkc < expectedWKC) || ec_group[currentgroup].docheckstate)) + { + if (needlf) + { + needlf = FALSE; + printf("\n"); + } + /* one ore more slaves are not responding */ + ec_group[currentgroup].docheckstate = FALSE; + ec_readstate(); + for (slave = 1; slave <= ec_slavecount; slave++) + { + if ((ec_slave[slave].group == currentgroup) && (ec_slave[slave].state != EC_STATE_OPERATIONAL)) + { + ec_group[currentgroup].docheckstate = TRUE; + if (ec_slave[slave].state == (EC_STATE_SAFE_OP + EC_STATE_ERROR)) + { + printf("ERROR : slave %d is in SAFE_OP + ERROR, attempting ack.\n", slave); + ec_slave[slave].state = (EC_STATE_SAFE_OP + EC_STATE_ACK); + ec_writestate(slave); + } + else if(ec_slave[slave].state == EC_STATE_SAFE_OP) + { + printf("WARNING : slave %d is in SAFE_OP, change to OPERATIONAL.\n", slave); + ec_slave[slave].state = EC_STATE_OPERATIONAL; + ec_writestate(slave); + } + else if(ec_slave[slave].state > EC_STATE_NONE) + { + if (ec_reconfig_slave(slave, EC_TIMEOUTMON)) + { + ec_slave[slave].islost = FALSE; + printf("MESSAGE : slave %d reconfigured\n",slave); + } + } + else if(!ec_slave[slave].islost) + { + /* re-check state */ + ec_statecheck(slave, EC_STATE_OPERATIONAL, EC_TIMEOUTRET); + if (ec_slave[slave].state == EC_STATE_NONE) + { + ec_slave[slave].islost = TRUE; + printf("ERROR : slave %d lost\n",slave); + } + } + } + if (ec_slave[slave].islost) + { + if(ec_slave[slave].state == EC_STATE_NONE) + { + if (ec_recover_slave(slave, EC_TIMEOUTMON)) + { + ec_slave[slave].islost = FALSE; + printf("MESSAGE : slave %d recovered\n",slave); + } + } + else + { + ec_slave[slave].islost = FALSE; + printf("MESSAGE : slave %d found\n",slave); + } + } + } + if(!ec_group[currentgroup].docheckstate) + printf("OK : all slaves resumed OPERATIONAL.\n"); + } + osal_usleep(10000); + } +} + +#define stack64k (64 * 1024) + +int main(int argc, char *argv[]) +{ + int ctime; + + printf("SOEM (Simple Open EtherCAT Master)\nRedundancy test\n"); + + if (argc > 3) + { + dorun = 0; + ctime = atoi(argv[3]); + + /* create RT thread */ + osal_thread_create_rt(&thread1, stack64k * 2, &ecatthread, (void*) &ctime); + + /* create thread to handle slave error handling in OP */ + osal_thread_create(&thread2, stack64k * 4, &ecatcheck, NULL); + + /* start acyclic part */ + redtest(argv[1],argv[2]); + } + else + { + printf("Usage: red_test ifname1 ifname2 cycletime\nifname = eth0 for example\ncycletime in us\n"); + } + + printf("End program\n"); + + return (0); +} diff --git a/soem-src/test/linux/simple_test/simple_test.c b/soem-src/test/linux/simple_test/simple_test.c new file mode 100644 index 0000000..f74757c --- /dev/null +++ b/soem-src/test/linux/simple_test/simple_test.c @@ -0,0 +1,260 @@ +/** \file + * \brief Example code for Simple Open EtherCAT master + * + * Usage : simple_test [ifname1] + * ifname is NIC interface, f.e. eth0 + * + * This is a minimal test. + * + * (c)Arthur Ketels 2010 - 2011 + */ + +#include +#include +#include + +#include "ethercat.h" + +#define EC_TIMEOUTMON 500 + +char IOmap[4096]; +OSAL_THREAD_HANDLE thread1; +int expectedWKC; +boolean needlf; +volatile int wkc; +boolean inOP; +uint8 currentgroup = 0; +boolean forceByteAlignment = FALSE; + +void simpletest(char *ifname) +{ + int i, j, oloop, iloop, chk; + needlf = FALSE; + inOP = FALSE; + + printf("Starting simple test\n"); + + /* initialise SOEM, bind socket to ifname */ + if (ec_init(ifname)) + { + printf("ec_init on %s succeeded.\n",ifname); + /* find and auto-config slaves */ + + + if ( ec_config_init(FALSE) > 0 ) + { + printf("%d slaves found and configured.\n",ec_slavecount); + + if (forceByteAlignment) + { + ec_config_map_aligned(&IOmap); + } + else + { + ec_config_map(&IOmap); + } + + ec_configdc(); + + printf("Slaves mapped, state to SAFE_OP.\n"); + /* wait for all slaves to reach SAFE_OP state */ + ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE * 4); + + oloop = ec_slave[0].Obytes; + if ((oloop == 0) && (ec_slave[0].Obits > 0)) oloop = 1; + if (oloop > 8) oloop = 8; + iloop = ec_slave[0].Ibytes; + if ((iloop == 0) && (ec_slave[0].Ibits > 0)) iloop = 1; + if (iloop > 8) iloop = 8; + + printf("segments : %d : %d %d %d %d\n",ec_group[0].nsegments ,ec_group[0].IOsegment[0],ec_group[0].IOsegment[1],ec_group[0].IOsegment[2],ec_group[0].IOsegment[3]); + + printf("Request operational state for all slaves\n"); + expectedWKC = (ec_group[0].outputsWKC * 2) + ec_group[0].inputsWKC; + printf("Calculated workcounter %d\n", expectedWKC); + ec_slave[0].state = EC_STATE_OPERATIONAL; + /* send one valid process data to make outputs in slaves happy*/ + ec_send_processdata(); + ec_receive_processdata(EC_TIMEOUTRET); + /* request OP state for all slaves */ + ec_writestate(0); + chk = 200; + /* wait for all slaves to reach OP state */ + do + { + ec_send_processdata(); + ec_receive_processdata(EC_TIMEOUTRET); + ec_statecheck(0, EC_STATE_OPERATIONAL, 50000); + } + while (chk-- && (ec_slave[0].state != EC_STATE_OPERATIONAL)); + if (ec_slave[0].state == EC_STATE_OPERATIONAL ) + { + printf("Operational state reached for all slaves.\n"); + inOP = TRUE; + /* cyclic loop */ + for(i = 1; i <= 10000; i++) + { + ec_send_processdata(); + wkc = ec_receive_processdata(EC_TIMEOUTRET); + + if(wkc >= expectedWKC) + { + printf("Processdata cycle %4d, WKC %d , O:", i, wkc); + + for(j = 0 ; j < oloop; j++) + { + printf(" %2.2x", *(ec_slave[0].outputs + j)); + } + + printf(" I:"); + for(j = 0 ; j < iloop; j++) + { + printf(" %2.2x", *(ec_slave[0].inputs + j)); + } + printf(" T:%"PRId64"\r",ec_DCtime); + needlf = TRUE; + } + osal_usleep(5000); + + } + inOP = FALSE; + } + else + { + printf("Not all slaves reached operational state.\n"); + ec_readstate(); + for(i = 1; i<=ec_slavecount ; i++) + { + if(ec_slave[i].state != EC_STATE_OPERATIONAL) + { + printf("Slave %d State=0x%2.2x StatusCode=0x%4.4x : %s\n", + i, ec_slave[i].state, ec_slave[i].ALstatuscode, ec_ALstatuscode2string(ec_slave[i].ALstatuscode)); + } + } + } + printf("\nRequest init state for all slaves\n"); + ec_slave[0].state = EC_STATE_INIT; + /* request INIT state for all slaves */ + ec_writestate(0); + } + else + { + printf("No slaves found!\n"); + } + printf("End simple test, close socket\n"); + /* stop SOEM, close socket */ + ec_close(); + } + else + { + printf("No socket connection on %s\nExecute as root\n",ifname); + } +} + +OSAL_THREAD_FUNC ecatcheck( void *ptr ) +{ + int slave; + (void)ptr; /* Not used */ + + while(1) + { + if( inOP && ((wkc < expectedWKC) || ec_group[currentgroup].docheckstate)) + { + if (needlf) + { + needlf = FALSE; + printf("\n"); + } + /* one ore more slaves are not responding */ + ec_group[currentgroup].docheckstate = FALSE; + ec_readstate(); + for (slave = 1; slave <= ec_slavecount; slave++) + { + if ((ec_slave[slave].group == currentgroup) && (ec_slave[slave].state != EC_STATE_OPERATIONAL)) + { + ec_group[currentgroup].docheckstate = TRUE; + if (ec_slave[slave].state == (EC_STATE_SAFE_OP + EC_STATE_ERROR)) + { + printf("ERROR : slave %d is in SAFE_OP + ERROR, attempting ack.\n", slave); + ec_slave[slave].state = (EC_STATE_SAFE_OP + EC_STATE_ACK); + ec_writestate(slave); + } + else if(ec_slave[slave].state == EC_STATE_SAFE_OP) + { + printf("WARNING : slave %d is in SAFE_OP, change to OPERATIONAL.\n", slave); + ec_slave[slave].state = EC_STATE_OPERATIONAL; + ec_writestate(slave); + } + else if(ec_slave[slave].state > EC_STATE_NONE) + { + if (ec_reconfig_slave(slave, EC_TIMEOUTMON)) + { + ec_slave[slave].islost = FALSE; + printf("MESSAGE : slave %d reconfigured\n",slave); + } + } + else if(!ec_slave[slave].islost) + { + /* re-check state */ + ec_statecheck(slave, EC_STATE_OPERATIONAL, EC_TIMEOUTRET); + if (ec_slave[slave].state == EC_STATE_NONE) + { + ec_slave[slave].islost = TRUE; + printf("ERROR : slave %d lost\n",slave); + } + } + } + if (ec_slave[slave].islost) + { + if(ec_slave[slave].state == EC_STATE_NONE) + { + if (ec_recover_slave(slave, EC_TIMEOUTMON)) + { + ec_slave[slave].islost = FALSE; + printf("MESSAGE : slave %d recovered\n",slave); + } + } + else + { + ec_slave[slave].islost = FALSE; + printf("MESSAGE : slave %d found\n",slave); + } + } + } + if(!ec_group[currentgroup].docheckstate) + printf("OK : all slaves resumed OPERATIONAL.\n"); + } + osal_usleep(10000); + } +} + +int main(int argc, char *argv[]) +{ + printf("SOEM (Simple Open EtherCAT Master)\nSimple test\n"); + + if (argc > 1) + { + /* create thread to handle slave error handling in OP */ + osal_thread_create(&thread1, 128000, &ecatcheck, NULL); + /* start cyclic part */ + simpletest(argv[1]); + } + else + { + ec_adaptert * adapter = NULL; + ec_adaptert * head = NULL; + printf("Usage: simple_test ifname1\nifname = eth0 for example\n"); + + printf ("\nAvailable adapters:\n"); + head = adapter = ec_find_adapters (); + while (adapter != NULL) + { + printf (" - %s (%s)\n", adapter->name, adapter->desc); + adapter = adapter->next; + } + ec_free_adapters(head); + } + + printf("End program\n"); + return (0); +} diff --git a/soem-src/test/linux/slaveinfo/slaveinfo.c b/soem-src/test/linux/slaveinfo/slaveinfo.c new file mode 100644 index 0000000..e81ba3b --- /dev/null +++ b/soem-src/test/linux/slaveinfo/slaveinfo.c @@ -0,0 +1,729 @@ +/** \file + * \brief Example code for Simple Open EtherCAT master + * + * Usage : slaveinfo [ifname] [-sdo] [-map] + * Ifname is NIC interface, f.e. eth0. + * Optional -sdo to display CoE object dictionary. + * Optional -map to display slave PDO mapping + * + * This shows the configured slave data. + * + * (c)Arthur Ketels 2010 - 2011 + */ + +#include +#include +#include + +#include "ethercat.h" + +char IOmap[4096]; +ec_ODlistt ODlist; +ec_OElistt OElist; +boolean printSDO = FALSE; +boolean printMAP = FALSE; +char usdo[128]; + + +#define OTYPE_VAR 0x0007 +#define OTYPE_ARRAY 0x0008 +#define OTYPE_RECORD 0x0009 + +#define ATYPE_Rpre 0x01 +#define ATYPE_Rsafe 0x02 +#define ATYPE_Rop 0x04 +#define ATYPE_Wpre 0x08 +#define ATYPE_Wsafe 0x10 +#define ATYPE_Wop 0x20 + + +char* dtype2string(uint16 dtype, uint16 bitlen) +{ + static char str[32] = { 0 }; + + switch(dtype) + { + case ECT_BOOLEAN: + sprintf(str, "BOOLEAN"); + break; + case ECT_INTEGER8: + sprintf(str, "INTEGER8"); + break; + case ECT_INTEGER16: + sprintf(str, "INTEGER16"); + break; + case ECT_INTEGER32: + sprintf(str, "INTEGER32"); + break; + case ECT_INTEGER24: + sprintf(str, "INTEGER24"); + break; + case ECT_INTEGER64: + sprintf(str, "INTEGER64"); + break; + case ECT_UNSIGNED8: + sprintf(str, "UNSIGNED8"); + break; + case ECT_UNSIGNED16: + sprintf(str, "UNSIGNED16"); + break; + case ECT_UNSIGNED32: + sprintf(str, "UNSIGNED32"); + break; + case ECT_UNSIGNED24: + sprintf(str, "UNSIGNED24"); + break; + case ECT_UNSIGNED64: + sprintf(str, "UNSIGNED64"); + break; + case ECT_REAL32: + sprintf(str, "REAL32"); + break; + case ECT_REAL64: + sprintf(str, "REAL64"); + break; + case ECT_BIT1: + sprintf(str, "BIT1"); + break; + case ECT_BIT2: + sprintf(str, "BIT2"); + break; + case ECT_BIT3: + sprintf(str, "BIT3"); + break; + case ECT_BIT4: + sprintf(str, "BIT4"); + break; + case ECT_BIT5: + sprintf(str, "BIT5"); + break; + case ECT_BIT6: + sprintf(str, "BIT6"); + break; + case ECT_BIT7: + sprintf(str, "BIT7"); + break; + case ECT_BIT8: + sprintf(str, "BIT8"); + break; + case ECT_VISIBLE_STRING: + sprintf(str, "VISIBLE_STR(%d)", bitlen); + break; + case ECT_OCTET_STRING: + sprintf(str, "OCTET_STR(%d)", bitlen); + break; + default: + sprintf(str, "dt:0x%4.4X (%d)", dtype, bitlen); + } + return str; +} + +char* otype2string(uint16 otype) +{ + static char str[32] = { 0 }; + + switch(otype) + { + case OTYPE_VAR: + sprintf(str, "VAR"); + break; + case OTYPE_ARRAY: + sprintf(str, "ARRAY"); + break; + case OTYPE_RECORD: + sprintf(str, "RECORD"); + break; + default: + sprintf(str, "ot:0x%4.4X", otype); + } + return str; +} + +char* access2string(uint16 access) +{ + static char str[32] = { 0 }; + + sprintf(str, "%s%s%s%s%s%s", + ((access & ATYPE_Rpre) != 0 ? "R" : "_"), + ((access & ATYPE_Wpre) != 0 ? "W" : "_"), + ((access & ATYPE_Rsafe) != 0 ? "R" : "_"), + ((access & ATYPE_Wsafe) != 0 ? "W" : "_"), + ((access & ATYPE_Rop) != 0 ? "R" : "_"), + ((access & ATYPE_Wop) != 0 ? "W" : "_")); + return str; +} + +char* SDO2string(uint16 slave, uint16 index, uint8 subidx, uint16 dtype) +{ + int l = sizeof(usdo) - 1, i; + uint8 *u8; + int8 *i8; + uint16 *u16; + int16 *i16; + uint32 *u32; + int32 *i32; + uint64 *u64; + int64 *i64; + float *sr; + double *dr; + char es[32]; + + memset(&usdo, 0, 128); + ec_SDOread(slave, index, subidx, FALSE, &l, &usdo, EC_TIMEOUTRXM); + if (EcatError) + { + return ec_elist2string(); + } + else + { + static char str[64] = { 0 }; + switch(dtype) + { + case ECT_BOOLEAN: + u8 = (uint8*) &usdo[0]; + if (*u8) sprintf(str, "TRUE"); + else sprintf(str, "FALSE"); + break; + case ECT_INTEGER8: + i8 = (int8*) &usdo[0]; + sprintf(str, "0x%2.2x / %d", *i8, *i8); + break; + case ECT_INTEGER16: + i16 = (int16*) &usdo[0]; + sprintf(str, "0x%4.4x / %d", *i16, *i16); + break; + case ECT_INTEGER32: + case ECT_INTEGER24: + i32 = (int32*) &usdo[0]; + sprintf(str, "0x%8.8x / %d", *i32, *i32); + break; + case ECT_INTEGER64: + i64 = (int64*) &usdo[0]; + sprintf(str, "0x%16.16"PRIx64" / %"PRId64, *i64, *i64); + break; + case ECT_UNSIGNED8: + u8 = (uint8*) &usdo[0]; + sprintf(str, "0x%2.2x / %u", *u8, *u8); + break; + case ECT_UNSIGNED16: + u16 = (uint16*) &usdo[0]; + sprintf(str, "0x%4.4x / %u", *u16, *u16); + break; + case ECT_UNSIGNED32: + case ECT_UNSIGNED24: + u32 = (uint32*) &usdo[0]; + sprintf(str, "0x%8.8x / %u", *u32, *u32); + break; + case ECT_UNSIGNED64: + u64 = (uint64*) &usdo[0]; + sprintf(str, "0x%16.16"PRIx64" / %"PRIu64, *u64, *u64); + break; + case ECT_REAL32: + sr = (float*) &usdo[0]; + sprintf(str, "%f", *sr); + break; + case ECT_REAL64: + dr = (double*) &usdo[0]; + sprintf(str, "%f", *dr); + break; + case ECT_BIT1: + case ECT_BIT2: + case ECT_BIT3: + case ECT_BIT4: + case ECT_BIT5: + case ECT_BIT6: + case ECT_BIT7: + case ECT_BIT8: + u8 = (uint8*) &usdo[0]; + sprintf(str, "0x%x / %u", *u8, *u8); + break; + case ECT_VISIBLE_STRING: + strcpy(str, "\""); + strcat(str, usdo); + strcat(str, "\""); + break; + case ECT_OCTET_STRING: + str[0] = 0x00; + for (i = 0 ; i < l ; i++) + { + sprintf(es, "0x%2.2x ", usdo[i]); + strcat( str, es); + } + break; + default: + sprintf(str, "Unknown type"); + } + return str; + } +} + +/** Read PDO assign structure */ +int si_PDOassign(uint16 slave, uint16 PDOassign, int mapoffset, int bitoffset) +{ + uint16 idxloop, nidx, subidxloop, rdat, idx, subidx; + uint8 subcnt; + int wkc, bsize = 0, rdl; + int32 rdat2; + uint8 bitlen, obj_subidx; + uint16 obj_idx; + int abs_offset, abs_bit; + + rdl = sizeof(rdat); rdat = 0; + /* read PDO assign subindex 0 ( = number of PDO's) */ + wkc = ec_SDOread(slave, PDOassign, 0x00, FALSE, &rdl, &rdat, EC_TIMEOUTRXM); + rdat = etohs(rdat); + /* positive result from slave ? */ + if ((wkc > 0) && (rdat > 0)) + { + /* number of available sub indexes */ + nidx = rdat; + bsize = 0; + /* read all PDO's */ + for (idxloop = 1; idxloop <= nidx; idxloop++) + { + rdl = sizeof(rdat); rdat = 0; + /* read PDO assign */ + wkc = ec_SDOread(slave, PDOassign, (uint8)idxloop, FALSE, &rdl, &rdat, EC_TIMEOUTRXM); + /* result is index of PDO */ + idx = etohs(rdat); + if (idx > 0) + { + rdl = sizeof(subcnt); subcnt = 0; + /* read number of subindexes of PDO */ + wkc = ec_SDOread(slave,idx, 0x00, FALSE, &rdl, &subcnt, EC_TIMEOUTRXM); + subidx = subcnt; + /* for each subindex */ + for (subidxloop = 1; subidxloop <= subidx; subidxloop++) + { + rdl = sizeof(rdat2); rdat2 = 0; + /* read SDO that is mapped in PDO */ + wkc = ec_SDOread(slave, idx, (uint8)subidxloop, FALSE, &rdl, &rdat2, EC_TIMEOUTRXM); + rdat2 = etohl(rdat2); + /* extract bitlength of SDO */ + bitlen = LO_BYTE(rdat2); + bsize += bitlen; + obj_idx = (uint16)(rdat2 >> 16); + obj_subidx = (uint8)((rdat2 >> 8) & 0x000000ff); + abs_offset = mapoffset + (bitoffset / 8); + abs_bit = bitoffset % 8; + ODlist.Slave = slave; + ODlist.Index[0] = obj_idx; + OElist.Entries = 0; + wkc = 0; + /* read object entry from dictionary if not a filler (0x0000:0x00) */ + if(obj_idx || obj_subidx) + wkc = ec_readOEsingle(0, obj_subidx, &ODlist, &OElist); + printf(" [0x%4.4X.%1d] 0x%4.4X:0x%2.2X 0x%2.2X", abs_offset, abs_bit, obj_idx, obj_subidx, bitlen); + if((wkc > 0) && OElist.Entries) + { + printf(" %-12s %s\n", dtype2string(OElist.DataType[obj_subidx], bitlen), OElist.Name[obj_subidx]); + } + else + printf("\n"); + bitoffset += bitlen; + }; + }; + }; + }; + /* return total found bitlength (PDO) */ + return bsize; +} + +int si_map_sdo(int slave) +{ + int wkc, rdl; + int retVal = 0; + uint8 nSM, iSM, tSM; + int Tsize, outputs_bo, inputs_bo; + uint8 SMt_bug_add; + + printf("PDO mapping according to CoE :\n"); + SMt_bug_add = 0; + outputs_bo = 0; + inputs_bo = 0; + rdl = sizeof(nSM); nSM = 0; + /* read SyncManager Communication Type object count */ + wkc = ec_SDOread(slave, ECT_SDO_SMCOMMTYPE, 0x00, FALSE, &rdl, &nSM, EC_TIMEOUTRXM); + /* positive result from slave ? */ + if ((wkc > 0) && (nSM > 2)) + { + /* make nSM equal to number of defined SM */ + nSM--; + /* limit to maximum number of SM defined, if true the slave can't be configured */ + if (nSM > EC_MAXSM) + nSM = EC_MAXSM; + /* iterate for every SM type defined */ + for (iSM = 2 ; iSM <= nSM ; iSM++) + { + rdl = sizeof(tSM); tSM = 0; + /* read SyncManager Communication Type */ + wkc = ec_SDOread(slave, ECT_SDO_SMCOMMTYPE, iSM + 1, FALSE, &rdl, &tSM, EC_TIMEOUTRXM); + if (wkc > 0) + { + if((iSM == 2) && (tSM == 2)) // SM2 has type 2 == mailbox out, this is a bug in the slave! + { + SMt_bug_add = 1; // try to correct, this works if the types are 0 1 2 3 and should be 1 2 3 4 + printf("Activated SM type workaround, possible incorrect mapping.\n"); + } + if(tSM) + tSM += SMt_bug_add; // only add if SMt > 0 + + if (tSM == 3) // outputs + { + /* read the assign RXPDO */ + printf(" SM%1d outputs\n addr b index: sub bitl data_type name\n", iSM); + Tsize = si_PDOassign(slave, ECT_SDO_PDOASSIGN + iSM, (int)(ec_slave[slave].outputs - (uint8 *)&IOmap[0]), outputs_bo ); + outputs_bo += Tsize; + } + if (tSM == 4) // inputs + { + /* read the assign TXPDO */ + printf(" SM%1d inputs\n addr b index: sub bitl data_type name\n", iSM); + Tsize = si_PDOassign(slave, ECT_SDO_PDOASSIGN + iSM, (int)(ec_slave[slave].inputs - (uint8 *)&IOmap[0]), inputs_bo ); + inputs_bo += Tsize; + } + } + } + } + + /* found some I/O bits ? */ + if ((outputs_bo > 0) || (inputs_bo > 0)) + retVal = 1; + return retVal; +} + +int si_siiPDO(uint16 slave, uint8 t, int mapoffset, int bitoffset) +{ + uint16 a , w, c, e, er; + uint8 eectl; + uint16 obj_idx; + uint8 obj_subidx; + uint8 obj_name; + uint8 obj_datatype; + uint8 bitlen; + int totalsize; + ec_eepromPDOt eepPDO; + ec_eepromPDOt *PDO; + int abs_offset, abs_bit; + char str_name[EC_MAXNAME + 1]; + + eectl = ec_slave[slave].eep_pdi; + + totalsize = 0; + PDO = &eepPDO; + PDO->nPDO = 0; + PDO->Length = 0; + PDO->Index[1] = 0; + for (c = 0 ; c < EC_MAXSM ; c++) PDO->SMbitsize[c] = 0; + if (t > 1) + t = 1; + PDO->Startpos = ec_siifind(slave, ECT_SII_PDO + t); + if (PDO->Startpos > 0) + { + a = PDO->Startpos; + w = ec_siigetbyte(slave, a++); + w += (ec_siigetbyte(slave, a++) << 8); + PDO->Length = w; + c = 1; + /* traverse through all PDOs */ + do + { + PDO->nPDO++; + PDO->Index[PDO->nPDO] = ec_siigetbyte(slave, a++); + PDO->Index[PDO->nPDO] += (ec_siigetbyte(slave, a++) << 8); + PDO->BitSize[PDO->nPDO] = 0; + c++; + /* number of entries in PDO */ + e = ec_siigetbyte(slave, a++); + PDO->SyncM[PDO->nPDO] = ec_siigetbyte(slave, a++); + a++; + obj_name = ec_siigetbyte(slave, a++); + a += 2; + c += 2; + if (PDO->SyncM[PDO->nPDO] < EC_MAXSM) /* active and in range SM? */ + { + str_name[0] = 0; + if(obj_name) + ec_siistring(str_name, slave, obj_name); + if (t) + printf(" SM%1d RXPDO 0x%4.4X %s\n", PDO->SyncM[PDO->nPDO], PDO->Index[PDO->nPDO], str_name); + else + printf(" SM%1d TXPDO 0x%4.4X %s\n", PDO->SyncM[PDO->nPDO], PDO->Index[PDO->nPDO], str_name); + printf(" addr b index: sub bitl data_type name\n"); + /* read all entries defined in PDO */ + for (er = 1; er <= e; er++) + { + c += 4; + obj_idx = ec_siigetbyte(slave, a++); + obj_idx += (ec_siigetbyte(slave, a++) << 8); + obj_subidx = ec_siigetbyte(slave, a++); + obj_name = ec_siigetbyte(slave, a++); + obj_datatype = ec_siigetbyte(slave, a++); + bitlen = ec_siigetbyte(slave, a++); + abs_offset = mapoffset + (bitoffset / 8); + abs_bit = bitoffset % 8; + + PDO->BitSize[PDO->nPDO] += bitlen; + a += 2; + + /* skip entry if filler (0x0000:0x00) */ + if(obj_idx || obj_subidx) + { + str_name[0] = 0; + if(obj_name) + ec_siistring(str_name, slave, obj_name); + + printf(" [0x%4.4X.%1d] 0x%4.4X:0x%2.2X 0x%2.2X", abs_offset, abs_bit, obj_idx, obj_subidx, bitlen); + printf(" %-12s %s\n", dtype2string(obj_datatype, bitlen), str_name); + } + bitoffset += bitlen; + totalsize += bitlen; + } + PDO->SMbitsize[ PDO->SyncM[PDO->nPDO] ] += PDO->BitSize[PDO->nPDO]; + c++; + } + else /* PDO deactivated because SM is 0xff or > EC_MAXSM */ + { + c += 4 * e; + a += 8 * e; + c++; + } + if (PDO->nPDO >= (EC_MAXEEPDO - 1)) c = PDO->Length; /* limit number of PDO entries in buffer */ + } + while (c < PDO->Length); + } + if (eectl) ec_eeprom2pdi(slave); /* if eeprom control was previously pdi then restore */ + return totalsize; +} + + +int si_map_sii(int slave) +{ + int retVal = 0; + int Tsize, outputs_bo, inputs_bo; + + printf("PDO mapping according to SII :\n"); + + outputs_bo = 0; + inputs_bo = 0; + /* read the assign RXPDOs */ + Tsize = si_siiPDO(slave, 1, (int)(ec_slave[slave].outputs - (uint8*)&IOmap), outputs_bo ); + outputs_bo += Tsize; + /* read the assign TXPDOs */ + Tsize = si_siiPDO(slave, 0, (int)(ec_slave[slave].inputs - (uint8*)&IOmap), inputs_bo ); + inputs_bo += Tsize; + /* found some I/O bits ? */ + if ((outputs_bo > 0) || (inputs_bo > 0)) + retVal = 1; + return retVal; +} + +void si_sdo(int cnt) +{ + int i, j; + + ODlist.Entries = 0; + memset(&ODlist, 0, sizeof(ODlist)); + if( ec_readODlist(cnt, &ODlist)) + { + printf(" CoE Object Description found, %d entries.\n",ODlist.Entries); + for( i = 0 ; i < ODlist.Entries ; i++) + { + uint8_t max_sub; + char name[128] = { 0 }; + + ec_readODdescription(i, &ODlist); + while(EcatError) printf(" - %s\n", ec_elist2string()); + snprintf(name, sizeof(name) - 1, "\"%s\"", ODlist.Name[i]); + if (ODlist.ObjectCode[i] == OTYPE_VAR) + { + printf("0x%04x %-40s [%s]\n", ODlist.Index[i], name, + otype2string(ODlist.ObjectCode[i])); + } + else + { + printf("0x%04x %-40s [%s maxsub(0x%02x / %d)]\n", + ODlist.Index[i], name, otype2string(ODlist.ObjectCode[i]), + ODlist.MaxSub[i], ODlist.MaxSub[i]); + } + memset(&OElist, 0, sizeof(OElist)); + ec_readOE(i, &ODlist, &OElist); + while(EcatError) printf("- %s\n", ec_elist2string()); + + if(ODlist.ObjectCode[i] != OTYPE_VAR) + { + int l = sizeof(max_sub); + ec_SDOread(cnt, ODlist.Index[i], 0, FALSE, &l, &max_sub, EC_TIMEOUTRXM); + } + else { + max_sub = ODlist.MaxSub[i]; + } + + for( j = 0 ; j < max_sub+1 ; j++) + { + if ((OElist.DataType[j] > 0) && (OElist.BitLength[j] > 0)) + { + snprintf(name, sizeof(name) - 1, "\"%s\"", OElist.Name[j]); + printf(" 0x%02x %-40s [%-16s %6s] ", j, name, + dtype2string(OElist.DataType[j], OElist.BitLength[j]), + access2string(OElist.ObjAccess[j])); + if ((OElist.ObjAccess[j] & 0x0007)) + { + printf("%s", SDO2string(cnt, ODlist.Index[i], j, OElist.DataType[j])); + } + printf("\n"); + } + } + } + } + else + { + while(EcatError) printf("%s", ec_elist2string()); + } +} + +void slaveinfo(char *ifname) +{ + int cnt, i, j, nSM; + uint16 ssigen; + int expectedWKC; + + printf("Starting slaveinfo\n"); + + /* initialise SOEM, bind socket to ifname */ + if (ec_init(ifname)) + { + printf("ec_init on %s succeeded.\n",ifname); + /* find and auto-config slaves */ + if ( ec_config(FALSE, &IOmap) > 0 ) + { + ec_configdc(); + while(EcatError) printf("%s", ec_elist2string()); + printf("%d slaves found and configured.\n",ec_slavecount); + expectedWKC = (ec_group[0].outputsWKC * 2) + ec_group[0].inputsWKC; + printf("Calculated workcounter %d\n", expectedWKC); + /* wait for all slaves to reach SAFE_OP state */ + ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE * 3); + if (ec_slave[0].state != EC_STATE_SAFE_OP ) + { + printf("Not all slaves reached safe operational state.\n"); + ec_readstate(); + for(i = 1; i<=ec_slavecount ; i++) + { + if(ec_slave[i].state != EC_STATE_SAFE_OP) + { + printf("Slave %d State=%2x StatusCode=%4x : %s\n", + i, ec_slave[i].state, ec_slave[i].ALstatuscode, ec_ALstatuscode2string(ec_slave[i].ALstatuscode)); + } + } + } + + + ec_readstate(); + for( cnt = 1 ; cnt <= ec_slavecount ; cnt++) + { + printf("\nSlave:%d\n Name:%s\n Output size: %dbits\n Input size: %dbits\n State: %d\n Delay: %d[ns]\n Has DC: %d\n", + cnt, ec_slave[cnt].name, ec_slave[cnt].Obits, ec_slave[cnt].Ibits, + ec_slave[cnt].state, ec_slave[cnt].pdelay, ec_slave[cnt].hasdc); + if (ec_slave[cnt].hasdc) printf(" DCParentport:%d\n", ec_slave[cnt].parentport); + printf(" Activeports:%d.%d.%d.%d\n", (ec_slave[cnt].activeports & 0x01) > 0 , + (ec_slave[cnt].activeports & 0x02) > 0 , + (ec_slave[cnt].activeports & 0x04) > 0 , + (ec_slave[cnt].activeports & 0x08) > 0 ); + printf(" Configured address: %4.4x\n", ec_slave[cnt].configadr); + printf(" Man: %8.8x ID: %8.8x Rev: %8.8x\n", (int)ec_slave[cnt].eep_man, (int)ec_slave[cnt].eep_id, (int)ec_slave[cnt].eep_rev); + for(nSM = 0 ; nSM < EC_MAXSM ; nSM++) + { + if(ec_slave[cnt].SM[nSM].StartAddr > 0) + printf(" SM%1d A:%4.4x L:%4d F:%8.8x Type:%d\n",nSM, etohs(ec_slave[cnt].SM[nSM].StartAddr), etohs(ec_slave[cnt].SM[nSM].SMlength), + etohl(ec_slave[cnt].SM[nSM].SMflags), ec_slave[cnt].SMtype[nSM]); + } + for(j = 0 ; j < ec_slave[cnt].FMMUunused ; j++) + { + printf(" FMMU%1d Ls:%8.8x Ll:%4d Lsb:%d Leb:%d Ps:%4.4x Psb:%d Ty:%2.2x Act:%2.2x\n", j, + etohl(ec_slave[cnt].FMMU[j].LogStart), etohs(ec_slave[cnt].FMMU[j].LogLength), ec_slave[cnt].FMMU[j].LogStartbit, + ec_slave[cnt].FMMU[j].LogEndbit, etohs(ec_slave[cnt].FMMU[j].PhysStart), ec_slave[cnt].FMMU[j].PhysStartBit, + ec_slave[cnt].FMMU[j].FMMUtype, ec_slave[cnt].FMMU[j].FMMUactive); + } + printf(" FMMUfunc 0:%d 1:%d 2:%d 3:%d\n", + ec_slave[cnt].FMMU0func, ec_slave[cnt].FMMU1func, ec_slave[cnt].FMMU2func, ec_slave[cnt].FMMU3func); + printf(" MBX length wr: %d rd: %d MBX protocols : %2.2x\n", ec_slave[cnt].mbx_l, ec_slave[cnt].mbx_rl, ec_slave[cnt].mbx_proto); + ssigen = ec_siifind(cnt, ECT_SII_GENERAL); + /* SII general section */ + if (ssigen) + { + ec_slave[cnt].CoEdetails = ec_siigetbyte(cnt, ssigen + 0x07); + ec_slave[cnt].FoEdetails = ec_siigetbyte(cnt, ssigen + 0x08); + ec_slave[cnt].EoEdetails = ec_siigetbyte(cnt, ssigen + 0x09); + ec_slave[cnt].SoEdetails = ec_siigetbyte(cnt, ssigen + 0x0a); + if((ec_siigetbyte(cnt, ssigen + 0x0d) & 0x02) > 0) + { + ec_slave[cnt].blockLRW = 1; + ec_slave[0].blockLRW++; + } + ec_slave[cnt].Ebuscurrent = ec_siigetbyte(cnt, ssigen + 0x0e); + ec_slave[cnt].Ebuscurrent += ec_siigetbyte(cnt, ssigen + 0x0f) << 8; + ec_slave[0].Ebuscurrent += ec_slave[cnt].Ebuscurrent; + } + printf(" CoE details: %2.2x FoE details: %2.2x EoE details: %2.2x SoE details: %2.2x\n", + ec_slave[cnt].CoEdetails, ec_slave[cnt].FoEdetails, ec_slave[cnt].EoEdetails, ec_slave[cnt].SoEdetails); + printf(" Ebus current: %d[mA]\n only LRD/LWR:%d\n", + ec_slave[cnt].Ebuscurrent, ec_slave[cnt].blockLRW); + if ((ec_slave[cnt].mbx_proto & ECT_MBXPROT_COE) && printSDO) + si_sdo(cnt); + if(printMAP) + { + if (ec_slave[cnt].mbx_proto & ECT_MBXPROT_COE) + si_map_sdo(cnt); + else + si_map_sii(cnt); + } + } + } + else + { + printf("No slaves found!\n"); + } + printf("End slaveinfo, close socket\n"); + /* stop SOEM, close socket */ + ec_close(); + } + else + { + printf("No socket connection on %s\nExcecute as root\n",ifname); + } +} + +char ifbuf[1024]; + +int main(int argc, char *argv[]) +{ + ec_adaptert * adapter = NULL; + ec_adaptert * head = NULL; + printf("SOEM (Simple Open EtherCAT Master)\nSlaveinfo\n"); + + if (argc > 1) + { + if ((argc > 2) && (strncmp(argv[2], "-sdo", sizeof("-sdo")) == 0)) printSDO = TRUE; + if ((argc > 2) && (strncmp(argv[2], "-map", sizeof("-map")) == 0)) printMAP = TRUE; + /* start slaveinfo */ + strcpy(ifbuf, argv[1]); + slaveinfo(ifbuf); + } + else + { + printf("Usage: slaveinfo ifname [options]\nifname = eth0 for example\nOptions :\n -sdo : print SDO info\n -map : print mapping\n"); + + printf ("Available adapters\n"); + head = adapter = ec_find_adapters (); + while (adapter != NULL) + { + printf ("Description : %s, Device to use for wpcap: %s\n", adapter->desc,adapter->name); + adapter = adapter->next; + } + ec_free_adapters(head); + } + + printf("End program\n"); + return (0); +} diff --git a/soem-src/test/rtk/main.c b/soem-src/test/rtk/main.c new file mode 100644 index 0000000..820ef7c --- /dev/null +++ b/soem-src/test/rtk/main.c @@ -0,0 +1,518 @@ +/** \file + * \brief Example code for Simple Open EtherCAT master + * + * This is a minimal example running rt-kernel TTOS. Check + * tutorial files in documentations for som steps to get your + * system running. + * (c)Andreas Karlsson 2012 + */ + +#include +#include "ethercat.h" +#include "string.h" +#include +#include + +#include + +#define pPORTFIO_SET ((vuint16_t *)PORTFIO_SET) +#define pPORTFIO_CLEAR ((vuint16_t *)PORTFIO_CLEAR) +#define pPORTFIO_DIR ((vuint16_t *)PORTFIO_DIR) + +#define EK1100_1 1 +#define EL4001_1 2 +#define EL3061_1 3 +#define EL1008_1 4 +#define EL1008_2 5 +#define EL2622_1 6 +#define EL2622_2 7 +#define EL2622_3 8 +#define EL2622_4 9 +#define NUMBER_OF_SLAVES 9 + + +typedef struct +{ + uint8 in1; + uint8 in2; + uint8 in3; + uint8 in4; + uint8 in5; + uint8 in6; + uint8 in7; + uint8 in8; +} in_EL1008_t; + +typedef struct +{ + uint8 out1; + uint8 out2; +} out_EL2622_t; + +typedef struct +{ + int16 out1; +} out_EL4001_t; + +typedef struct +{ + int32 in1; +} in_EL3061_t; + +out_EL4001_t slave_EL4001_1; +in_EL3061_t slave_EL3061_1; +in_EL1008_t slave_EL1008_1; +in_EL1008_t slave_EL1008_2; +out_EL2622_t slave_EL2622_1; +out_EL2622_t slave_EL2622_2; +out_EL2622_t slave_EL2622_3; + +uint32 network_configuration(void) +{ + /* Do we got expected number of slaves from config */ + if (ec_slavecount < NUMBER_OF_SLAVES) + return 0; + + /* Verify slave by slave that it is correct*/ + if (strcmp(ec_slave[EK1100_1].name,"EK1100")) + return 0; + else if (strcmp(ec_slave[EL4001_1].name,"EL4001")) + return 0; + else if (strcmp(ec_slave[EL3061_1].name,"EL3061")) + return 0; + else if (strcmp(ec_slave[EL1008_1].name,"EL1008")) + return 0; + else if (strcmp(ec_slave[EL1008_2].name,"EL1008")) + return 0; + else if (strcmp(ec_slave[EL2622_1].name,"EL2622")) + return 0; + else if (strcmp(ec_slave[EL2622_2].name,"EL2622")) + return 0; + else if (strcmp(ec_slave[EL2622_3].name,"EL2622")) + return 0; + else if (strcmp(ec_slave[EL2622_4].name,"EL2622")) + return 0; + + return 1; +} + +int32 get_input_int32(uint16 slave_no,uint8 module_index) +{ + int32 return_value; + uint8 *data_ptr; + /* Get the IO map pointer from the ec_slave struct */ + data_ptr = ec_slave[slave_no].inputs; + /* Move pointer to correct module index */ + data_ptr += module_index * 4; + /* Read value byte by byte since all targets can't handle misaligned + * addresses + */ + return_value = *data_ptr++; + return_value += (*data_ptr++ << 8); + return_value += (*data_ptr++ << 16); + return_value += (*data_ptr++ << 24); + + return return_value; +} + +void set_input_int32 (uint16 slave_no, uint8 module_index, int32 value) +{ + uint8 *data_ptr; + /* Get the IO map pointer from the ec_slave struct */ + data_ptr = ec_slave[slave_no].inputs; + /* Move pointer to correct module index */ + data_ptr += module_index * 4; + /* Read value byte by byte since all targets can handle misaligned + * addresses + */ + *data_ptr++ = (value >> 0) & 0xFF; + *data_ptr++ = (value >> 8) & 0xFF; + *data_ptr++ = (value >> 16) & 0xFF; + *data_ptr++ = (value >> 24) & 0xFF; +} + +uint8 get_input_bit (uint16 slave_no,uint8 module_index) +{ + /* Get the the startbit position in slaves IO byte */ + uint8 startbit = ec_slave[slave_no].Istartbit; + /* Mask bit and return boolean 0 or 1 */ + if (*ec_slave[slave_no].inputs & BIT (module_index - 1 + startbit)) + return 1; + else + return 0; +} + +int16 get_output_int16(uint16 slave_no,uint8 module_index) +{ + int16 return_value; + uint8 *data_ptr; + + /* Get the IO map pointer from the ec_slave struct */ + data_ptr = ec_slave[slave_no].outputs; + /* Move pointer to correct module index */ + data_ptr += module_index * 2; + /* Read value byte by byte since all targets can handle misaligned + * addresses + */ + return_value = *data_ptr++; + return_value += (*data_ptr++ << 8); + + return return_value; +} + +void set_output_int16 (uint16 slave_no, uint8 module_index, int16 value) +{ + uint8 *data_ptr; + /* Get the IO map pointer from the ec_slave struct */ + data_ptr = ec_slave[slave_no].outputs; + /* Move pointer to correct module index */ + data_ptr += module_index * 2; + /* Read value byte by byte since all targets can handle misaligned + * addresses + */ + *data_ptr++ = (value >> 0) & 0xFF; + *data_ptr++ = (value >> 8) & 0xFF; +} + +uint8 get_output_bit (uint16 slave_no,uint8 module_index) +{ + /* Get the the startbit position in slaves IO byte */ + uint8 startbit = ec_slave[slave_no].Ostartbit; + /* Mask bit and return boolean 0 or 1 */ + if (*ec_slave[slave_no].outputs & BIT (module_index - 1 + startbit)) + return 1; + else + return 0; +} + +void set_output_bit (uint16 slave_no, uint8 module_index, uint8 value) +{ + /* Get the the startbit position in slaves IO byte */ + uint8 startbit = ec_slave[slave_no].Ostartbit; + /* Set or Clear bit */ + if (value == 0) + *ec_slave[slave_no].outputs &= ~(1 << (module_index - 1 + startbit)); + else + *ec_slave[slave_no].outputs |= (1 << (module_index - 1 + startbit)); +} + + +extern tt_sched_t * tt_sched[]; + +char IOmap[128]; +int dorun = 0; + +uint8_t load1s, load5s, load10s; +uint32_t error_counter = 0; + +void tt_error (uint32_t task_ix); +void tt_error (uint32_t task_ix) +{ + error_counter++; +} + +static void my_cyclic_callback (void * arg) +{ + while (1) + { + stats_get_load (&load1s, &load5s, &load10s); + rprintp ("Processor load was %d:%d:%d (1s:5s:10s) with TT errors: %d\n", + load1s, load5s, load10s,error_counter); + task_delay(20000); + } +} + + +void read_io (void * arg) +{ + /* Function connected to cyclic TTOS task + * The function is executed cyclic according + * sceduel specified in schedule.tt + */ + *pPORTFIO_SET = BIT (6); + /* Send and receive processdata + * Note that you need som synchronization + * case you modifey IO in local application + */ + ec_send_processdata(); + ec_receive_processdata(EC_TIMEOUTRET); + *pPORTFIO_CLEAR = BIT (6); +} + + +void simpletest(void *arg) +{ + + char *ifname = arg; + int cnt, i, j; + + *pPORTFIO_DIR |= BIT (6); + + rprintp("Starting simple test\n"); + + /* initialise SOEM */ + if (ec_init(ifname)) + { + rprintp("ec_init succeeded.\n"); + + /* find and auto-config slaves */ + if ( ec_config_init(FALSE) > 0 ) + { + rprintp("%d slaves found and configured.\n",ec_slavecount); + + /* Check network setup */ + if (network_configuration()) + { + /* Run IO mapping */ + ec_config_map(&IOmap); + + rprintp("Slaves mapped, state to SAFE_OP.\n"); + /* wait for all slaves to reach SAFE_OP state */ + ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE); + + /* Print som information on the mapped network */ + for( cnt = 1 ; cnt <= ec_slavecount ; cnt++) + { + rprintp("\nSlave:%d\n Name:%s\n Output size: %dbits\n Input size: %dbits\n State: %d\n Delay: %d[ns]\n Has DC: %d\n", + cnt, ec_slave[cnt].name, ec_slave[cnt].Obits, ec_slave[cnt].Ibits, + ec_slave[cnt].state, ec_slave[cnt].pdelay, ec_slave[cnt].hasdc); + rprintp(" Configured address: %x\n", ec_slave[cnt].configadr); + rprintp(" Outputs address: %x\n", ec_slave[cnt].outputs); + rprintp(" Inputs address: %x\n", ec_slave[cnt].inputs); + + for(j = 0 ; j < ec_slave[cnt].FMMUunused ; j++) + { + rprintp(" FMMU%1d Ls:%x Ll:%4d Lsb:%d Leb:%d Ps:%x Psb:%d Ty:%x Act:%x\n", j, + (int)ec_slave[cnt].FMMU[j].LogStart, ec_slave[cnt].FMMU[j].LogLength, ec_slave[cnt].FMMU[j].LogStartbit, + ec_slave[cnt].FMMU[j].LogEndbit, ec_slave[cnt].FMMU[j].PhysStart, ec_slave[cnt].FMMU[j].PhysStartBit, + ec_slave[cnt].FMMU[j].FMMUtype, ec_slave[cnt].FMMU[j].FMMUactive); + } + rprintp(" FMMUfunc 0:%d 1:%d 2:%d 3:%d\n", + ec_slave[cnt].FMMU0func, ec_slave[cnt].FMMU1func, ec_slave[cnt].FMMU2func, ec_slave[cnt].FMMU3func); + + } + + rprintp("Request operational state for all slaves\n"); + ec_slave[0].state = EC_STATE_OPERATIONAL; + /* send one valid process data to make outputs in slaves happy*/ + ec_send_processdata(); + ec_receive_processdata(EC_TIMEOUTRET); + /* request OP state for all slaves */ + ec_writestate(0); + /* wait for all slaves to reach OP state */ + ec_statecheck(0, EC_STATE_OPERATIONAL, EC_TIMEOUTSTATE); + if (ec_slave[0].state == EC_STATE_OPERATIONAL ) + { + rprintp("Operational state reached for all slaves.\n"); + } + else + { + rprintp("Not all slaves reached operational state.\n"); + ec_readstate(); + for(i = 1; i<=ec_slavecount ; i++) + { + if(ec_slave[i].state != EC_STATE_OPERATIONAL) + { + rprintp("Slave %d State=0x%04x StatusCode=0x%04x\n", + i, ec_slave[i].state, ec_slave[i].ALstatuscode); + } + } + } + + + /* Simple blinking lamps BOX demo */ + uint8 digout = 0; + + slave_EL4001_1.out1 = (int16)0x3FFF; + set_output_int16(EL4001_1,0,slave_EL4001_1.out1); + + task_spawn ("t_StatsPrint", my_cyclic_callback, 20, 1024, (void *)NULL); + tt_start_wait (tt_sched[0]); + + while(1) + { + dorun = 0; + slave_EL1008_1.in1 = get_input_bit(EL1008_1,1); // Start button + slave_EL1008_1.in2 = get_input_bit(EL1008_1,2); // Turnkey RIGHT + slave_EL1008_1.in3 = get_input_bit(EL1008_1,3); // Turnkey LEFT + + /* (Turnkey MIDDLE + Start button) OR Turnkey RIGHT OR Turnkey LEFT + Turnkey LEFT: Light positions bottom to top. Loop, slow operation. + Turnkey MIDDLE: Press start button to light positions bottom to top. No loop, fast operation. + Turnkey RIGHT: Light positions bottom to top. Loop, fast operation. + */ + if (slave_EL1008_1.in1 || slave_EL1008_1.in2 || slave_EL1008_1.in3) + { + digout = 0; + /* *ec_slave[6].outputs = digout; */ + /* set_output_bit(slave_name #,index as 1 output on module , value */ + set_output_bit(EL2622_1,1,(digout & BIT (0))); /* Start button */ + set_output_bit(EL2622_1,2,(digout & BIT (1))); /* Turnkey RIGHT */ + set_output_bit(EL2622_2,1,(digout & BIT (2))); /* Turnkey LEFT */ + set_output_bit(EL2622_2,2,(digout & BIT (3))); + set_output_bit(EL2622_3,1,(digout & BIT (4))); + set_output_bit(EL2622_3,2,(digout & BIT (5))); + + while(dorun < 95) + { + dorun++; + + if (slave_EL1008_1.in3) + task_delay(tick_from_ms(20)); + else + task_delay(tick_from_ms(5)); + + digout = (uint8) (digout | BIT((dorun / 16) & 0xFF)); + + set_output_bit(EL2622_1,1,(digout & BIT (0))); /* LED1 */ + set_output_bit(EL2622_1,2,(digout & BIT (1))); /* LED2 */ + set_output_bit(EL2622_2,1,(digout & BIT (2))); /* LED3 */ + set_output_bit(EL2622_2,2,(digout & BIT (3))); /* LED4 */ + set_output_bit(EL2622_3,1,(digout & BIT (4))); /* LED5 */ + set_output_bit(EL2622_3,2,(digout & BIT (5))); /* LED6 */ + + slave_EL1008_1.in1 = get_input_bit(EL1008_1,2); /* Turnkey RIGHT */ + slave_EL1008_1.in2 = get_input_bit(EL1008_1,3); /* Turnkey LEFT */ + slave_EL3061_1.in1 = get_input_int32(EL3061_1,0); /* Read AI */ + } + } + task_delay(tick_from_ms(2)); + + } + } + else + { + rprintp("Mismatch of network units!\n"); + } + } + else + { + rprintp("No slaves found!\n"); + } + rprintp("End simple test, close socket\n"); + /* stop SOEM, close socket */ + ec_close(); + } + else + { + rprintp("ec_init failed"); + } + rprintp("End program\n"); +} + +static void test_osal_timer_timeout_us (const uint32 timeout_us) +{ + osal_timert timer; + + ASSERT (timeout_us > 4000); + + osal_timer_start (&timer, timeout_us); + ASSERT (osal_timer_is_expired (&timer) == false); + osal_usleep (timeout_us - 2000); + ASSERT (osal_timer_is_expired (&timer) == false); + osal_usleep (4000); + ASSERT (osal_timer_is_expired (&timer)); +} + +static void test_osal_timer (void) +{ + test_osal_timer_timeout_us (10*1000); /* 10ms */ + test_osal_timer_timeout_us (100*1000); /* 100ms */ + test_osal_timer_timeout_us (1000*1000); /* 1s */ + test_osal_timer_timeout_us (2000*1000); /* 2s */ +} + +#define USECS_PER_SEC 1000000 +#define USECS_PER_TICK (USECS_PER_SEC / CFG_TICKS_PER_SECOND) +#ifndef ABS +#define ABS(x) ((x) < 0 ? -(x) : (x)) +#endif + +static int32 time_difference_us (const ec_timet stop, const ec_timet start) +{ + int32 difference_us; + + ASSERT (stop.sec >= start.sec); + if (stop.sec == start.sec) + { + ASSERT (stop.usec >= start.usec); + } + + difference_us = (stop.sec - start.sec) * USECS_PER_SEC; + difference_us += ((int32)stop.usec - (int32)start.usec); + + ASSERT (difference_us >= 0); + return difference_us; +} + +/** + * Test osal_current_time() by using it for measuring how long an osal_usleep() + * takes, in specified number of microseconds. + */ +static void test_osal_current_time_for_delay_us (const int32 sleep_time_us) +{ + ec_timet start; + ec_timet stop; + int32 measurement_us; + int32 deviation_us; + const int32 usleep_accuracy_us = USECS_PER_TICK; + boolean is_deviation_within_tolerance; + + start = osal_current_time (); + osal_usleep (sleep_time_us); + stop = osal_current_time (); + + measurement_us = time_difference_us (stop, start); + deviation_us = ABS (measurement_us - sleep_time_us); + is_deviation_within_tolerance = deviation_us <= usleep_accuracy_us; + ASSERT (is_deviation_within_tolerance); +} + +static void test_osal_current_time (void) +{ + test_osal_current_time_for_delay_us (0); + test_osal_current_time_for_delay_us (1); + test_osal_current_time_for_delay_us (500); + test_osal_current_time_for_delay_us (999); + test_osal_current_time_for_delay_us (USECS_PER_TICK); + test_osal_current_time_for_delay_us (USECS_PER_TICK-1); + test_osal_current_time_for_delay_us (USECS_PER_TICK+1); + test_osal_current_time_for_delay_us (2 * 1000 * 1000); /* 2s */ +} + +#include + +static void test_oshw_htons (void) +{ + uint16 network; + uint16 host; + + host = 0x1234; + network = oshw_htons (host); + ASSERT (network == htons (host)); +} + +static void test_oshw_ntohs (void) +{ + uint16 host; + uint16 network; + + network = 0x1234; + host = oshw_ntohs (network); + ASSERT (host == ntohs (network)); +} + +int main (void) +{ + test_oshw_htons (); + test_oshw_ntohs (); + test_osal_timer (); + test_osal_current_time (); + + rprintp("SOEM (Simple Open EtherCAT Master)\nSimple test\n"); + + task_spawn ("simpletest", simpletest, 9, 8192, NULL); + + return (0); +} + diff --git a/soem-src/test/simple_ng/simple_ng.c b/soem-src/test/simple_ng/simple_ng.c new file mode 100644 index 0000000..82eb1ba --- /dev/null +++ b/soem-src/test/simple_ng/simple_ng.c @@ -0,0 +1,327 @@ +/** \file + * \brief Example code for Simple Open EtherCAT master + * + * Usage: simple_ng IFNAME1 + * IFNAME1 is the NIC interface name, e.g. 'eth0' + * + * This is a minimal test. + */ + +#include "ethercat.h" +#include +#include +#include + + +typedef struct { + ecx_contextt context; + char * iface; + uint8 group; + int roundtrip_time; + + /* Used by the context */ + uint8 map[4096]; + ecx_portt port; + ec_slavet slavelist[EC_MAXSLAVE]; + int slavecount; + ec_groupt grouplist[EC_MAXGROUP]; + uint8 esibuf[EC_MAXEEPBUF]; + uint32 esimap[EC_MAXEEPBITMAP]; + ec_eringt elist; + ec_idxstackT idxstack; + boolean ecaterror; + int64 DCtime; + ec_SMcommtypet SMcommtype[EC_MAX_MAPT]; + ec_PDOassignt PDOassign[EC_MAX_MAPT]; + ec_PDOdesct PDOdesc[EC_MAX_MAPT]; + ec_eepromSMt eepSM; + ec_eepromFMMUt eepFMMU; +} Fieldbus; + + +static void +fieldbus_initialize(Fieldbus *fieldbus, char *iface) +{ + ecx_contextt *context; + + /* Let's start by 0-filling `fieldbus` to avoid surprises */ + memset(fieldbus, 0, sizeof(*fieldbus)); + + fieldbus->iface = iface; + fieldbus->group = 0; + fieldbus->roundtrip_time = 0; + fieldbus->ecaterror = FALSE; + + /* Initialize the ecx_contextt data structure */ + context = &fieldbus->context; + context->port = &fieldbus->port; + context->slavelist = fieldbus->slavelist; + context->slavecount = &fieldbus->slavecount; + context->maxslave = EC_MAXSLAVE; + context->grouplist = fieldbus->grouplist; + context->maxgroup = EC_MAXGROUP; + context->esibuf = fieldbus->esibuf; + context->esimap = fieldbus->esimap; + context->esislave = 0; + context->elist = &fieldbus->elist; + context->idxstack = &fieldbus->idxstack; + context->ecaterror = &fieldbus->ecaterror; + context->DCtime = &fieldbus->DCtime; + context->SMcommtype = fieldbus->SMcommtype; + context->PDOassign = fieldbus->PDOassign; + context->PDOdesc = fieldbus->PDOdesc; + context->eepSM = &fieldbus->eepSM; + context->eepFMMU = &fieldbus->eepFMMU; + context->FOEhook = NULL; + context->EOEhook = NULL; + context->manualstatechange = 0; +} + +static int +fieldbus_roundtrip(Fieldbus *fieldbus) +{ + ecx_contextt *context; + ec_timet start, end, diff; + int wkc; + + context = &fieldbus->context; + + start = osal_current_time(); + ecx_send_processdata(context); + wkc = ecx_receive_processdata(context, EC_TIMEOUTRET); + end = osal_current_time(); + osal_time_diff(&start, &end, &diff); + fieldbus->roundtrip_time = diff.sec * 1000000 + diff.usec; + + return wkc; +} + +static boolean +fieldbus_start(Fieldbus *fieldbus) +{ + ecx_contextt *context; + ec_groupt *grp; + ec_slavet *slave; + int i; + + context = &fieldbus->context; + grp = fieldbus->grouplist + fieldbus->group; + + printf("Initializing SOEM on '%s'... ", fieldbus->iface); + if (! ecx_init(context, fieldbus->iface)) { + printf("no socket connection\n"); + return FALSE; + } + printf("done\n"); + + printf("Finding autoconfig slaves... "); + if (ecx_config_init(context, FALSE) <= 0) { + printf("no slaves found\n"); + return FALSE; + } + printf("%d slaves found\n", fieldbus->slavecount); + + printf("Sequential mapping of I/O... "); + ecx_config_map_group(context, fieldbus->map, fieldbus->group); + printf("mapped %dO+%dI bytes from %d segments", + grp->Obytes, grp->Ibytes, grp->nsegments); + if (grp->nsegments > 1) { + /* Show how slaves are distrubuted */ + for (i = 0; i < grp->nsegments; ++i) { + printf("%s%d", i == 0 ? " (" : "+", grp->IOsegment[i]); + } + printf(" slaves)"); + } + printf("\n"); + + printf("Configuring distributed clock... "); + ecx_configdc(context); + printf("done\n"); + + printf("Waiting for all slaves in safe operational... "); + ecx_statecheck(context, 0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE * 4); + printf("done\n"); + + printf("Send a roundtrip to make outputs in slaves happy... "); + fieldbus_roundtrip(fieldbus); + printf("done\n"); + + printf("Setting operational state.."); + /* Act on slave 0 (a virtual slave used for broadcasting) */ + slave = fieldbus->slavelist; + slave->state = EC_STATE_OPERATIONAL; + ecx_writestate(context, 0); + /* Poll the result ten times before giving up */ + for (i = 0; i < 10; ++i) { + printf("."); + fieldbus_roundtrip(fieldbus); + ecx_statecheck(context, 0, EC_STATE_OPERATIONAL, EC_TIMEOUTSTATE / 10); + if (slave->state == EC_STATE_OPERATIONAL) { + printf(" all slaves are now operational\n"); + return TRUE; + } + } + + printf(" failed,"); + ecx_readstate(context); + for (i = 1; i <= fieldbus->slavecount; ++i) { + slave = fieldbus->slavelist + i; + if (slave->state != EC_STATE_OPERATIONAL) { + printf(" slave %d is 0x%04X (AL-status=0x%04X %s)", + i, slave->state, slave->ALstatuscode, + ec_ALstatuscode2string(slave->ALstatuscode)); + } + } + printf("\n"); + + return FALSE; +} + +static void +fieldbus_stop(Fieldbus *fieldbus) +{ + ecx_contextt *context; + ec_slavet *slave; + + context = &fieldbus->context; + /* Act on slave 0 (a virtual slave used for broadcasting) */ + slave = fieldbus->slavelist; + + printf("Requesting init state on all slaves... "); + slave->state = EC_STATE_INIT; + ecx_writestate(context, 0); + printf("done\n"); + + printf("Close socket... "); + ecx_close(context); + printf("done\n"); +} + +static boolean +fieldbus_dump(Fieldbus *fieldbus) +{ + ec_groupt *grp; + uint32 n; + int wkc, expected_wkc; + + grp = fieldbus->grouplist + fieldbus->group; + + wkc = fieldbus_roundtrip(fieldbus); + expected_wkc = grp->outputsWKC * 2 + grp->inputsWKC; + printf("%6d usec WKC %d", fieldbus->roundtrip_time, wkc); + if (wkc < expected_wkc) { + printf(" wrong (expected %d)\n", expected_wkc); + return FALSE; + } + + printf(" O:"); + for (n = 0; n < grp->Obytes; ++n) { + printf(" %02X", grp->outputs[n]); + } + printf(" I:"); + for (n = 0; n < grp->Ibytes; ++n) { + printf(" %02X", grp->inputs[n]); + } + printf(" T: %lld\r", (long long) fieldbus->DCtime); + return TRUE; +} + +static void +fieldbus_check_state(Fieldbus *fieldbus) +{ + ecx_contextt *context; + ec_groupt *grp; + ec_slavet *slave; + int i; + + context = &fieldbus->context; + grp = context->grouplist + fieldbus->group; + grp->docheckstate = FALSE; + ecx_readstate(context); + for (i = 1; i <= fieldbus->slavecount; ++i) { + slave = context->slavelist + i; + if (slave->group != fieldbus->group) { + /* This slave is part of another group: do nothing */ + } else if (slave->state != EC_STATE_OPERATIONAL) { + grp->docheckstate = TRUE; + if (slave->state == EC_STATE_SAFE_OP + EC_STATE_ERROR) { + printf("* Slave %d is in SAFE_OP+ERROR, attempting ACK\n", i); + slave->state = EC_STATE_SAFE_OP + EC_STATE_ACK; + ecx_writestate(context, i); + } else if(slave->state == EC_STATE_SAFE_OP) { + printf("* Slave %d is in SAFE_OP, change to OPERATIONAL\n", i); + slave->state = EC_STATE_OPERATIONAL; + ecx_writestate(context, i); + } else if(slave->state > EC_STATE_NONE) { + if (ecx_reconfig_slave(context, i, EC_TIMEOUTRET)) { + slave->islost = FALSE; + printf("* Slave %d reconfigured\n", i); + } + } else if(! slave->islost) { + ecx_statecheck(context, i, EC_STATE_OPERATIONAL, EC_TIMEOUTRET); + if (slave->state == EC_STATE_NONE) { + slave->islost = TRUE; + printf("* Slave %d lost\n", i); + } + } + } else if (slave->islost) { + if(slave->state != EC_STATE_NONE) { + slave->islost = FALSE; + printf("* Slave %d found\n", i); + } else if (ecx_recover_slave(context, i, EC_TIMEOUTRET)) { + slave->islost = FALSE; + printf("* Slave %d recovered\n", i); + } + } + } + + if (! grp->docheckstate) { + printf("All slaves resumed OPERATIONAL\n"); + } +} + +int +main(int argc, char *argv[]) +{ + Fieldbus fieldbus; + + if (argc != 2) { + ec_adaptert * adapter = NULL; + ec_adaptert * head = NULL; + printf("Usage: simple_ng IFNAME1\n" + "IFNAME1 is the NIC interface name, e.g. 'eth0'\n"); + + printf("\nAvailable adapters:\n"); + head = adapter = ec_find_adapters(); + while (adapter != NULL) + { + printf(" - %s (%s)\n", adapter->name, adapter->desc); + adapter = adapter->next; + } + ec_free_adapters(head); + return 1; + } + + fieldbus_initialize(&fieldbus, argv[1]); + if (fieldbus_start(&fieldbus)) { + int i, min_time, max_time; + min_time = max_time = 0; + for (i = 1; i <= 10000; ++i) { + printf("Iteration %4d:", i); + if (! fieldbus_dump(&fieldbus)) { + fieldbus_check_state(&fieldbus); + } else if (i == 1) { + min_time = max_time = fieldbus.roundtrip_time; + } else if (fieldbus.roundtrip_time < min_time) { + min_time = fieldbus.roundtrip_time; + } else if (fieldbus.roundtrip_time > max_time) { + max_time = fieldbus.roundtrip_time; + } + osal_usleep(5000); + } + printf("\nRoundtrip time (usec): min %d max %d\n", min_time, max_time); + fieldbus_stop(&fieldbus); + } + + return 0; +} diff --git a/soem-src/test/win32/ebox/ebox.c b/soem-src/test/win32/ebox/ebox.c new file mode 100644 index 0000000..cd1fb42 --- /dev/null +++ b/soem-src/test/win32/ebox/ebox.c @@ -0,0 +1,388 @@ +/** \file + * \brief Example code for Simple Open EtherCAT master + * + * Usage : ebox [ifname] [cycletime] + * ifname is NIC interface, f.e. eth0 + * cycletime in us, f.e. 500 + * + * This test is specifically build for the E/BOX. + * + * (c)Arthur Ketels 2011 + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "ethercat.h" + +#define NSEC_PER_SEC 1000000000 + +typedef struct PACKED +{ + uint8 status; + uint8 counter; + uint8 din; + int32 ain[2]; + uint32 tsain; + int32 enc[2]; +} in_EBOXt; + +typedef struct PACKED +{ + uint8 counter; + int16 stream[100]; +} in_EBOX_streamt; + +typedef struct PACKED +{ + uint8 control; + uint8 dout; + int16 aout[2]; + uint16 pwmout[2]; +} out_EBOXt; + +typedef struct PACKED +{ + uint8 control; +} out_EBOX_streamt; + +// total samples to capture +#define MAXSTREAM 200000 +// sample interval in ns, here 8us -> 125kHz +// maximum data rate for E/BOX v1.0.1 is around 150kHz +#define SYNC0TIME 8000 + +struct sched_param schedp; +char IOmap[4096]; +pthread_t thread1; +struct timeval tv,t1,t2; +int dorun = 0; +int deltat, tmax=0; +int64 toff; +int DCdiff; +int os; +uint32 ob; +int16 ob2; +uint8 ob3; +pthread_cond_t cond = PTHREAD_COND_INITIALIZER; +pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER; +int64 integral=0; +uint32 cyclecount; +in_EBOX_streamt *in_EBOX; +out_EBOX_streamt *out_EBOX; +double ain[2]; +int ainc; +int streampos; +int16 stream1[MAXSTREAM]; +int16 stream2[MAXSTREAM]; + +int output_cvs(char *fname, int length) +{ + FILE *fp; + + int i; + + fp = fopen(fname, "w"); + if(fp == NULL) + return 0; + for (i = 0; i < length; i++) + { + fprintf(fp, "%d %d %d\n", i, stream1[i], stream2[i]); + } + fclose(fp); + + return 1; +} + +void eboxtest(char *ifname) +{ + int cnt, i; + + printf("Starting E/BOX test\n"); + + /* initialise SOEM, bind socket to ifname */ + if (ec_init(ifname)) + { + printf("ec_init on %s succeeded.\n",ifname); + /* find and auto-config slaves */ + if ( ec_config_init(FALSE) > 0 ) + { + printf("%d slaves found and configured.\n",ec_slavecount); + + // check if first slave is an E/BOX + if (( ec_slavecount >= 1 ) && + (strcmp(ec_slave[1].name,"E/BOX") == 0)) + { + // reprogram PDO mapping to set slave in stream mode + // this can only be done in pre-OP state + os=sizeof(ob2); ob2 = 0x1601; + ec_SDOwrite(1,0x1c12,01,FALSE,os,&ob2,EC_TIMEOUTRXM); + os=sizeof(ob2); ob2 = 0x1a01; + ec_SDOwrite(1,0x1c13,01,FALSE,os,&ob2,EC_TIMEOUTRXM); + } + + ec_config_map(&IOmap); + + ec_configdc(); + + /* wait for all slaves to reach SAFE_OP state */ + ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE); + + /* configure DC options for every DC capable slave found in the list */ + printf("DC capable : %d\n",ec_configdc()); + + /* check configuration */ + if (( ec_slavecount >= 1 ) && + (strcmp(ec_slave[1].name,"E/BOX") == 0) + ) + { + printf("E/BOX found.\n"); + + /* connect struct pointers to slave I/O pointers */ + in_EBOX = (in_EBOX_streamt*) ec_slave[1].inputs; + out_EBOX = (out_EBOX_streamt*) ec_slave[1].outputs; + + /* read indevidual slave state and store in ec_slave[] */ + ec_readstate(); + for(cnt = 1; cnt <= ec_slavecount ; cnt++) + { + printf("Slave:%d Name:%s Output size:%3dbits Input size:%3dbits State:%2d delay:%d.%d\n", + cnt, ec_slave[cnt].name, ec_slave[cnt].Obits, ec_slave[cnt].Ibits, + ec_slave[cnt].state, (int)ec_slave[cnt].pdelay, ec_slave[cnt].hasdc); + } + printf("Request operational state for all slaves\n"); + + /* send one processdata cycle to init SM in slaves */ + ec_send_processdata(); + ec_receive_processdata(EC_TIMEOUTRET); + + ec_slave[0].state = EC_STATE_OPERATIONAL; + /* request OP state for all slaves */ + ec_writestate(0); + /* wait for all slaves to reach OP state */ + ec_statecheck(0, EC_STATE_OPERATIONAL, EC_TIMEOUTSTATE); + if (ec_slave[0].state == EC_STATE_OPERATIONAL ) + { + printf("Operational state reached for all slaves.\n"); + ain[0] = 0; + ain[1] = 0; + ainc = 0; + dorun = 1; + usleep(100000); // wait for linux to sync on DC + ec_dcsync0(1, TRUE, SYNC0TIME, 0); // SYNC0 on slave 1 + /* acyclic loop 20ms */ + for(i = 1; i <= 200; i++) + { + /* read DC difference register for slave 2 */ + // ec_FPRD(ec_slave[1].configadr, ECT_REG_DCSYSDIFF, sizeof(DCdiff), &DCdiff, EC_TIMEOUTRET); + // if(DCdiff<0) { DCdiff = - (int32)((uint32)DCdiff & 0x7ffffff); } + printf("PD cycle %5d DCtime %12lld Cnt:%3d Data: %6d %6d %6d %6d %6d %6d %6d %6d \n", + cyclecount, ec_DCtime, in_EBOX->counter, in_EBOX->stream[0], in_EBOX->stream[1], + in_EBOX->stream[2], in_EBOX->stream[3], in_EBOX->stream[4], in_EBOX->stream[5], + in_EBOX->stream[98], in_EBOX->stream[99]); + usleep(20000); + } + dorun = 0; + // printf("\nCnt %d : Ain0 = %f Ain2 = %f\n", ainc, ain[0] / ainc, ain[1] / ainc); + } + else + { + printf("Not all slaves reached operational state.\n"); + } + } + else + { + printf("E/BOX not found in slave configuration.\n"); + } + ec_dcsync0(1, FALSE, 8000, 0); // SYNC0 off + printf("Request safe operational state for all slaves\n"); + ec_slave[0].state = EC_STATE_SAFE_OP; + /* request SAFE_OP state for all slaves */ + ec_writestate(0); + /* wait for all slaves to reach state */ + ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE); + ec_slave[0].state = EC_STATE_PRE_OP; + /* request SAFE_OP state for all slaves */ + ec_writestate(0); + /* wait for all slaves to reach state */ + ec_statecheck(0, EC_STATE_PRE_OP, EC_TIMEOUTSTATE); + if (( ec_slavecount >= 1 ) && + (strcmp(ec_slave[1].name,"E/BOX") == 0)) + { + // restore PDO to standard mode + // this can only be done is pre-op state + os=sizeof(ob2); ob2 = 0x1600; + ec_SDOwrite(1,0x1c12,01,FALSE,os,&ob2,EC_TIMEOUTRXM); + os=sizeof(ob2); ob2 = 0x1a00; + ec_SDOwrite(1,0x1c13,01,FALSE,os,&ob2,EC_TIMEOUTRXM); + } + printf("Streampos %d\n", streampos); + output_cvs("stream.txt", streampos); + } + else + { + printf("No slaves found!\n"); + } + printf("End E/BOX, close socket\n"); + /* stop SOEM, close socket */ + ec_close(); + } + else + { + printf("No socket connection on %s\nExcecute as root\n",ifname); + } +} + +/* add ns to timespec */ +void add_timespec(struct timespec *ts, int64 addtime) +{ + int64 sec, nsec; + + nsec = addtime % NSEC_PER_SEC; + sec = (addtime - nsec) / NSEC_PER_SEC; + ts->tv_sec += sec; + ts->tv_nsec += nsec; + if ( ts->tv_nsec >= NSEC_PER_SEC ) + { + nsec = ts->tv_nsec % NSEC_PER_SEC; + ts->tv_sec += (ts->tv_nsec - nsec) / NSEC_PER_SEC; + ts->tv_nsec = nsec; + } +} + +/* PI calculation to get linux time synced to DC time */ +void ec_sync(int64 reftime, int64 cycletime , int64 *offsettime) +{ + int64 delta; + /* set linux sync point 50us later than DC sync, just as example */ + delta = (reftime - 50000) % cycletime; + if(delta> (cycletime /2)) { delta= delta - cycletime; } + if(delta>0){ integral++; } + if(delta<0){ integral--; } + *offsettime = -(delta / 100) - (integral /20); +} + +/* RT EtherCAT thread */ +void ecatthread( void *ptr ) +{ + struct timespec ts; + struct timeval tp; + int rc; + int ht; + int i; + int pcounter = 0; + int64 cycletime; + + rc = pthread_mutex_lock(&mutex); + rc = gettimeofday(&tp, NULL); + + /* Convert from timeval to timespec */ + ts.tv_sec = tp.tv_sec; + ht = (tp.tv_usec / 1000) + 1; /* round to nearest ms */ + ts.tv_nsec = ht * 1000000; + cycletime = *(int*)ptr * 1000; /* cycletime in ns */ + toff = 0; + dorun = 0; + while(1) + { + /* calculate next cycle start */ + add_timespec(&ts, cycletime + toff); + /* wait to cycle start */ + rc = pthread_cond_timedwait(&cond, &mutex, &ts); + if (dorun>0) + { + rc = gettimeofday(&tp, NULL); + + ec_send_processdata(); + + ec_receive_processdata(EC_TIMEOUTRET); + + cyclecount++; + + + if((in_EBOX->counter != pcounter) && (streampos < (MAXSTREAM - 1))) + { + // check if we have timing problems in master + // if so, overwrite stream data so it shows up clearly in plots. + if(in_EBOX->counter > (pcounter + 1)) + { + for(i = 0 ; i < 50 ; i++) + { + stream1[streampos] = 20000; + stream2[streampos++] = -20000; + } + } + else + { + for(i = 0 ; i < 50 ; i++) + { + stream1[streampos] = in_EBOX->stream[i * 2]; + stream2[streampos++] = in_EBOX->stream[(i * 2) + 1]; + } + } + pcounter = in_EBOX->counter; + } + + /* calulate toff to get linux time and DC synced */ + ec_sync(ec_DCtime, cycletime, &toff); + } + } +} + +int main(int argc, char *argv[]) +{ + int iret1; + int ctime; + struct sched_param param; + int policy = SCHED_OTHER; + + printf("SOEM (Simple Open EtherCAT Master)\nE/BOX test\n"); + + memset(&schedp, 0, sizeof(schedp)); + /* do not set priority above 49, otherwise sockets are starved */ + schedp.sched_priority = 30; + sched_setscheduler(0, SCHED_FIFO, &schedp); + + do + { + usleep(1000); + } + while (dorun); + + if (argc > 1) + { + dorun = 1; + if( argc > 2) + ctime = atoi(argv[2]); + else + ctime = 1000; // 1ms cycle time + /* create RT thread */ + iret1 = pthread_create( &thread1, NULL, (void *) &ecatthread, (void*) &ctime); + memset(¶m, 0, sizeof(param)); + /* give it higher priority */ + param.sched_priority = 40; + iret1 = pthread_setschedparam(thread1, policy, ¶m); + + /* start acyclic part */ + eboxtest(argv[1]); + } + else + { + printf("Usage: ebox ifname [cycletime]\nifname = eth0 for example\ncycletime in us\n"); + } + + schedp.sched_priority = 0; + sched_setscheduler(0, SCHED_OTHER, &schedp); + + printf("End program\n"); + + return (0); +} diff --git a/soem-src/test/win32/eepromtool/eepromtool.c b/soem-src/test/win32/eepromtool/eepromtool.c new file mode 100644 index 0000000..66575d8 --- /dev/null +++ b/soem-src/test/win32/eepromtool/eepromtool.c @@ -0,0 +1,390 @@ +/** \file + * \brief EEprom tool for Simple Open EtherCAT master + * + * Usage : eepromtool ifname slave OPTION fname + * ifname is NIC interface, f.e. eth0 + * slave = slave number in EtherCAT order 1..n + * -r read EEPROM, output binary format + * -ri read EEPROM, output Intel Hex format + * -w write EEPROM, input binary format + * -wi write EEPROM, input Intel Hex format + * + * (c)Arthur Ketels 2010 + */ + +#include +#include +#include + +#include "ethercat.h" + +#define MAXBUF 32768 +#define STDBUF 2048 +#define MINBUF 128 + +#define MODE_NONE 0 +#define MODE_READBIN 1 +#define MODE_READINTEL 2 +#define MODE_WRITEBIN 3 +#define MODE_WRITEINTEL 4 + +#define MAXSLENGTH 256 + +uint8 ebuf[MAXBUF]; +uint8 ob; +uint16 ow; +int os; +int slave; +ec_timet tstart,tend, tdif; +int wkc; +int mode; +char sline[MAXSLENGTH]; + +#define IHEXLENGTH 0x20 + +int input_bin(char *fname, int *length) +{ + FILE *fp; + + int cc = 0, c; + + fp = fopen(fname, "rb"); + if(fp == NULL) + return 0; + while (((c = fgetc(fp)) != EOF) && (cc < MAXBUF)) + ebuf[cc++] = (uint8)c; + *length = cc; + fclose(fp); + + return 1; +} + +int input_intelhex(char *fname, int *start, int *length) +{ + FILE *fp; + + int c, sc, retval = 1; + int ll, ladr, lt, sn, i, lval; + int hstart, hlength, sum; + + fp = fopen(fname, "r"); + if(fp == NULL) + return 0; + hstart = MAXBUF; + hlength = 0; + sum = 0; + do + { + memset(sline, 0x00, MAXSLENGTH); + sc = 0; + while (((c = fgetc(fp)) != EOF) && (c != 0x0A) && (sc < (MAXSLENGTH -1))) + sline[sc++] = (uint8)c; + if ((c != EOF) && ((sc < 11) || (sline[0] != ':'))) + { + c = EOF; + retval = 0; + printf("Invalid Intel Hex format.\n"); + } + if (c != EOF) + { + sn = sscanf(sline , ":%2x%4x%2x", &ll, &ladr, <); + if ((sn == 3) && ((ladr + ll) <= MAXBUF) && (lt == 0)) + { + sum = ll + (ladr >> 8) + (ladr & 0xff) + lt; + if(ladr < hstart) hstart = ladr; + for(i = 0; i < ll ; i++) + { + sn = sscanf(&sline[9 + (i << 1)], "%2x", &lval); + ebuf[ladr + i] = (uint8)lval; + sum += (uint8)lval; + } + if(((ladr + ll) - hstart) > hlength) + hlength = (ladr + ll) - hstart; + sum = (0x100 - sum) & 0xff; + sn = sscanf(&sline[9 + (i << 1)], "%2x", &lval); + if (!sn || ((sum - lval) != 0)) + { + c = EOF; + retval = 0; + printf("Invalid checksum.\n"); + } + } + } + } + while (c != EOF); + if (retval) + { + *length = hlength; + *start = hstart; + } + fclose(fp); + + return retval; +} + +int output_bin(char *fname, int length) +{ + FILE *fp; + + int cc; + + fp = fopen(fname, "wb"); + if(fp == NULL) + return 0; + for (cc = 0 ; cc < length ; cc++) + fputc( ebuf[cc], fp); + fclose(fp); + + return 1; +} + +int output_intelhex(char *fname, int length) +{ + FILE *fp; + + int cc = 0, ll, sum, i; + + fp = fopen(fname, "w"); + if(fp == NULL) + return 0; + while (cc < length) + { + ll = length - cc; + if (ll > IHEXLENGTH) ll = IHEXLENGTH; + sum = ll + (cc >> 8) + (cc & 0xff); + fprintf(fp, ":%2.2X%4.4X00", ll, cc); + for (i = 0; i < ll; i++) + { + fprintf(fp, "%2.2X", ebuf[cc + i]); + sum += ebuf[cc + i]; + } + fprintf(fp, "%2.2X\n", (0x100 - sum) & 0xff); + cc += ll; + } + fprintf(fp, ":00000001FF\n"); + fclose(fp); + + return 1; +} + +int eeprom_read(int slave, int start, int length) +{ + int i, wkc, ainc = 4; + uint16 estat, aiadr; + uint32 b4; + uint64 b8; + uint8 eepctl; + + if((ec_slavecount >= slave) && (slave > 0) && ((start + length) <= MAXBUF)) + { + aiadr = 1 - slave; + eepctl = 2; + wkc = ec_APWR(aiadr, ECT_REG_EEPCFG, sizeof(eepctl), &eepctl , EC_TIMEOUTRET3); /* force Eeprom from PDI */ + eepctl = 0; + wkc = ec_APWR(aiadr, ECT_REG_EEPCFG, sizeof(eepctl), &eepctl , EC_TIMEOUTRET3); /* set Eeprom to master */ + + estat = 0x0000; + aiadr = 1 - slave; + wkc=ec_APRD(aiadr, ECT_REG_EEPSTAT, sizeof(estat), &estat, EC_TIMEOUTRET3); /* read eeprom status */ + estat = etohs(estat); + if (estat & EC_ESTAT_R64) + { + ainc = 8; + for (i = start ; i < (start + length) ; i+=ainc) + { + b8 = ec_readeepromAP(aiadr, i >> 1 , EC_TIMEOUTEEP); + ebuf[i] = (uint8) b8; + ebuf[i+1] = (uint8) (b8 >> 8); + ebuf[i+2] = (uint8) (b8 >> 16); + ebuf[i+3] = (uint8) (b8 >> 24); + ebuf[i+4] = (uint8) (b8 >> 32); + ebuf[i+5] = (uint8) (b8 >> 40); + ebuf[i+6] = (uint8) (b8 >> 48); + ebuf[i+7] = (uint8) (b8 >> 56); + } + } + else + { + for (i = start ; i < (start + length) ; i+=ainc) + { + b4 = (uint32)ec_readeepromAP(aiadr, i >> 1 , EC_TIMEOUTEEP); + ebuf[i] = (uint8) b4; + ebuf[i+1] = (uint8) (b4 >> 8); + ebuf[i+2] = (uint8) (b4 >> 16); + ebuf[i+3] = (uint8) (b4 >> 24); + } + } + + return 1; + } + + return 0; +} + +int eeprom_write(int slave, int start, int length) +{ + int i, wkc, dc = 0; + uint16 aiadr, *wbuf; + uint8 eepctl; + int ret; + + if((ec_slavecount >= slave) && (slave > 0) && ((start + length) <= MAXBUF)) + { + aiadr = 1 - slave; + eepctl = 2; + wkc = ec_APWR(aiadr, ECT_REG_EEPCFG, sizeof(eepctl), &eepctl , EC_TIMEOUTRET3); /* force Eeprom from PDI */ + eepctl = 0; + wkc = ec_APWR(aiadr, ECT_REG_EEPCFG, sizeof(eepctl), &eepctl , EC_TIMEOUTRET3); /* set Eeprom to master */ + + aiadr = 1 - slave; + wbuf = (uint16 *)&ebuf[0]; + for (i = start ; i < (start + length) ; i+=2) + { + ret = ec_writeeepromAP(aiadr, i >> 1 , *(wbuf + (i >> 1)), EC_TIMEOUTEEP); + if (++dc >= 100) + { + dc = 0; + printf("."); + fflush(stdout); + } + } + + return 1; + } + + return 0; +} + +void eepromtool(char *ifname, int slave, int mode, char *fname) +{ + int w, rc = 0, estart, esize; + uint16 *wbuf; + + /* initialise SOEM, bind socket to ifname */ + if (ec_init(ifname)) + { + printf("ec_init on %s succeeded.\n",ifname); + + w = 0x0000; + wkc = ec_BRD(0x0000, ECT_REG_TYPE, sizeof(w), &w, EC_TIMEOUTSAFE); /* detect number of slaves */ + if (wkc > 0) + { + ec_slavecount = wkc; + + printf("%d slaves found.\n",ec_slavecount); + if((ec_slavecount >= slave) && (slave > 0)) + { + if ((mode == MODE_READBIN) || (mode == MODE_READINTEL)) + { + tstart = osal_current_time(); + eeprom_read(slave, 0x0000, MINBUF); // read first 128 bytes + + wbuf = (uint16 *)&ebuf[0]; + printf("Slave %d data\n", slave); + printf(" PDI Control : %4.4X\n",*(wbuf + 0x00)); + printf(" PDI Config : %4.4X\n",*(wbuf + 0x01)); + printf(" Config Alias : %4.4X\n",*(wbuf + 0x04)); + printf(" Checksum : %4.4X\n",*(wbuf + 0x07)); + printf(" Vendor ID : %8.8X\n",*(uint32 *)(wbuf + 0x08)); + printf(" Product Code : %8.8X\n",*(uint32 *)(wbuf + 0x0A)); + printf(" Revision Number : %8.8X\n",*(uint32 *)(wbuf + 0x0C)); + printf(" Serial Number : %8.8X\n",*(uint32 *)(wbuf + 0x0E)); + printf(" Mailbox Protocol : %4.4X\n",*(wbuf + 0x1C)); + esize = (*(wbuf + 0x3E) + 1) * 128; + if (esize > MAXBUF) esize = MAXBUF; + printf(" Size : %4.4X = %d bytes\n",*(wbuf + 0x3E), esize); + printf(" Version : %4.4X\n",*(wbuf + 0x3F)); + + if (esize > MINBUF) + eeprom_read(slave, MINBUF, esize - MINBUF); // read reminder + + tend = osal_current_time(); + osal_time_diff(&tstart, &tend, &tdif); + if (mode == MODE_READINTEL) output_intelhex(fname, esize); + if (mode == MODE_READBIN) output_bin(fname, esize); + + printf("\nTotal EEPROM read time :%ldms\n", (tdif.usec+(tdif.sec*1000000L)) / 1000); + } + if ((mode == MODE_WRITEBIN) || (mode == MODE_WRITEINTEL)) + { + estart = 0; + if (mode == MODE_WRITEINTEL) rc = input_intelhex(fname, &estart, &esize); + if (mode == MODE_WRITEBIN) rc = input_bin(fname, &esize); + + if (rc > 0) + { + wbuf = (uint16 *)&ebuf[0]; + printf("Slave %d\n", slave); + printf(" Vendor ID : %8.8X\n",*(uint32 *)(wbuf + 0x08)); + printf(" Product Code : %8.8X\n",*(uint32 *)(wbuf + 0x0A)); + printf(" Revision Number : %8.8X\n",*(uint32 *)(wbuf + 0x0C)); + printf(" Serial Number : %8.8X\n",*(uint32 *)(wbuf + 0x0E)); + + printf("Busy"); + fflush(stdout); + tstart = osal_current_time(); + eeprom_write(slave, estart, esize); + tend = osal_current_time(); + osal_time_diff(&tstart, &tend, &tdif); + + printf("\nTotal EEPROM write time :%ldms\n", (tdif.usec+(tdif.sec*1000000L)) / 1000); + } + else + printf("Error reading file, abort.\n"); + } + } + else + printf("Slave number outside range.\n"); + } + else + printf("No slaves found!\n"); + printf("End, close socket\n"); + /* stop SOEM, close socket */ + ec_close(); + } + else + printf("No socket connection on %s\nExcecute as root\n",ifname); +} + +int main(int argc, char *argv[]) +{ + ec_adaptert * adapter = NULL; + ec_adaptert * head = NULL; + printf("SOEM (Simple Open EtherCAT Master)\nEEPROM tool\n"); + + if (argc > 4) + { + slave = atoi(argv[2]); + mode = MODE_NONE; + if ((strncmp(argv[3], "-r", sizeof("-r")) == 0)) mode = MODE_READBIN; + if ((strncmp(argv[3], "-ri", sizeof("-ri")) == 0)) mode = MODE_READINTEL; + if ((strncmp(argv[3], "-w", sizeof("-w")) == 0)) mode = MODE_WRITEBIN; + if ((strncmp(argv[3], "-wi", sizeof("-wi")) == 0)) mode = MODE_WRITEINTEL; + + /* start tool */ + eepromtool(argv[1],slave,mode,argv[4]); + } + else + { + printf("Usage: eepromtool ifname slave OPTION fname\n"); + printf("ifname = adapter name\n"); + printf("slave = slave number in EtherCAT order 1..n\n"); + printf(" -r read EEPROM, output binary format\n"); + printf(" -ri read EEPROM, output Intel Hex format\n"); + printf(" -w write EEPROM, input binary format\n"); + printf(" -wi write EEPROM, input Intel Hex format\n"); + /* Print the list */ + printf ("Available adapters\n"); + head = adapter = ec_find_adapters (); + while (adapter != NULL) + { + printf ("Description : %s, Device to use for wpcap: %s\n", adapter->desc,adapter->name); + adapter = adapter->next; + } + ec_free_adapters(adapter); + } + + printf("End program\n"); + + return (0); +} diff --git a/soem-src/test/win32/firm_update/firm_update.c b/soem-src/test/win32/firm_update/firm_update.c new file mode 100644 index 0000000..0cd511e --- /dev/null +++ b/soem-src/test/win32/firm_update/firm_update.c @@ -0,0 +1,159 @@ +/** \file + * \brief Example code for Simple Open EtherCAT master + * + * Usage: firm_update ifname1 slave fname + * ifname is NIC interface, f.e. eth0 + * slave = slave number in EtherCAT order 1..n + * fname = binary file to store in slave + * CAUTION! Using the wrong file can result in a bricked slave! + * + * This is a slave firmware update test. + * + * (c)Arthur Ketels 2011 + */ + +#include +#include +#include +#include +#include + +#include "ethercat.h" + +#define FWBUFSIZE (8 * 1024 * 1024) + +uint8 ob; +uint16 ow; +uint32 data; +char filename[256]; +char filebuffer[FWBUFSIZE]; // 8MB buffer +int filesize; +int j; +uint16 argslave; + +int input_bin(char *fname, int *length) +{ + FILE *fp; + + int cc = 0, c; + + fp = fopen(fname, "rb"); + if(fp == NULL) + return 0; + while (((c = fgetc(fp)) != EOF) && (cc < FWBUFSIZE)) + filebuffer[cc++] = (uint8)c; + *length = cc; + fclose(fp); + return 1; +} + + +void boottest(char *ifname, uint16 slave, char *filename) +{ + printf("Starting firmware update example\n"); + + /* initialise SOEM, bind socket to ifname */ + if (ec_init(ifname)) + { + printf("ec_init on %s succeeded.\n",ifname); + /* find and auto-config slaves */ + + + if ( ec_config_init(FALSE) > 0 ) + { + printf("%d slaves found and configured.\n",ec_slavecount); + + printf("Request init state for slave %d\n", slave); + ec_slave[slave].state = EC_STATE_INIT; + ec_writestate(slave); + + /* wait for slave to reach INIT state */ + ec_statecheck(slave, EC_STATE_INIT, EC_TIMEOUTSTATE * 4); + printf("Slave %d state to INIT.\n", slave); + + /* read BOOT mailbox data, master -> slave */ + data = ec_readeeprom(slave, ECT_SII_BOOTRXMBX, EC_TIMEOUTEEP); + ec_slave[slave].SM[0].StartAddr = (uint16)LO_WORD(data); + ec_slave[slave].SM[0].SMlength = (uint16)HI_WORD(data); + /* store boot write mailbox address */ + ec_slave[slave].mbx_wo = (uint16)LO_WORD(data); + /* store boot write mailbox size */ + ec_slave[slave].mbx_l = (uint16)HI_WORD(data); + + /* read BOOT mailbox data, slave -> master */ + data = ec_readeeprom(slave, ECT_SII_BOOTTXMBX, EC_TIMEOUTEEP); + ec_slave[slave].SM[1].StartAddr = (uint16)LO_WORD(data); + ec_slave[slave].SM[1].SMlength = (uint16)HI_WORD(data); + /* store boot read mailbox address */ + ec_slave[slave].mbx_ro = (uint16)LO_WORD(data); + /* store boot read mailbox size */ + ec_slave[slave].mbx_rl = (uint16)HI_WORD(data); + + printf(" SM0 A:%4.4x L:%4d F:%8.8x\n", ec_slave[slave].SM[0].StartAddr, ec_slave[slave].SM[0].SMlength, + (int)ec_slave[slave].SM[0].SMflags); + printf(" SM1 A:%4.4x L:%4d F:%8.8x\n", ec_slave[slave].SM[1].StartAddr, ec_slave[slave].SM[1].SMlength, + (int)ec_slave[slave].SM[1].SMflags); + /* program SM0 mailbox in for slave */ + ec_FPWR (ec_slave[slave].configadr, ECT_REG_SM0, sizeof(ec_smt), &ec_slave[slave].SM[0], EC_TIMEOUTRET); + /* program SM1 mailbox out for slave */ + ec_FPWR (ec_slave[slave].configadr, ECT_REG_SM1, sizeof(ec_smt), &ec_slave[slave].SM[1], EC_TIMEOUTRET); + + printf("Request BOOT state for slave %d\n", slave); + ec_slave[slave].state = EC_STATE_BOOT; + ec_writestate(slave); + + /* wait for slave to reach BOOT state */ + if (ec_statecheck(slave, EC_STATE_BOOT, EC_TIMEOUTSTATE * 10) == EC_STATE_BOOT) + { + printf("Slave %d state to BOOT.\n", slave); + + if (input_bin(filename, &filesize)) + { + printf("File read OK, %d bytes.\n",filesize); + printf("FoE write...."); + j = ec_FOEwrite(slave, filename, 0, filesize , &filebuffer, EC_TIMEOUTSTATE); + printf("result %d.\n",j); + printf("Request init state for slave %d\n", slave); + ec_slave[slave].state = EC_STATE_INIT; + ec_writestate(slave); + } + else + printf("File not read OK.\n"); + } + + } + else + { + printf("No slaves found!\n"); + } + printf("End firmware update example, close socket\n"); + /* stop SOEM, close socket */ + ec_close(); + } + else + { + printf("No socket connection on %s\nExcecute as root\n",ifname); + } +} + +int main(int argc, char *argv[]) +{ + printf("SOEM (Simple Open EtherCAT Master)\nFirmware update example\n"); + + if (argc > 3) + { + argslave = atoi(argv[2]); + boottest(argv[1], argslave, argv[3]); + } + else + { + printf("Usage: firm_update ifname1 slave fname\n"); + printf("ifname = eth0 for example\n"); + printf("slave = slave number in EtherCAT order 1..n\n"); + printf("fname = binary file to store in slave\n"); + printf("CAUTION! Using the wrong file can result in a bricked slave!\n"); + } + + printf("End program\n"); + return (0); +} diff --git a/soem-src/test/win32/red_test/red_test.c b/soem-src/test/win32/red_test/red_test.c new file mode 100644 index 0000000..0a5d5e4 --- /dev/null +++ b/soem-src/test/win32/red_test/red_test.c @@ -0,0 +1,257 @@ +/** \file + * \brief Example code for Simple Open EtherCAT master + * + * Usage : red_test [ifname1] [ifname2] [cycletime] + * ifname is NIC interface, f.e. eth0 + * cycletime in us, f.e. 500 + * + * This is a redundancy test. + * + * (c)Arthur Ketels 2008 + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "ethercat.h" + +#define NSEC_PER_SEC 1000000000 + +struct sched_param schedp; +char IOmap[4096]; +pthread_t thread1; +struct timeval tv, t1, t2; +int dorun = 0; +int deltat, tmax = 0; +int64 toff; +int DCdiff; +int os; +uint8 ob; +uint16 ob2; +pthread_cond_t cond = PTHREAD_COND_INITIALIZER; +pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER; +uint8 *digout = 0; +int wcounter; + +void redtest(char *ifname, char *ifname2) +{ + int cnt, i, j, oloop, iloop; + + printf("Starting Redundant test\n"); + + /* initialise SOEM, bind socket to ifname */ + if (ec_init_redundant(ifname, ifname2)) + { + printf("ec_init on %s succeeded.\n",ifname); + /* find and auto-config slaves */ + if ( ec_config(FALSE, &IOmap) > 0 ) + { + printf("%d slaves found and configured.\n",ec_slavecount); + /* wait for all slaves to reach SAFE_OP state */ + ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE); + + /* configure DC options for every DC capable slave found in the list */ + ec_configdc(); + + /* read indevidual slave state and store in ec_slave[] */ + ec_readstate(); + for(cnt = 1; cnt <= ec_slavecount ; cnt++) + { + printf("Slave:%d Name:%s Output size:%3dbits Input size:%3dbits State:%2d delay:%d.%d\n", + cnt, ec_slave[cnt].name, ec_slave[cnt].Obits, ec_slave[cnt].Ibits, + ec_slave[cnt].state, (int)ec_slave[cnt].pdelay, ec_slave[cnt].hasdc); + printf(" Out:%8.8x,%4d In:%8.8x,%4d\n", + (int)ec_slave[cnt].outputs, ec_slave[cnt].Obytes, (int)ec_slave[cnt].inputs, ec_slave[cnt].Ibytes); + /* check for EL2004 or EL2008 */ + if( !digout && ((ec_slave[cnt].eep_id == 0x07d43052) || (ec_slave[cnt].eep_id == 0x07d83052))) + { + digout = ec_slave[cnt].outputs; + } + } + printf("Request operational state for all slaves\n"); + ec_slave[0].state = EC_STATE_OPERATIONAL; + /* request OP state for all slaves */ + ec_writestate(0); + /* wait for all slaves to reach OP state */ + ec_statecheck(0, EC_STATE_OPERATIONAL, 5 * EC_TIMEOUTSTATE); + oloop = ec_slave[0].Obytes; + if ((oloop == 0) && (ec_slave[0].Obits > 0)) oloop = 1; + if (oloop > 8) oloop = 8; + iloop = ec_slave[0].Ibytes; + if ((iloop == 0) && (ec_slave[0].Ibits > 0)) iloop = 1; + if (iloop > 8) iloop = 8; + if (ec_slave[0].state == EC_STATE_OPERATIONAL ) + { + printf("Operational state reached for all slaves.\n"); + dorun = 1; + /* acyclic loop 5000 x 20ms = 10s */ + for(i = 1; i <= 5000; i++) + { + printf("Processdata cycle %5d , Wck %3d, DCtime %12lld, O:", dorun, wcounter , ec_DCtime); + for(j = 0 ; j < oloop; j++) + { + printf(" %2.2x", *(ec_slave[0].outputs + j)); + } + printf(" I:"); + for(j = 0 ; j < iloop; j++) + { + printf(" %2.2x", *(ec_slave[0].inputs + j)); + } + printf("\n"); + usleep(20000); + } + dorun = 0; + } + else + { + printf("Not all slaves reached operational state.\n"); + } + printf("Request safe operational state for all slaves\n"); + ec_slave[0].state = EC_STATE_SAFE_OP; + /* request SAFE_OP state for all slaves */ + ec_writestate(0); + } + else + { + printf("No slaves found!\n"); + } + printf("End redundant test, close socket\n"); + /* stop SOEM, close socket */ + ec_close(); + } + else + { + printf("No socket connection on %s\nExcecute as root\n",ifname); + } +} + +/* add ns to timespec */ +void add_timespec(struct timespec *ts, int64 addtime) +{ + int64 sec, nsec; + + nsec = addtime % NSEC_PER_SEC; + sec = (addtime - nsec) / NSEC_PER_SEC; + ts->tv_sec += sec; + ts->tv_nsec += nsec; + if ( ts->tv_nsec >= NSEC_PER_SEC ) + { + nsec = ts->tv_nsec % NSEC_PER_SEC; + ts->tv_sec += (ts->tv_nsec - nsec) / NSEC_PER_SEC; + ts->tv_nsec = nsec; + } +} + +/* PI calculation to get linux time synced to DC time */ +void ec_sync(int64 reftime, int64 cycletime , int64 *offsettime) +{ + static int64 integral = 0; + int64 delta; + /* set linux sync point 50us later than DC sync, just as example */ + delta = (reftime - 50000) % cycletime; + if(delta> (cycletime / 2)) { delta= delta - cycletime; } + if(delta>0){ integral++; } + if(delta<0){ integral--; } + *offsettime = -(delta / 100) - (integral / 20); +} + +/* RT EtherCAT thread */ +void ecatthread( void *ptr ) +{ + struct timespec ts; + struct timeval tp; + int rc; + int ht; + int64 cycletime; + + rc = pthread_mutex_lock(&mutex); + rc = gettimeofday(&tp, NULL); + + /* Convert from timeval to timespec */ + ts.tv_sec = tp.tv_sec; + ht = (tp.tv_usec / 1000) + 1; /* round to nearest ms */ + ts.tv_nsec = ht * 1000000; + cycletime = *(int*)ptr * 1000; /* cycletime in ns */ + toff = 0; + dorun = 0; + while(1) + { + /* calculate next cycle start */ + add_timespec(&ts, cycletime + toff); + /* wait to cycle start */ + rc = pthread_cond_timedwait(&cond, &mutex, &ts); + if (dorun>0) + { + rc = gettimeofday(&tp, NULL); + + ec_send_processdata(); + + wcounter = ec_receive_processdata(EC_TIMEOUTRET); + + dorun++; + /* if we have some digital output, cycle */ + if( digout ) *digout = (uint8) ((dorun / 16) & 0xff); + + if (ec_slave[0].hasdc) + { + /* calulate toff to get linux time and DC synced */ + ec_sync(ec_DCtime, cycletime, &toff); + } + } + } +} + +int main(int argc, char *argv[]) +{ + int iret1; + int ctime; + struct sched_param param; + int policy = SCHED_OTHER; + + printf("SOEM (Simple Open EtherCAT Master)\nRedundancy test\n"); + + memset(&schedp, 0, sizeof(schedp)); + /* do not set priority above 49, otherwise sockets are starved */ + schedp.sched_priority = 30; + sched_setscheduler(0, SCHED_FIFO, &schedp); + + do + { + usleep(1000); + } + while (dorun); + + if (argc > 3) + { + dorun = 1; + ctime = atoi(argv[3]); + /* create RT thread */ + iret1 = pthread_create( &thread1, NULL, (void *) &ecatthread, (void*) &ctime); + memset(¶m, 0, sizeof(param)); + /* give it higher priority */ + param.sched_priority = 40; + iret1 = pthread_setschedparam(thread1, policy, ¶m); + + /* start acyclic part */ + redtest(argv[1],argv[2]); + } + else + { + printf("Usage: red_test ifname1 ifname2 cycletime\nifname = eth0 for example\ncycletime in us\n"); + } + + schedp.sched_priority = 0; + sched_setscheduler(0, SCHED_OTHER, &schedp); + + printf("End program\n"); + + return (0); +} diff --git a/soem-src/test/win32/simple_test/simple_test.c b/soem-src/test/win32/simple_test/simple_test.c new file mode 100644 index 0000000..ae481ff --- /dev/null +++ b/soem-src/test/win32/simple_test/simple_test.c @@ -0,0 +1,375 @@ +/** \file + * \brief Example code for Simple Open EtherCAT master + * + * Usage : simple_test [ifname1] + * ifname is NIC interface, f.e. eth0 + * + * This is a minimal test. + * + * (c)Arthur Ketels 2010 - 2011 + */ + +#include +#include +//#include + +#include "osal.h" +#include "ethercat.h" + +#define EC_TIMEOUTMON 500 + +char IOmap[4096]; +OSAL_THREAD_HANDLE thread1; +int expectedWKC; +boolean needlf; +volatile int wkc; +volatile int rtcnt; +boolean inOP; +uint8 currentgroup = 0; + +/* most basic RT thread for process data, just does IO transfer */ +void CALLBACK RTthread(UINT uTimerID, UINT uMsg, DWORD_PTR dwUser, DWORD_PTR dw1, DWORD_PTR dw2) +{ + IOmap[0]++; + + ec_send_processdata(); + wkc = ec_receive_processdata(EC_TIMEOUTRET); + rtcnt++; + /* do RT control stuff here */ +} + +int EL7031setup(uint16 slave, void *not_used) +{ + int retval; + uint16 u16val; + + // map velocity + uint16 map_1c12[4] = {0x0003, 0x1601, 0x1602, 0x1604}; + uint16 map_1c13[3] = {0x0002, 0x1a01, 0x1a03}; + + retval = 0; + + // Set PDO mapping using Complete Access + // Strange, writing CA works, reading CA doesn't + // This is a protocol error of the slave. + retval += ec_SDOwrite(slave, 0x1c12, 0x00, TRUE, sizeof(map_1c12), &map_1c12, EC_TIMEOUTSAFE); + retval += ec_SDOwrite(slave, 0x1c13, 0x00, TRUE, sizeof(map_1c13), &map_1c13, EC_TIMEOUTSAFE); + + // bug in EL7031 old firmware, CompleteAccess for reading is not supported even if the slave says it is. + ec_slave[slave].CoEdetails &= ~ECT_COEDET_SDOCA; + + // set some motor parameters, just as example + u16val = 1200; // max motor current in mA +// retval += ec_SDOwrite(slave, 0x8010, 0x01, FALSE, sizeof(u16val), &u16val, EC_TIMEOUTSAFE); + u16val = 150; // motor coil resistance in 0.01ohm +// retval += ec_SDOwrite(slave, 0x8010, 0x04, FALSE, sizeof(u16val), &u16val, EC_TIMEOUTSAFE); + + // set other nescessary parameters as needed + // ..... + + while(EcatError) printf("%s", ec_elist2string()); + + printf("EL7031 slave %d set, retval = %d\n", slave, retval); + return 1; +} + +int AEPsetup(uint16 slave, void *not_used) +{ + int retval; + uint8 u8val; + uint16 u16val; + + retval = 0; + + u8val = 0; + retval += ec_SDOwrite(slave, 0x1c12, 0x00, FALSE, sizeof(u8val), &u8val, EC_TIMEOUTRXM); + u16val = 0x1603; + retval += ec_SDOwrite(slave, 0x1c12, 0x01, FALSE, sizeof(u16val), &u16val, EC_TIMEOUTRXM); + u8val = 1; + retval += ec_SDOwrite(slave, 0x1c12, 0x00, FALSE, sizeof(u8val), &u8val, EC_TIMEOUTRXM); + + u8val = 0; + retval += ec_SDOwrite(slave, 0x1c13, 0x00, FALSE, sizeof(u8val), &u8val, EC_TIMEOUTRXM); + u16val = 0x1a03; + retval += ec_SDOwrite(slave, 0x1c13, 0x01, FALSE, sizeof(u16val), &u16val, EC_TIMEOUTRXM); + u8val = 1; + retval += ec_SDOwrite(slave, 0x1c13, 0x00, FALSE, sizeof(u8val), &u8val, EC_TIMEOUTRXM); + + u8val = 8; + retval += ec_SDOwrite(slave, 0x6060, 0x00, FALSE, sizeof(u8val), &u8val, EC_TIMEOUTRXM); + + // set some motor parameters, just as example + u16val = 1200; // max motor current in mA +// retval += ec_SDOwrite(slave, 0x8010, 0x01, FALSE, sizeof(u16val), &u16val, EC_TIMEOUTSAFE); + u16val = 150; // motor coil resistance in 0.01ohm +// retval += ec_SDOwrite(slave, 0x8010, 0x04, FALSE, sizeof(u16val), &u16val, EC_TIMEOUTSAFE); + + // set other nescessary parameters as needed + // ..... + + while(EcatError) printf("%s", ec_elist2string()); + + printf("AEP slave %d set, retval = %d\n", slave, retval); + return 1; +} + +void simpletest(char *ifname) +{ + int i, j, oloop, iloop, wkc_count, chk, slc; + UINT mmResult; + + needlf = FALSE; + inOP = FALSE; + + printf("Starting simple test\n"); + + /* initialise SOEM, bind socket to ifname */ + if (ec_init(ifname)) + { + printf("ec_init on %s succeeded.\n",ifname); + /* find and auto-config slaves */ + + + if ( ec_config_init(FALSE) > 0 ) + { + printf("%d slaves found and configured.\n",ec_slavecount); + + if((ec_slavecount > 1)) + { + for(slc = 1; slc <= ec_slavecount; slc++) + { + // beckhoff EL7031, using ec_slave[].name is not very reliable + if((ec_slave[slc].eep_man == 0x00000002) && (ec_slave[slc].eep_id == 0x1b773052)) + { + printf("Found %s at position %d\n", ec_slave[slc].name, slc); + // link slave specific setup to preop->safeop hook + ec_slave[slc].PO2SOconfig = &EL7031setup; + } + // Copley Controls EAP, using ec_slave[].name is not very reliable + if((ec_slave[slc].eep_man == 0x000000ab) && (ec_slave[slc].eep_id == 0x00000380)) + { + printf("Found %s at position %d\n", ec_slave[slc].name, slc); + // link slave specific setup to preop->safeop hook + ec_slave[slc].PO2SOconfig = &AEPsetup; + } + } + } + + + ec_config_map(&IOmap); + + ec_configdc(); + + printf("Slaves mapped, state to SAFE_OP.\n"); + /* wait for all slaves to reach SAFE_OP state */ + ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE * 4); + + oloop = ec_slave[0].Obytes; + if ((oloop == 0) && (ec_slave[0].Obits > 0)) oloop = 1; + if (oloop > 8) oloop = 8; + iloop = ec_slave[0].Ibytes; + if ((iloop == 0) && (ec_slave[0].Ibits > 0)) iloop = 1; + if (iloop > 8) iloop = 8; + + printf("segments : %d : %d %d %d %d\n",ec_group[0].nsegments ,ec_group[0].IOsegment[0],ec_group[0].IOsegment[1],ec_group[0].IOsegment[2],ec_group[0].IOsegment[3]); + + printf("Request operational state for all slaves\n"); + expectedWKC = (ec_group[0].outputsWKC * 2) + ec_group[0].inputsWKC; + printf("Calculated workcounter %d\n", expectedWKC); + ec_slave[0].state = EC_STATE_OPERATIONAL; + /* send one valid process data to make outputs in slaves happy*/ + ec_send_processdata(); + ec_receive_processdata(EC_TIMEOUTRET); + + /* start RT thread as periodic MM timer */ + mmResult = timeSetEvent(1, 0, RTthread, 0, TIME_PERIODIC); + + /* request OP state for all slaves */ + ec_writestate(0); + chk = 200; + /* wait for all slaves to reach OP state */ + do + { + ec_statecheck(0, EC_STATE_OPERATIONAL, 50000); + } + while (chk-- && (ec_slave[0].state != EC_STATE_OPERATIONAL)); + if (ec_slave[0].state == EC_STATE_OPERATIONAL ) + { + printf("Operational state reached for all slaves.\n"); + wkc_count = 0; + inOP = TRUE; + + + /* cyclic loop, reads data from RT thread */ + for(i = 1; i <= 500; i++) + { + if(wkc >= expectedWKC) + { + printf("Processdata cycle %4d, WKC %d , O:", rtcnt, wkc); + + for(j = 0 ; j < oloop; j++) + { + printf(" %2.2x", *(ec_slave[0].outputs + j)); + } + + printf(" I:"); + for(j = 0 ; j < iloop; j++) + { + printf(" %2.2x", *(ec_slave[0].inputs + j)); + } + printf(" T:%lld\r",ec_DCtime); + needlf = TRUE; + } + osal_usleep(50000); + + } + inOP = FALSE; + } + else + { + printf("Not all slaves reached operational state.\n"); + ec_readstate(); + for(i = 1; i<=ec_slavecount ; i++) + { + if(ec_slave[i].state != EC_STATE_OPERATIONAL) + { + printf("Slave %d State=0x%2.2x StatusCode=0x%4.4x : %s\n", + i, ec_slave[i].state, ec_slave[i].ALstatuscode, ec_ALstatuscode2string(ec_slave[i].ALstatuscode)); + } + } + } + + /* stop RT thread */ + timeKillEvent(mmResult); + + printf("\nRequest init state for all slaves\n"); + ec_slave[0].state = EC_STATE_INIT; + /* request INIT state for all slaves */ + ec_writestate(0); + } + else + { + printf("No slaves found!\n"); + } + printf("End simple test, close socket\n"); + /* stop SOEM, close socket */ + ec_close(); + } + else + { + printf("No socket connection on %s\nExcecute as root\n",ifname); + } +} + +//DWORD WINAPI ecatcheck( LPVOID lpParam ) +OSAL_THREAD_FUNC ecatcheck(void *lpParam) +{ + int slave; + + while(1) + { + if( inOP && ((wkc < expectedWKC) || ec_group[currentgroup].docheckstate)) + { + if (needlf) + { + needlf = FALSE; + printf("\n"); + } + /* one ore more slaves are not responding */ + ec_group[currentgroup].docheckstate = FALSE; + ec_readstate(); + for (slave = 1; slave <= ec_slavecount; slave++) + { + if ((ec_slave[slave].group == currentgroup) && (ec_slave[slave].state != EC_STATE_OPERATIONAL)) + { + ec_group[currentgroup].docheckstate = TRUE; + if (ec_slave[slave].state == (EC_STATE_SAFE_OP + EC_STATE_ERROR)) + { + printf("ERROR : slave %d is in SAFE_OP + ERROR, attempting ack.\n", slave); + ec_slave[slave].state = (EC_STATE_SAFE_OP + EC_STATE_ACK); + ec_writestate(slave); + } + else if(ec_slave[slave].state == EC_STATE_SAFE_OP) + { + printf("WARNING : slave %d is in SAFE_OP, change to OPERATIONAL.\n", slave); + ec_slave[slave].state = EC_STATE_OPERATIONAL; + ec_writestate(slave); + } + else if(ec_slave[slave].state > EC_STATE_NONE) + { + if (ec_reconfig_slave(slave, EC_TIMEOUTMON)) + { + ec_slave[slave].islost = FALSE; + printf("MESSAGE : slave %d reconfigured\n",slave); + } + } + else if(!ec_slave[slave].islost) + { + /* re-check state */ + ec_statecheck(slave, EC_STATE_OPERATIONAL, EC_TIMEOUTRET); + if (ec_slave[slave].state == EC_STATE_NONE) + { + ec_slave[slave].islost = TRUE; + printf("ERROR : slave %d lost\n",slave); + } + } + } + if (ec_slave[slave].islost) + { + if(ec_slave[slave].state == EC_STATE_NONE) + { + if (ec_recover_slave(slave, EC_TIMEOUTMON)) + { + ec_slave[slave].islost = FALSE; + printf("MESSAGE : slave %d recovered\n",slave); + } + } + else + { + ec_slave[slave].islost = FALSE; + printf("MESSAGE : slave %d found\n",slave); + } + } + } + if(!ec_group[currentgroup].docheckstate) + printf("OK : all slaves resumed OPERATIONAL.\n"); + } + osal_usleep(10000); + } + + return 0; +} + +char ifbuf[1024]; + +int main(int argc, char *argv[]) +{ + ec_adaptert * adapter = NULL; + ec_adaptert * head = NULL; + printf("SOEM (Simple Open EtherCAT Master)\nSimple test\n"); + + if (argc > 1) + { + /* create thread to handle slave error handling in OP */ + osal_thread_create(&thread1, 128000, &ecatcheck, NULL); + strcpy(ifbuf, argv[1]); + /* start cyclic part */ + simpletest(ifbuf); + } + else + { + printf("Usage: simple_test ifname1\n"); + /* Print the list */ + printf ("Available adapters\n"); + head = adapter = ec_find_adapters (); + while (adapter != NULL) + { + printf ("Description : %s, Device to use for wpcap: %s\n", adapter->desc,adapter->name); + adapter = adapter->next; + } + ec_free_adapters(adapter); + } + + printf("End program\n"); + return (0); +} diff --git a/soem-src/test/win32/slaveinfo/slaveinfo.c b/soem-src/test/win32/slaveinfo/slaveinfo.c new file mode 100644 index 0000000..057db73 --- /dev/null +++ b/soem-src/test/win32/slaveinfo/slaveinfo.c @@ -0,0 +1,648 @@ +/** \file + * \brief Example code for Simple Open EtherCAT master + * + * Usage : slaveinfo [ifname] [-sdo] [-map] + * Ifname is NIC interface, f.e. eth0. + * Optional -sdo to display CoE object dictionary. + * Optional -map to display slave PDO mapping + * + * This shows the configured slave data. + * + * (c)Arthur Ketels 2010 - 2011 + */ + +#include +#include +#include + +#include "ethercat.h" + +char IOmap[4096]; +ec_ODlistt ODlist; +ec_OElistt OElist; +boolean printSDO = FALSE; +boolean printMAP = FALSE; +char usdo[128]; +char hstr[1024]; + +char* dtype2string(uint16 dtype) +{ + switch(dtype) + { + case ECT_BOOLEAN: + sprintf(hstr, "BOOLEAN"); + break; + case ECT_INTEGER8: + sprintf(hstr, "INTEGER8"); + break; + case ECT_INTEGER16: + sprintf(hstr, "INTEGER16"); + break; + case ECT_INTEGER32: + sprintf(hstr, "INTEGER32"); + break; + case ECT_INTEGER24: + sprintf(hstr, "INTEGER24"); + break; + case ECT_INTEGER64: + sprintf(hstr, "INTEGER64"); + break; + case ECT_UNSIGNED8: + sprintf(hstr, "UNSIGNED8"); + break; + case ECT_UNSIGNED16: + sprintf(hstr, "UNSIGNED16"); + break; + case ECT_UNSIGNED32: + sprintf(hstr, "UNSIGNED32"); + break; + case ECT_UNSIGNED24: + sprintf(hstr, "UNSIGNED24"); + break; + case ECT_UNSIGNED64: + sprintf(hstr, "UNSIGNED64"); + break; + case ECT_REAL32: + sprintf(hstr, "REAL32"); + break; + case ECT_REAL64: + sprintf(hstr, "REAL64"); + break; + case ECT_BIT1: + sprintf(hstr, "BIT1"); + break; + case ECT_BIT2: + sprintf(hstr, "BIT2"); + break; + case ECT_BIT3: + sprintf(hstr, "BIT3"); + break; + case ECT_BIT4: + sprintf(hstr, "BIT4"); + break; + case ECT_BIT5: + sprintf(hstr, "BIT5"); + break; + case ECT_BIT6: + sprintf(hstr, "BIT6"); + break; + case ECT_BIT7: + sprintf(hstr, "BIT7"); + break; + case ECT_BIT8: + sprintf(hstr, "BIT8"); + break; + case ECT_VISIBLE_STRING: + sprintf(hstr, "VISIBLE_STRING"); + break; + case ECT_OCTET_STRING: + sprintf(hstr, "OCTET_STRING"); + break; + default: + sprintf(hstr, "Type 0x%4.4X", dtype); + } + return hstr; +} + +char* SDO2string(uint16 slave, uint16 index, uint8 subidx, uint16 dtype) +{ + int l = sizeof(usdo) - 1, i; + uint8 *u8; + int8 *i8; + uint16 *u16; + int16 *i16; + uint32 *u32; + int32 *i32; + uint64 *u64; + int64 *i64; + float *sr; + double *dr; + char es[32]; + + memset(&usdo, 0, 128); + ec_SDOread(slave, index, subidx, FALSE, &l, &usdo, EC_TIMEOUTRXM); + if (EcatError) + { + return ec_elist2string(); + } + else + { + switch(dtype) + { + case ECT_BOOLEAN: + u8 = (uint8*) &usdo[0]; + if (*u8) sprintf(hstr, "TRUE"); + else sprintf(hstr, "FALSE"); + break; + case ECT_INTEGER8: + i8 = (int8*) &usdo[0]; + sprintf(hstr, "0x%2.2x %d", *i8, *i8); + break; + case ECT_INTEGER16: + i16 = (int16*) &usdo[0]; + sprintf(hstr, "0x%4.4x %d", *i16, *i16); + break; + case ECT_INTEGER32: + case ECT_INTEGER24: + i32 = (int32*) &usdo[0]; + sprintf(hstr, "0x%8.8x %d", *i32, *i32); + break; + case ECT_INTEGER64: + i64 = (int64*) &usdo[0]; + sprintf(hstr, "0x%16.16"PRIx64" %"PRId64, *i64, *i64); + break; + case ECT_UNSIGNED8: + u8 = (uint8*) &usdo[0]; + sprintf(hstr, "0x%2.2x %u", *u8, *u8); + break; + case ECT_UNSIGNED16: + u16 = (uint16*) &usdo[0]; + sprintf(hstr, "0x%4.4x %u", *u16, *u16); + break; + case ECT_UNSIGNED32: + case ECT_UNSIGNED24: + u32 = (uint32*) &usdo[0]; + sprintf(hstr, "0x%8.8x %u", *u32, *u32); + break; + case ECT_UNSIGNED64: + u64 = (uint64*) &usdo[0]; + sprintf(hstr, "0x%16.16"PRIx64" %"PRIu64, *u64, *u64); + break; + case ECT_REAL32: + sr = (float*) &usdo[0]; + sprintf(hstr, "%f", *sr); + break; + case ECT_REAL64: + dr = (double*) &usdo[0]; + sprintf(hstr, "%f", *dr); + break; + case ECT_BIT1: + case ECT_BIT2: + case ECT_BIT3: + case ECT_BIT4: + case ECT_BIT5: + case ECT_BIT6: + case ECT_BIT7: + case ECT_BIT8: + u8 = (uint8*) &usdo[0]; + sprintf(hstr, "0x%x", *u8); + break; + case ECT_VISIBLE_STRING: + strcpy(hstr, usdo); + break; + case ECT_OCTET_STRING: + hstr[0] = 0x00; + for (i = 0 ; i < l ; i++) + { + sprintf(es, "0x%2.2x ", usdo[i]); + strcat( hstr, es); + } + break; + default: + sprintf(hstr, "Unknown type"); + } + return hstr; + } +} + +/** Read PDO assign structure */ +int si_PDOassign(uint16 slave, uint16 PDOassign, int mapoffset, int bitoffset) +{ + uint16 idxloop, nidx, subidxloop, rdat, idx, subidx; + uint8 subcnt; + int wkc, bsize = 0, rdl; + int32 rdat2; + uint8 bitlen, obj_subidx; + uint16 obj_idx; + int abs_offset, abs_bit; + + rdl = sizeof(rdat); rdat = 0; + /* read PDO assign subindex 0 ( = number of PDO's) */ + wkc = ec_SDOread(slave, PDOassign, 0x00, FALSE, &rdl, &rdat, EC_TIMEOUTRXM); + rdat = etohs(rdat); + /* positive result from slave ? */ + if ((wkc > 0) && (rdat > 0)) + { + /* number of available sub indexes */ + nidx = rdat; + bsize = 0; + /* read all PDO's */ + for (idxloop = 1; idxloop <= nidx; idxloop++) + { + rdl = sizeof(rdat); rdat = 0; + /* read PDO assign */ + wkc = ec_SDOread(slave, PDOassign, (uint8)idxloop, FALSE, &rdl, &rdat, EC_TIMEOUTRXM); + /* result is index of PDO */ + idx = etohl(rdat); + if (idx > 0) + { + rdl = sizeof(subcnt); subcnt = 0; + /* read number of subindexes of PDO */ + wkc = ec_SDOread(slave,idx, 0x00, FALSE, &rdl, &subcnt, EC_TIMEOUTRXM); + subidx = subcnt; + /* for each subindex */ + for (subidxloop = 1; subidxloop <= subidx; subidxloop++) + { + rdl = sizeof(rdat2); rdat2 = 0; + /* read SDO that is mapped in PDO */ + wkc = ec_SDOread(slave, idx, (uint8)subidxloop, FALSE, &rdl, &rdat2, EC_TIMEOUTRXM); + rdat2 = etohl(rdat2); + /* extract bitlength of SDO */ + bitlen = LO_BYTE(rdat2); + bsize += bitlen; + obj_idx = (uint16)(rdat2 >> 16); + obj_subidx = (uint8)((rdat2 >> 8) & 0x000000ff); + abs_offset = mapoffset + (bitoffset / 8); + abs_bit = bitoffset % 8; + ODlist.Slave = slave; + ODlist.Index[0] = obj_idx; + OElist.Entries = 0; + wkc = 0; + /* read object entry from dictionary if not a filler (0x0000:0x00) */ + if(obj_idx || obj_subidx) + wkc = ec_readOEsingle(0, obj_subidx, &ODlist, &OElist); + printf(" [0x%4.4X.%1d] 0x%4.4X:0x%2.2X 0x%2.2X", abs_offset, abs_bit, obj_idx, obj_subidx, bitlen); + if((wkc > 0) && OElist.Entries) + { + printf(" %-12s %s\n", dtype2string(OElist.DataType[obj_subidx]), OElist.Name[obj_subidx]); + } + else + printf("\n"); + bitoffset += bitlen; + }; + }; + }; + }; + /* return total found bitlength (PDO) */ + return bsize; +} + +int si_map_sdo(int slave) +{ + int wkc, rdl; + int retVal = 0; + uint8 nSM, iSM, tSM; + int Tsize, outputs_bo, inputs_bo; + uint8 SMt_bug_add; + + printf("PDO mapping according to CoE :\n"); + SMt_bug_add = 0; + outputs_bo = 0; + inputs_bo = 0; + rdl = sizeof(nSM); nSM = 0; + /* read SyncManager Communication Type object count */ + wkc = ec_SDOread(slave, ECT_SDO_SMCOMMTYPE, 0x00, FALSE, &rdl, &nSM, EC_TIMEOUTRXM); + /* positive result from slave ? */ + if ((wkc > 0) && (nSM > 2)) + { + /* make nSM equal to number of defined SM */ + nSM--; + /* limit to maximum number of SM defined, if true the slave can't be configured */ + if (nSM > EC_MAXSM) + nSM = EC_MAXSM; + /* iterate for every SM type defined */ + for (iSM = 2 ; iSM <= nSM ; iSM++) + { + rdl = sizeof(tSM); tSM = 0; + /* read SyncManager Communication Type */ + wkc = ec_SDOread(slave, ECT_SDO_SMCOMMTYPE, iSM + 1, FALSE, &rdl, &tSM, EC_TIMEOUTRXM); + if (wkc > 0) + { + if((iSM == 2) && (tSM == 2)) // SM2 has type 2 == mailbox out, this is a bug in the slave! + { + SMt_bug_add = 1; // try to correct, this works if the types are 0 1 2 3 and should be 1 2 3 4 + printf("Activated SM type workaround, possible incorrect mapping.\n"); + } + if(tSM) + tSM += SMt_bug_add; // only add if SMt > 0 + + if (tSM == 3) // outputs + { + /* read the assign RXPDO */ + printf(" SM%1d outputs\n addr b index: sub bitl data_type name\n", iSM); + Tsize = si_PDOassign(slave, ECT_SDO_PDOASSIGN + iSM, (int)(ec_slave[slave].outputs - (uint8 *)&IOmap[0]), outputs_bo ); + outputs_bo += Tsize; + } + if (tSM == 4) // inputs + { + /* read the assign TXPDO */ + printf(" SM%1d inputs\n addr b index: sub bitl data_type name\n", iSM); + Tsize = si_PDOassign(slave, ECT_SDO_PDOASSIGN + iSM, (int)(ec_slave[slave].inputs - (uint8 *)&IOmap[0]), inputs_bo ); + inputs_bo += Tsize; + } + } + } + } + + /* found some I/O bits ? */ + if ((outputs_bo > 0) || (inputs_bo > 0)) + retVal = 1; + return retVal; +} + +int si_siiPDO(uint16 slave, uint8 t, int mapoffset, int bitoffset) +{ + uint16 a , w, c, e, er, Size; + uint8 eectl; + uint16 obj_idx; + uint8 obj_subidx; + uint8 obj_name; + uint8 obj_datatype; + uint8 bitlen; + int totalsize; + ec_eepromPDOt eepPDO; + ec_eepromPDOt *PDO; + int abs_offset, abs_bit; + char str_name[EC_MAXNAME + 1]; + + eectl = ec_slave[slave].eep_pdi; + Size = 0; + totalsize = 0; + PDO = &eepPDO; + PDO->nPDO = 0; + PDO->Length = 0; + PDO->Index[1] = 0; + for (c = 0 ; c < EC_MAXSM ; c++) PDO->SMbitsize[c] = 0; + if (t > 1) + t = 1; + PDO->Startpos = ec_siifind(slave, ECT_SII_PDO + t); + if (PDO->Startpos > 0) + { + a = PDO->Startpos; + w = ec_siigetbyte(slave, a++); + w += (ec_siigetbyte(slave, a++) << 8); + PDO->Length = w; + c = 1; + /* traverse through all PDOs */ + do + { + PDO->nPDO++; + PDO->Index[PDO->nPDO] = ec_siigetbyte(slave, a++); + PDO->Index[PDO->nPDO] += (ec_siigetbyte(slave, a++) << 8); + PDO->BitSize[PDO->nPDO] = 0; + c++; + /* number of entries in PDO */ + e = ec_siigetbyte(slave, a++); + PDO->SyncM[PDO->nPDO] = ec_siigetbyte(slave, a++); + a++; + obj_name = ec_siigetbyte(slave, a++); + a += 2; + c += 2; + if (PDO->SyncM[PDO->nPDO] < EC_MAXSM) /* active and in range SM? */ + { + str_name[0] = 0; + if(obj_name) + ec_siistring(str_name, slave, obj_name); + if (t) + printf(" SM%1d RXPDO 0x%4.4X %s\n", PDO->SyncM[PDO->nPDO], PDO->Index[PDO->nPDO], str_name); + else + printf(" SM%1d TXPDO 0x%4.4X %s\n", PDO->SyncM[PDO->nPDO], PDO->Index[PDO->nPDO], str_name); + printf(" addr b index: sub bitl data_type name\n"); + /* read all entries defined in PDO */ + for (er = 1; er <= e; er++) + { + c += 4; + obj_idx = ec_siigetbyte(slave, a++); + obj_idx += (ec_siigetbyte(slave, a++) << 8); + obj_subidx = ec_siigetbyte(slave, a++); + obj_name = ec_siigetbyte(slave, a++); + obj_datatype = ec_siigetbyte(slave, a++); + bitlen = ec_siigetbyte(slave, a++); + abs_offset = mapoffset + (bitoffset / 8); + abs_bit = bitoffset % 8; + + PDO->BitSize[PDO->nPDO] += bitlen; + a += 2; + + str_name[0] = 0; + if(obj_name) + ec_siistring(str_name, slave, obj_name); + + printf(" [0x%4.4X.%1d] 0x%4.4X:0x%2.2X 0x%2.2X", abs_offset, abs_bit, obj_idx, obj_subidx, bitlen); + printf(" %-12s %s\n", dtype2string(obj_datatype), str_name); + bitoffset += bitlen; + totalsize += bitlen; + } + PDO->SMbitsize[ PDO->SyncM[PDO->nPDO] ] += PDO->BitSize[PDO->nPDO]; + Size += PDO->BitSize[PDO->nPDO]; + c++; + } + else /* PDO deactivated because SM is 0xff or > EC_MAXSM */ + { + c += 4 * e; + a += 8 * e; + c++; + } + if (PDO->nPDO >= (EC_MAXEEPDO - 1)) c = PDO->Length; /* limit number of PDO entries in buffer */ + } + while (c < PDO->Length); + } + if (eectl) ec_eeprom2pdi(slave); /* if eeprom control was previously pdi then restore */ + return totalsize; +} + + +int si_map_sii(int slave) +{ + int retVal = 0; + int Tsize, outputs_bo, inputs_bo; + + printf("PDO mapping according to SII :\n"); + + outputs_bo = 0; + inputs_bo = 0; + /* read the assign RXPDOs */ + Tsize = si_siiPDO(slave, 1, (int)(ec_slave[slave].outputs - (uint8*)&IOmap), outputs_bo ); + outputs_bo += Tsize; + /* read the assign TXPDOs */ + Tsize = si_siiPDO(slave, 0, (int)(ec_slave[slave].inputs - (uint8*)&IOmap), inputs_bo ); + inputs_bo += Tsize; + /* found some I/O bits ? */ + if ((outputs_bo > 0) || (inputs_bo > 0)) + retVal = 1; + return retVal; +} + +void si_sdo(int cnt) +{ + int i, j; + + ODlist.Entries = 0; + memset(&ODlist, 0, sizeof(ODlist)); + if( ec_readODlist(cnt, &ODlist)) + { + printf(" CoE Object Description found, %d entries.\n",ODlist.Entries); + for( i = 0 ; i < ODlist.Entries ; i++) + { + ec_readODdescription(i, &ODlist); + while(EcatError) printf("%s", ec_elist2string()); + printf(" Index: %4.4x Datatype: %4.4x Objectcode: %2.2x Name: %s\n", + ODlist.Index[i], ODlist.DataType[i], ODlist.ObjectCode[i], ODlist.Name[i]); + memset(&OElist, 0, sizeof(OElist)); + ec_readOE(i, &ODlist, &OElist); + while(EcatError) printf("%s", ec_elist2string()); + for( j = 0 ; j < ODlist.MaxSub[i]+1 ; j++) + { + if ((OElist.DataType[j] > 0) && (OElist.BitLength[j] > 0)) + { + printf(" Sub: %2.2x Datatype: %4.4x Bitlength: %4.4x Obj.access: %4.4x Name: %s\n", + j, OElist.DataType[j], OElist.BitLength[j], OElist.ObjAccess[j], OElist.Name[j]); + if ((OElist.ObjAccess[j] & 0x0007)) + { + printf(" Value :%s\n", SDO2string(cnt, ODlist.Index[i], j, OElist.DataType[j])); + } + } + } + } + } + else + { + while(EcatError) printf("%s", ec_elist2string()); + } +} + +void slaveinfo(char *ifname) +{ + int cnt, i, j, nSM; + uint16 ssigen; + int expectedWKC; + + printf("Starting slaveinfo\n"); + + /* initialise SOEM, bind socket to ifname */ + if (ec_init(ifname)) + { + printf("ec_init on %s succeeded.\n",ifname); + /* find and auto-config slaves */ + if ( ec_config(FALSE, &IOmap) > 0 ) + { + ec_configdc(); + while(EcatError) printf("%s", ec_elist2string()); + printf("%d slaves found and configured.\n",ec_slavecount); + expectedWKC = (ec_group[0].outputsWKC * 2) + ec_group[0].inputsWKC; + printf("Calculated workcounter %d\n", expectedWKC); + /* wait for all slaves to reach SAFE_OP state */ + ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE * 3); + if (ec_slave[0].state != EC_STATE_SAFE_OP ) + { + printf("Not all slaves reached safe operational state.\n"); + ec_readstate(); + for(i = 1; i<=ec_slavecount ; i++) + { + if(ec_slave[i].state != EC_STATE_SAFE_OP) + { + printf("Slave %d State=%2x StatusCode=%4x : %s\n", + i, ec_slave[i].state, ec_slave[i].ALstatuscode, ec_ALstatuscode2string(ec_slave[i].ALstatuscode)); + } + } + } + + + ec_readstate(); + for( cnt = 1 ; cnt <= ec_slavecount ; cnt++) + { + printf("\nSlave:%d\n Name:%s\n Output size: %dbits\n Input size: %dbits\n State: %d\n Delay: %d[ns]\n Has DC: %d\n", + cnt, ec_slave[cnt].name, ec_slave[cnt].Obits, ec_slave[cnt].Ibits, + ec_slave[cnt].state, ec_slave[cnt].pdelay, ec_slave[cnt].hasdc); + if (ec_slave[cnt].hasdc) printf(" DCParentport:%d\n", ec_slave[cnt].parentport); + printf(" Activeports:%d.%d.%d.%d\n", (ec_slave[cnt].activeports & 0x01) > 0 , + (ec_slave[cnt].activeports & 0x02) > 0 , + (ec_slave[cnt].activeports & 0x04) > 0 , + (ec_slave[cnt].activeports & 0x08) > 0 ); + printf(" Configured address: %4.4x\n", ec_slave[cnt].configadr); + printf(" Man: %8.8x ID: %8.8x Rev: %8.8x\n", (int)ec_slave[cnt].eep_man, (int)ec_slave[cnt].eep_id, (int)ec_slave[cnt].eep_rev); + for(nSM = 0 ; nSM < EC_MAXSM ; nSM++) + { + if(ec_slave[cnt].SM[nSM].StartAddr > 0) + printf(" SM%1d A:%4.4x L:%4d F:%8.8x Type:%d\n",nSM, ec_slave[cnt].SM[nSM].StartAddr, ec_slave[cnt].SM[nSM].SMlength, + (int)ec_slave[cnt].SM[nSM].SMflags, ec_slave[cnt].SMtype[nSM]); + } + for(j = 0 ; j < ec_slave[cnt].FMMUunused ; j++) + { + printf(" FMMU%1d Ls:%8.8x Ll:%4d Lsb:%d Leb:%d Ps:%4.4x Psb:%d Ty:%2.2x Act:%2.2x\n", j, + (int)ec_slave[cnt].FMMU[j].LogStart, ec_slave[cnt].FMMU[j].LogLength, ec_slave[cnt].FMMU[j].LogStartbit, + ec_slave[cnt].FMMU[j].LogEndbit, ec_slave[cnt].FMMU[j].PhysStart, ec_slave[cnt].FMMU[j].PhysStartBit, + ec_slave[cnt].FMMU[j].FMMUtype, ec_slave[cnt].FMMU[j].FMMUactive); + } + printf(" FMMUfunc 0:%d 1:%d 2:%d 3:%d\n", + ec_slave[cnt].FMMU0func, ec_slave[cnt].FMMU1func, ec_slave[cnt].FMMU2func, ec_slave[cnt].FMMU3func); + printf(" MBX length wr: %d rd: %d MBX protocols : %2.2x\n", ec_slave[cnt].mbx_l, ec_slave[cnt].mbx_rl, ec_slave[cnt].mbx_proto); + ssigen = ec_siifind(cnt, ECT_SII_GENERAL); + /* SII general section */ + if (ssigen) + { + ec_slave[cnt].CoEdetails = ec_siigetbyte(cnt, ssigen + 0x07); + ec_slave[cnt].FoEdetails = ec_siigetbyte(cnt, ssigen + 0x08); + ec_slave[cnt].EoEdetails = ec_siigetbyte(cnt, ssigen + 0x09); + ec_slave[cnt].SoEdetails = ec_siigetbyte(cnt, ssigen + 0x0a); + if((ec_siigetbyte(cnt, ssigen + 0x0d) & 0x02) > 0) + { + ec_slave[cnt].blockLRW = 1; + ec_slave[0].blockLRW++; + } + ec_slave[cnt].Ebuscurrent = ec_siigetbyte(cnt, ssigen + 0x0e); + ec_slave[cnt].Ebuscurrent += ec_siigetbyte(cnt, ssigen + 0x0f) << 8; + ec_slave[0].Ebuscurrent += ec_slave[cnt].Ebuscurrent; + } + printf(" CoE details: %2.2x FoE details: %2.2x EoE details: %2.2x SoE details: %2.2x\n", + ec_slave[cnt].CoEdetails, ec_slave[cnt].FoEdetails, ec_slave[cnt].EoEdetails, ec_slave[cnt].SoEdetails); + printf(" Ebus current: %d[mA]\n only LRD/LWR:%d\n", + ec_slave[cnt].Ebuscurrent, ec_slave[cnt].blockLRW); + if ((ec_slave[cnt].mbx_proto & ECT_MBXPROT_COE) && printSDO) + si_sdo(cnt); + if(printMAP) + { + if (ec_slave[cnt].mbx_proto & ECT_MBXPROT_COE) + si_map_sdo(cnt); + else + si_map_sii(cnt); + } + } + } + else + { + printf("No slaves found!\n"); + } + printf("End slaveinfo, close socket\n"); + /* stop SOEM, close socket */ + ec_close(); + } + else + { + printf("No socket connection on %s\nExcecute as root\n",ifname); + } +} + +char ifbuf[1024]; + +int main(int argc, char *argv[]) +{ + ec_adaptert * adapter = NULL; + ec_adaptert * head = NULL; + printf("SOEM (Simple Open EtherCAT Master)\nSlaveinfo\n"); + + if (argc > 1) + { + if ((argc > 2) && (strncmp(argv[2], "-sdo", sizeof("-sdo")) == 0)) printSDO = TRUE; + if ((argc > 2) && (strncmp(argv[2], "-map", sizeof("-map")) == 0)) printMAP = TRUE; + /* start slaveinfo */ + strcpy(ifbuf, argv[1]); + slaveinfo(ifbuf); + } + else + { + printf("Usage: slaveinfo ifname [options]\nifname = eth0 for example\nOptions :\n -sdo : print SDO info\n -map : print mapping\n"); + /* Print the list */ + printf ("Available adapters\n"); + head = adapter = ec_find_adapters (); + while (adapter != NULL) + { + printf ("Description : %s, Device to use for wpcap: %s\n", adapter->desc,adapter->name); + adapter = adapter->next; + } + ec_free_adapters(head); + } + + printf("End program\n"); + return (0); +} diff --git a/src/ecat_server.py b/src/ecat_server.py index b0c1f97..9468cb4 100644 --- a/src/ecat_server.py +++ b/src/ecat_server.py @@ -22,10 +22,15 @@ 所有 pysoem 访问经全局锁串行化。日志随每个响应以 log[] 返回。 启动模式: --window 用 pywebview 桌面窗口(ecat_app.vbs); 默认浏览器 tab(ecat_web.vbs/bat)。 + +单实例: 本工具独占 Npcap 适配器, 两个实例并存时后起的那个抓不到从站。 +故每次启动先清掉旧实例(先请求其 /api/shutdown 让从站回 INIT, 再强杀兜底), +见 _kill_stale_instances()。 """ import argparse import collections import ctypes +import gc import io import json import os @@ -752,6 +757,9 @@ RXB_MAX_FRAMES = RXB_BYTES // (FRM_HDR_SIZE + CAN_FD_DATA_MAX) # 3 (全 DLC64) MOTOR_MAX_NODES = 9 MOTOR_CHANNELS = 3 MOTOR_SLOT_COUNT = MOTOR_MAX_NODES * MOTOR_CHANNELS # 27 +# 扫完全表需要的窗口数(27 槽 / 7 槽每窗 = 4)。仅用于开机首圈全表扫描与发现 +# 扫描的节奏；**稳态下单槽刷新率不再除以它** —— 窗口会收缩到只盖有主的槽， +# 见 _cover_bases。前端要显示"数据多新"应读快照里的 slot_period_ms。 MOTOR_SCAN_CYCLES = -(-MOTOR_SLOT_COUNT // MOT_SLOTS_PER_PDO) # 4 # 发送序号:每调一次 /api/can/send 自增。固件按"变化沿"触发发送,所以 @@ -1062,9 +1070,477 @@ _motor_cmd_q = collections.deque() # 待下发的 CAN 帧队列(线程安全) # maxlen 防前端长时间不取时内存无限增长 —— 满了丢最旧的，快照里 lost/pending # 已经能告诉前端"有没有漏"。 _motor_rx_log = collections.deque(maxlen=2000) -_motor_period_ms = 4 # 后台周期，默认 4ms(250Hz) —— 远快于 1s 看门狗 +# 最近若干个 PDO 周期的完整时间线 (t0, send_start, recv_start, recv_end, +# loop_end, grid_skips)，单位 perf_counter。t0=循环顶(取锁前)，loop_end=算完 +# 下一拍截止点的瞬间 —— 这两点之间才是"这一拍真正占用的时间"。 +# 为什么要拆到这一层：cyc_us 只量 send/receive 一段，而 250us/200us 档 96~97% +# 的达成率**不随档位变化** —— 说明丢的拍不是"总线差几十 us"这种比例性的事， +# 更像有一类固定频率的事件(GC 暂停/HTTP 线程抢 GIL/系统杂活)一拍吃掉一整个 +# 时隙。没被量到的区间(锁等待、窗口解码+快照构建、每 8 拍的分位数排序、自旋 +# 结束后重拿 GIL)全在 loop_end-t0 与 recv_end-t0 的差里。原始数据由 +# /api/motor/dbg 导出，和 GC/HTTP 事件时间线做交集归因；热循环只多一次元组重建。 +_cyc_t = collections.deque(maxlen=2048) +_skip_total = 0 # 本周期设置以来网格跳过的拍数合计(仅 PDO 线程写) + +# GC .stop-the-world 事件 (结束时刻, 时长us, 代数, 触发线程名)。gen2 收集会停 +# 所有线程几 ms，是"某一拍凭空超时"的头号嫌疑；记下来才能和时间线对质。 +_gc_events = collections.deque(maxlen=1024) +_gc_start = {} +# 增量计数器 [c0,c1,c2, max0,max1,max2, pdo线程触发数]。_cyc_stats 每次拍快照 +# 都拷一份进 cyc_us.gc —— 必须是 O(1)，绝不在热循环里扫 deque。 +_gc_stat = [0, 0, 0, 0.0, 0.0, 0.0, 0] + + +def _gc_cb(event, info=None): + """gc.callbacks 回调，记录每次分代收集的起止。CPython 3.12 的调用形状是 + callback(event: str, info: dict) —— event 为 'start'/'stop'，info 带 + generation/collected；参数顺序写反会让 .get 抛 AttributeError 被自己的 + try 吞掉，得到"GC 从没发生过"的假象(2026-09-04 实踩)。回调在"触发收集的 + 那个线程"上跑，所以 PDO 线程自己分配触发 gen0 时线程名就是 pdo-cycle。""" + try: + gen = (info or {}).get("generation") + if gen is None or gen < 0 or gen > 2: + return + if event == "start": + _gc_start[gen] = time.perf_counter() + elif event == "stop": + t = _gc_start.pop(gen, None) + dur = (time.perf_counter() - t) * 1e6 if t else 0.0 + name = threading.current_thread().name + _gc_events.append((time.perf_counter(), round(dur, 1), gen, name)) + _gc_stat[gen] += 1 + if dur > _gc_stat[3 + gen]: + _gc_stat[3 + gen] = round(dur, 1) + if name == "pdo-cycle": + _gc_stat[6] += 1 + except Exception: + pass # 观测器绝不允许把被观测的系统带崩 + + +gc.callbacks.append(_gc_cb) + +# HTTP 请求进/出时间线 (t_in, t_out, path)。ThreadingHTTPServer 每个请求一个 +# 线程，序列化快照的那几百 us 都持着 GIL —— PDO 线程自旋结束时要在 GIL 门口 +# 排队，这就是"自旋明明是准的，拍还是迟了"的另一号嫌疑。 +_http_t = collections.deque(maxlen=2048) +# ── cyc_us 分位数**绝不在 PDO 循环里算**(2026-09-04 实测定罪) ── +# 旧版每 8 拍调一次 _cyc_stats：7 段×400 样本的排序要 400~600us，比一整个 +# 250us 时隙还大 —— 时间线按拍号 mod 8 严丝合缝地超时(真机 194/250 个超时拍 +# 恰好间隔 8)，达成率被自家观测代码从 97% 打到 86.6%。分位数只有 HTTP 读者 +# 要看，就让他们(在请求线程里)按需算，带 TTL 去重；没人看时热循环零成本。 +_CYC_TTL_S = 0.25 +_cyc_cached = None # 最近一次算好的分位数(任何线程可写，GIL 下原子) +_cyc_cached_t = 0.0 +_motor_period_ms = 4.0 # 后台周期，默认 4ms(250Hz) —— 远快于 1s 看门狗 _motor_win = 0 # 当前窗口起始槽号，由本线程独占推进 +# PDO 周期的允许范围。下限 62.5us(16kHz) 只是**允许请求**的最小值；实际跑不跑 +# 得到由物理决定：真机一送一回 ≈165us，比这更快会自然自限(周期跑超目标值， +# 循环本身不会坏)。留这么低是为了极限测速时能直接报目标档而不用改代码。 +MOTOR_PERIOD_MIN_MS = 0.0625 +MOTOR_PERIOD_MAX_MS = 1000.0 + + +def _fmt_period(ms): + """周期的显示写法：<1ms 用 µs，与前端状态栏保持一致。""" + return f"{ms * 1000:g}us" if ms < 1 else f"{ms:g}ms" + + +def _parse_period_ms(req): + """从请求里取 period_ms(float，允许亚毫秒) 或 period_us(int)，钳到允许范围。 + + 亚毫秒档位(0.25ms=4kHz 等)只在自旋节拍下才有意义：内核睡眠在 Windows 上 + 粒度 ~0.5ms，根本睡不出 250us。周期与自旋的组合由 _spin_effective 判断， + 这里只管解析，不掺节拍策略。 + """ + v = req.get("period_us") + if v is None: + v = req.get("period_ms") + if v is None: + return None + ms = float(v) + else: + ms = float(v) / 1000.0 + if ms != ms: # NaN 请求直接当没给 + return None + if ms <= 0: # 0/负数一律视为"没给"，别被钳成最小周期 + return None + return max(MOTOR_PERIOD_MIN_MS, min(MOTOR_PERIOD_MAX_MS, ms)) + +# ── 窗口自适应：别把 EtherCAT 当以太网用 ── +# +# 旧实现每周期把窗口 +7 无脑轮扫 27 个槽(4 周期一圈)，于是**单个电机**的状态 +# 只有 周期率/4 的更新速度 —— 实测 4ms 周期下总线跑 225Hz，而每个槽只有 56Hz。 +# 而 27 个槽里通常只有 2 个有主(9 节点×3 通道是**容量**，不是实际接入数)， +# 也就是说 4 个窗口里 3 个整窗都在传空槽，280B/周期只有 80B 有效。 +# +# 关键事实(读固件 ethercat_can_bridge.cpp:248 得到，不是猜的)：从站的 win_base +# 是**任意槽号**，它只是从 base 开始逐槽填到越界即停，并不要求 base 是 7 的倍数。 +# 所以只要让窗口盖住"有主的那几个槽"，一个窗口就够 —— 每周期都刷全部在线电机， +# 单槽刷新率直接等于 PDO 周期率，快 4 倍且一行总线开销都没多加。 +# +# 仍需周期性全表扫描：新插上的电机若落在窗口外，不扫就永远发现不了。 +# 每 MOTOR_DISCOVERY_EVERY 周期插一个发现窗口，4ms 周期下全表 ≈1s 覆盖一轮, +# 小于前端 STALE_OFFLINE_MS(2s)，不会把新电机拖到误判离线。 +MOTOR_DISCOVERY_EVERY = 64 +# 窗口能落的最大起点：再往后窗口尾部就越界了，白扔覆盖能力。 +MOTOR_WIN_BASE_MAX = max(0, MOTOR_SLOT_COUNT - MOT_SLOTS_PER_PDO) +# 发现扫描要走的窗口起点序列。必须用它而不是"每次 +7 到越界就归零"： +# 27 槽 / 7 槽一窗时 0,7,14 之后是 21，而 21 > MOTOR_WIN_BASE_MAX(20)， +# 朴素写法会在这里直接绕回 0，于是 slot 21..26 永远扫不到 —— 挂在 node8/node9 +# 上的电机热插后再也发现不了。末窗钳到 20 才能盖住尾部。 +MOTOR_DISCOVERY_BASES = sorted(set( + [min(b, MOTOR_WIN_BASE_MAX) + for b in range(0, MOTOR_SLOT_COUNT, MOT_SLOTS_PER_PDO)])) + +# ── 自旋调度：短周期下唯一能跑准的办法 ── +# +# 分段实测(真机, period_ms=1)把一次周期的开销拆开看： +# send_processdata 调用 ≈ 48 us +# receive_processdata 等帧 ≈ 118 us ← EtherCAT 真正的总线时间 +# 解码 + 建快照 ≈ 5 us +# 合计 ≈ 165 us → 理论上能跑 ≈6000 Hz +# 可实测周期是 1360 us。差的 1.2ms **全在"睡到下一拍"这一步**： +# Windows 的内核定时器粒度太粗，Event.wait(1ms) 实测 p50 1.075 / p90 2.003ms， +# 稳定超调 0.4~1.0ms。而且没法"先睡后自旋"补救 —— 睡过头时截止点已经过了， +# 自旋无事可做(实测 hybrid 与裸 wait 完全一样)。 +# +# 唯一能跑准的就是自旋到绝对截止点：实测 p50 1.000 / p90 1.001ms，抖动 0.000ms。 +# 代价是**吃满一个核**(≈99% CPU)，所以默认只在短周期自动开启，且必须能让用户关。 +MOTOR_SPIN_AUTO_MAX_MS = 2 # 周期 ≤ 此值时自动用自旋(内核睡眠在这个档根本睡不准) +_motor_spin = None # None=按周期自动；True/False=用户显式指定 + + +# ── 自旋线程钉到 P 核（快核组）── +# +# 自旋必然 100% 占住一个逻辑处理器。这条线程是全进程最latency敏感的执行体 +# (PDO 节拍)，钉到 P 核让它跑在单核性能最高、解释器段最短的核上；E 核留给 +# 系统后台杂活和 HTTP 线程的溢出。2026-09-05 按用户要求从"钉 E 核"改为"钉 P 核"。 +# +# 为什么用"实测吞吐"而不是查 API：这台机器上 +# GetLogicalProcessorInformationEx → 直接 ERROR_BAD_LENGTH(31)，连 size 查询都失败 +# GetSystemCpuSetInformation → 能返回但结构布局对不上(32 组/896 字节， +# 元素大小不整除)，EfficiencyClass 拿不到 +# GetDynamicProcessorCounts → 该构建里根本没导出 +# 处理器组 → 只有 1 个组，组号区分不出 P/E +# 所以退而求其次：把线程逐个钉到每个逻辑处理器上跑同一段纯计算，按耗时聚类。 +# 实测本机 P 核 11.9~16.7ms、E 核 24.7~29.7ms，中间有 8ms 的空档，分得很开。 +# 用"最大间隙"而不是固定倍数(如 0.75×最慢)分类 —— 后者会把被 SMT 兄弟拖累的 +# P 核(实测 16.7ms)误判成 E 核。 +# +# 关掉/覆盖：ECAT_PIN_PCORES=0 完全不钉；ECAT_PCORE_MASK=0xFFFF 手工指定。 +_PCORE_PROBE_ITERS = 300000 +_PCORE_PROBE_REPS = 3 +g_pcore_mask = 0 # 探测结果缓存(0 = 未探测/不可用) +g_pcore_probe_done = threading.Event() + + +def _start_pcore_probe(): + """服务启动时后台探测 P 核，赶在任何总线操作之前跑完。 + + 必须在启动阶段做而不是等 PDO 线程自己探：探测期间那个线程不喂过程数据， + 从站的 1000ms 看门狗会把它踢出 OP，重进时又可能撞上 SSC 邮箱楔死。 + """ + if os.name != "nt" or not _pcore_pin_enabled(): + g_pcore_probe_done.set() + return + env = _pcore_mask_from_env() + if env: + g_pcore_mask = env + g_pcore_probe_done.set() + return + + def _run(): + global g_pcore_mask + try: + g_pcore_mask = _detect_pcore_mask() + except Exception as e: # noqa: BLE001 - 探测失败只是不钉核，不能影响服务 + print(f"[PCORE] 探测失败，自旋线程不钉核: {type(e).__name__}: {e}", flush=True) + finally: + g_pcore_probe_done.set() + threading.Thread(target=_run, name="pcore-probe", daemon=True).start() + + +def _pcore_pin_enabled(): + """ECAT_PIN_PCORES=0 可完全关掉钉核(比如用户已经用别的工具管过亲和性)。""" + return os.environ.get("ECAT_PIN_PCORES", "1") != "0" + + +def _pcore_mask_from_env(): + """ECAT_PCORE_MASK=0xFFFF 手工指定，跳过实测(实测偶尔会被后台负载干扰)。""" + v = os.environ.get("ECAT_PCORE_MASK", "").strip() + if not v: + return 0 + try: + return int(v, 0) & 0xFFFFFFFFFFFFFFFF + except ValueError: + return 0 + + +def _probe_core_speed(lp, iters=_PCORE_PROBE_ITERS, reps=_PCORE_PROBE_REPS): + """把当前线程钉到 lp 上跑 reps 次纯计算，返回最快一次的墙钟毫秒。""" + box = [] + + def run(): + _set_thread_affinity(lp) + x = 0 + for i in range(2000): # 预热，顺带让调度器落位 + x += i + for _ in range(reps): + t0 = time.perf_counter() + y = 0 + for i in range(iters): + y += i * i + box.append((time.perf_counter() - t0) * 1000.0) + th = threading.Thread(target=run) + th.start() + th.join(timeout=10.0) + return min(box) if box else None + + +def _classify_fast(speeds): + """从 [(逻辑处理器号, 耗时ms)] 里挑出快核(P 核)，返回位掩码；分不开返回 0。 + + 用"最大间隙"而不是固定倍数(如 0.75×最慢)：本机实测 P 核 11.9~16.7ms、 + E 核 24.7~29.7ms，但被 SMT 兄弟拖累的 P 核也能到 16.7ms —— 固定倍数会把 + 它们误判成 E 核，间隙法不会。 + 额外要求间隙足够大(≥最快核的 25%)且两组各至少 2 个核，否则视为"这台机器 + 分不出两类核"(纯对称 CPU 或噪声太大)，宁可不钉也别钉错。 + """ + if len(speeds) < 4: + return 0 + vals = sorted(s for _, s in speeds) + best_gap, best_cut = 0.0, None + for i in range(1, len(vals)): + gap = vals[i] - vals[i - 1] + if gap > best_gap: + best_gap, best_cut = gap, vals[i] + if best_cut is None or best_gap < vals[0] * 0.25: + return 0 + fast = [lp for lp, s in speeds if s < best_cut] + slow = [lp for lp, s in speeds if s >= best_cut] + if len(fast) < 2 or len(slow) < 2: + return 0 + mask = 0 + for lp in fast: + mask |= 1 << lp + return mask + + +def _detect_pcore_mask(): + """实测找出快核(P 核)集合，返回亲和性位掩码；拿不到结果返回 0。""" + n = os.cpu_count() or 0 + if n < 4: + return 0 + speeds = [] + for lp in range(n): + s = _probe_core_speed(lp) + if s is None: + return 0 + speeds.append((lp, s)) + return _classify_fast(speeds) + + +class _GROUP_AFFINITY(ctypes.Structure): + _fields_ = [("Mask", ctypes.c_ulonglong), ("Group", ctypes.c_ushort), + ("Reserved", ctypes.c_ushort * 3)] + + +def _get_thread_affinity_mask(): + """当前线程的亲和性掩码(自测用来确认钉核只影响本线程)。 + + 不能用 SetThreadAffinityMask(0) 来"只查不改" —— 掩码必须非零，那样只会失败。 + """ + if os.name != "nt": + return 0 + try: + k32 = ctypes.windll.kernel32 + ga = _GROUP_AFFINITY() + k32.GetThreadGroupAffinity.restype = ctypes.c_int + k32.GetThreadGroupAffinity.argtypes = [ctypes.c_void_p, + ctypes.POINTER(_GROUP_AFFINITY)] + if not k32.GetThreadGroupAffinity(k32.GetCurrentThread(), ctypes.byref(ga)): + return 0 + return ga.Mask or 0 + except Exception: + return 0 + + +def _set_thread_affinity(lp): + return _set_thread_affinity_mask(1 << lp) + + +def _set_thread_affinity_mask(mask): + if os.name != "nt": + return 0 + try: + k32 = ctypes.windll.kernel32 + h = k32.GetCurrentThread() + k32.SetThreadAffinityMask.restype = ctypes.c_ulonglong + k32.SetThreadAffinityMask.argtypes = [ctypes.c_void_p, ctypes.c_ulonglong] + return k32.SetThreadAffinityMask(h, mask) or 0 + except Exception: + return 0 + + +# ── 自旋必须放掉 GIL，否则会把整个服务饿死 ── +# +# 第一版自旋写成 `while time.perf_counter() < deadline: pass`，真机实测 +# HTTP 读快照的 p50 从 0.6ms 直接劣化到 **40ms** —— 页面等于废了。 +# 原因：CPython 的 GIL 切换间隔默认 5ms，而纯 Python 循环里 perf_counter() +# 是带 GIL 调用的，自旋线程可以连拿好几个 5ms 不放，其它线程(每个 HTTP 请求 +# 一个线程)全排在后面。单独复现：自旋线程存在时，别的线程 sleep(0.1ms) +# 实测 p50 5.70ms；换成下面这种放 GIL 的自旋后回到 0.54ms(即基线)。 +# +# 做法是用 ctypes 调 QueryPerformanceCounter —— ctypes 外部调用默认**释放 GIL**， +# 所以自旋期间其它线程能自由跑。精度 0.1us(QPC 频率 10MHz)，绰绰有余。 +class _QpcTS(ctypes.Structure): + _fields_ = [("lo", ctypes.c_ulong), ("hi", ctypes.c_long)] + + +if os.name == "nt": + _qpc = ctypes.windll.kernel32.QueryPerformanceCounter + _qpc.restype = ctypes.c_int + _qpc.argtypes = [ctypes.POINTER(_QpcTS)] + _QPC_HZ = 1e7 # QPC 固定 10MHz + + def _spin_until(deadline_s): + """自旋到 time.perf_counter() 的绝对时刻，全程不持 GIL。""" + target = int(deadline_s * _QPC_HZ) + t = _QpcTS() + while True: + _qpc(ctypes.byref(t)) + if (t.hi * 4294967296 + t.lo) >= target: + return +else: # 非 Windows 兜底：精度差但不致命 + def _spin_until(deadline_s): + while time.perf_counter() < deadline_s: + time.sleep(0) + + +def _cover_bases(slots): + """求覆盖 slots 所需的最少窗口起点集合(贪心，窗口长 MOT_SLOTS_PER_PDO)。 + + slots 为空(还没发现任何电机)时回退成全表轮扫，否则一个槽都读不到。 + """ + if not slots: + return list(range(0, MOTOR_SLOT_COUNT, MOT_SLOTS_PER_PDO)) + bases = [] + todo = sorted(slots) + i = 0 + while i < len(todo): + # 起点尽量靠后(但不超过 todo[i])，让窗口多盖住后面的槽 + base = min(todo[i], MOTOR_WIN_BASE_MAX) + bases.append(base) + end = base + MOT_SLOTS_PER_PDO # 覆盖 [base, end) + while i < len(todo) and todo[i] < end: + i += 1 + return bases + + +def _cyc_stats(n=400): + """把最近 n 个周期的完整时间线压成分位数(微秒)。样本不足返回 None。 + + 字段口径(单位 us): + send/rx/total 取锁后→发完→收完(旧字段，前端在用，口径不能动) + pre 取锁本身(等 g_lock) + 装帧。周期越小它越显眼 —— HTTP 的 + 独占路由持锁做总线操作时会直接顶在这里。 + tail 收完→循环尾：窗口解码、快照 dict、每 8 拍的分位数排序。 + cyc_us 时代完全没量这一段，它是"看不见的税"。 + work t0→loop_end，这一拍真正占用的总时间。work>周期 才会跳拍。 + iv 相邻两拍 t0 的间隔 = 实际拍间期，均值倒数就是实测 Hz。 + late/lost 窗口里超时拍数与网格损失拍数。 + gc 分代收集计数(全周期累计，来自 _gc_stat 增量器，O(1))。 + """ + s = list(_cyc_t)[-n:] + if len(s) < 20: + return None + send = sorted((b - a) * 1e6 for _, a, b, _, _, _ in s) + rx = sorted((c - b) * 1e6 for _, _, b, c, _, _ in s) + tot = sorted((c - a) * 1e6 for _, a, _, c, _, _ in s) + pre = sorted((a - t) * 1e6 for t, a, _, _, _, _ in s) + tail = sorted((e - c) * 1e6 for _, _, _, c, e, _ in s) + work = sorted((e - t) * 1e6 for t, _, _, _, e, _ in s) + iv = sorted((s[i][0] - s[i - 1][0]) * 1e6 for i in range(1, len(s))) + + def pct(v, p): + return round(v[min(len(v) - 1, int(len(v) * p))], 1) + + def sec(v): + return {"p50": pct(v, .5), "p90": pct(v, .9), "max": round(v[-1], 1)} + return {"n": len(s), + "send": sec(send), "rx": sec(rx), "total": sec(tot), + "pre": sec(pre), "tail": sec(tail), "work": sec(work), "iv": sec(iv), + "late": sum(1 for x in s if x[5] > 0), + "lost": sum(x[5] for x in s), + "gc": {"n": list(_gc_stat[:3]), "max_us": list(_gc_stat[3:6]), + "pdo": _gc_stat[6]}} + + +def _cyc_fresh(max_age=_CYC_TTL_S): + """读侧按需取分位数：距上次计算超过 max_age 秒才重算，否则吃缓存。 + 计算发生在**调用方线程**(HTTP)，PDO 线程绝不碰排序。见 _CYC_TTL_S 注释。""" + global _cyc_cached, _cyc_cached_t + now = time.perf_counter() + if _cyc_cached is None or now - _cyc_cached_t > max_age: + _cyc_cached = _cyc_stats() + _cyc_cached_t = now + return _cyc_cached + + +def _spin_effective(): + """本周期到底会不会自旋(供日志/快照/前端显示，别让用户猜 CPU 去哪了)。""" + if _motor_spin is not None: + return _motor_spin + return _motor_period_ms <= MOTOR_SPIN_AUTO_MAX_MS + + +if os.name == "nt": + _SET_TPR = ctypes.windll.kernel32.SetThreadPriority + _SET_TPR.argtypes = [ctypes.c_void_p, ctypes.c_int] + _GET_CUR_TH = ctypes.windll.kernel32.GetCurrentThread + _GET_CUR_TH.restype = ctypes.c_void_p # 伪句柄 -2，必须按指针宽度传，x64 上不能当 int + + def _set_spin_priority(spin: bool): + """自旋拍给线程挂 TIME_CRITICAL，普通拍回落 NORMAL。 + + 250us 档实测卡在 ~89% 达成率：截止点自旋本身是准的，但线程按普通优先级 + 时零星被系统杂活抢占，每次迟到几十 us 就从周期预算里真扣掉几十 us。 + 钉核只是选了一群核，没规定这群核上谁先跑。钉到 P 核组后这条理由依然 + 成立：P 组里还有 HTTP 线程和解释器主线程，TIME_CRITICAL 保证自旋线程 + 优先占住自己那个 LP，HTTP 溢出到组内其余核(本机 16 个 P 线程，远多于 + 并发 HTTP 线程数)；不自旋时回落 NORMAL，别白白压着内核睡眠路径的响应性。 + """ + try: + _SET_TPR(_GET_CUR_TH(), 15 if spin else 0) # 15=TIME_CRITICAL, 0=NORMAL + except Exception: # 优先级只是锦上添花 + pass +else: + def _set_spin_priority(spin: bool): + pass + + +def _motor_wait_until(deadline_s): + """等 perf_counter 时间轴上的**绝对**截止点，可被 _motor_stop 立刻打断。 + + 自旋：精度到 us(实测抖动 0.000ms)，代价是吃满一个核。 + 内核睡眠：省 CPU，但 Windows 粒度会让它稳定超调 0.4~1.0ms。 + 已过点直接返回(调用方靠重排网格决定是否追拍，见 _pdo_worker)。 + """ + now = time.perf_counter() + if deadline_s <= now: + _motor_stop.is_set() + return + if _spin_effective(): + # 自旋必须走 _spin_until(内部放 GIL)。写成纯 Python 循环会把 HTTP + # 线程饿死 —— 真机实测读快照 p50 从 0.6ms 劣化到 40ms。 + # 代价是自旋期间不响应 _motor_stop，但单次自旋 ≤ 一个周期(≤1s)， + # 停线程最坏多等一拍，可接受。 + _spin_until(deadline_s) + _motor_stop.is_set() + else: + _motor_stop.wait(deadline_s - now) + def _motor_snapshot(): return _motor_snap @@ -1072,10 +1548,27 @@ def _motor_snapshot(): def _pdo_worker(): """后台 PDO 循环：持续喂看门狗、滚动扫描电机状态窗口、下发排队的 CAN 帧。""" - global _motor_snap, _motor_win, g_op_active + global _motor_snap, _motor_win, g_op_active, _skip_total + # 自旋会 100% 占住一个逻辑处理器，钉到 P 核组让节拍跑在单核性能最高的核上。 + # **只读缓存，绝不在这儿现场探测**：探测要跑 1.6s，而这段时间本线程不喂 + # 过程数据，从站 SM2 的 1000ms 看门狗会直接把它踢出 OP —— 正是把板子弄楔死 + # 的那条路径。探测由 _start_pcore_probe() 在服务启动时(还没碰总线)后台做完。 + if _pcore_pin_enabled() and g_pcore_probe_done.is_set() and g_pcore_mask: + _set_thread_affinity_mask(g_pcore_mask) slots = {} # 槽号 -> 槽快照(跨周期累积，窗口只覆盖自己那部分) head = {} errors = 0 + # 窗口自适应状态(见 _cover_bases 上方注释)。 + bases = _cover_bases(()) # 还没发现电机 → 先全表轮扫 + bi = 0 # 当前用 bases 里的第几个窗口 + discovery = 0 # 距下次插入"发现窗口"还差几周期 + disco_base = 0 # 发现扫描游标: MOTOR_DISCOVERY_BASES 的下标 + # 启动后先老老实实扫完一整圈再切自适应。否则第一个窗口刚看见 1 号电机就把 + # bases 收缩到那一个窗口，后面的槽再也扫不到 —— 自测第 2 项(要求聚合齐 + # 全部 27 槽)正是这么炸的。一圈 = len(bases) 个周期，4ms 下仅 16ms。 + sweep_left = len(bases) + rt_state = None # 线程优先级是否已按自旋状态设置 + next_t = None # 下一拍的绝对截止点(perf_counter 时间轴) while not _motor_stop.is_set(): t0 = time.perf_counter() try: @@ -1102,19 +1595,35 @@ def _pdo_worker(): sent = _fill_can_out(ch, can_id, dlc, data, batch=batch or None) nsent = 1 + len(batch) - # 推进窗口：每周期换一段，MOTOR_SCAN_CYCLES 个周期扫完全表 + # 告诉从站本周期要哪一段槽。窗口起点由循环末尾自适应推进， + # 从站接受任意 base(不必是 7 的倍数)，见 _cover_bases 注释。 _can_out[TX_OFF_WIN] = _motor_win & 0xFF out_len = len(bytes(s.output)) if out_len > 0: s.output = bytes(_can_out[:out_len]).ljust(out_len, b"\x00") + # 分段计时: 定位"周期为什么压不到 1ms"到底花在哪一步。 + # 总线上一帧 1024B @100Mbit 只要约 82us, 实测周期却是 ~1360us, + # 差一个数量级 —— 说明瓶颈在驱动/协议栈, 不在 EtherCAT。 + _ts = time.perf_counter() g_master.send_processdata() + _tm = time.perf_counter() wkc = g_master.receive_processdata(2000) + _tr = time.perf_counter() + _cyc_t.append((t0, _ts, _tm, _tr, _tr, 0)) inp = bytes(s.input) expected = g_master.expected_wkc win_slots, win_head = _decode_motor_window(inp) + # 出现没见过的槽就**立刻**重算覆盖，不等下一次发现扫描。 + # 否则从启动(或热插)到下一个发现边界之间最多 MOTOR_DISCOVERY_EVERY + # 个周期都在跑全表轮扫，这段时间里单槽刷新率仍是慢的 —— 自测里 + # 表现为命中率只有 80% 而不是 ~100%。 + fresh = [k for k in win_slots if k not in slots] slots.update(win_slots) + if fresh and sweep_left <= 0: + bases = _cover_bases(slots.keys()) + bi = 0 if win_head: head = win_head rxb = _decode_rx_batch(inp) @@ -1122,12 +1631,27 @@ def _pdo_worker(): # 用 deque(maxlen) 而不是无界 list：前端若长时间不取，内存不能无限涨。 if rxb["frames"]: _motor_rx_log.extend(rxb["frames"]) + cyc = (_motor_snap or {}).get("cycles", 0) + 1 _motor_snap = { "ts": time.time(), "wkc": wkc, "expected_wkc": expected, - "cycles": (_motor_snap or {}).get("cycles", 0) + 1, + "cycles": cyc, "period_ms": _motor_period_ms, + # 单个电机状态的真实刷新周期。窗口自适应后通常 = period_ms + # (一个窗口盖住全部在线槽 → 每周期都刷)，而不再是 period_ms×4。 + # 前端要显示"数据多新"就得用这个，拿 period_ms 硬算会谎报。 + "slot_period_ms": _motor_period_ms * len(bases), + "win_bases": list(bases), + # 周期分段耗时(us): 读侧(/api/motor/state → _cyc_fresh)会把它刷 + # 成新值再回给调用方；放在这儿只是让旧消费者拿到的键不消失。 + # 样本不足时为 None。PDO 线程自己**不算**分位数(见 _CYC_TTL_S)。 + "cyc_us": _cyc_cached, + # 自旋调度是否生效。True 时本进程会吃满一个核，必须让前端看得见。 + "spin": _spin_effective(), + # 自旋线程被钉到的逻辑处理器掩码(0=没钉，说明探测失败或已关掉)。 + # 前端把它显示出来，用户才知道那个核落在哪组。 + "pcore_mask": g_pcore_mask, "sent_seq": sent, "sent_frames": nsent, "queue_depth": len(_motor_cmd_q), @@ -1138,8 +1662,32 @@ def _pdo_worker(): "motors": sorted(slots.values(), key=lambda m: (m["ch"], m["node"])), } errors = 0 - _motor_win = (_motor_win + MOT_SLOTS_PER_PDO) % ( - MOTOR_SCAN_CYCLES * MOT_SLOTS_PER_PDO) + + # ── 推进窗口 ── + # 正常情况在 bases(只覆盖有主的槽)里轮转；每 MOTOR_DISCOVERY_EVERY + # 周期让位给一个发现窗口，好让新插上的电机能被看见。 + if sweep_left > 0: + # 开机首圈：走完整轮扫再切自适应(见 sweep_left 定义处) + sweep_left -= 1 + if sweep_left == 0: + bases = _cover_bases(slots.keys()) + bi = 0 + _motor_win = bases[0] + else: + bi = (bi + 1) % len(bases) + _motor_win = bases[bi] + elif (discovery := discovery + 1) >= MOTOR_DISCOVERY_EVERY: + discovery = 0 + _motor_win = MOTOR_DISCOVERY_BASES[disco_base] + disco_base = (disco_base + 1) % len(MOTOR_DISCOVERY_BASES) + # 发现窗口可能带回新槽 → 重算覆盖集合。用已知槽号(slots 的 key) + # 而不是 online 标志：电机短暂掉线时不该立刻把它踢出窗口， + # 否则它一恢复就得等下一轮发现扫描才被看到。 + bases = _cover_bases(slots.keys()) + bi = 0 + else: + bi = (bi + 1) % len(bases) + _motor_win = bases[bi] except Exception as e: # noqa: BLE001 - 循环不能因单次异常退出 errors += 1 if errors in (1, 50) or errors % 500 == 0: @@ -1153,18 +1701,61 @@ def _pdo_worker(): _motor_stop.wait(0.2) continue # 睡眠必须在锁外：持锁 sleep 会把所有 HTTP 路由一起冻住 - dt = time.perf_counter() - t0 - _motor_stop.wait(max(0.0, _motor_period_ms / 1000.0 - dt)) + # 节拍源可能被人热改(spin 标志随时可变)，翻转时才动一次线程优先级。 + want_rt = _spin_effective() + if want_rt != rt_state: + _set_spin_priority(want_rt) + rt_state = want_rt + # ── 下一拍 = 绝对网格上的截止点，误差不逐拍累加 ── + # 旧写法"截止点 = 本拍 t0 + 周期"：自旋/重拿 GIL 每拍迟到 ε，下拍截止点 + # 就跟着后移 ε，ε 是**每拍固定税**(250us 档实测 ε≈30us → 只有 89%)。 + # 改成 网格截止点 = 锚点 + n×周期：这拍的迟到由下拍吸收，长期平均率 + # = 精确 1/周期；工作耗时真的超周期时重排网格，不去追越拉越远的旧账。 + now = time.perf_counter() + per = _motor_period_ms / 1000.0 + if next_t is None: + next_t = t0 + per + if next_t <= now: + # 这拍工作耗时超了周期：**跳过**过期的网格拍而不是挪锚点 —— + # 挪锚点会把每次超出的零头白丢，那正是 250us 档 8% 系统缺口的来源。 + # 跳过则平均率仍收敛到 1/周期；持续超周期时循环自然跑满(自限)。 + k = 0 + while next_t <= now: + next_t += per + k += 1 + _skip_total += k + try: + _cyc_t[-1] = (t0, _ts, _tm, _tr, now, k) + except IndexError: + pass # 热改周期时整条被清了，这拍丢弃 + else: + try: + _cyc_t[-1] = (t0, _ts, _tm, _tr, now, 0) + except IndexError: + pass + _motor_wait_until(next_t) + next_t += per -def _motor_thread_start(period_ms=None): - """启动后台 PDO 循环(幂等)。""" - global _motor_thread, _motor_period_ms - if period_ms: - _motor_period_ms = max(1, min(1000, int(period_ms))) +def _motor_thread_start(period_ms=None, spin=None): + """启动后台 PDO 循环(幂等)。 + + spin: None=按周期自动(≤MOTOR_SPIN_AUTO_MAX_MS 才自旋)，True/False=显式指定。 + """ + global _motor_thread, _motor_period_ms, _motor_spin, _cyc_cached, _skip_total + p = _parse_period_ms({"period_ms": period_ms}) + if p is not None: + _motor_period_ms = p + # 每次调用都按本次请求重算，**不能**沿用上一次：spin 省略就是"交回自动"。 + # 早先写成 `if spin is not None` 才赋值，结果用户手动开过一次自旋后， + # 之后所有长周期请求都会继续带着 spin=True，自动阈值形同失效。 + _motor_spin = None if spin is None else bool(spin) if _motor_thread is not None and _motor_thread.is_alive(): return False _motor_stop.clear() + _cyc_t.clear() # 换周期后旧样本没意义，别混进分段统计 + _cyc_cached = None # 缓存的分位数同样作废，等新周期的第一批样本 + _skip_total = 0 _motor_thread = threading.Thread(target=_pdo_worker, name="pdo-cycle", daemon=True) _motor_thread.start() @@ -1182,26 +1773,145 @@ def _motor_thread_stop(): return True +def api_motor_spin(req): + """只换节拍源(自旋/内核睡眠)，**绝不动 PDO 循环**。 + + 为什么不复用 /api/motor/start：那条路由在 _BUS_EXCLUSIVE_PATHS 里， + dispatcher 会先 `_motor_thread_stop()` 停掉循环再执行。从站 SM2 的过程数据 + 看门狗只有 1000ms，停手超过这个数从站就掉出 OP(AL 0x001B)，而 g_op_active + 当时仍是 True，于是 motor/start 跳过重进 OP、循环空转报错，再想重枚举就撞上 + SSC 邮箱楔死(2026-09-04 实测把一块好板子这么弄死过两次)。 + 自旋只是循环里读的一个标志，热改即可，没有任何理由重启它。 + + 请求: {spin: true|false|null} null/省略 = 交回按周期自动决定 + """ + global _motor_spin + v = req.get("spin") + _motor_spin = None if v is None else bool(v) + return {"ok": True, "log": [], "spin": _spin_effective(), + "period_ms": _motor_period_ms} + + +def api_motor_period(req): + """热换 PDO 周期，**绝不动 PDO 循环**(理由同 api_motor_spin)。 + + 请求: {period_ms: float} 或 {period_us: int}。允许亚毫秒 —— 0.25ms 即 4kHz。 + 后台循环每拍都重读 _motor_period_ms，改完下一拍就生效，过程数据喂狗全程 + 不断流，所以随时可调、可反复调；与 motor/start(会停循环、有看门狗风险) + 是本质不同的两条路。 + + 物理上限参考(真机分段实测)：一送一回 ≈165us，比这更快的请求会被循环 + 自然自限(实测率低于目标)，不会把循环弄坏。用 rate_probe 现场量。 + """ + global _motor_period_ms, _cyc_cached, _skip_total + ms = _parse_period_ms(req) + if ms is None: + return {"ok": False, "log": ["缺少 period_ms(>0, 单位 ms) 或 period_us(µs)"]} + old = _motor_period_ms + _motor_period_ms = ms + _cyc_t.clear() # 旧周期的分段样本与新周期无关 + _cyc_cached = None + _skip_total = 0 + _gc_events.clear() # dbg 时间线同理：跨档的样本会互相污染归因 + spin = _spin_effective() + log = [f"PDO 周期 {_fmt_period(old)} → {_fmt_period(ms)}"] + if _motor_thread is None or not _motor_thread.is_alive(): + log.append("后台循环当前没在跑，新周期将在下次 /api/motor/start 时生效") + if _motor_spin is False and ms <= MOTOR_SPIN_AUTO_MAX_MS: + log.append("注意: 节拍源被强制为内核睡眠，Windows 睡眠粒度 ~0.5ms 大于" + "目标周期，实测率会远低于目标。要跑准请走 /api/motor/spin " + "设 spin:true(或省略交回自动)。") + return {"ok": True, "log": log, "period_ms": _motor_period_ms, "spin": spin} + + +def api_motor_dbg(req): + """"这一拍的耗时到底藏在哪一段"的原始时间线导出。见 _cyc_t 注释块。 + + 默认动作是 dump：最近 2048 拍 (t0,send,rx,rx_end,loop_end,跳拍数)、GC 事件 + (结束时刻/时长/代数/线程)、HTTP 请求进出。归因要把三条时间线对齐求交集 + (cadence_diag.py)，所以给原始样本而不是分位数 —— 分位数会把"一次 3ms 的 + gen2 收集"抹平成看不见的 p90。 + + action=gc 开关垃圾回收跑 A/B：若关掉 GC 后超时拍大幅消失，责任即锁定。 + 关 GC 会让循环的分配不再回收(短测无害，长跑泄漏)，纯诊断开关，别留常态。 + """ + if req.get("action") == "gc": + en = req.get("enable") + if en is True: + gc.enable() + elif en is False: + gc.disable() + return {"ok": True, "gc_enabled": gc.isenabled()} + return {"ok": True, + "period_ms": _motor_period_ms, "spin": _spin_effective(), + "gc_enabled": gc.isenabled(), + "cycles": (_motor_snap or {}).get("cycles", 0), + "skip_total": _skip_total, + "cyc": list(_cyc_t), "gc_events": list(_gc_events), + "http": list(_http_t)} + + def api_motor_start(req): """进 OP 并启动后台 PDO 循环。 + 请求: {period_ms?: float, spin?: bool} + period_ms —— 后台周期，默认 4ms。允许亚毫秒(float，如 0.25=4kHz)， + 下限 MOTOR_PERIOD_MIN_MS。**运行中改周期请走 + /api/motor/period**，别用本路由(会重启循环)。 + spin —— 是否自旋调度。不给就按周期自动(≤2ms 自旋)。自旋能把周期 + 跑准(实测抖动 0.000ms)但吃满一个核；关掉则退回内核睡眠， + 省 CPU 但周期会超调 0.4~1.0ms。见 _motor_wait_until 注释。 + 亚毫秒周期只有自旋才跑得动(睡眠粒度 ~0.5ms)。 + 只想改这一项请走 /api/motor/spin，别用本路由(会重启循环)。 + @note dispatcher 已在取 g_lock 前停掉旧循环，本函数在持锁状态下启动新线程。 新线程第一次取 g_lock 会等到本路由返回，属正常排队，不是死锁。 """ + # 下面要写 g_op_active（发现从站已掉出 OP 时清掉它），必须声明 global， + # 否则函数里一有赋值，前面的读取就会抛 UnboundLocalError。 + global g_op_active if g_master is None: return {"ok": False, "log": ["未连接"]} + # g_op_active 只是我们"上次成功进过 OP"的记忆，**不是**从站的真实状态。 + # 停掉后台循环超过 1000ms，从站的过程数据看门狗就会把它踢出 OP(AL 0x001B)， + # 而这个标志仍是 True —— 于是这里跳过重进 OP，新循环每拍都撞"未进 OP"， + # 用户看到的是"服务活着但全是 --"。所以必须现读一次状态来核实。 + really_op = False + if g_op_active: + try: + g_master.read_state() + really_op = (g_master.slaves[0].state & 0x0F) == 8 # OP_STATE + except Exception: + really_op = False + if not really_op: + g_op_active = False + log0 = ["检测到从站已掉出 OP(过程数据看门狗超时)，重新走 PREOP→SAFEOP→OP"] + else: + log0 = [] + else: + log0 = [] if not g_op_active: log, ok = _require_op_ready(*_dc_args(req)) + # 失败时也要带上 log0：否则"我们检测到从站掉出 OP 才去重进、结果重进失败" + # 这条关键上下文被丢掉，用户只看到一串 config_map 报错，无从判断起因。 if not ok: - return {"ok": False, "log": log} + return {"ok": False, "log": log0 + log} else: log = [] - started = _motor_thread_start(req.get("period_ms")) + log = log0 + log + spin = req.get("spin") + started = _motor_thread_start(req.get("period_ms"), + None if spin is None else bool(spin)) + spinning = _spin_effective() log.append(f"后台 PDO 循环{'已启动' if started else '已在运行'}: " - f"周期 {_motor_period_ms}ms，窗口 {MOT_SLOTS_PER_PDO} 槽/周期，" - f"{MOTOR_SCAN_CYCLES} 周期扫完 {MOTOR_SLOT_COUNT} 槽 " - f"(全表刷新 ≈{_motor_period_ms * MOTOR_SCAN_CYCLES}ms)") - return {"ok": True, "log": log, "period_ms": _motor_period_ms} + f"周期 {_fmt_period(_motor_period_ms)}，窗口 {MOT_SLOTS_PER_PDO} 槽/周期，" + f"窗口自适应(只覆盖有主的槽，在线电机每周期都刷新)，" + f"每 {MOTOR_DISCOVERY_EVERY} 周期插一次发现扫描以找新电机") + log.append("节拍源: " + ("自旋(周期跑得准，但会占满一个 CPU 核)" + if spinning else + "内核睡眠(省 CPU，但 Windows 粒度会让周期超调约 0.5ms)")) + return {"ok": True, "log": log, "period_ms": _motor_period_ms, + "spin": spinning} def api_motor_stop(req): @@ -1210,17 +1920,24 @@ def api_motor_stop(req): @note 实际的 stop/join 由 dispatcher 在锁外完成(见 _BUS_EXCLUSIVE_PATHS)， 这里只报告结果 —— 持锁 join 一个要抢同一把锁的线程会自锁死。 """ - return {"ok": True, "log": [f"后台 PDO 循环已停止(周期曾为 {_motor_period_ms}ms)"]} + return {"ok": True, "log": [f"后台 PDO 循环已停止(周期曾为 {_fmt_period(_motor_period_ms)})"]} def api_motor_state(req): - """读最新电机状态快照（不碰总线，纯读内存，可高频轮询）。""" + """读最新电机状态快照（不碰总线，纯读内存，可高频轮询）。 + + cyc_us 分位数在这里(请求线程)经 _cyc_fresh 现算 —— 绝不能在 PDO 循环里 + 排序：那 400~600us 的排序恰好塞爆一整个 250us 时隙，实测把每第 8 拍顶 + 超时。读侧算，最坏是这条 HTTP 回复自己慢 0.5ms，热循环毫发无损。 + """ snap = _motor_snapshot() if snap is None: return {"ok": False, "log": ["后台 PDO 循环未启动 —— 先调 /api/motor/start"]} if "error" in snap: return {"ok": False, "log": [f"PDO 周期异常: {snap['error']}"], **snap} - return {"ok": True, "log": [], **snap} + out = {"ok": True, "log": [], **snap} + out["cyc_us"] = _cyc_fresh() + return out def api_motor_send(req): @@ -1446,18 +2163,6 @@ def _gui_bringToFront(): _win32_foreground(h) -def _focus_existing(port): - """让已有实例的窗口到前台。返回 True 表示确实存在旧实例。""" - try: - urllib.request.urlopen(urllib.request.Request( - f"http://127.0.0.1:{port}/api/focus", data=b"{}", - headers={"Content-Type": "application/json"}, method="POST"), - timeout=2) - return True - except Exception: - return False - - def _acquire_single_instance(): """命名 mutex 单实例守卫。返回 False=已有实例在跑。""" global g_mutex @@ -1489,8 +2194,16 @@ ROUTES = { "/api/can/state": ("POST", api_can_state), # 电机控制台(motor.html)：状态读快照、命令走后台循环队列 "/api/motor/start": ("POST", api_motor_start), + # 故意**不**放进 _BUS_EXCLUSIVE_PATHS：它只改一个标志，重启循环反而会 + # 掐断过程数据喂狗把从站弄掉出 OP。见 api_motor_spin 的注释。 + "/api/motor/spin": ("POST", api_motor_spin), + # 同理：周期热改只是换个循环每拍重读的变量，喂狗不断流。亚毫秒周期 + # (250us=4kHz)全靠这条路随时调，不必重启循环。见 api_motor_period。 + "/api/motor/period": ("POST", api_motor_period), "/api/motor/stop": ("POST", api_motor_stop), "/api/motor/state": ("POST", api_motor_state), + # 诊断时间线导出(读内存，不碰总线)，同样不进独占路由。见 api_motor_dbg。 + "/api/motor/dbg": ("POST", api_motor_dbg), "/api/motor/send": ("POST", api_motor_send), "/api/motor/rx": ("POST", api_motor_rx), "/api/sdo": ("POST", api_sdo), @@ -1514,6 +2227,27 @@ _BUS_EXCLUSIVE_PATHS = frozenset({ "/api/shutdown", }) +# ── 原生守护进程转发（原型 A：把 PDO 热循环交给 C/SOEM，抬天花板 ~5kHz→~7kHz）── +# 设了 ECAT_NATIVE_URL(如 http://127.0.0.1:8091) 就把下面 3 条热路由原样转给 native， +# 让现有前端/脚本透明驱动原生循环。前提：native 独占网卡时 Python 自己的 PDO 循环 +# 必须没在跑（别调 motor/start），否则两个主站抢同一条总线。默认关，不影响现有链路。 +_NATIVE_URL = os.environ.get("ECAT_NATIVE_URL", "").rstrip("/") +_NATIVE_PATHS = frozenset({"/api/motor/state", "/api/motor/period", "/api/motor/dbg"}) + + +def _native_forward(path, raw_body): + """原样转发 POST 到 native，返回其响应字节。 + + 绝不给 urlopen 设 timeout —— 设了会让 connect 走 select 轮询，每请求白加约 + 10ms（见记忆 esp32-ecat-proxy-must-not-set-socket-timeout / + python-socket-timeout-nonblocking-connect-windows）。 + """ + req = urllib.request.Request( + _NATIVE_URL + path, data=raw_body or b"{}", + headers={"Content-Type": "application/json"}, method="POST") + with urllib.request.urlopen(req) as resp: + return resp.read() + class Handler(BaseHTTPRequestHandler): server_version = "EcatTool/1.0" @@ -1571,6 +2305,15 @@ class Handler(BaseHTTPRequestHandler): } def do_GET(self): + # 包一层进出时间戳进 _http_t：PDO 拍超时要和"HTTP 线程正持着 GIL 序列化 + # 快照"的时间窗求交集，才知道丢的拍是不是被抢了 GIL。见 api_motor_dbg。 + t_in = time.perf_counter() + try: + self._do_GET() + finally: + _http_t.append((t_in, time.perf_counter(), self.path.split("?")[0])) + + def _do_GET(self): path = self.path.split("?")[0] name = self._STATIC_MAP.get(path) if name: @@ -1602,6 +2345,13 @@ class Handler(BaseHTTPRequestHandler): self._not_found(path) def do_POST(self): + t_in = time.perf_counter() + try: + self._do_POST() + finally: + _http_t.append((t_in, time.perf_counter(), self.path.split("?")[0])) + + def _do_POST(self): path = self.path.split("?")[0] if path == "/api/focus": body = b'{"ok":true,"focused":true}' @@ -1621,6 +2371,16 @@ class Handler(BaseHTTPRequestHandler): {"ok": False, "log": [f"请求解析失败: {e}"]}).encode("utf-8")) return + # 原生守护进程转发（原型 A）：ECAT_NATIVE_URL 设了就把 3 条热路由原样转给 + # native。失败时如实报"不可达"，绝不静默回退到本地循环（那会掩盖 native 挂了）。 + if _NATIVE_URL and path in _NATIVE_PATHS: + try: + self._send(200, _native_forward(path, raw)) + except Exception as e: + self._send(200, json.dumps({"ok": False, "log": [ + f"native 守护进程不可达({_NATIVE_URL}): {type(e).__name__}: {e}"]}).encode("utf-8")) + return + # /api/motor/state 与 /api/motor/rx 只读内存(快照 / 接收缓冲)，不碰总线 # —— 放在 g_lock 外面。否则高频轮询会和后台 PDO 线程(每 4ms 取一次锁) # 排队互等，既拖慢响应又给循环加抖动。 @@ -1671,6 +2431,116 @@ def _port_taken(port): return s.connect_ex(("127.0.0.1", port)) == 0 +# 本工具要**独占** Npcap 适配器: 两个实例并存时后起的那个 config_init 抓不到 +# 从站(身份读成 M:00010001), 而 _bind_server 的端口自动挪移让它静默跑到 8091, +# 页面于是永远"探测中→连接失败"。故启动时主动清掉旧实例, 而不是与之共存。 +_INSTANCE_PORTS = (8090, 8091, 8092, 8093, 8094, 8095) + + +def _kill_stale_instances(): + """杀掉本机其它 ecat_server 实例, 返回日志行列表。 + + 优雅优先: 先按端口调旧实例的 /api/shutdown, 让它把从站带回 INIT 再退。 + 直接强杀会让 LAN9252 停在 OP 且 SSC 邮箱可能楔死, 下次 config_map 报一片 + MailboxError, 必须复位 MCU 才能恢复 —— 那正是本次要消除的故障。 + 强杀只作兜底(进程已挂/HTTP 不响应)。 + """ + logs = [] + for port in _INSTANCE_PORTS: + if not _port_taken(port): + continue + try: + urllib.request.urlopen(urllib.request.Request( + f"http://127.0.0.1:{port}/api/shutdown", data=b"{}", + headers={"Content-Type": "application/json"}, method="POST"), + timeout=4) + logs.append(f"旧实例(端口 {port}): 已请求优雅退出(从站回 INIT)") + except Exception: + continue + + # 等端口真正释放: 旧实例要走 停PDO→api_close→httpd.shutdown, 需要一点时间。 + deadline = time.time() + 8 + while time.time() < deadline: + if not any(_port_taken(p) for p in _INSTANCE_PORTS): + break + time.sleep(0.3) + + killed = _force_kill_ecat_processes() + if killed: + logs.append(f"强杀残留 ecat_server 进程 {len(killed)} 个: " + + ",".join(str(p) for p in killed)) + # Npcap 句柄随进程退出由内核回收, 但要给它一拍时间, 否则新实例 open 偶发失败 + if logs: + time.sleep(1.0) + return logs + + +def _force_kill_ecat_processes(): + """按命令行匹配强杀残留 ecat_server 进程, 返回被杀 pid 列表。 + + 用 WMI 查命令行而不是按进程名: 进程名一律是 python/pythonw, 按名字杀会 + 连带干掉 ESP32 工具和用户其它 Python 程序。 + + 两层收窄, 避免误杀: + 1. 进程名必须是 python*.exe —— 只在命令行里**提到**过 ecat_server.py 的 + shell/编辑器/grep 不能杀(实测 bash -c "... ecat_server.py ..." 会命中); + 2. 自身与自身祖先豁免: venv\\Scripts\\python(w).exe 是个转发壳, 会把系统 + 解释器当子进程再起一遍, 同一次启动在 WMI 里因此有多条匹配记录。 + """ + procs = {} + try: + out = subprocess.run( + ["powershell", "-NoProfile", "-Command", + "Get-CimInstance Win32_Process | Where-Object { " + "$_.Name -like 'python*' -and " + "$_.CommandLine -match 'ecat_server\\.py' } | ForEach-Object " + "{ \"$($_.ProcessId) $($_.ParentProcessId)\" }"], + capture_output=True, text=True, timeout=20, + creationflags=getattr(subprocess, "CREATE_NO_WINDOW", 0)).stdout + except Exception: + return [] + for line in out.splitlines(): + parts = line.split() + if len(parts) == 2 and parts[0].isdigit() and parts[1].isdigit(): + procs[int(parts[0])] = int(parts[1]) + + me = os.getpid() + protected = {me} + cur = me # 沿 ppid 向上豁免(venv 转发壳链) + for _ in range(8): # 上限防父子环导致死循环 + parent = procs.get(cur) + # 只在**匹配集合内**向上走: 若把不匹配的启动器(explorer/wscript)也豁免, + # 下面的后代展开会连旧实例一起保护 —— 新旧实例往往同一个启动器, + # 那样清场会变成一次空操作(已由单测覆盖)。 + if parent is None or parent not in procs or parent in protected: + break + protected.add(parent) + cur = parent + changed = True # 再豁免自己的后代(窗口模式的服务子进程) + while changed: + changed = False + for pid, ppid in procs.items(): + if ppid in protected and pid not in protected: + protected.add(pid) + changed = True + + killed = [] + for pid in procs: + if pid in protected: + continue + # PROCESS_TERMINATE(0x0001): 杀不掉(权限/已退出)就跳过, 不让启动流程卡住 + try: + k32 = ctypes.windll.kernel32 + h = k32.OpenProcess(0x0001, False, pid) + if h: + k32.TerminateProcess(h, 1) + k32.CloseHandle(h) + killed.append(pid) + except Exception: + pass + return killed + + def _bind_server(port): for _ in range(6): if _port_taken(port): @@ -1703,8 +2573,17 @@ def _watch_parent(ppid): def _run_browser(args): """浏览器模式: 服务跑在本进程主线程 (纯 pysoem 进程, 无 GUI 库)。""" global g_httpd + # 开机就后台探测 P 核(约 1.6s)。放在这里是因为此刻还没碰过总线， + # 探测占用的 CPU 不会打断任何过程数据交换。见 _start_pcore_probe 注释。 + _start_pcore_probe() if getattr(args, "parent_pid", 0): _watch_parent(args.parent_pid) + else: + # 独立启动(ecat_web.vbs/bat 或直接命令行): 自己负责清场。 + # 窗口模式的服务子进程不走这里 —— GUI 父进程已经清过, 子进程再清一遍 + # 会把父进程连带杀掉(父子命令行都匹配 ecat_server.py)。 + for line in _kill_stale_instances(): + print(line, flush=True) httpd, port = _bind_server(args.port) if httpd is None: raise SystemExit("无法绑定端口") @@ -1852,18 +2731,21 @@ def _run_window(webview): """桌面窗口模式: 本进程只做 GUI, HTTP/pysoem 跑在子进程。 实测 pythonnet 环境下 Python 后台线程会静默死亡, 进程隔离根治。""" global g_child, g_port, g_win - if not _acquire_single_instance(): - probed = False - for p in (8090, 8091, 8092): - if _focus_existing(p): - probed = True - break - if probed: - os._exit(0) + # 每次启动先清场, 而不是"发现旧实例就唤醒它然后自己退出"。 + # 旧行为的问题: 用户遇到界面卡死/连接失败时的第一反应就是重新打开 app, + # 而唤醒的正是那个已经坏掉的实例, 看起来像"重开也没用"。 + for line in _kill_stale_instances(): + print(line, flush=True) + # 清场后 mutex 随旧进程退出被内核释放, 但释放不是瞬时的, 重试几次。 + for _ in range(10): + if _acquire_single_instance(): + break + time.sleep(0.3) + else: _fail_box("EtherCAT 控制台", - "已有一个实例在运行, 但无法唤醒其窗口。\n" + "已有一个实例在运行且无法结束。\n" "请到任务管理器结束所有 pythonw.exe 后重新打开。") - os._exit(0) + os._exit(1) child, port, focus_evt = _spawn_service_child( ["--no-browser", "--parent-pid", str(os.getpid())], diff --git a/src/test_motor_backend.py b/src/test_motor_backend.py index f9c8d73..254ce75 100644 --- a/src/test_motor_backend.py +++ b/src/test_motor_backend.py @@ -9,11 +9,20 @@ 6. 【1.5.56】批量发送区：多帧在**同一个周期**发出（同步下发） 7. 【1.5.56】批量接收区：一周期回收多帧，含 DLC64 CANFD 8. 【1.5.56】兼容性：批量区为空时，字节布局与 1.5.55 完全一致 + 9. 窗口自适应：稀疏总线(27 槽只有 2 个有主)下单槽每周期都刷新， + 而不是旧实现的每 4 周期一次 + 10. 发现扫描：热插上来的电机仍能被找到并纳入常规窗口 + （含 slot 21..26 这段尾部 —— 朴素的"+7 到越界归零"游标会永久漏掉它） + 11. 自旋节拍源：自动阈值/显式覆盖，以及 1ms 档确实跑得准 + 12. 自旋不得持 GIL 空转（否则 HTTP 线程被饿死，真机实测劣化到 40ms） + 13. P 核识别与钉核：混合架构下自旋钉到快核(P 核)组，且钉核是线程级的 + 14. api_motor_start 的 OP 状态核实：已在 OP 能正常启动，从站掉出 OP 能发现 跑法: python test_motor_backend.py """ import struct import sys +import threading import time import types import os @@ -37,7 +46,14 @@ NSLOT = S.MOT_SLOTS_PER_PDO class FakeSlave: - """按 comm/ethercat_can_bridge.cpp 的语义模拟从站侧。""" + """按 comm/ethercat_can_bridge.cpp 的语义模拟从站侧。 + + state/al_status 是 api_motor_start 核实"是否真在 OP"时要读的字段（pysoem + 的 Slave 也有这两个），默认给 OP，掉线场景由测试临时改。 + """ + + state = 8 # OP_STATE + al_status = 0 def __init__(self): self._out = bytes(S.PDO_OUT_LEN) @@ -136,7 +152,53 @@ class FakeSlave: return bytes(b) +class SparseSlave(FakeSlave): + """只有少数槽有主的从站 —— 真实现场的样子(27 槽容量，实际接 2 台)。 + + FakeSlave 把 27 个槽全填满，那正好是自适应窗口毫无收益的最坏情况， + 测不出窗口浪费。这里按 `present` 集合填槽，并记录每个槽被窗口命中的次数， + 用来算"单个电机每几个周期才被读到一次"。 + """ + + def __init__(self, present): + super().__init__() + self.present = set(present) + self.slot_hits = {} # 槽号 -> 被窗口盖住并回传的次数 + + @property + def input(self): + b = bytearray(super().input) + # 覆盖掉父类填的"全满"槽区，改按 present 填 + win = self._out[S.TX_OFF_WIN] + for i in range(NSLOT): + off = S.MOTW_OFF_SLOTS + i * SLOT + b[off:off + SLOT] = bytes(SLOT) # 先清成空槽 + cnt = 0 + for i in range(NSLOT): + idx = win + i + if idx >= S.MOTOR_SLOT_COUNT: + break + cnt += 1 + if idx not in self.present: + continue # 空槽: node_id 留 0 + off = S.MOTW_OFF_SLOTS + i * SLOT + b[off] = idx // S.MOTOR_CHANNELS + 1 + b[off + 1] = idx % S.MOTOR_CHANNELS + b[off + 2] = 1 + b[off + 3] = 32 + # seq 每次命中都要变，否则前端的 seq 活性判断看不出在刷新 + self.slot_hits[idx] = self.slot_hits.get(idx, 0) + 1 + struct.pack_into(" 0.90, f"slot {slot} 命中率仅 {ratio*100:.0f}%，窗口没盖住" + # 空槽不该再白占窗口：整体扫描量应远低于旧实现的 27 槽/4 周期 + print(f" [OK] 在线电机每周期都刷新（旧实现每 4 周期才 1 次 = 25%）") + + print("\n=== 10. 发现扫描：新插上的电机仍能被找到 ===") + # 自适应窗口只盯"已知的槽"，必须靠周期性发现扫描找到新电机， + # 否则热插拔上来的电机永远不出现 —— 这是自适应的必要配套。 + sparse2 = SparseSlave({2}) + S.g_master.slaves[0] = sparse2 + sparse2.output = bytes(S.PDO_OUT_LEN) + S._motor_snap = None + S._motor_thread_start(period_ms=1) + time.sleep(0.3) + sparse2.present.add(24) # 热插：node9/ch0 上线 + deadline = time.time() + 5.0 + found = False + while time.time() < deadline: + time.sleep(0.1) + snap = S.api_motor_state({}) + if any(m["slot"] == 24 for m in (snap.get("motors") or [])): + found = True + break + S._motor_thread_stop() + print(f" 发现间隔 MOTOR_DISCOVERY_EVERY={S.MOTOR_DISCOVERY_EVERY} 周期") + assert found, "发现扫描没找到新上线的 slot 24（自适应窗口把它永久漏掉了）" + print(f" bases={snap['win_bases']} → 新槽已纳入常规覆盖") + assert any(b <= 24 < b + S.MOT_SLOTS_PER_PDO for b in snap["win_bases"]), \ + "slot 24 被发现了但没纳入常规窗口，下一拍又会漏" + print(" [OK] 热插的电机能被发现并纳入每周期刷新") + + print("\n=== 11. 自旋节拍源：策略 + 实际精度 ===") + # 分段实测表明一次周期的总线开销只有 ~165us，1.36ms 里 1.2ms 是 + # Windows 内核睡眠的粒度超调。自旋能把周期跑准，代价是占满一个核， + # 所以"什么时候自旋"和"自旋到底准不准"都得有测试守着。 + S._motor_spin = None + S._motor_period_ms = 4 + assert S._spin_effective() is False, "4ms 默认不该自旋(内核睡眠够用，别白烧 CPU)" + # 显式开一次之后，下一次不带 spin 的请求必须回到"自动"，不能沿用 True。 + # 早先 _motor_thread_start 写成 `if spin is not None` 才赋值，于是手动开过 + # 一次自旋就永久粘住，长周期也照样烧一个核，自动阈值形同失效。 + S._motor_thread_start(4, True) + assert S._spin_effective() is True, "显式 spin=True 必须生效" + S._motor_thread_stop() + S._motor_thread_start(4, None) + assert S._spin_effective() is False, \ + "省略 spin 必须回到自动模式(4ms→不自旋)，不能粘住上一次的 True" + S._motor_thread_stop() + S._motor_period_ms = 2 + S._motor_period_ms = 2 + assert S._spin_effective() is True, "2ms 该自动自旋(内核睡眠在这个档睡不准)" + S._motor_period_ms = 1 + assert S._spin_effective() is True, "1ms 该自动自旋" + S._motor_period_ms = 1 + S._motor_spin = False + assert S._spin_effective() is False, "用户显式关掉就必须关(省 CPU 优先)" + S._motor_period_ms = 8 + S._motor_spin = True + assert S._spin_effective() is True, "用户显式打开就必须在长周期也打开" + S._motor_spin = None + print(" [OK] 自动阈值 / 显式覆盖 都按预期") + + # 真量一次精度。**只断言自旋档达标，不断言"自旋比睡眠快"** —— + # Windows 的定时器分辨率是全局且粘滞的(别的进程调过 timeBeginPeriod 就会 + # 一直生效)，粗粒度时睡眠能慢到 700Hz、细粒度时睡眠也能跑到 ~1000Hz。 + # 拿两者比值当判据会让这条测试随机翻脸，真正要保证的是： + # 1) 自旋档能稳定跑到设定周期(下面第 12 项再保证它不饿死别人) + S.g_master.slaves[0] = SparseSlave({2}) + S.g_master.slaves[0].output = bytes(S.PDO_OUT_LEN) + def rate(spin, period=1): + S._motor_spin = spin + S._motor_period_ms = period + S._motor_snap = None + S._cyc_t.clear() + S._motor_thread_start() + time.sleep(1.0) # 先让周期稳定下来 + a = S._motor_snapshot()["cycles"]; ta = time.perf_counter() + time.sleep(1.5) + b = S._motor_snapshot()["cycles"]; tb = time.perf_counter() + S._motor_thread_stop() + return (b - a) / (tb - ta) + h_sleep = rate(False) + h_spin = rate(True) + print(f" 内核睡眠: {h_sleep:6.1f} Hz 自旋: {h_spin:6.1f} Hz (目标 1000 Hz)") + print(f" (睡眠档快慢取决于本机定时器分辨率, 只作参考不作判据)") + assert h_spin > 850, f"1ms 自旋档只跑到 {h_spin:.0f}Hz，离目标太远" + print(f" [OK] 自旋稳定跑到设定周期的 {h_spin/10:.0f}%（1ms 档）") + + print("\n=== 12. 自旋不得饿死其它线程(GIL) ===") + # 真机踩过的坑：第一版自旋写成 `while time.perf_counter() < d: pass`， + # 于是读快照的 HTTP p50 从 0.6ms 劣化到 40ms —— 页面直接废了。 + # 原因是 CPython GIL 切换间隔默认 5ms，纯 Python 循环可以连拿好几个 5ms。 + # 这一项量"自旋在跑时，别的线程被延迟多久"，是那条修复的回归守卫。 + S.g_master.slaves[0] = SparseSlave({2}) + S.g_master.slaves[0].output = bytes(S.PDO_OUT_LEN) + S._motor_spin = True + S._motor_period_ms = 1 + S._motor_snap = None + S._cyc_t.clear() + S._motor_thread_start() + time.sleep(0.6) # 让自旋先跑起来 + lat = [] + t_end = time.perf_counter() + 1.5 + while time.perf_counter() < t_end: # 模拟 HTTP 线程的活: 醒来干点事再睡 + t0 = time.perf_counter() + time.sleep(0.0002) + lat.append((time.perf_counter() - t0) * 1000) + S._motor_thread_stop() + S._motor_spin = None + lat.sort() + p50, p90, mx = lat[len(lat)//2], lat[int(len(lat)*.9)], lat[-1] + print(f" 自旋跑 1ms 周期时，其它线程 sleep(0.2ms) 实测 " + f"p50 {p50:.2f}ms p90 {p90:.2f}ms max {mx:.2f}ms (n={len(lat)})") + # 纯 Python 自旋时 p50 约 5.7ms(=GIL 切换间隔)；放 GIL 的自旋回到 ~0.5ms 基线 + assert p50 < 2.0, ( + f"自旋把其它线程饿到 p50 {p50:.1f}ms —— 自旋循环一定又在持 GIL 空转，" + "必须走 _spin_until(ctypes QPC)，否则 HTTP 路由会全排在 GIL 后面") + print(" [OK] 自旋不持 GIL，HTTP 线程不受影响") + + print("\n=== 13. P 核识别与钉核 ===") + # 自旋要占满一个逻辑处理器，钉到 P 核(快核组)让节拍跑在单核性能最高的核上。 + # 分类逻辑单独测(不跑 1.6s 的真实探测)，用本机实测到的两组数字喂它。 + def bits(m): + return [b for b in range(64) if m >> b & 1] + + # 本机 i7-14700K 的真实测量值：P 核 11.9~16.7ms(含被 SMT 拖累的)，E 核 24.7~29.7ms + real = ([(lp, 13.0 + (lp % 4)) for lp in range(16)] + + [(lp, 25.0 + (lp % 5)) for lp in range(16, 28)]) + got = bits(S._classify_fast(real)) + assert got == list(range(16)), f"本机拓扑应识别出 P 核 0..15，实际 {got}" + print(f" [OK] 真实混合拓扑 → P 核 {got[0]}..{got[-1]}（16 个）") + + # 关键回归：固定倍数阈值会把慢一点的 P 核误判成 E 核，间隙法不会 —— + # 被 SMT 拖累的 16.5ms 核必须仍留在 P 组里 + smt = [(lp, 11.0 if lp < 8 else 16.5) for lp in range(16)] + \ + [(lp, 28.0) for lp in range(16, 24)] + got = bits(S._classify_fast(smt)) + assert got == list(range(16)), f"被 SMT 拖累的 P 核不该被踢出快核组，实际 {got}" + print(" [OK] 被 SMT 拖慢的 P 核没被误判成 E 核") + + # 纯对称 CPU(全同速)必须老实承认分不出来 → 返回 0 = 不钉 + flat = [(lp, 15.0 + lp * 0.01) for lp in range(8)] + assert S._classify_fast(flat) == 0, "对称 CPU 不该硬分出一个'快核'集合" + assert S._classify_fast([(0, 1.0), (1, 9.0)]) == 0, "样本太少不该下结论" + only_one_fast = [(0, 10.0)] + [(lp, 30.0) for lp in range(1, 8)] + assert S._classify_fast(only_one_fast) == 0, "快核只剩 1 个说明分错了" + only_one_slow = [(lp, 10.0) for lp in range(7)] + [(7, 30.0)] + assert S._classify_fast(only_one_slow) == 0, "慢核只剩 1 个说明分错了(离群噪声不是拓扑)" + print(" [OK] 对称/样本不足/单核离群 都安全返回不钉核") + + # 钉核只影响 PDO 线程自己，不能把整个进程(含 HTTP 线程)钉死 + S.g_pcore_mask = 0xF0 + S.g_pcore_probe_done.set() + box = {} + def _pinned(): + S.g_pcore_probe_done.wait(2.0) + if S._pcore_pin_enabled() and S.g_pcore_probe_done.is_set() and S.g_pcore_mask: + S._set_thread_affinity_mask(S.g_pcore_mask) + box["m"] = S._get_thread_affinity_mask() + th = threading.Thread(target=_pinned); th.start(); th.join(timeout=5) + assert bits(box.get("m", 0)) == [4, 5, 6, 7], f"线程亲和性没生效: {box}" + assert bits(S._get_thread_affinity_mask()) != [4, 5, 6, 7], \ + "主线程亲和性被改了 —— 钉核必须是线程级的" + print(" [OK] 钉核是线程级的，HTTP 线程与主线程不受影响") + S.g_pcore_mask = 0 + + print("\n=== 14. api_motor_start 的 OP 状态核实 ===") + # 真机踩过的坑：给 api_motor_start 加了"读从站真实状态"的核实逻辑后， + # 函数里出现了对 g_op_active 的赋值却没声明 global —— 整个路由直接抛 + # UnboundLocalError，电机页 EtherCAT 链路全挂。假从站自测原先都绕开这个 + # 函数(直接调 _motor_thread_start)，所以完全没抓到，这里补上。 + S.g_master.slaves[0] = SparseSlave({2}) + S.g_master.slaves[0].output = bytes(S.PDO_OUT_LEN) + S.g_op_active = True + S._motor_spin = None + r = S.api_motor_start({"period_ms": 2}) + assert r["ok"], f"已在 OP 时重复 start 不该失败: {r.get('log')}" + assert r["spin"] is True, "2ms 档应自动自旋" + print(f" [OK] 已在 OP 时 start 正常（spin={r['spin']}，无 UnboundLocalError）") + S._motor_thread_stop() + + # 从站被看门狗踢出 OP 后，g_op_active 是过期缓存；必须现读状态发现并走重进， + # 否则新循环每拍都撞"未进 OP"，界面绿着却全是 --。 + # 这里只验证"检测到 + 走了重进流程"，不验证重进成功 —— 完整 OP 序列要 + # config_init/config_map/config_dc/state_check/write_state/sdo_read/ + # set_watchdog/dc_sync/_fprd 一整套，假从站撑不起来，硬凑会变成在测桩本身。 + S.g_op_active = True + S.g_master.slaves[0].state = 4 # SAFEOP + r = S.api_motor_start({"period_ms": 4}) + joined = " / ".join(r.get("log") or []) + assert "掉出 OP" in joined, f"没检测到从站已掉出 OP，仍拿过期标志空转: {joined[:160]}" + assert not r["ok"], "重进失败时必须如实报错，不能假装启动成功" + print(" [OK] 检测到从站掉出 OP 并走重进流程（不拿过期标志空转）") + S.g_master.slaves[0].state = 8 + S._motor_thread_stop() + S.g_op_active = True + S._motor_spin = None + + print("\n=== 15. 亚毫秒周期 + /api/motor/period 热调 ===") + # 需求：内核级要能跑 250us(4kHz)，且周期随时可调。守两条： + # a) period 允许亚毫秒 float，并按 [MIN,MAX] 钳位； + # b) 热调路由**不停循环**、也不进总线互斥表 —— 停循环 = 掐断喂狗， + # 正是把真机板子弄楔死的那条路(见 api_motor_spin 注释)。 + assert "/api/motor/period" not in S._BUS_EXCLUSIVE_PATHS, \ + "周期热调绝不能进总线互斥表：dispatcher 停循环会掐断喂狗" + + assert S._parse_period_ms({"period_ms": 0.25}) == 0.25 + assert S._parse_period_ms({"period_us": 500}) == 0.5 + assert S._parse_period_ms({"period_ms": 0.0001}) == S.MOTOR_PERIOD_MIN_MS + assert S._parse_period_ms({"period_ms": 5000}) == S.MOTOR_PERIOD_MAX_MS + assert S._parse_period_ms({"period_ms": 0}) is None + assert S._parse_period_ms({"period_ms": float("nan")}) is None + assert S._parse_period_ms({}) is None + print(" [OK] period_ms/period_us 解析与钳位(0/NaN/缺省视为没给)") + + # 热调速测：假从站的收发是纯内存操作(µs 级)，这里量的是 + # "自旋+float 周期"这套机制能不能把循环真正驱动到 4kHz 档， + # 不含真机 ~165us 的总线时间(那部分由 rate_probe 在板上量)。 + S.g_master.slaves[0] = SparseSlave({2, 5}) + S.g_master.slaves[0].output = bytes(S.PDO_OUT_LEN) + S._motor_spin = None + S._motor_thread_start(period_ms=4) + time.sleep(0.15) + r = S.api_motor_period({"period_ms": 0.25}) + assert r["ok"] and r["period_ms"] == 0.25, r + assert r["spin"] is True, "0.25ms 周期在自动档下必须自旋(睡眠粒度睡不出)" + assert S._motor_thread.is_alive(), "热调周期把循环弄停了" + + def _rate(sample_s=1.0): + t0 = time.perf_counter() + c0 = (S._motor_snapshot() or {}).get("cycles", 0) + time.sleep(sample_s) + c1 = (S._motor_snapshot() or {}).get("cycles", 0) + return (c1 - c0) / (time.perf_counter() - t0) + + hz = _rate(1.0) + assert hz > 3000, f"0.25ms 周期只跑出 {hz:.0f}Hz，机制没到 4kHz 档" + snap = S._motor_snapshot() + assert snap["period_ms"] == 0.25 and snap["slot_period_ms"] == 0.25, snap + print(f" [OK] 热调 4ms→0.25ms 循环不停，实测 {hz:.0f}Hz(假从站 ≥3000)") + + # 强制睡眠 + 亚毫秒周期 → 必须明确警告(睡眠粒度 > 目标周期，跑不到) + S._motor_spin = False + r = S.api_motor_period({"period_us": 250}) + assert r["ok"] and r["spin"] is False + assert "内核睡眠" in " ".join(r["log"]), f"该警告的没警告: {r['log']}" + S._motor_spin = None + + # 换回长周期：自动档应退回内核睡眠(不白烧一个核) + r = S.api_motor_period({"period_ms": 4}) + assert r["ok"] and r["spin"] is False + assert S._motor_thread.is_alive() + print(" [OK] 换回 4ms 自动退回内核睡眠，循环全程没停过") + S._motor_thread_stop() + + # float 周期也要能从 motor/start 一路传进来(前端首启路径) + S._motor_spin = None + assert S._motor_thread_start(period_ms=0.5) is True + assert S._motor_period_ms == 0.5 + S._motor_thread_stop() + S._motor_period_ms = 4 + S._motor_spin = None + print(" [OK] motor/start 也收 float 周期(0.5ms)") + + print("\n=== 16. 真机总线耗时下的网格收敛（4kHz 保底的忠实代理）===") + # 第 15 项的假从站 send/receive 是 µs 级内存桩，**没建模真机 165µs 总线时间**， + # 所以它证明不了"166us 工作 + 250us 网格"这个真机核心交互能不能收敛到 4000Hz。 + # 这里给收发注入按真机分段实测拆开的耗时(send 48us + receive 118us = 166us)， + # 且用 _spin_until 释放 GIL —— 和真机 pysoem 在 Npcap 调用期间放 GIL 的行为一致。 + # 这是不接硬件能产出的最强预测：工作 166us < 周期 250us，网格不该丢拍， + # 平均率应收敛到 ~4000Hz；若收敛不了，说明缺口是结构性的，得上真机前就预警。 + BUS_SEND_US, BUS_RX_US = 48.0, 118.0 + + class BusCostMaster(FakeMaster): + def send_processdata(self): + S._spin_until(time.perf_counter() + BUS_SEND_US / 1e6) + + def receive_processdata(self, timeout): + S._spin_until(time.perf_counter() + BUS_RX_US / 1e6) + return 3 + + S.g_master = BusCostMaster() + S.g_master.slaves[0] = SparseSlave({2, 5}) + S.g_master.slaves[0].output = bytes(S.PDO_OUT_LEN) + S._motor_spin = None + S._motor_thread_start(period_ms=0.25) + time.sleep(0.2) + + def _rate(sample_s): + t0 = time.perf_counter() + c0 = (S._motor_snapshot() or {}).get("cycles", 0) + time.sleep(sample_s) + c1 = (S._motor_snapshot() or {}).get("cycles", 0) + return (c1 - c0) / (time.perf_counter() - t0) + + hz = _rate(1.5) + # 分位数改读侧现算(_cyc_fresh)，从 api_motor_state 拿顺带验证这条路径 + cyc = S.api_motor_state({}).get("cyc_us") or {} + tot = (cyc.get("total") or {}).get("p50") + print(f" 166us 总线负载下 250us 网格实测 {hz:.0f}Hz (cyc_us.total p50={tot}us)") + # 工作 166us 远小于 250us 周期，网格不该丢拍 → 应收敛到接近 4000。 + # 阈值取 3700 留 GIL/调度噪声余量；真机若明显低于这个值才需要动 send/receive 合并。 + assert hz > 3700, \ + f"166us 工作负载下网格只收敛到 {hz:.0f}Hz，缺口是结构性的，真机大概率也到不了 4kHz" + print(" [OK] 真机同等总线负载下网格仍收敛到 ~4kHz → 保底 4kHz 的强预测") + # 防回归：分位数排序一旦爬回 PDO 循环，每第 8 拍必超时(真机实测 86.6%)。 + # 用源码级检查钉死，比任何数值断言都稳。 + import inspect + assert "_cyc_stats(" not in inspect.getsource(S._pdo_worker), \ + "_cyc_stats 又回到 PDO 热循环了 —— 必须走读侧 _cyc_fresh" + S._motor_thread_stop() + S.g_master = FakeMaster() + S._motor_period_ms = 4 + S._motor_spin = None + print("\n全部通过 ✅") return 0 diff --git a/src/web/ecat.css b/src/web/ecat.css index f634fee..491ad7d 100644 --- a/src/web/ecat.css +++ b/src/web/ecat.css @@ -143,23 +143,41 @@ label { display: block; } .led-pulse { animation: ledPulse 1.2s ease-in-out infinite; } @keyframes ledPulse { 0%,100% { opacity: 1; } 50% { opacity: .35; } } -.ch-select { - padding: 5px 8px; background: var(--bg-root); - border: 1px solid var(--border-strong); border-radius: var(--radius-sm); - font-family: var(--font-mono); font-size: 0.82rem; outline: none; -} -.ch-select:focus { border-color: var(--primary); } -/* 轮询卡顿：通道下拉框变黄并呼吸发光（fixed-tick 下某拍未按时完成） */ -.ch-select.stuck { - border-color: color-mix(in srgb, var(--warn) 65%, transparent); - animation: stuckPulse 1.2s ease-in-out infinite; -} -@keyframes stuckPulse { - 0%, 100% { box-shadow: 0 0 2px color-mix(in srgb, var(--warn) 15%, transparent); } - 50% { box-shadow: 0 0 10px color-mix(in srgb, var(--warn) 40%, transparent); } +/* ── 通道选择：三个并排按钮，各带一颗活动指示灯 ── + 用按钮组而不是 - - - - +
+ +
+ + + +
diff --git a/src/web/motor.js b/src/web/motor.js index 69f9626..83bd32a 100644 --- a/src/web/motor.js +++ b/src/web/motor.js @@ -425,7 +425,9 @@ function loadUiState() { const el = document.getElementById(id); if (el) el.value = o.vals[id]; } - if (typeof o.ch === 'number') { state.ch = o.ch; $('#chSelect').value = String(o.ch); } + /* 通道按钮的选中态由 renderChannelBar() 统一渲染(boot 里会调)，这里只恢复值。 + * 越界值钳到 0：localStorage 可能是旧版本/手工改过的。 */ + if (typeof o.ch === 'number') state.ch = (o.ch >= 0 && o.ch <= 2) ? o.ch : 0; if (o.node) state.selectedNode = o.node; if (o.versions) for (const k of Object.keys(o.versions)) state.versions.set(+k, o.versions[k]); } @@ -711,6 +713,92 @@ function renderLink(snap) { $('#sbLed').className = 'led ' + (state.connected ? 'led-ok' : 'led-err'); } +/* ═══════════════════════ 通道活动指示 ═══════════════════════ */ + +/* 每通道活动状态。online 来自状态表(权威)，rxSeq 用来判断"正在收帧"。 + * 三个通道的数据每周期都在快照里(9 节点 × 3 通道 = 27 槽全带回)， + * 以前只取当前通道、其余丢掉，所以看不出别的通道有没有信息。 */ +const chAct = [ + { online: 0, seqSum: 0, lastRxAt: 0 }, + { online: 0, seqSum: 0, lastRxAt: 0 }, + { online: 0, seqSum: 0, lastRxAt: 0 }, +]; +/* 认定"仍在活动"的时间窗。取 1.2s：电机 STATUS 是 100ms 级， + * 留足余量避免正常抖动让灯闪灭；又短到拔线后 ~1s 就能看出静默。 */ +const CH_LIVE_MS = 1200; + +function updateChannelActivity(motors) { + const now = nowMs(); + const online = [0, 0, 0]; + const seqSum = [0, 0, 0]; + for (const m of motors) { + const c = m.ch; + if (c < 0 || c > 2) continue; // 固件只有 3 路，越界值不该影响指示 + if (m.online) online[c] += 1; + /* seq 是该槽 STATUS 的刷新序号：和上一拍比较有没有变， + * 就知道这一通道这段时间有没有真的收到帧 —— 比 online 标志更灵敏， + * online 在掉线判定超时前会一直是 1。 */ + seqSum[c] += m.seq || 0; + } + for (let c = 0; c < 3; c++) { + const a = chAct[c]; + if (seqSum[c] !== a.seqSum) { + a.seqSum = seqSum[c]; + a.lastRxAt = now; + blinkChDot(c); + } + a.online = online[c]; + } + renderChannelBar(); +} + +/* 收帧闪灯：重启动画要先摘 class 再强制 reflow，否则同一 class 连续两次 + * 添加浏览器不会重播动画（高频收帧时灯会看起来一直不闪）。 */ +function blinkChDot(ch) { + const el = document.getElementById('chDot' + ch); + if (!el) return; + el.classList.remove('blink'); + void el.offsetWidth; + el.classList.add('blink'); +} + +function renderChannelBar() { + const now = nowMs(); + for (let c = 0; c < 3; c++) { + const a = chAct[c]; + const live = a.lastRxAt && (now - a.lastRxAt) < CH_LIVE_MS; + const dot = document.getElementById('chDot' + c); + if (dot) { + /* 绿=有在线电机；黄=有帧但认不出在线电机(接了东西但不是电机/正在上线)； + * 灰=静默。保留 blink 类，别把正在播的闪烁动画摘掉。 */ + const blinking = dot.classList.contains('blink'); + dot.className = 'ch-dot' + (a.online ? ' on' : (live ? ' rx' : '')) + + (blinking ? ' blink' : ''); + } + const cnt = document.getElementById('chCnt' + c); + if (cnt) cnt.textContent = a.online ? String(a.online) : ''; + const btn = document.querySelector(`.ch-btn[data-ch="${c}"]`); + if (btn) { + btn.classList.toggle('sel', c === state.ch); + btn.classList.toggle('live', !!(a.online || live)); + btn.setAttribute('aria-checked', c === state.ch ? 'true' : 'false'); + const parts = [`CH${c}`]; + parts.push(a.online ? `${a.online} 个电机在线` : (live ? '有帧到达，无在线电机' : '静默')); + btn.title = parts.join(' · '); + } + } +} + +function setChannel(ch) { + if (ch === state.ch) return; + state.ch = ch; + clearScope(); + state.decoded.clear(); + $('#sendHint').textContent = `POST /api/motor/send · CH${state.ch}`; + renderChannelBar(); + renderNodeList(); renderStatus(); saveUiState(); +} + /* ═══════════════════════ 轮询：读快照 ═══════════════════════ */ async function pollOnce() { @@ -731,6 +819,9 @@ async function pollOnce() { if (typeof snap.head.win_base === 'number') state.winBase = snap.head.win_base; } + /* 通道活动指示：用**全部**通道的槽算，必须在下面按 state.ch 过滤之前做。 */ + updateChannelActivity(state.motors); + /* 解码当前通道各节点的 STATUS。data 是 64 字符 hex（32B）， * 直接喂给与 ESP32 页共用的 decodeStatus。 */ state.decoded.clear(); @@ -821,6 +912,14 @@ function setConnected(ok, why) { : (why || '未连接 — 请先在 EtherCAT 主页点「连接」，再回到本页启动 PDO 循环'); } if (changed) logLine(ok ? 'PDO 循环就绪，状态开始刷新' : `链路异常: ${why || '未知'}`, ok ? 'ok' : 'warn'); + /* 链路断开时必须清掉通道活动指示：轮询停了就没人再更新它， + 三颗灯会**冻在**断线前的状态，看起来像"还在收帧"。 */ + if (!ok && changed) clearChannelActivity(); +} + +function clearChannelActivity() { + for (const a of chAct) { a.online = 0; a.seqSum = 0; a.lastRxAt = 0; } + renderChannelBar(); } function startPolling() { @@ -891,12 +990,20 @@ function quickRunTarget() { } function bindEvents() { - $('#chSelect').addEventListener('change', () => { - state.ch = parseInt($('#chSelect').value, 10) || 0; - clearScope(); - state.decoded.clear(); - $('#sendHint').textContent = `POST /api/motor/send · CH${state.ch}`; - renderNodeList(); renderStatus(); saveUiState(); + /* 事件委托绑在容器上：三个按钮各绑一次也行，但委托后加/减通道不用改绑定 */ + $('#chGroup').addEventListener('click', (e) => { + const btn = e.target.closest('.ch-btn'); + if (btn) setChannel(parseInt(btn.dataset.ch, 10) || 0); + }); + /* 键盘左右方向键切通道(radiogroup 的惯例操作)，Tab 仍逐个走 */ + $('#chGroup').addEventListener('keydown', (e) => { + const d = e.key === 'ArrowRight' ? 1 : (e.key === 'ArrowLeft' ? -1 : 0); + if (!d) return; + e.preventDefault(); + const next = (state.ch + d + 3) % 3; + setChannel(next); + const btn = document.querySelector(`.ch-btn[data-ch="${next}"]`); + if (btn) btn.focus(); }); $('#modeSelect').addEventListener('change', () => { updateModeUi(); saveUiState(); }); @@ -1081,6 +1188,7 @@ async function boot() { setVerFields(state.versions.get(state.selectedNode)); } $('#sendHint').textContent = `POST /api/motor/send · CH${state.ch}`; + renderChannelBar(); /* 首帧就画出选中态，别等第一次轮询回来才高亮 */ renderScanBar(0, state.scanCells); /* 进页面即启动后台 PDO 循环：从站 SM2 的 1000ms 过程数据看门狗要求