├── .gitignore ├── programmer ├── qlareloadfw.bash ├── pgm1394multi.bash ├── CMakeLists.txt ├── mcsFile.h └── mcsFile.cpp ├── util ├── Makefile ├── CMakeLists.txt ├── info1394.c ├── time1394.c └── block1394.c ├── tests ├── MotorVoltage.h ├── qlacloserelays.cpp ├── MotorVoltage.cpp ├── qlacommand.cpp ├── CMakeLists.txt ├── fwPortTest.cpp ├── block1394eth.cpp ├── instrument.cpp └── ethswitch.cpp ├── lib ├── PortFactory.h ├── Amp1394Types.h ├── Amp1394Time.h ├── Amp1394BSwap.h ├── AmpIORevision.h.in ├── Amp1394Console.h ├── code │ ├── PortFactory.cpp │ ├── Amp1394Time.cpp │ ├── BoardIO.cpp │ ├── ZynqEmioPort.cpp │ ├── Amp1394Console.cpp │ ├── EncoderVelocity.cpp │ └── FirewirePort.cpp ├── EthRawPort.h ├── EthUdpPort.h ├── ZynqEmioPort.h ├── CMakeLists.txt ├── Amp1394.i ├── FirewirePort.h ├── BoardIO.h ├── EncoderVelocity.h ├── EthBasePort.h └── FpgaIO.h ├── README.md ├── Amp1394Config.cmake.in ├── python ├── test_amp1394.py └── test_eth1394.py ├── .github └── workflows │ └── cmake-multi-platform.yml ├── CMakeLists.txt └── CHANGELOG.md /.gitignore: -------------------------------------------------------------------------------- 1 | cisstLog.txt 2 | **/cisstLog.txt 3 | *~ 4 | CMakeLists.txt.user 5 | *.so 6 | *.cxx 7 | Amp1394Python.py 8 | -------------------------------------------------------------------------------- /programmer/qlareloadfw.bash: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | rmmod firewire_ohci 3 | rmmod firewire_core 4 | modprobe firewire_ohci 5 | modprobe firewire_core 6 | -------------------------------------------------------------------------------- /util/Makefile: -------------------------------------------------------------------------------- 1 | CC = gcc 2 | CFLAGS = -Wall 3 | LDFLAGS = -lraw1394 4 | EXEC = info1394 quad1394 block1394 time1394 5 | 6 | all: $(EXEC) 7 | 8 | %: %.c 9 | $(CC) $(CFLAGS) $< -o $@ $(LDFLAGS) 10 | 11 | quad1394: block1394 12 | cp block1394 quad1394 13 | 14 | clean: 15 | rm -rf $(EXEC) 16 | -------------------------------------------------------------------------------- /tests/MotorVoltage.h: -------------------------------------------------------------------------------- 1 | /* -*- Mode: C++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */ 2 | /* ex: set filetype=cpp softtabstop=4 shiftwidth=4 tabstop=4 cindent expandtab: */ 3 | 4 | #ifndef __MOTORVOLTAGE_H__ 5 | #define __MOTORVOLTAGE_H__ 6 | 7 | class BasePort; 8 | class AmpIO; 9 | 10 | // Returns the measured motor supply voltage, for QLA V1.5+ 11 | // index: 0 for QLA; 1 or 2 for DQLA 12 | double MeasureMotorSupplyVoltage(BasePort *curPort, AmpIO *curBoard, unsigned int index = 0); 13 | 14 | #endif // __MOTORVOLTAGE_H__ 15 | -------------------------------------------------------------------------------- /lib/PortFactory.h: -------------------------------------------------------------------------------- 1 | /* -*- Mode: C++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */ 2 | /* ex: set filetype=cpp softtabstop=4 shiftwidth=4 tabstop=4 cindent expandtab: */ 3 | 4 | /* 5 | Author(s): Anton Deguet 6 | 7 | (C) Copyright 2020 Johns Hopkins University (JHU), All Rights Reserved. 8 | 9 | --- begin cisst license - do not edit --- 10 | 11 | This software is provided "as is" under an open source license, with 12 | no warranty. The complete license can be found in license.txt and 13 | http://www.cisst.org/cisst/license.txt. 14 | 15 | --- end cisst license --- 16 | */ 17 | 18 | #ifndef __PortFactory_H__ 19 | #define __PortFactory_H__ 20 | 21 | #include "BasePort.h" 22 | 23 | // Create a port based on options, as parsed by BasePort::ParseOptions 24 | BasePort * PortFactory(const char * args = 0, 25 | std::ostream & debugStream = std::cerr); 26 | 27 | #endif 28 | -------------------------------------------------------------------------------- /tests/qlacloserelays.cpp: -------------------------------------------------------------------------------- 1 | /* -*- Mode: C++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */ 2 | /* ex: set filetype=cpp softtabstop=4 shiftwidth=4 tabstop=4 cindent expandtab: */ 3 | 4 | /* 5 | Author(s): Zihan Chen, Anton Deguet 6 | Created on: 2015 7 | 8 | (C) Copyright 2015-2023 Johns Hopkins University (JHU), All Rights Reserved. 9 | 10 | --- begin cisst license - do not edit --- 11 | 12 | This software is provided "as is" under an open source license, with 13 | no warranty. The complete license can be found in license.txt and 14 | http://www.cisst.org/cisst/license.txt. 15 | 16 | --- end cisst license --- 17 | */ 18 | 19 | #include 20 | 21 | int main(int, char**) 22 | { 23 | std::cerr << "qlacloserelays is deprecated, use `qlacommand` instead. Examples:" << std::endl 24 | << "> qlacommand -c close-relays" << std::endl 25 | << "> qlacommand -pfw:0 -c close-relays" << std::endl 26 | << "> qlacommand -c open-relays" << std::endl; 27 | return 0; 28 | } 29 | -------------------------------------------------------------------------------- /lib/Amp1394Types.h: -------------------------------------------------------------------------------- 1 | /* -*- Mode: C++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */ 2 | /* ex: set filetype=cpp softtabstop=4 shiftwidth=4 tabstop=4 cindent expandtab: */ 3 | 4 | /* 5 | Author(s): Zihan Chen, Peter Kazanzides, Jie Ying Wu 6 | 7 | (C) Copyright 2011-2021 Johns Hopkins University (JHU), All Rights Reserved. 8 | 9 | --- begin cisst license - do not edit --- 10 | 11 | This software is provided "as is" under an open source license, with 12 | no warranty. The complete license can be found in license.txt and 13 | http://www.cisst.org/cisst/license.txt. 14 | 15 | --- end cisst license --- 16 | */ 17 | 18 | 19 | #ifndef __AMP1394_TYPES_H__ 20 | #define __AMP1394_TYPES_H__ 21 | 22 | #ifdef _MSC_VER 23 | typedef unsigned __int8 uint8_t; 24 | typedef unsigned __int16 uint16_t; 25 | typedef unsigned __int32 uint32_t; 26 | typedef unsigned __int64 uint64_t; 27 | typedef __int16 int16_t; 28 | typedef __int32 int32_t; 29 | #else 30 | #include 31 | #endif 32 | 33 | #endif // __AMP1394_TYPES_H__ 34 | -------------------------------------------------------------------------------- /lib/Amp1394Time.h: -------------------------------------------------------------------------------- 1 | /* -*- Mode: C++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */ 2 | /* ex: set filetype=cpp softtabstop=4 shiftwidth=4 tabstop=4 cindent expandtab: */ 3 | 4 | /* 5 | (C) Copyright 2016 Johns Hopkins University (JHU), All Rights Reserved. 6 | 7 | --- begin cisst license - do not edit --- 8 | 9 | This software is provided "as is" under an open source license, with 10 | no warranty. The complete license can be found in license.txt and 11 | http://www.cisst.org/cisst/license.txt. 12 | 13 | --- end cisst license --- 14 | */ 15 | 16 | // These are simple cross-platform implementations to get the current time (in seconds) and 17 | // to sleep for a specified number of seconds. They are based on the implementations from 18 | // osaGetTime and osaSleep in cisstOSAbstraction. 19 | 20 | #ifndef __AMP1394TIME_H__ 21 | #define __AMP1394TIME_H__ 22 | 23 | // Return the time in seconds 24 | double Amp1394_GetTime(void); 25 | 26 | // Sleep for the desired number of seconds 27 | void Amp1394_Sleep(double sec); 28 | 29 | #endif 30 | 31 | -------------------------------------------------------------------------------- /programmer/pgm1394multi.bash: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # 3 | # Script to run pgm1394 on multiple boards. This script doesn't reboot the boards. 4 | # Usage: pgm1394multi 0 1 6 7 # parameters are board IDs 5 | # 6 | 7 | boards="$@" 8 | 9 | for boardId in $boards 10 | do 11 | echo "Programming board $boardId" 12 | pgm1394 $boardId -a 13 | result=$? 14 | echo "Result: $result (0 is good)" 15 | if [[ "$result" -ne 0 ]]; then 16 | echo "------> pgm1394 -a failed for board $boardId" 17 | echo "------> DO NOT REBOOT OR POWER OFF this board" 18 | echo "------> Try to reprogram this board using pgm1394" 19 | exit $result 20 | fi 21 | done 22 | 23 | echo "------> You now need to reboot your controllers. You can either" 24 | echo "------> power cycle them or use: qlacommand -c reboot" 25 | echo "" 26 | echo "------> If your PC is connected to the controllers via FireWire" 27 | echo "------> you might also need to unplug/replug the FireWire cable to" 28 | echo "------> rediscover the boards (or use sudo qlareloadfw.bash)" 29 | 30 | exit 0 31 | -------------------------------------------------------------------------------- /lib/Amp1394BSwap.h: -------------------------------------------------------------------------------- 1 | /* -*- Mode: C++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */ 2 | /* ex: set filetype=cpp softtabstop=4 shiftwidth=4 tabstop=4 cindent expandtab: */ 3 | 4 | /* 5 | (C) Copyright 2020-2021 Johns Hopkins University (JHU), All Rights Reserved. 6 | 7 | --- begin cisst license - do not edit --- 8 | 9 | This software is provided "as is" under an open source license, with 10 | no warranty. The complete license can be found in license.txt and 11 | http://www.cisst.org/cisst/license.txt. 12 | 13 | --- end cisst license --- 14 | */ 15 | 16 | #ifndef __AMP1394BSWAP_H__ 17 | #define __AMP1394BSWAP_H__ 18 | 19 | #include "Amp1394Types.h" 20 | 21 | #ifdef _MSC_VER 22 | #include // for byteswap functions 23 | inline uint16_t bswap_16(uint16_t data) { return _byteswap_ushort(data); } 24 | inline uint32_t bswap_32(uint32_t data) { return _byteswap_ulong(data); } 25 | 26 | #elif defined(__APPLE__) 27 | #include 28 | #define bswap_32(x) OSSwapInt32(x) 29 | #define bswap_16(x) OSSwapInt16(x) 30 | 31 | #else 32 | #include 33 | #endif 34 | 35 | #endif 36 | -------------------------------------------------------------------------------- /lib/AmpIORevision.h.in: -------------------------------------------------------------------------------- 1 | /* -*- Mode: C++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */ 2 | /* ex: set filetype=cpp softtabstop=4 shiftwidth=4 tabstop=4 cindent expandtab: */ 3 | 4 | /* 5 | Author(s): Anton Deguet 6 | Created on: 2014-04-10 7 | 8 | (C) Copyright 2015-2023 Johns Hopkins University (JHU), All Rights Reserved. 9 | 10 | --- begin cisst license - do not edit --- 11 | This software is provided "as is" under an open source license, with 12 | no warranty. The complete license can be found in license.txt and 13 | http://www.cisst.org/cisst/license.txt. 14 | --- end cisst license --- 15 | */ 16 | 17 | #pragma once 18 | 19 | #ifndef _AmpIORevision_h 20 | #define _AmpIORevision_h 21 | 22 | #define Amp1394_VERSION_MAJOR ${Amp1394_VERSION_MAJOR} 23 | #define Amp1394_VERSION_MINOR ${Amp1394_VERSION_MINOR} 24 | #define Amp1394_VERSION_PATCH ${Amp1394_VERSION_PATCH} 25 | #define Amp1394_VERSION "${Amp1394_VERSION}" 26 | 27 | #cmakedefine01 Amp1394_HAS_RAW1394 28 | #cmakedefine01 Amp1394_HAS_PCAP 29 | #cmakedefine01 Amp1394_HAS_EMIO 30 | 31 | #cmakedefine01 Amp1394Console_HAS_CURSES 32 | 33 | #endif // _AmpIORevision_h 34 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Mechatronics Software 2 | 3 | This is the software interface to the FPGA boards developed for the 4 | [Open Source Mechatronics](https://jhu-cisst.github.io/mechatronics) project. 5 | It is designed to have no external dependencies, other than `libraw1394` for 6 | the Firewire communications and `libpcap` if raw Ethernet is used. 7 | 8 | The following directories are included: 9 | * `lib` -- library to interface with the FPGA boards 10 | * `programmer` -- application to program the FPGA boards via Firewire 11 | * `python` -- Python test programs 12 | * `tests` -- example programs (qladisp, qlatest, etc.) 13 | * `util` -- low-level Firewire utility programs (do not depend on `lib`) 14 | 15 | Documentation for the software is on the [wiki](http://github.com/jhu-cisst/mechatronics-software/wiki). 16 | 17 | # Compilation status 18 | 19 | Linux gcc/clang and Windows cl for both Debug and Release configurations:
20 | [![CMake on multiple platforms](https://github.com/jhu-cisst/mechatronics-software/actions/workflows/cmake-multi-platform.yml/badge.svg)](https://github.com/jhu-cisst/mechatronics-software/actions/workflows/cmake-multi-platform.yml) 21 | -------------------------------------------------------------------------------- /Amp1394Config.cmake.in: -------------------------------------------------------------------------------- 1 | # - Config file for the Amp1394 package 2 | # It defines the following variables 3 | # Amp1394_INCLUDE_DIR - include directories for Amp1394 4 | # Amp1394_LIBRARY_DIR - link library directories for Amp1394 5 | # Amp1394_LIBRARIES - libraries to link against 6 | # Amp1394Console_LIBRARIES - libraries for Amp1394Console 7 | 8 | # Version 9 | set (Amp1394_VERSION_MAJOR "@Amp1394_VERSION_MAJOR@") 10 | set (Amp1394_VERSION_MINOR "@Amp1394_VERSION_MINOR@") 11 | set (Amp1394_VERSION_PATCH "@Amp1394_VERSION_PATCH@") 12 | set (Amp1394_VERSION "@Amp1394_VERSION@") 13 | 14 | # Compute paths 15 | set (Amp1394_INCLUDE_DIR "@CONF_INCLUDE_DIR@") 16 | set (Amp1394_LIBRARY_DIR "@CONF_LIBRARY_DIR@") 17 | 18 | # Libraries to link against 19 | set (Amp1394_LIBRARIES "@CONF_LIBRARIES@") 20 | set (Amp1394Console_LIBRARIES "@CONF_CONSOLE_LIBRARIES@") 21 | 22 | # FireWire/Ethernet/Zynq-EMIO support 23 | set (Amp1394_HAS_RAW1394 "@Amp1394_HAS_RAW1394@") 24 | set (Amp1394_HAS_PCAP "@Amp1394_HAS_PCAP@") 25 | set (Amp1394_HAS_EMIO "@Amp1394_HAS_EMIO@") 26 | 27 | # Whether using curses for console 28 | set (Amp1394Console_HAS_CURSES "@Amp1394Console_HAS_CURSES@") 29 | -------------------------------------------------------------------------------- /util/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # 2 | # (C) Copyright 2012-2024 Johns Hopkins University (JHU), All Rights Reserved. 3 | # 4 | # --- begin cisst license - do not edit --- 5 | # 6 | # This software is provided "as is" under an open source license, with 7 | # no warranty. The complete license can be found in license.txt and 8 | # http://www.cisst.org/cisst/license.txt. 9 | # 10 | # --- end cisst license --- 11 | 12 | project (Amp1394Utils VERSION 2.1.0) 13 | 14 | if (NOT EXECUTABLE_OUTPUT_PATH) 15 | set (EXECUTABLE_OUTPUT_PATH ${CMAKE_CURRENT_BINARY_DIR}) 16 | endif (NOT EXECUTABLE_OUTPUT_PATH) 17 | 18 | # for FireWire 1394 19 | if (Amp1394_HAS_RAW1394) 20 | set (PROGRAMS block1394 info1394 time1394) 21 | foreach (program ${PROGRAMS}) 22 | add_executable (${program} ${program}.c) 23 | target_link_libraries (${program} raw1394) 24 | endforeach (program) 25 | 26 | # Add post-build command to copy block1394 to quad1394 27 | add_custom_command (TARGET block1394 POST_BUILD 28 | COMMAND ${CMAKE_COMMAND} -E copy 29 | ${EXECUTABLE_OUTPUT_PATH}/block1394 30 | ${EXECUTABLE_OUTPUT_PATH}/quad1394 31 | COMMENT "Generating quad1394") 32 | 33 | endif (Amp1394_HAS_RAW1394) 34 | -------------------------------------------------------------------------------- /python/test_amp1394.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import Amp1394Python as amp1394 4 | from Amp1394Python import bswap32 5 | 6 | def TEST_ReadQuadlet(fw, eth): 7 | # bid = 1, addr = 0x04 (QLA1) 8 | bid = 1 9 | addr = 0x04 10 | ret_fw, data_fw = fw.ReadQuadlet(bid, addr) 11 | ret_eth, data_eth = eth.ReadQuadlet(bid, addr) 12 | if (not ret_fw): 13 | print 'Read Firewire failed' 14 | if (not ret_eth): 15 | print 'Read Ethernet failed' 16 | 17 | if (data_fw != data_eth): 18 | print 'Firewire & Ethernet read do not match' 19 | print 'Firewire data = ' + hex(data_fw) 20 | print 'Ethernet data = ' + hex(data_eth) 21 | return False 22 | else: 23 | print 'Passed test' 24 | return True 25 | pass 26 | 27 | 28 | if __name__ == '__main__': 29 | bid = 3; 30 | eth = amp1394.EthUdpPort(0) 31 | bd1eth = amp1394.AmpIO(bid) 32 | eth.AddBoard(bd1eth) 33 | 34 | fw = amp1394.FirewirePort(0) 35 | bd1fw = amp1394.AmpIO(bid) 36 | fw.AddBoard(bd1fw) 37 | 38 | # Do something here 39 | 40 | fw.RemoveBoard(bid) 41 | eth.RemoveBoard(bid) 42 | 43 | #try: 44 | # import ipdb; ipdb.set_trace() 45 | #except ImportError: 46 | # print 'Failed to import ipdb' 47 | -------------------------------------------------------------------------------- /programmer/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # 2 | # (C) Copyright 2012-2021 Johns Hopkins University (JHU), All Rights Reserved. 3 | # 4 | # --- begin cisst license - do not edit --- 5 | # 6 | # This software is provided "as is" under an open source license, with 7 | # no warranty. The complete license can be found in license.txt and 8 | # http://www.cisst.org/cisst/license.txt. 9 | # 10 | # --- end cisst license --- 11 | 12 | project (Amp1394Programmer) 13 | 14 | include_directories(${Amp1394_INCLUDE_DIR}) 15 | link_directories(${Amp1394_LIBRARY_DIR} ${Amp1394_EXTRA_LIBRARY_DIR}) 16 | 17 | # programmer, pgm1394 18 | add_executable(pgm1394 pgm1394.cpp mcsFile.h mcsFile.cpp) 19 | target_link_libraries(pgm1394 ${Amp1394_LIBRARIES} ${Amp1394_EXTRA_LIBRARIES}) 20 | install (TARGETS pgm1394 21 | COMPONENT Amp1394-utils 22 | RUNTIME DESTINATION bin) 23 | 24 | if (NOT WIN32) 25 | 26 | # script to program multiple boards 27 | add_custom_command(TARGET pgm1394 POST_BUILD 28 | COMMAND ${CMAKE_COMMAND} -E 29 | copy "${Amp1394Programmer_SOURCE_DIR}/pgm1394multi.bash" ${EXECUTABLE_OUTPUT_PATH}) 30 | 31 | # script to unload/reload FireWire kernel modules 32 | add_custom_command(TARGET pgm1394 POST_BUILD 33 | COMMAND ${CMAKE_COMMAND} -E 34 | copy "${Amp1394Programmer_SOURCE_DIR}/qlareloadfw.bash" ${EXECUTABLE_OUTPUT_PATH}) 35 | 36 | # install scripts 37 | install (FILES "${Amp1394Programmer_SOURCE_DIR}/pgm1394multi.bash" "${Amp1394Programmer_SOURCE_DIR}/qlareloadfw.bash" 38 | COMPONENT Amp1394-utils 39 | DESTINATION bin) 40 | 41 | endif (NOT WIN32) 42 | -------------------------------------------------------------------------------- /util/info1394.c: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * 3 | * This is a minimal Linux command line program that displays information 4 | * about the connected IEEE 1394 (Firewire) devices. 5 | * 6 | * Compile: gcc -Wall -lraw1394 -o 7 | * Usage: 8 | * 9 | ******************************************************************************/ 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | /******************************************************************************* 16 | * main program 17 | */ 18 | int main(int argc, char** argv) 19 | { 20 | raw1394handle_t handle; 21 | struct raw1394_portinfo *ports; 22 | int nports; 23 | int i, rc; 24 | 25 | /* get handle to device and check for errors */ 26 | handle = raw1394_new_handle(); 27 | if (handle == NULL) { 28 | fprintf(stderr, "**** Error: could not open 1394 handle\n"); 29 | exit(-1); 30 | } 31 | 32 | /* get number of ports and check for errors */ 33 | nports = raw1394_get_port_info(handle, NULL, 0); 34 | if (nports < 0) { 35 | fprintf(stderr, "**** Error: could not get port info\n"); 36 | exit(-1); 37 | } 38 | 39 | ports = (struct raw1394_portinfo *) malloc(nports*sizeof(struct raw1394_portinfo)); 40 | nports = raw1394_get_port_info(handle, ports, nports); 41 | for (i = 0; i < nports; i++) { 42 | printf("Port %d: %s (%d nodes)", i, ports[i].name, ports[i].nodes); 43 | if (i == 0) { 44 | rc = raw1394_set_port(handle, i); 45 | if (!rc) { 46 | int id = raw1394_get_local_id(handle); 47 | printf(", local id 0x%4X (%d)", id, id&0x3f); 48 | } 49 | printf("\n"); 50 | } 51 | } 52 | free(ports); 53 | 54 | raw1394_destroy_handle(handle); 55 | return 0; 56 | } 57 | -------------------------------------------------------------------------------- /lib/Amp1394Console.h: -------------------------------------------------------------------------------- 1 | /* -*- Mode: C++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */ 2 | /* ex: set filetype=cpp softtabstop=4 shiftwidth=4 tabstop=4 cindent expandtab: */ 3 | 4 | /* 5 | Author(s): Peter Kazanzides 6 | 7 | (C) Copyright 2021 Johns Hopkins University (JHU), All Rights Reserved. 8 | 9 | --- begin cisst license - do not edit --- 10 | 11 | This software is provided "as is" under an open source license, with 12 | no warranty. The complete license can be found in license.txt and 13 | http://www.cisst.org/cisst/license.txt. 14 | 15 | --- end cisst license --- 16 | */ 17 | 18 | #ifndef __AMP1394CONSOLE_H__ 19 | #define __AMP1394CONSOLE_H__ 20 | 21 | 22 | // Portable class for console display/input 23 | 24 | class Amp1394Console { 25 | bool isOk; 26 | bool noEcho; // Whether to echo input characters and show cursor 27 | bool noBlock; // Whether character input is blocking 28 | struct ConsoleInternals; 29 | ConsoleInternals *Internals; 30 | public: 31 | enum { FLAG_DEFAULT = 0x00, FLAG_ECHO = 0x01, FLAG_BLOCKING = 0x02 }; 32 | 33 | Amp1394Console(unsigned int flags = FLAG_DEFAULT) : 34 | isOk(false), noEcho(!(flags&FLAG_ECHO)), noBlock(!(flags&FLAG_BLOCKING)), Internals(0) 35 | { } 36 | ~Amp1394Console() 37 | { End(); } 38 | 39 | bool IsOK() const { return isOk; } 40 | bool Init(); 41 | void End(); 42 | 43 | // Print the specified string at the specified cursor position (row, col). 44 | // Uses control string (cstr) with variable arguments (like printf). 45 | static void Print(int row, int col, const char *cstr, ...); 46 | // Update console display. Does nothing for implementations where Print() 47 | // immediately updates display. 48 | static void Refresh(); 49 | // Return input character. Note that the behavior of this function is 50 | // affected by the blocking mode. 51 | int GetChar(); 52 | // Read (up to n) characters until newline or carriage return. 53 | // This function is always blocking. 54 | static bool GetString(char *str, int n); 55 | }; 56 | 57 | #endif 58 | -------------------------------------------------------------------------------- /tests/MotorVoltage.cpp: -------------------------------------------------------------------------------- 1 | /* -*- Mode: C++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */ 2 | /* ex: set filetype=cpp softtabstop=4 shiftwidth=4 tabstop=4 cindent expandtab: */ 3 | 4 | #include "BasePort.h" 5 | #include "AmpIO.h" 6 | #include "Amp1394Time.h" 7 | 8 | const double bits2volts = (2.5/65535)*21.0; 9 | 10 | double MeasureMotorSupplyVoltage(BasePort *curPort, AmpIO *curBoard, unsigned int index) 11 | { 12 | // Firmware version must be >= 8 13 | if (curBoard->GetFirmwareVersion() < 8) 14 | return 0.0; 15 | 16 | // QLA IO expander must be present (QLA 1.5+) 17 | curPort->ReadAllBoards(); 18 | if (!curBoard->IsQLAExpanded(index)) { 19 | std::cerr << "MeasureMotorSupplyVoltage: QLA does not support voltage measurement" << std::endl; 20 | return 0.0; 21 | } 22 | 23 | // Make sure board power is on and amplifier power is off 24 | curBoard->WriteSafetyRelay(true); 25 | curBoard->WritePowerEnable(true); 26 | curBoard->WriteAmpEnable(0xff, 0); 27 | Amp1394_Sleep(1.0); 28 | if (!curBoard->ReadPowerStatus()) { 29 | std::cerr << "MeasureMotorSupplyVoltage: motor supply not enabled, check safety circuit" << std::endl; 30 | return 0.0; 31 | } 32 | 33 | double V = 0.0; // Measured voltage (in volts) 34 | uint32_t volts; // Measured voltage (in bits) 35 | uint32_t vinc = 0x1000; 36 | nodeaddr_t dac_addr = (index == 2) ? 0x81 : 0x41; 37 | for (volts = 0; volts <= 0x0000ffff; volts += vinc) { 38 | // Write voltage to DAC 4 or 8 (for DQLA) 39 | curPort->WriteQuadlet(curBoard->GetBoardId(), dac_addr, volts | 0x80000000); 40 | Amp1394_Sleep(0.001); 41 | bool dac_lt_supply = curBoard->ReadMotorSupplyVoltageBit(index); 42 | if (!dac_lt_supply) { 43 | // if DAC voltage greater than supply, 44 | // backup and reduce resolution 45 | if (vinc <= 1) break; 46 | volts -= vinc; 47 | vinc >>= 1; 48 | } 49 | } 50 | curPort->WriteQuadlet(curBoard->GetBoardId(), dac_addr, 0x80008000); 51 | curBoard->WritePowerEnable(false); 52 | if (volts <= 0x0000ffff) 53 | V = volts*bits2volts; 54 | return V; 55 | } 56 | -------------------------------------------------------------------------------- /programmer/mcsFile.h: -------------------------------------------------------------------------------- 1 | /* -*- Mode: C++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */ 2 | /* ex: set filetype=cpp softtabstop=4 shiftwidth=4 tabstop=4 cindent expandtab: */ 3 | 4 | #ifndef _MCS_H 5 | #define _MCS_H 6 | 7 | // mcsFile 8 | // 9 | // This class reads an Intel MCS-86 format file, which is produced by the Xilinx 10 | // ISE software. It is not a complete implementation, and probably will not work 11 | // for MCS-86 files produced by other software packages. 12 | 13 | #include 14 | #include 15 | 16 | class mcsFile { 17 | std::ifstream file; 18 | int line_num; // current line number 19 | unsigned long startAddr; // start address 20 | unsigned long numBytes; // number of bytes in sector 21 | unsigned char curSector[65536]; // current sector 22 | 23 | struct RecInfo { 24 | enum { DATA_MAX = 16 }; 25 | unsigned char ndata; 26 | unsigned short addr; 27 | unsigned char type; 28 | unsigned char data[DATA_MAX]; 29 | }; 30 | 31 | bool ProcessNextLine(RecInfo &rec); 32 | bool toHex(const char *p2, unsigned char &result) const; 33 | 34 | enum RecordTypes { 35 | RECORD_DATA = 0, 36 | RECORD_EOF = 1, 37 | RECORD_EXT_SEGMENT = 2, // not supported here 38 | RECORD_EXT_LINEAR = 4 39 | }; 40 | 41 | public: 42 | mcsFile(); 43 | ~mcsFile(); 44 | // Open file 45 | bool OpenFile(const std::string &fileName); 46 | // Read next sector 47 | bool ReadNextSector(); 48 | unsigned long GetSectorAddress() const { return startAddr; } 49 | unsigned long GetSectorNumBytes() const { return numBytes; } 50 | const unsigned char *GetSectorData() const { return curSector; } 51 | // Compare current sector to specified data 52 | bool VerifySector(const unsigned char *data, unsigned long len) const; 53 | // Seek back to beginning of file 54 | void Rewind(); 55 | void CloseFile(); 56 | 57 | // Support for writing MCS file 58 | static void WriteSectorHeader(std::ofstream &file, unsigned int i); 59 | static void WriteDataLine(std::ofstream &file, unsigned long addr, unsigned char *bytes, unsigned int numBytes); 60 | static void WriteEOF(std::ofstream &file); 61 | }; 62 | 63 | #endif // _MCS_H 64 | -------------------------------------------------------------------------------- /lib/code/PortFactory.cpp: -------------------------------------------------------------------------------- 1 | /* -*- Mode: C++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */ 2 | /* ex: set filetype=cpp softtabstop=4 shiftwidth=4 tabstop=4 cindent expandtab: */ 3 | 4 | /* 5 | Author(s): Anton Deguet 6 | 7 | (C) Copyright 2014-2023 Johns Hopkins University (JHU), All Rights Reserved. 8 | 9 | --- begin cisst license - do not edit --- 10 | 11 | This software is provided "as is" under an open source license, with 12 | no warranty. The complete license can be found in license.txt and 13 | http://www.cisst.org/cisst/license.txt. 14 | 15 | --- end cisst license --- 16 | */ 17 | 18 | #include "PortFactory.h" 19 | 20 | #include 21 | #if Amp1394_HAS_RAW1394 22 | #include "FirewirePort.h" 23 | #endif 24 | #if Amp1394_HAS_PCAP 25 | #include "EthRawPort.h" 26 | #endif 27 | #if Amp1394_HAS_EMIO 28 | #include "ZynqEmioPort.h" 29 | #endif 30 | #include "EthUdpPort.h" 31 | 32 | BasePort * PortFactory(const char * args, std::ostream & debugStream) 33 | { 34 | BasePort * port = 0; 35 | int portNumber = 0; 36 | std::string IPaddr = ETH_UDP_DEFAULT_IP; 37 | bool fwBridge = false; 38 | 39 | BasePort::PortType portType = BasePort::DefaultPortType(); 40 | 41 | if (!BasePort::ParseOptions(args, portType, portNumber, IPaddr, fwBridge)) { 42 | debugStream << "PortFactory: Failed to parse option: " << args << std::endl; 43 | return port; 44 | } 45 | 46 | switch (portType) { 47 | 48 | case BasePort::PORT_FIREWIRE: 49 | #if Amp1394_HAS_RAW1394 50 | port = new FirewirePort(portNumber, debugStream); 51 | #else 52 | debugStream << "PortFactory: FireWire not available (set Amp1394_HAS_RAW1394 in CMake)" << std::endl; 53 | #endif 54 | break; 55 | 56 | case BasePort::PORT_ETH_UDP: 57 | port = new EthUdpPort(portNumber, IPaddr, fwBridge, debugStream); 58 | break; 59 | 60 | case BasePort::PORT_ETH_RAW: 61 | #if Amp1394_HAS_PCAP 62 | port = new EthRawPort(portNumber, fwBridge, debugStream); 63 | #else 64 | debugStream << "PortFactory: Raw Ethernet not available (set Amp1394_HAS_PCAP in CMake)" << std::endl; 65 | #endif 66 | break; 67 | 68 | case BasePort::PORT_ZYNQ_EMIO: 69 | #if Amp1394_HAS_EMIO 70 | port = new ZynqEmioPort(portNumber, debugStream); 71 | #else 72 | debugStream << "PortFactory: Zynq EMIO not available" << std::endl; 73 | #endif 74 | break; 75 | 76 | default: 77 | debugStream << "PortFactory: Unsupported port type" << std::endl; 78 | break; 79 | } 80 | 81 | return port; 82 | } 83 | -------------------------------------------------------------------------------- /lib/code/Amp1394Time.cpp: -------------------------------------------------------------------------------- 1 | /* -*- Mode: C++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */ 2 | /* ex: set filetype=cpp softtabstop=4 shiftwidth=4 tabstop=4 cindent expandtab: */ 3 | 4 | /* 5 | (C) Copyright 2016 Johns Hopkins University (JHU), All Rights Reserved. 6 | 7 | --- begin cisst license - do not edit --- 8 | 9 | This software is provided "as is" under an open source license, with 10 | no warranty. The complete license can be found in license.txt and 11 | http://www.cisst.org/cisst/license.txt. 12 | 13 | --- end cisst license --- 14 | */ 15 | 16 | #include "Amp1394Time.h" 17 | 18 | #include 19 | 20 | #ifdef _MSC_VER // Windows 21 | #include 22 | #else // Linux, OS X, Solaris 23 | #include 24 | #include 25 | #endif 26 | 27 | // See osaGetTime.cpp (cisstOSAbstraction) if support for other platforms needed. 28 | 29 | double Amp1394_GetTime(void) 30 | { 31 | #ifdef _MSC_VER 32 | LARGE_INTEGER liTimerFrequency, liTimeNow; 33 | double timerFrequency, time; 34 | // According to MSDN, these functions are guaranteed to work 35 | // on Windows XP or later. 36 | if ((QueryPerformanceCounter(&liTimeNow) == 0) || 37 | (QueryPerformanceFrequency(&liTimerFrequency) == 0)) { 38 | // No performance counter available 39 | return 0.0; 40 | } 41 | timerFrequency = (double)liTimerFrequency.QuadPart; 42 | // Also, the frequency is guaranteed to be non-zero on Windows XP or later. 43 | if (timerFrequency == 0.0) return 0.0; 44 | time = (double)liTimeNow.QuadPart/timerFrequency; 45 | return time; 46 | #else 47 | struct timeval currentTime; 48 | gettimeofday(¤tTime, NULL); 49 | return ((double) currentTime.tv_sec) + ((double)currentTime.tv_usec) * 1e-6; 50 | #endif 51 | } 52 | 53 | // See osaSleep.cpp (cisstOSAbstraction) if support for other platforms needed. 54 | 55 | void Amp1394_Sleep(double sec) 56 | { 57 | #ifdef _MSC_VER 58 | // A waitable timer seems to be better than the Windows Sleep(). 59 | HANDLE WaitTimer; 60 | LARGE_INTEGER dueTime; 61 | sec *= -10.0 * 1000.0 * 1000.0; 62 | dueTime.QuadPart = static_cast(sec); //dueTime is in 100ns 63 | // We don't name the timer (third parameter) because CreateWaitableTimer will fail if the name 64 | // matches an existing name (e.g., if two threads call osaSleep). 65 | WaitTimer = CreateWaitableTimer(NULL, true, NULL); 66 | SetWaitableTimer(WaitTimer, &dueTime, 0, NULL, NULL, 0); 67 | WaitForSingleObject(WaitTimer, INFINITE); 68 | CloseHandle(WaitTimer); 69 | #else 70 | const long nSecInSec = 1000L * 1000L * 1000L; 71 | struct timespec ts; 72 | ts.tv_sec = static_cast (sec); 73 | ts.tv_nsec = static_cast ( (sec-ts.tv_sec) * nSecInSec ); 74 | nanosleep(&ts, NULL); 75 | #endif 76 | } 77 | -------------------------------------------------------------------------------- /util/time1394.c: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * 3 | * This is a minimal Linux command line program that displays timing information 4 | * for reading/writing blocks of data over IEEE-1394. 5 | * 6 | * Compile: gcc -Wall -lraw1394 -o 7 | * Usage: 8 | * 9 | ******************************************************************************/ 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #define N 1024 18 | 19 | raw1394handle_t handle; 20 | 21 | void signal_handler(int sig) { 22 | signal(SIGINT, SIG_DFL); 23 | raw1394_destroy_handle(handle); 24 | exit(0); 25 | } 26 | 27 | double gettime_us(struct timeval* tv) { 28 | gettimeofday(tv, 0); 29 | return (double)tv->tv_usec + (double)1e6 * (double)tv->tv_sec; 30 | } 31 | 32 | int main(int argc, char** argv) 33 | { 34 | struct raw1394_portinfo ports[4]; 35 | int nports; 36 | int i, rc; 37 | double tr, tw; 38 | quadlet_t data[N]; 39 | struct timeval tv; 40 | 41 | if (argc != 3) { 42 | printf("Usage: %s interval repetitions\n", argv[0]); 43 | return 0; 44 | } 45 | 46 | int sz_interval = strtoul(argv[1], 0, 10); 47 | int sz_q, reps; 48 | int num_reps = strtoul(argv[2], 0, 10);; 49 | 50 | signal(SIGINT, signal_handler); 51 | if (!(handle=raw1394_new_handle())) exit(4); 52 | nports = raw1394_get_port_info(handle, ports, 4); 53 | if (nports < 0) { 54 | fprintf(stderr, "**** Error: could not get port info\n"); 55 | exit(-1); 56 | } 57 | 58 | for (i=0; iGetFirmwareVersion(BoardId) : 0); 28 | } 29 | 30 | unsigned int BoardIO::GetFpgaVersionMajor(void) const 31 | { 32 | return (port ? port->GetFpgaVersionMajor(BoardId) : 0); 33 | } 34 | 35 | unsigned int BoardIO::GetFpgaVersionMajorFromStatus(uint32_t status) 36 | { 37 | // FPGA V1: all bits are 0 38 | // FPGA V2: bit 31 is 1 39 | // FPGA V3: bits[31:30] are 01 40 | unsigned int fpga_ver = 0; 41 | if (status == 0) 42 | fpga_ver = 1; 43 | else if (status&0x80000000) 44 | fpga_ver = 2; 45 | else if ((status&0x40000000) == 0x40000000) 46 | fpga_ver = 3; 47 | 48 | return fpga_ver; 49 | } 50 | 51 | uint32_t BoardIO::GetHardwareVersion(void) const 52 | { 53 | return (port ? port->GetHardwareVersion(BoardId) : 0); 54 | } 55 | 56 | std::string BoardIO::GetHardwareVersionString(void) const 57 | { 58 | return (port ? port->GetHardwareVersionString(BoardId) : ""); 59 | } 60 | 61 | /******************************************************************************* 62 | * Read commands 63 | */ 64 | 65 | uint32_t BoardIO::ReadStatus(void) const 66 | { 67 | uint32_t read_data = 0; 68 | if (port) port->ReadQuadlet(BoardId, BoardIO::BOARD_STATUS, read_data); 69 | return read_data; 70 | } 71 | 72 | uint32_t BoardIO::ReadIPv4Address(void) const 73 | { 74 | if (GetFirmwareVersion() < 7) { 75 | std::cerr << "AmpIO::ReadIPv4Address: requires firmware 7 or above" << std::endl; 76 | return 0; 77 | } 78 | uint32_t read_data = 0; 79 | if (port) 80 | port->ReadQuadlet(BoardId, BoardIO::IP_ADDR, read_data); 81 | return read_data; 82 | } 83 | 84 | /******************************************************************************* 85 | * Write commands 86 | */ 87 | 88 | bool BoardIO::WriteIPv4Address(uint32_t IPaddr) 89 | { 90 | if (GetFirmwareVersion() < 7) { 91 | std::cerr << "BoardIO::WriteIPv4Address: requires firmware 7 or above" << std::endl; 92 | return false; 93 | } 94 | return (port ? port->WriteQuadlet(BoardId, BoardIO::IP_ADDR, IPaddr) : false); 95 | } 96 | -------------------------------------------------------------------------------- /tests/qlacommand.cpp: -------------------------------------------------------------------------------- 1 | /* -*- Mode: C++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */ 2 | /* ex: set filetype=cpp softtabstop=4 shiftwidth=4 tabstop=4 cindent expandtab: */ 3 | 4 | /* 5 | Author(s): Anton Deguet, Zihan Chen 6 | Created on: 2015 (from qlacloserelays) 7 | 8 | (C) Copyright 2015-2023 Johns Hopkins University (JHU), All Rights Reserved. 9 | 10 | --- begin cisst license - do not edit --- 11 | 12 | This software is provided "as is" under an open source license, with 13 | no warranty. The complete license can be found in license.txt and 14 | http://www.cisst.org/cisst/license.txt. 15 | 16 | --- end cisst license --- 17 | */ 18 | 19 | #include 20 | #include 21 | 22 | #include 23 | #include 24 | #include "Amp1394Time.h" 25 | 26 | int main(int argc, char** argv) 27 | { 28 | unsigned int i; 29 | unsigned int nbArgs = static_cast(argc); 30 | BasePort * port = 0; 31 | std::string portArgs; 32 | std::string command; 33 | 34 | for (i = 1; i < nbArgs; i++) { 35 | if ((argv[i][0] == '-') && (argv[i][1] == 'p')) { 36 | portArgs = argv[i] + 2; 37 | } 38 | if ((argv[i][0] == '-') && (argv[i][1] == 'c')) { 39 | // make sure we have an extra argument 40 | if ((i + 1) >= nbArgs) { 41 | std::cerr << "Missing argument after -c" << std::endl; 42 | return -1; 43 | } 44 | command = argv[i + 1]; 45 | } 46 | } 47 | 48 | // check that command is supported before creating anything 49 | if (! ((command == "open-relays") 50 | || (command == "close-relays") 51 | || (command == "reboot") 52 | || (command == "reset-eth") 53 | || (command == "reset-encoder-preload") 54 | )) { 55 | std::cerr << "Invalid command \"" << command 56 | << "\". Supported commands: `qlacommand -c {open-relays,close-relays,reboot,reset-eth,reset-encoder-preload}`" << std::endl 57 | << "Port can be specified using `-p`: -pupd, -pupd:X.X.X.X, -pfw, -pfw:X" << std::endl; 58 | return -1; 59 | } 60 | 61 | // if port is not specified 62 | port = PortFactory(portArgs.c_str()); 63 | if (!port) { 64 | std::cerr << "Failed to create port using: " << portArgs << std::endl; 65 | return -1; 66 | } 67 | 68 | std::cout << "Executing command \"" << command << "\" on " << port->GetNumOfNodes() << " nodes" << std::endl; 69 | if (command == "open-relays") { 70 | AmpIO::WriteSafetyRelayAll(port, false); 71 | } else if (command == "close-relays") { 72 | AmpIO::WriteSafetyRelayAll(port, true); 73 | } else if (command == "reboot") { 74 | AmpIO::WriteRebootAll(port); 75 | } else if (command == "reset-eth") { 76 | AmpIO::ResetEthernetAll(port); 77 | } else if (command == "reset-encoder-preload") { 78 | AmpIO::WriteEncoderPreloadAll(port, 0); 79 | } 80 | 81 | delete port; 82 | 83 | return 0; 84 | } 85 | -------------------------------------------------------------------------------- /lib/EthRawPort.h: -------------------------------------------------------------------------------- 1 | /* -*- Mode: C++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */ 2 | /* ex: set filetype=cpp softtabstop=4 shiftwidth=4 tabstop=4 cindent expandtab: */ 3 | 4 | /* 5 | Author(s): Zihan Chen, Peter Kazanzides 6 | 7 | (C) Copyright 2014-2024 Johns Hopkins University (JHU), All Rights Reserved. 8 | 9 | --- begin cisst license - do not edit --- 10 | 11 | This software is provided "as is" under an open source license, with 12 | no warranty. The complete license can be found in license.txt and 13 | http://www.cisst.org/cisst/license.txt. 14 | 15 | --- end cisst license --- 16 | */ 17 | 18 | 19 | #ifndef __EthRawPort_H__ 20 | #define __EthRawPort_H__ 21 | 22 | #include "EthBasePort.h" 23 | 24 | // Forward declaration 25 | struct pcap; 26 | typedef struct pcap pcap_t; 27 | 28 | const unsigned int ETH_FRAME_HEADER_SIZE = 14; // dest addr (6), src addr (6), length (2) 29 | const unsigned int ETH_FRAME_LENGTH_OFFSET = 12; // offset to length 30 | 31 | //const unsigned int ETH_RAW_FRAME_MAX_SIZE = 1500; // maximum raw Ethernet frame size 32 | const unsigned int ETH_RAW_FRAME_MAX_SIZE = 1024; // Temporary firmware limit 33 | 34 | class EthRawPort : public EthBasePort 35 | { 36 | protected: 37 | 38 | pcap_t *handle; 39 | uint8_t frame_hdr[ETH_FRAME_HEADER_SIZE]; 40 | 41 | bool headercheck(const unsigned char *header, bool toPC) const; 42 | 43 | // Make Ethernet header 44 | void make_ethernet_header(unsigned char *packet, unsigned int numBytes, nodeid_t node, unsigned char flags); 45 | 46 | // Check Ethernet header 47 | bool CheckEthernetHeader(const unsigned char *packet, bool useEthernetBroadcast); 48 | 49 | //! Initialize EthRaw port 50 | bool Init(void); 51 | 52 | //! Cleanup EthRaw port 53 | void Cleanup(void); 54 | 55 | //! Initialize nodes on the bus; called by ScanNodes 56 | // \return Maximum number of nodes on bus (0 if error) 57 | nodeid_t InitNodes(void); 58 | 59 | // Send packet via PCAP 60 | bool PacketSend(nodeid_t node, unsigned char *packet, size_t nbytes, bool useEthernetBroadcast); 61 | 62 | // Receive packet via PCAP 63 | int PacketReceive(unsigned char *packet, size_t nbytes); 64 | 65 | // Flush all packets in receive buffer 66 | int PacketFlushAll(void); 67 | 68 | public: 69 | EthRawPort(int portNum, bool forceFwBridge = false, 70 | std::ostream &debugStream = std::cerr, EthCallbackType cb = 0); 71 | 72 | ~EthRawPort(); 73 | 74 | //****************** BasePort virtual methods *********************** 75 | 76 | PortType GetPortType(void) const { return PORT_ETH_RAW; } 77 | 78 | bool IsOK(void); 79 | 80 | unsigned int GetPrefixOffset(MsgType msg) const; 81 | unsigned int GetWritePostfixSize(void) const 82 | { return FW_CRC_SIZE; } 83 | unsigned int GetReadPostfixSize(void) const 84 | { return (FW_CRC_SIZE+FW_EXTRA_SIZE); } 85 | 86 | unsigned int GetWriteQuadAlign(void) const 87 | { return ((ETH_FRAME_HEADER_SIZE+FW_CTRL_SIZE)%sizeof(quadlet_t)); } 88 | unsigned int GetReadQuadAlign(void) const 89 | { return (ETH_FRAME_HEADER_SIZE%sizeof(quadlet_t)); } 90 | 91 | // Get the maximum number of data bytes that can be read 92 | // (via ReadBlock) or written (via WriteBlock). 93 | unsigned int GetMaxReadDataSize(void) const; 94 | unsigned int GetMaxWriteDataSize(void) const; 95 | }; 96 | 97 | #endif // __EthRawPort_H__ 98 | -------------------------------------------------------------------------------- /.github/workflows/cmake-multi-platform.yml: -------------------------------------------------------------------------------- 1 | # This starter workflow is for a CMake project running on multiple platforms. There is a different starter workflow if you just want a single platform. 2 | # See: https://github.com/actions/starter-workflows/blob/main/ci/cmake-single-platform.yml 3 | name: CMake on multiple platforms 4 | 5 | on: 6 | push: 7 | branches: [ "devel" ] 8 | pull_request: 9 | branches: [ "devel" ] 10 | 11 | jobs: 12 | 13 | build: 14 | runs-on: ${{ matrix.os }} 15 | 16 | strategy: 17 | # Set fail-fast to false to ensure that feedback is delivered for all matrix combinations. Consider changing this to true when your workflow is stable. 18 | fail-fast: true 19 | 20 | # Set up a matrix to run the following 3 configurations: 21 | # 1. 22 | # 2. 23 | # 3. 24 | # 25 | # To add more build types (Release, Debug, RelWithDebInfo, etc.) customize the build_type list. 26 | matrix: 27 | os: [ubuntu-latest, windows-latest] 28 | build_type: [Release, Debug] 29 | c_compiler: [gcc, clang, cl] 30 | include: 31 | - os: windows-latest 32 | c_compiler: cl 33 | cpp_compiler: cl 34 | - os: ubuntu-latest 35 | c_compiler: gcc 36 | cpp_compiler: g++ 37 | - os: ubuntu-latest 38 | c_compiler: clang 39 | cpp_compiler: clang++ 40 | exclude: 41 | - os: windows-latest 42 | c_compiler: gcc 43 | - os: windows-latest 44 | c_compiler: clang 45 | - os: ubuntu-latest 46 | c_compiler: cl 47 | 48 | steps: 49 | - uses: actions/checkout@v3 50 | 51 | - name: Set reusable strings 52 | # Turn repeated input strings (such as the build output directory) into step outputs. These step outputs can be used throughout the workflow file. 53 | id: strings 54 | shell: bash 55 | run: | 56 | echo "build-output-dir=${{ github.workspace }}/build" >> "$GITHUB_OUTPUT" 57 | 58 | - name: Install dependencies 59 | shell: bash 60 | run: | 61 | if [ "$RUNNER_OS" == "Linux" ]; then 62 | sudo apt install libraw1394-dev libncurses5-dev 63 | fi 64 | 65 | - name: Configure CMake 66 | # Configure CMake in a 'build' subdirectory. `CMAKE_BUILD_TYPE` is only required if you are using a single-configuration generator such as make. 67 | # See https://cmake.org/cmake/help/latest/variable/CMAKE_BUILD_TYPE.html?highlight=cmake_build_type 68 | run: > 69 | cmake -B ${{ steps.strings.outputs.build-output-dir }} 70 | -DCMAKE_CXX_COMPILER=${{ matrix.cpp_compiler }} 71 | -DCMAKE_C_COMPILER=${{ matrix.c_compiler }} 72 | -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} 73 | -S ${{ github.workspace }} 74 | 75 | - name: Build 76 | # Build your program with the given configuration. Note that --config is needed because the default Windows generator is a multi-config generator (Visual Studio generator). 77 | run: cmake --build ${{ steps.strings.outputs.build-output-dir }} --config ${{ matrix.build_type }} 78 | 79 | -------------------------------------------------------------------------------- /tests/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # 2 | # (C) Copyright 2011-2024 Johns Hopkins University (JHU), All Rights Reserved. 3 | # 4 | # --- begin cisst license - do not edit --- 5 | # 6 | # This software is provided "as is" under an open source license, with 7 | # no warranty. The complete license can be found in license.txt and 8 | # http://www.cisst.org/cisst/license.txt. 9 | # 10 | # --- end cisst license --- 11 | 12 | project(Amp1394Tests VERSION 2.1.0) 13 | 14 | include_directories(${Amp1394_INCLUDE_DIR}) 15 | link_directories(${Amp1394_LIBRARY_DIR} ${Amp1394_EXTRA_LIBRARY_DIR}) 16 | 17 | add_executable(qladisp qladisp.cpp) 18 | target_link_libraries(qladisp ${Amp1394_LIBRARIES} ${Amp1394_EXTRA_LIBRARIES} 19 | ${Amp1394Console_LIBRARIES} ${Amp1394Console_EXTRA_LIBRARIES}) 20 | 21 | add_executable(qlatest qlatest.cpp) 22 | target_link_libraries(qlatest ${Amp1394_LIBRARIES} ${Amp1394_EXTRA_LIBRARIES} 23 | ${Amp1394Console_LIBRARIES} ${Amp1394Console_EXTRA_LIBRARIES}) 24 | 25 | install (TARGETS qladisp qlatest 26 | COMPONENT Amp1394-utils 27 | RUNTIME DESTINATION bin) 28 | 29 | if (Amp1394_HAS_RAW1394) 30 | add_executable(fwPortTest fwPortTest.cpp) 31 | target_link_libraries(fwPortTest ${Amp1394_LIBRARIES} ${Amp1394_EXTRA_LIBRARIES}) 32 | 33 | install (TARGETS fwPortTest 34 | COMPONENT Amp1394-utils 35 | RUNTIME DESTINATION bin) 36 | endif (Amp1394_HAS_RAW1394) 37 | 38 | add_executable(qlacloserelays qlacloserelays.cpp) 39 | target_link_libraries(qlacloserelays ${Amp1394_LIBRARIES} ${Amp1394_EXTRA_LIBRARIES}) 40 | 41 | add_executable(qlacommand qlacommand.cpp) 42 | target_link_libraries(qlacommand ${Amp1394_LIBRARIES} ${Amp1394_EXTRA_LIBRARIES}) 43 | 44 | add_executable(fpgatest fpgatest.cpp MotorVoltage.h MotorVoltage.cpp) 45 | target_link_libraries(fpgatest ${Amp1394_LIBRARIES} ${Amp1394_EXTRA_LIBRARIES}) 46 | 47 | add_executable(instrument instrument.cpp) 48 | target_link_libraries(instrument ${Amp1394_LIBRARIES} ${Amp1394_EXTRA_LIBRARIES}) 49 | 50 | add_executable (block1394eth block1394eth.cpp) 51 | target_link_libraries (block1394eth ${Amp1394_LIBRARIES} ${Amp1394_EXTRA_LIBRARIES}) 52 | 53 | add_custom_command (TARGET block1394eth POST_BUILD 54 | COMMAND ${CMAKE_COMMAND} -E copy 55 | ${EXECUTABLE_OUTPUT_PATH}/${CMAKE_CFG_INTDIR}/block1394eth${CMAKE_EXECUTABLE_SUFFIX} 56 | ${EXECUTABLE_OUTPUT_PATH}/${CMAKE_CFG_INTDIR}/quad1394eth${CMAKE_EXECUTABLE_SUFFIX} 57 | COMMENT "Generating quad1394eth") 58 | 59 | add_executable(enctest enctest.cpp) 60 | target_link_libraries (enctest ${Amp1394_LIBRARIES} ${Amp1394_EXTRA_LIBRARIES}) 61 | 62 | add_executable(ethswitch ethswitch.cpp) 63 | target_link_libraries(ethswitch ${Amp1394_LIBRARIES} ${Amp1394_EXTRA_LIBRARIES} 64 | ${Amp1394Console_LIBRARIES} ${Amp1394Console_EXTRA_LIBRARIES}) 65 | 66 | install (PROGRAMS ${EXECUTABLE_OUTPUT_PATH}/quad1394eth 67 | COMPONENT Amp1394-utils 68 | DESTINATION bin) 69 | 70 | install (TARGETS qlacloserelays qlacommand fpgatest instrument block1394eth enctest ethswitch 71 | COMPONENT Amp1394-utils 72 | RUNTIME DESTINATION bin) 73 | 74 | add_executable(dvrktest dvrktest.cpp MotorVoltage.h MotorVoltage.cpp) 75 | target_link_libraries(dvrktest ${Amp1394_LIBRARIES} ${Amp1394_EXTRA_LIBRARIES}) 76 | install (TARGETS dvrktest 77 | COMPONENT Amp1394-utils 78 | RUNTIME DESTINATION bin) 79 | -------------------------------------------------------------------------------- /lib/EthUdpPort.h: -------------------------------------------------------------------------------- 1 | /* -*- Mode: C++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */ 2 | /* ex: set filetype=cpp softtabstop=4 shiftwidth=4 tabstop=4 cindent expandtab: */ 3 | 4 | /* 5 | Author(s): Zihan Chen, Peter Kazanzides 6 | 7 | (C) Copyright 2014-2024 Johns Hopkins University (JHU), All Rights Reserved. 8 | 9 | --- begin cisst license - do not edit --- 10 | 11 | This software is provided "as is" under an open source license, with 12 | no warranty. The complete license can be found in license.txt and 13 | http://www.cisst.org/cisst/license.txt. 14 | 15 | --- end cisst license --- 16 | */ 17 | 18 | 19 | #ifndef __EthUdpPort_H__ 20 | #define __EthUdpPort_H__ 21 | 22 | #include 23 | #include "EthBasePort.h" 24 | 25 | struct SocketInternals; 26 | 27 | // Default MTU=1500 (does not count 18 bytes for Ethernet frame header and CRC) 28 | const unsigned int ETH_MTU_DEFAULT = 1500; 29 | // Size of IPv4 header (20) and UDP header (8), counts against MTU limit 30 | const unsigned int ETH_UDP_HEADER = 28; 31 | 32 | class EthUdpPort : public EthBasePort 33 | { 34 | protected: 35 | SocketInternals *sockPtr; // OS-specific internals 36 | std::string ServerIP; // IP address of server (string) 37 | std::string MulticastIP; // IP address for multicast (string) 38 | unsigned short UDP_port; // Port on server (FPGA) 39 | 40 | //! Initialize EthUdp port 41 | bool Init(void); 42 | 43 | //! Cleanup EthUdp port 44 | void Cleanup(void); 45 | 46 | //! Initialize nodes on the bus; called by ScanNodes 47 | // \return Maximum number of nodes on bus (0 if error) 48 | nodeid_t InitNodes(void); 49 | 50 | // Send packet via UDP 51 | bool PacketSend(nodeid_t node, unsigned char *packet, size_t nbytes, bool useEthernetBroadcast); 52 | 53 | // Receive packet via UDP 54 | int PacketReceive(unsigned char *packet, size_t nbytes); 55 | 56 | // Flush all packets in receive buffer 57 | int PacketFlushAll(void); 58 | 59 | public: 60 | 61 | EthUdpPort(int portNum, const std::string &serverIP = ETH_UDP_DEFAULT_IP, 62 | bool forceFwBridge = false, 63 | std::ostream &debugStream = std::cerr, EthCallbackType cb = 0); 64 | 65 | ~EthUdpPort(); 66 | 67 | std::string GetMultiCastIP() const { return MulticastIP; } 68 | bool SetMulticastIP(const std::string &multicast); 69 | 70 | //****************** BasePort virtual methods *********************** 71 | 72 | PortType GetPortType(void) const { return PORT_ETH_UDP; } 73 | 74 | bool IsOK(void); 75 | 76 | unsigned int GetPrefixOffset(MsgType msg) const; 77 | unsigned int GetWritePostfixSize(void) const { return FW_CRC_SIZE; } 78 | unsigned int GetReadPostfixSize(void) const { return (FW_CRC_SIZE+FW_EXTRA_SIZE); } 79 | 80 | unsigned int GetWriteQuadAlign(void) const { return (FW_CTRL_SIZE%sizeof(quadlet_t)); } 81 | unsigned int GetReadQuadAlign(void) const { return 0; } 82 | 83 | // Get the maximum number of data bytes that can be read 84 | // (via ReadBlock) or written (via WriteBlock). 85 | unsigned int GetMaxReadDataSize(void) const; 86 | unsigned int GetMaxWriteDataSize(void) const; 87 | 88 | //****************** Static methods *************************** 89 | 90 | // Convert IP address from uint32_t to string 91 | static std::string IP_String(uint32_t IPaddr); 92 | 93 | // Convert IP address from string to uint32_t 94 | static uint32_t IP_ULong(const std::string &IPaddr); 95 | }; 96 | 97 | #endif // __EthUdpPort_H__ 98 | -------------------------------------------------------------------------------- /lib/code/ZynqEmioPort.cpp: -------------------------------------------------------------------------------- 1 | /* -*- Mode: C++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */ 2 | /* ex: set filetype=cpp softtabstop=4 shiftwidth=4 tabstop=4 cindent expandtab: */ 3 | 4 | /* 5 | Author(s): Peter Kazanzides 6 | 7 | (C) Copyright 2023-2024 Johns Hopkins University (JHU), All Rights Reserved. 8 | 9 | --- begin cisst license - do not edit --- 10 | 11 | This software is provided "as is" under an open source license, with 12 | no warranty. The complete license can be found in license.txt and 13 | http://www.cisst.org/cisst/license.txt. 14 | 15 | --- end cisst license --- 16 | */ 17 | 18 | #include "ZynqEmioPort.h" 19 | #include "Amp1394Time.h" 20 | #include "fpgav3_emio_mmap.h" 21 | #include "fpgav3_emio_gpiod.h" 22 | #include "fpgav3_lib.h" 23 | 24 | ZynqEmioPort::ZynqEmioPort(int portNum, std::ostream &debugStream): 25 | BasePort(portNum, debugStream), emio(0) 26 | { 27 | Init(); 28 | } 29 | 30 | bool ZynqEmioPort::Init(void) 31 | { 32 | memset(Node2Board, BoardIO::MAX_BOARDS, sizeof(Node2Board)); 33 | 34 | // Initialize EMIO interface to FPGA 35 | if (PortNum == 1) { 36 | outStr << "ZynqEmioPort: using EMIO gpiod interface" << std::endl; 37 | emio = new EMIO_Interface_Gpiod; 38 | } 39 | else { 40 | outStr << "ZynqEmioPort: using EMIO mmap interface" << std::endl; 41 | emio = new EMIO_Interface_Mmap; 42 | } 43 | // Print version info for this program and (if different) libfgpav3 44 | print_fpgav3_versions(outStr); 45 | 46 | bool ret = ScanNodes(); 47 | if (ret) 48 | SetDefaultProtocol(); 49 | return ret; 50 | } 51 | 52 | ZynqEmioPort::~ZynqEmioPort() 53 | { 54 | Cleanup(); 55 | } 56 | 57 | void ZynqEmioPort::Cleanup(void) 58 | { 59 | delete emio; 60 | emio = 0; 61 | } 62 | 63 | unsigned int ZynqEmioPort::GetBusGeneration(void) const 64 | { 65 | return FwBusGeneration; 66 | } 67 | 68 | void ZynqEmioPort::UpdateBusGeneration(unsigned int gen) 69 | { 70 | FwBusGeneration = gen; 71 | } 72 | 73 | nodeid_t ZynqEmioPort::InitNodes(void) 74 | { 75 | return 1; 76 | } 77 | 78 | bool ZynqEmioPort::AddBoard(BoardIO *board) 79 | { 80 | bool ret = BasePort::AddBoard(board); 81 | if (ret) { 82 | unsigned int id = board->GetBoardId(); 83 | HubBoard = id; // last added board would be hub board 84 | } 85 | return ret; 86 | } 87 | 88 | bool ZynqEmioPort::RemoveBoard(unsigned char boardId) 89 | { 90 | return BasePort::RemoveBoard(boardId); 91 | } 92 | 93 | bool ZynqEmioPort::WriteBroadcastOutput(quadlet_t *buffer, unsigned int size) 94 | { 95 | return WriteBlockNode(FW_NODE_BROADCAST, 0, buffer, size); 96 | } 97 | 98 | void ZynqEmioPort::PromDelay(void) const 99 | { 100 | // Wait 5 msec 101 | Amp1394_Sleep(0.005); 102 | } 103 | 104 | bool ZynqEmioPort::WriteBroadcastReadRequest(unsigned int seq) 105 | { 106 | quadlet_t bcReqData = (seq << 16) | BoardInUseMask_; 107 | return WriteQuadlet(FW_NODE_BROADCAST, 0x1800, bcReqData); 108 | } 109 | 110 | // Node number should always be 0 (or broadcast) for EMIO 111 | bool ZynqEmioPort::CheckNodeId(nodeid_t node) const 112 | { 113 | return ((node == 0) || (node == FW_NODE_BROADCAST)); 114 | } 115 | 116 | bool ZynqEmioPort::ReadQuadletNode(nodeid_t node, nodeaddr_t addr, quadlet_t &data, unsigned char) 117 | { 118 | if (!CheckNodeId(node)) { 119 | outStr << "ReadQuadletNode: invalid node " << node << std::endl; 120 | return false; 121 | } 122 | 123 | return emio->ReadQuadlet(addr, data); 124 | } 125 | 126 | bool ZynqEmioPort::WriteQuadletNode(nodeid_t node, nodeaddr_t addr, quadlet_t data, unsigned char) 127 | { 128 | if (!CheckNodeId(node)) { 129 | outStr << "WriteQuadletNode: invalid node " << node << std::endl; 130 | return false; 131 | } 132 | 133 | return emio->WriteQuadlet(addr, data); 134 | } 135 | 136 | bool ZynqEmioPort::ReadBlockNode(nodeid_t node, nodeaddr_t addr, quadlet_t *rdata, 137 | unsigned int nbytes, unsigned char) 138 | { 139 | if (!CheckNodeId(node)) { 140 | outStr << "ReadBlockNode: invalid node " << node << std::endl; 141 | return false; 142 | } 143 | 144 | rtRead = true; // for debugging 145 | return emio->ReadBlock(addr, rdata, nbytes); 146 | } 147 | 148 | bool ZynqEmioPort::WriteBlockNode(nodeid_t node, nodeaddr_t addr, quadlet_t *wdata, 149 | unsigned int nbytes, unsigned char) 150 | { 151 | if (!CheckNodeId(node)) { 152 | outStr << "WriteBlockNode: invalid node " << node << std::endl; 153 | return false; 154 | } 155 | 156 | rtWrite = true; // for debugging 157 | return emio->WriteBlock(addr, wdata, nbytes); 158 | } 159 | -------------------------------------------------------------------------------- /lib/ZynqEmioPort.h: -------------------------------------------------------------------------------- 1 | /* -*- Mode: C++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */ 2 | /* ex: set filetype=cpp softtabstop=4 shiftwidth=4 tabstop=4 cindent expandtab: */ 3 | 4 | /* 5 | Author(s): Peter Kazanzides 6 | 7 | (C) Copyright 2023-2024 Johns Hopkins University (JHU), All Rights Reserved. 8 | 9 | --- begin cisst license - do not edit --- 10 | 11 | This software is provided "as is" under an open source license, with 12 | no warranty. The complete license can be found in license.txt and 13 | http://www.cisst.org/cisst/license.txt. 14 | 15 | --- end cisst license --- 16 | */ 17 | 18 | #ifndef __ZynqEmioPort_H__ 19 | #define __ZynqEmioPort_H__ 20 | 21 | #include 22 | #include "BoardIO.h" 23 | #include "BasePort.h" 24 | #include "fpgav3_emio.h" 25 | 26 | class ZynqEmioPort : public BasePort { 27 | public: 28 | protected: 29 | 30 | EMIO_Interface *emio; 31 | 32 | // Internal method to check whether node id is valid 33 | // (should be 0 or FW_NODE_BROADCAST) 34 | bool CheckNodeId(nodeid_t node) const; 35 | 36 | //! Read quadlet from node (internal method called by ReadQuadlet). 37 | // Parameter "flags" is not used for EMIO 38 | bool ReadQuadletNode(nodeid_t node, nodeaddr_t addr, quadlet_t &data, unsigned char flags = 0); 39 | 40 | //! Write quadlet to node (internal method called by WriteQuadlet) 41 | // Parameter "flags" is not used for EMIO 42 | bool WriteQuadletNode(nodeid_t node, nodeaddr_t addr, quadlet_t data, unsigned char flags = 0); 43 | 44 | // Initialize EMIO port 45 | bool Init(void); 46 | 47 | // Cleanup EMIO port 48 | void Cleanup(void); 49 | 50 | // Initialize nodes on the bus; called by ScanNodes 51 | // \return Maximum number of nodes on bus (0 if error) 52 | nodeid_t InitNodes(void); 53 | 54 | // Write the broadcast packet containing the DAC values and power control 55 | bool WriteBroadcastOutput(quadlet_t *buffer, unsigned int size); 56 | 57 | // Write a block to the specified node. Internal method called by WriteBlock and 58 | // WriteAllBoardsBroadcast. 59 | // Parameter "flags" is not used for EMIO. 60 | bool WriteBlockNode(nodeid_t node, nodeaddr_t addr, quadlet_t *wdata, 61 | unsigned int nbytes, unsigned char flags = 0); 62 | 63 | // Read a block from the specified node. Internal method called by ReadBlock. 64 | // Parameter "flags" is not used for EMIO. 65 | bool ReadBlockNode(nodeid_t node, nodeaddr_t addr, quadlet_t *rdata, 66 | unsigned int nbytes, unsigned char flags = 0); 67 | 68 | public: 69 | // Initialize Zynq EMIO port 70 | // 71 | // There are two available interfaces, one using mmap for direct register access, 72 | // and one using the gpiod driver. The mmap interface is much faster and therefore 73 | // is the default. To use the gpiod interface, set portNum to 1. 74 | ZynqEmioPort(int portNum = 0, std::ostream &debugStream = std::cerr); 75 | ~ZynqEmioPort(); 76 | 77 | // Get/set EMIO timeout in microseconds 78 | double GetTimeout_us(void) const { return emio->GetTimeout_us(); } 79 | void SetTimeout_us(double time_uSec) { emio->SetTimeout_us(time_uSec); } 80 | 81 | // Get/set EMIO verbose flag 82 | bool GetVerbose() const { return emio->GetVerbose(); } 83 | void SetVerbose(bool newState) { emio->SetVerbose(newState); } 84 | 85 | //****************** BasePort pure virtual methods *********************** 86 | 87 | PortType GetPortType(void) const { return PORT_ZYNQ_EMIO; } 88 | 89 | int NumberOfUsers(void) { return 1; } 90 | 91 | bool IsOK(void) { return emio->IsOK(); } 92 | 93 | unsigned int GetBusGeneration(void) const; 94 | 95 | void UpdateBusGeneration(unsigned int gen); 96 | 97 | unsigned int GetPrefixOffset(MsgType) const { return 0; } 98 | unsigned int GetWritePostfixSize(void) const { return 0; } 99 | unsigned int GetReadPrefixSize(void) const { return 0; } 100 | unsigned int GetReadPostfixSize(void) const { return 0; } 101 | 102 | unsigned int GetWriteQuadAlign(void) const { return 0; } 103 | unsigned int GetReadQuadAlign(void) const { return 0; } 104 | 105 | // Get the maximum number of data bytes that can be read 106 | // (via ReadBlock) or written (via WriteBlock). 107 | unsigned int GetMaxReadDataSize(void) const { return MAX_POSSIBLE_DATA_SIZE; } 108 | unsigned int GetMaxWriteDataSize(void) const { return MAX_POSSIBLE_DATA_SIZE; } 109 | 110 | // Adds board(s) 111 | bool AddBoard(BoardIO *board); 112 | 113 | // Removes board 114 | bool RemoveBoard(unsigned char boardId); 115 | 116 | /*! 117 | \brief Write the broadcast read request 118 | */ 119 | bool WriteBroadcastReadRequest(unsigned int seq); 120 | 121 | /*! 122 | \brief Wait for broadcast read data to be available 123 | */ 124 | void WaitBroadcastRead(void) {} 125 | 126 | /*! 127 | \brief Add delay (if needed) for PROM I/O operations 128 | */ 129 | void PromDelay(void) const; 130 | 131 | }; 132 | 133 | #endif // __ZynqEmioPort_H__ 134 | -------------------------------------------------------------------------------- /lib/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # 2 | # (C) Copyright 2011-2025 Johns Hopkins University (JHU), All Rights Reserved. 3 | # 4 | # --- begin cisst license - do not edit --- 5 | # 6 | # This software is provided "as is" under an open source license, with 7 | # no warranty. The complete license can be found in license.txt and 8 | # http://www.cisst.org/cisst/license.txt. 9 | # 10 | # --- end cisst license --- 11 | 12 | project (Amp1394Lib) 13 | 14 | set (HEADERS 15 | BoardIO.h 16 | FpgaIO.h 17 | AmpIO.h 18 | Amp1394Types.h 19 | Amp1394Time.h 20 | Amp1394BSwap.h 21 | EncoderVelocity.h 22 | BasePort.h 23 | EthBasePort.h 24 | EthUdpPort.h 25 | PortFactory.h) 26 | 27 | set (SOURCE_FILES 28 | code/BoardIO.cpp 29 | code/FpgaIO.cpp 30 | code/AmpIO.cpp 31 | code/Amp1394Time.cpp 32 | code/EncoderVelocity.cpp 33 | code/BasePort.cpp 34 | code/EthBasePort.cpp 35 | code/EthUdpPort.cpp 36 | code/PortFactory.cpp) 37 | 38 | 39 | if (Amp1394_HAS_RAW1394) 40 | set (HEADERS ${HEADERS} FirewirePort.h) 41 | set (SOURCE_FILES ${SOURCE_FILES} code/FirewirePort.cpp) 42 | endif (Amp1394_HAS_RAW1394) 43 | 44 | if (Amp1394_HAS_PCAP) 45 | set (HEADERS ${HEADERS} EthRawPort.h) 46 | set (SOURCE_FILES ${SOURCE_FILES} code/EthRawPort.cpp) 47 | endif (Amp1394_HAS_PCAP) 48 | 49 | if (Amp1394_HAS_EMIO) 50 | set (HEADERS ${HEADERS} ZynqEmioPort.h) 51 | set (SOURCE_FILES ${SOURCE_FILES} code/ZynqEmioPort.cpp) 52 | endif (Amp1394_HAS_EMIO) 53 | 54 | include_directories(${Amp1394_INCLUDE_DIR} ${Amp1394_EXTRA_INCLUDE_DIR}) 55 | link_directories(${Amp1394_LIBRARY_DIR} ${Amp1394_EXTRA_LIBRARY_DIR}) 56 | 57 | # Create Amp1394 library 58 | add_library(Amp1394 STATIC 59 | ${HEADERS} ${SOURCE_FILES}) 60 | 61 | target_link_libraries(Amp1394 ${Amp1394_EXTRA_LIBRARIES}) 62 | 63 | option (Amp1394_BUILD_SWIG "Build Amp1394 with Python wrapper" OFF) 64 | if (Amp1394_BUILD_SWIG) 65 | find_package(SWIG REQUIRED) 66 | include(${SWIG_USE_FILE}) 67 | 68 | # Find Python and NumPy 69 | set (AMP1394_PYTHON_VERSION_REQUIRED "" CACHE STRING "Required Python version (if not empty)") 70 | if (AMP1394_PYTHON_VERSION_REQUIRED) 71 | message (STATUS "Looking for Python ${AMP1394_PYTHON_VERSION_REQUIRED}") 72 | find_package (Python ${AMP1394_PYTHON_VERSION_REQUIRED} EXACT REQUIRED COMPONENTS Development NumPy) 73 | else () 74 | find_package (Python REQUIRED COMPONENTS Interpreter Development NumPy) 75 | endif () 76 | 77 | include_directories(${Python_INCLUDE_DIRS} ${Python_NumPy_INCLUDE_DIRS}) 78 | include_directories("${Amp1394_BINARY_DIR}/Amp1394") 79 | 80 | set_source_files_properties(Amp1394.i PROPERTIES CPLUSPLUS ON) 81 | swig_add_library(Amp1394Python 82 | LANGUAGE python 83 | SOURCES Amp1394.i ${HEADERS} ${SOURCE_FILES}) 84 | target_link_libraries(Amp1394Python ${Python_LIBRARIES} ${Amp1394_EXTRA_LIBRARIES}) 85 | 86 | if (WIN32) 87 | set_target_properties (Amp1394Python PROPERTIES SUFFIX .pyd) 88 | set_target_properties (Amp1394Python PROPERTIES DEBUG_POSTFIX "_d") 89 | endif (WIN32) 90 | 91 | # post build command 92 | add_custom_command(TARGET Amp1394Python POST_BUILD 93 | COMMAND ${CMAKE_COMMAND} 94 | ARGS -E copy_if_different 95 | ${CMAKE_CURRENT_BINARY_DIR}/Amp1394Python.py 96 | ${LIBRARY_OUTPUT_PATH}/${CMAKE_CFG_INTDIR}/ 97 | COMMAND ${CMAKE_COMMAND} 98 | ARGS -E copy_if_different 99 | ${Amp1394_SOURCE_DIR}/python/test_eth1394.py 100 | ${LIBRARY_OUTPUT_PATH}/${CMAKE_CFG_INTDIR}/ 101 | COMMAND ${CMAKE_COMMAND} 102 | ARGS -E copy_if_different 103 | ${Amp1394_SOURCE_DIR}/python/test_amp1394.py 104 | ${LIBRARY_OUTPUT_PATH}/${CMAKE_CFG_INTDIR}/ 105 | COMMENT "Copying Python files to ${LIBRARY_OUTPUT_PATH}" ) 106 | 107 | # install library and python file 108 | install ( 109 | TARGETS Amp1394Python 110 | RUNTIME DESTINATION bin 111 | LIBRARY DESTINATION lib 112 | COMPONENT Amp1394) 113 | install ( 114 | FILES ${CMAKE_CURRENT_BINARY_DIR}/Amp1394Python.py 115 | DESTINATION lib 116 | COMPONENT Amp1394) 117 | 118 | endif (Amp1394_BUILD_SWIG) 119 | 120 | # Create Amp1394 console library 121 | 122 | if (NOT WIN32) 123 | if (CURSES_FOUND) 124 | add_definitions(-DAmp1394Console_HAS_CURSES) 125 | include_directories (${CURSES_INCLUDE_DIR}) 126 | endif (CURSES_FOUND) 127 | endif (NOT WIN32) 128 | 129 | add_library(Amp1394Console STATIC Amp1394Console.h code/Amp1394Console.cpp) 130 | 131 | target_link_libraries(Amp1394Console ${Amp1394Console_EXTRA_LIBRARIES}) 132 | 133 | # revision file 134 | configure_file ("${CMAKE_CURRENT_SOURCE_DIR}/AmpIORevision.h.in" 135 | "${Amp1394_BINARY_DIR}/Amp1394/AmpIORevision.h") 136 | 137 | # Install files 138 | install (TARGETS Amp1394 Amp1394Console COMPONENT Amp1394 139 | RUNTIME DESTINATION bin 140 | LIBRARY DESTINATION lib 141 | ARCHIVE DESTINATION lib) 142 | 143 | install (FILES ${HEADERS} Amp1394Console.h 144 | DESTINATION include/Amp1394 145 | COMPONENT Amp1394-dev) 146 | -------------------------------------------------------------------------------- /lib/Amp1394.i: -------------------------------------------------------------------------------- 1 | /* File : Amp1394.i */ 2 | %module Amp1394Python 3 | %{ 4 | #define SWIG_FILE_WITH_INIT 5 | 6 | /*Put headers and other declarations here*/ 7 | #include "AmpIORevision.h" 8 | #include "Amp1394Time.h" 9 | #include "AmpIO.h" 10 | #include "EthUdpPort.h" 11 | 12 | #if Amp1394_HAS_RAW1394 13 | #include "FirewirePort.h" 14 | #endif 15 | 16 | #if Amp1394_HAS_PCAP 17 | #include "EthRawPort.h" 18 | #endif 19 | 20 | #if Amp1394_HAS_EMIO 21 | #include "ZynqEmioPort.h" 22 | #endif 23 | 24 | #include "PortFactory.h" 25 | 26 | #ifdef _MSC_VER 27 | #include 28 | inline uint16_t bswap_16(uint16_t data) { return _byteswap_ushort(data); } 29 | inline uint32_t bswap_32(uint32_t data) { return _byteswap_ulong(data); } 30 | #else 31 | #include 32 | #endif 33 | 34 | uint16_t bswap16(uint16_t in) { 35 | return bswap_16(in); 36 | } 37 | 38 | uint32_t bswap32(uint32_t in) { 39 | return bswap_32(in); 40 | } 41 | 42 | %} 43 | 44 | %include stdint.i 45 | %include std_string.i 46 | %include numpy.i 47 | 48 | %init %{ 49 | import_array(); 50 | %} 51 | 52 | uint16_t bswap16(uint16_t in); 53 | uint32_t bswap32(uint32_t in); 54 | 55 | /*%apply int *INPUT { Int32 *in };*/ 56 | %typemap(in,numinputs=0) 57 | (quadlet_t& ARGOUT_QUADLET_T) 58 | (quadlet_t temp) 59 | { 60 | $1 = &temp; 61 | } 62 | %typemap(argout) (quadlet_t& ARGOUT_QUADLET_T) 63 | { 64 | $result = SWIG_Python_AppendOutput($result, SWIG_From_unsigned_SS_int(*arg$argnum)); 65 | } 66 | 67 | %typemap(in,numinputs=0) 68 | (int32_t& ARGOUT_INT32_T) 69 | (int32_t temp) 70 | { 71 | $1 = &temp; 72 | } 73 | %typemap(argout) (int32_t& ARGOUT_INT32_T) 74 | { 75 | $result = SWIG_Python_AppendOutput($result, SWIG_From_int(*arg$argnum)); 76 | } 77 | 78 | %typemap(in,numinputs=0) 79 | (uint16_t& ARGOUT_UINT16_T) 80 | (uint16_t temp) 81 | { 82 | $1 = &temp; 83 | } 84 | %typemap(argout) (uint16_t& ARGOUT_UINT16_T) 85 | { 86 | $result = SWIG_Python_AppendOutput($result, SWIG_From_unsigned_SS_short(*arg$argnum)); 87 | } 88 | 89 | // ------------------------- 90 | // IN_ARRAY 91 | %typecheck(SWIG_TYPECHECK_DOUBLE_ARRAY, 92 | fragment="NumPy_Macros") 93 | (quadlet_t* IN_ARRAY1, unsigned int NBYTES) 94 | { 95 | $1 = is_array($input) || PySequence_Check($input); 96 | } 97 | %typemap(in, 98 | fragment="NumPy_Fragments") 99 | (quadlet_t* IN_ARRAY1, unsigned int NBYTES) 100 | (PyArrayObject* array=NULL, int is_new_object=0) 101 | { 102 | npy_intp size[1] = { -1 }; 103 | array = obj_to_array_contiguous_allow_conversion($input, 104 | NPY_UINT, 105 | &is_new_object); 106 | if (!array || !require_dimensions(array, 1) || 107 | !require_size(array, size, 1)) SWIG_fail; 108 | $1 = (quadlet_t*) array_data(array); 109 | $2 = (unsigned int) array_size(array,0) * 4; 110 | } 111 | %typemap(freearg) 112 | (quadlet_t* IN_ARRAY1, unsigned int NBYTES) 113 | { 114 | if (is_new_object$argnum && array$argnum) 115 | { Py_DECREF(array$argnum); } 116 | } 117 | 118 | // ---------------------------- 119 | // ARGOUT_ARRAY 120 | %typemap(in,numinputs=1, 121 | fragment="NumPy_Fragments") 122 | (quadlet_t* ARGOUT_ARRAY1, unsigned int NBYTES) 123 | (PyObject* array = NULL) 124 | { 125 | npy_intp dims[1]; 126 | if (!PyInt_Check($input)) 127 | { 128 | const char* typestring = pytype_string($input); 129 | PyErr_Format(PyExc_TypeError, 130 | "Int dimension expected. '%s' given.", 131 | typestring); 132 | SWIG_fail; 133 | } 134 | $2 = (unsigned int) PyInt_AsLong($input); 135 | dims[0] = (npy_intp) $2; 136 | array = PyArray_SimpleNew(1, dims, NPY_UINT); 137 | if (!array) SWIG_fail; 138 | $1 = (quadlet_t*) array_data(array); 139 | $2 = $2 * 4; 140 | } 141 | %typemap(argout) 142 | (quadlet_t* ARGOUT_ARRAY1, unsigned int NBYTES) 143 | { 144 | $result = SWIG_Python_AppendOutput($result,(PyObject*)array$argnum); 145 | } 146 | 147 | 148 | 149 | // AmpIO class 150 | %apply uint16_t& ARGOUT_UINT16_T {uint16_t &rdata}; 151 | // ReadDoutControl 152 | %apply uint16_t& ARGOUT_UINT16_T {uint16_t &countsHigh}; 153 | %apply uint16_t& ARGOUT_UINT16_T {uint16_t &countsLow}; 154 | // ReadEncoderPreload 155 | %apply int32_t& ARGOUT_INT32_T {int32_t &sdata}; 156 | 157 | %ignore FpgaIO::ReadKSZ8851Reg(uint8_t addr, uint8_t &rdata); 158 | %ignore FpgaIO::WriteKSZ8851Reg(uint8_t, uint8_t const &); 159 | 160 | %import "AmpIORevision.h" 161 | 162 | %constant int VERSION_MAJOR = Amp1394_VERSION_MAJOR; 163 | %constant int VERSION_MINOR = Amp1394_VERSION_MINOR; 164 | %constant int VERSION_PATCH = Amp1394_VERSION_PATCH; 165 | %constant std::string VERSION = Amp1394_VERSION; 166 | 167 | %include "Amp1394Types.h" 168 | %include "Amp1394Time.h" 169 | %include "EncoderVelocity.h" 170 | %include "BoardIO.h" 171 | %include "FpgaIO.h" 172 | %include "AmpIO.h" 173 | 174 | %apply (int* IN_ARRAY1, int DIM1) {(int* data, int size)}; 175 | %apply quadlet_t& ARGOUT_QUADLET_T {quadlet_t &data}; 176 | %apply (quadlet_t* ARGOUT_ARRAY1, unsigned int NBYTES) {(quadlet_t *rdata, unsigned int nbytes)}; 177 | %apply (quadlet_t* IN_ARRAY1, unsigned int NBYTES) {(quadlet_t *wdata, unsigned int nbytes)}; 178 | %include "BasePort.h" 179 | %include "EthBasePort.h" 180 | %include "EthUdpPort.h" 181 | #if Amp1394_HAS_RAW1394 182 | %include "FirewirePort.h" 183 | #endif 184 | #if Amp1394_HAS_PCAP 185 | %include "EthRawPort.h" 186 | #endif 187 | #if Amp1394_HAS_EMIO 188 | %include "ZynqEmioPort.h" 189 | #endif 190 | %include "PortFactory.h" 191 | -------------------------------------------------------------------------------- /tests/fwPortTest.cpp: -------------------------------------------------------------------------------- 1 | /* -*- Mode: C++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */ 2 | /* ex: set filetype=cpp softtabstop=4 shiftwidth=4 tabstop=4 cindent expandtab: */ 3 | 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include // for usleep 12 | 13 | #include "FirewirePort.h" 14 | #include "AmpIO.h" 15 | 16 | // ZC: maybe make it a unit testing ?? 17 | 18 | 19 | // This program tests FirewirePort API 20 | void ShowUsage(void) 21 | { 22 | std::cout << "fwPortTest: this program tests the FirewirePort class\n" 23 | << " -h Display this usage information\n" 24 | << " -p " 25 | << std::endl; 26 | } 27 | 28 | 29 | int main(int argc, char** argv) 30 | { 31 | // declare variables and default values 32 | int port = 0; 33 | const size_t numBoards = 4; 34 | int board_id[numBoards] = {BoardIO::MAX_BOARDS, BoardIO::MAX_BOARDS}; 35 | std::vector BoardList; 36 | 37 | // set board id 38 | board_id[0] = 2; 39 | board_id[1] = 3; 40 | board_id[2] = 6; 41 | board_id[3] = 7; 42 | 43 | // command line option setup 44 | const char short_options[] = "hp:"; 45 | int next_option; 46 | do { 47 | next_option = getopt(argc, argv, short_options); 48 | switch (next_option) 49 | { 50 | case 'h': 51 | ShowUsage(); 52 | break; 53 | case 'p': 54 | port = atoi(optarg); 55 | std::cout << "using port number" << std::endl; 56 | break; 57 | case -1: // parsing done 58 | break; 59 | default: // should NEVER happen 60 | abort(); 61 | } 62 | } 63 | while (next_option != -1); 64 | 65 | // --------------------------------------------------- 66 | // Firewire port 67 | // ZC: really this port number should be interactive 68 | // --------------------------------------------------- 69 | std::stringstream debugStream(std::stringstream::out|std::stringstream::in); 70 | FirewirePort fwport(port, std::cout); 71 | if (!fwport.IsOK()) { 72 | std::cerr << "Failed to initialize firewire port" << std::endl; 73 | return EXIT_FAILURE; 74 | } else { 75 | std::cout << "Open port on " << port << std::endl; 76 | } 77 | 78 | // Create AmpIO board 1 and 2 79 | for (size_t i = 0; i < numBoards; i++) { 80 | BoardList.push_back(new AmpIO(board_id[i])); 81 | fwport.AddBoard(BoardList[i]); 82 | std::cout << "new board with node_id = " << std::dec 83 | << fwport.GetNodeId(board_id[i]) << std::endl; 84 | } 85 | 86 | // ----- --------------------------------- 87 | // Quadlet broadcast 88 | // --------------------------------------- 89 | bool rc; 90 | nodeaddr_t nodeaddress_wd = 0xffffff000003; 91 | rc = fwport.WriteQuadlet(FW_NODE_BROADCAST, nodeaddress_wd, bswap_32(0x3888)); 92 | if (!rc) { 93 | std::cerr << "Quadlet broadcast error" << std::endl; 94 | } 95 | 96 | // now read back and verify 97 | quadlet_t dataQuadlet = 0x0000; 98 | for (size_t i = 0; i < BoardList.size(); i++) { 99 | fwport.ReadQuadlet(BoardList[i]->GetBoardId(), 0x03, dataQuadlet); 100 | std::cout << std::dec << "Board " << (int) BoardList[i]->GetBoardId() 101 | << " watchdog = " << std::hex << bswap_32(dataQuadlet) << std::endl; 102 | dataQuadlet = 0x0000; // clear value 103 | } 104 | 105 | // ----- --------------------------------- 106 | // Block broadcast 107 | // -------------------------------------- 108 | for (size_t i = 0; i < BoardList.size(); i++) { 109 | BoardList[i]->WriteSafetyRelay(true); 110 | BoardList[i]->WritePowerEnable(true); 111 | usleep(40000); // sleep 40 ms 112 | BoardList[i]->WriteAmpEnable(0x0f, 0x0f); 113 | } 114 | 115 | AmpIO* board; 116 | for (size_t i = 0; i < numBoards; i++) { 117 | board = BoardList[i]; 118 | // set current for all boards 119 | for (size_t j = 0; j < 4; j++) { 120 | board->SetMotorCurrent(j, 0x8000 + 0x200 * j); 121 | } 122 | } 123 | 124 | 125 | // ----- --------------------------------- 126 | // Broadcast Write/Read boards 127 | // --------------------------------------- 128 | 129 | int counter = 0; 130 | int csize = 100; 131 | timeval start, prev; 132 | timeval stop[csize]; 133 | 134 | gettimeofday(&start, NULL); 135 | while(counter < csize){ 136 | counter++; 137 | fwport.ReadAllBoardsBroadcast(); 138 | fwport.WriteAllBoardsBroadcast(); 139 | gettimeofday(&stop[counter-1], NULL); 140 | } 141 | 142 | prev = start; 143 | for (int i = 0; i < csize; i++) { 144 | double mtime; 145 | mtime = (stop[i].tv_sec - prev.tv_sec) * 1000000.0 + 146 | (stop[i].tv_usec - prev.tv_usec) + 0.5; 147 | prev = stop[i]; 148 | 149 | std::cout << "time " << i << " = " << std::dec << mtime << std::endl; 150 | } 151 | 152 | 153 | // Shut down boards and remove from port 154 | for (size_t i = 0; i < BoardList.size(); i++) { 155 | BoardList[i]->WriteSafetyRelay(false); 156 | BoardList[i]->WriteAmpEnable(0x0f, 0); // Turn power off 157 | BoardList[i]->WritePowerEnable(false); // Turn power off 158 | fwport.RemoveBoard(BoardList[i]->GetBoardId()); 159 | } 160 | 161 | return EXIT_SUCCESS; 162 | } 163 | -------------------------------------------------------------------------------- /lib/code/Amp1394Console.cpp: -------------------------------------------------------------------------------- 1 | /* -*- Mode: C++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */ 2 | /* ex: set filetype=cpp softtabstop=4 shiftwidth=4 tabstop=4 cindent expandtab: */ 3 | 4 | /* 5 | Author(s): Peter Kazanzides 6 | 7 | (C) Copyright 2021-2025 Johns Hopkins University (JHU), All Rights Reserved. 8 | 9 | --- begin cisst license - do not edit --- 10 | 11 | This software is provided "as is" under an open source license, with 12 | no warranty. The complete license can be found in license.txt and 13 | http://www.cisst.org/cisst/license.txt. 14 | 15 | --- end cisst license --- 16 | */ 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | #ifdef _MSC_VER 23 | #define WIN32_LEAN_AND_MEAN 24 | #include 25 | #include 26 | #else 27 | #include 28 | #ifdef Amp1394Console_HAS_CURSES 29 | #include 30 | #else 31 | #include 32 | #include 33 | #endif 34 | #endif 35 | 36 | #include "Amp1394Console.h" 37 | 38 | #ifdef Amp1394Console_HAS_CURSES 39 | 40 | // Implementation using Curses library 41 | 42 | bool Amp1394Console::Init() 43 | { 44 | initscr(); // initialize curses 45 | cbreak(); // disable buffered input 46 | keypad(stdscr, TRUE); // enable keypad values 47 | if (noEcho) 48 | noecho(); // do not echo input characters 49 | if (noBlock) 50 | nodelay(stdscr, TRUE); // getch non-blocking 51 | isOk = true; 52 | return true; 53 | } 54 | 55 | void Amp1394Console::End() 56 | { 57 | endwin(); 58 | } 59 | 60 | void Amp1394Console::Print(int row, int col, const char *cstr, ...) 61 | { 62 | va_list args; 63 | va_start(args, cstr); 64 | wmove(stdscr, row, col); 65 | vwprintw(stdscr, cstr, args); 66 | va_end(args); 67 | } 68 | 69 | void Amp1394Console::Refresh() 70 | { 71 | wrefresh(stdscr); 72 | } 73 | 74 | int Amp1394Console::GetChar() 75 | { 76 | return getch(); 77 | } 78 | 79 | bool Amp1394Console::GetString(char *str, int n) 80 | { 81 | return (getnstr(str, n) != ERR); 82 | } 83 | 84 | #else 85 | 86 | // Implementation using VT-100 escape codes 87 | #ifndef _MSC_VER 88 | struct Amp1394Console::ConsoleInternals { 89 | struct termios savedAttr; 90 | }; 91 | 92 | // Implementation of _kbhit for non-Windows systems 93 | int _kbhit() 94 | { 95 | struct timeval tv; 96 | fd_set fds; 97 | tv.tv_sec = 0; 98 | tv.tv_usec = 0; 99 | FD_ZERO(&fds); 100 | FD_SET(STDIN_FILENO, &fds); 101 | select(STDIN_FILENO+1, &fds, 0, 0, &tv); 102 | return FD_ISSET(STDIN_FILENO, &fds); 103 | } 104 | #endif 105 | 106 | bool Amp1394Console::Init() 107 | { 108 | isOk = false; 109 | #ifdef _MSC_VER 110 | HANDLE stdscr = GetStdHandle(STD_OUTPUT_HANDLE); 111 | if (stdscr == INVALID_HANDLE_VALUE) { 112 | std::cerr << "Could not get Std handle" << std::endl; 113 | return false; 114 | } 115 | DWORD mode; 116 | if (!GetConsoleMode(stdscr, &mode)) { 117 | std::cerr << "Could not get console mode" << std::endl; 118 | return false; 119 | } 120 | mode |= ENABLE_VIRTUAL_TERMINAL_PROCESSING; 121 | if (!SetConsoleMode(stdscr, mode)) { 122 | std::cerr << "Failed to set VT100 mode" << std::endl; 123 | return false; 124 | } 125 | #else 126 | Internals = new Amp1394Console::ConsoleInternals; 127 | struct termios termAttr; 128 | tcgetattr(STDIN_FILENO, &termAttr); 129 | Internals->savedAttr = termAttr; 130 | if (noBlock) { 131 | termAttr.c_lflag &= ~ICANON; 132 | termAttr.c_cc[VMIN] = 0; 133 | termAttr.c_cc[VTIME] = 0; 134 | } 135 | else 136 | termAttr.c_lflag |= ICANON; 137 | if (noEcho) 138 | termAttr.c_lflag &= ~ECHO; 139 | else 140 | termAttr.c_lflag |= ECHO; 141 | if (tcsetattr(STDIN_FILENO, TCSANOW, &termAttr) != 0) { 142 | std::cerr << "Failed to set terminal attributes" << std::endl; 143 | return false; 144 | } 145 | #endif 146 | if (noEcho) 147 | printf("\x1b[?25l"); // Hide cursor 148 | printf("\x1b[2J"); // Erase screen 149 | isOk = true; 150 | return true; 151 | } 152 | 153 | void Amp1394Console::End() 154 | { 155 | if (isOk) { 156 | printf("\x1b[0;0H"); // Move cursor to (0,0) 157 | printf("\x1b[!p"); // Soft reset (restore defaults) 158 | printf("\x1b[2J"); // Erase screen 159 | } 160 | #ifndef _MSC_VER 161 | if (Internals) { 162 | // Restore settings 163 | tcsetattr(STDIN_FILENO, TCSANOW, &(Internals->savedAttr)); 164 | delete Internals; 165 | Internals = 0; 166 | } 167 | #endif 168 | isOk = false; 169 | } 170 | 171 | void Amp1394Console::Print(int row, int col, const char *cstr, ...) 172 | { 173 | va_list args; 174 | va_start(args, cstr); 175 | printf("\x1b[%d;%dH", row, col); 176 | vprintf(cstr, args); 177 | va_end(args); 178 | } 179 | 180 | void Amp1394Console::Refresh() 181 | { 182 | } 183 | 184 | int Amp1394Console::GetChar() 185 | { 186 | int c = -1; // ERR in curses 187 | // If blocking, or if key pressed, get a character 188 | if (!noBlock || _kbhit()) { 189 | #ifdef _MSC_VER 190 | c = noEcho ? _getch() : _getche(); 191 | #else 192 | c = getchar(); 193 | #endif 194 | } 195 | return c; 196 | } 197 | 198 | bool Amp1394Console::GetString(char *str, int n) 199 | { 200 | char *ret = fgets(str, n, stdin); 201 | if (ret) { 202 | size_t len = strlen(str); 203 | if (len > 0) 204 | str[len-1] = 0; // remove newline 205 | } 206 | return (ret != 0); 207 | } 208 | 209 | #endif 210 | -------------------------------------------------------------------------------- /tests/block1394eth.cpp: -------------------------------------------------------------------------------- 1 | /* -*- Mode: C++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */ 2 | /* ex: set filetype=cpp softtabstop=4 shiftwidth=4 tabstop=4 cindent expandtab: */ 3 | 4 | /****************************************************************************** 5 | * 6 | * This is a variation of util/block1394.c that relies on the Amp1394 library, 7 | * thus it can support connecting via Firewire, Ethernet Raw (PCAP) or UDP. 8 | * 9 | ******************************************************************************/ 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include "Amp1394BSwap.h" 18 | 19 | #include 20 | #include "PortFactory.h" 21 | #include "AmpIO.h" 22 | 23 | void PrintDebugStream(std::stringstream &debugStream) 24 | { 25 | char line[80]; 26 | while (debugStream.getline(line, sizeof(line))) 27 | std::cerr << line << std::endl; 28 | debugStream.clear(); 29 | debugStream.str(""); 30 | } 31 | 32 | int main(int argc, char** argv) 33 | { 34 | int args_found = 0; 35 | nodeaddr_t addr = 0x0; 36 | int size = 1; 37 | quadlet_t data1; 38 | quadlet_t *data = &data1; 39 | bool isQuad1394 = (strstr(argv[0], "quad1394eth") != 0); 40 | 41 | int i, j = 0; 42 | int bid = 0; 43 | bool verbose = false; 44 | std::string portDescription = BasePort::DefaultPort(); 45 | 46 | for (i = 1; i < argc; i++) { 47 | if (argv[i][0] == '-') { 48 | if (argv[i][1] == 'p') { 49 | portDescription = argv[i]+2; 50 | } 51 | else if (argv[i][1] == 'b') { 52 | bid = atoi(argv[i]+2); 53 | std::cout << "Selecting board " << bid << "\n"; 54 | } 55 | else if (argv[i][1] == 'v') { 56 | verbose = true; 57 | } 58 | } 59 | else { 60 | if (args_found == 0) 61 | addr = strtoull(argv[i], 0, 16); 62 | else if ((args_found == 1) && (isQuad1394)) 63 | data1 = strtoul(argv[i], 0, 16); 64 | else if ((args_found == 1) && (!isQuad1394)) { 65 | std::cout << "" << std::endl; 66 | size = strtoul(argv[i], 0, 10); 67 | /* Allocate data array, initializing contents to 0 */ 68 | data = (quadlet_t *) calloc(sizeof(quadlet_t), size); 69 | if (!data) { 70 | std::cerr << "Failed to allocate memory for " << size << " quadlets\n"; 71 | exit(-1); 72 | } 73 | } 74 | else if (!isQuad1394 && (j < size)) 75 | data[j++] = bswap_32(strtoul(argv[i], 0, 16)); 76 | else 77 | std::cerr << "Warning: extra parameter: " << argv[i] << "\n"; 78 | 79 | args_found++; 80 | } 81 | } 82 | 83 | if (args_found < 1) { 84 | if (isQuad1394) 85 | std::cout << "Usage: " << argv[0] << " [-pP] [-bN] [-v]
[value to write in hex]" << std::endl; 86 | else 87 | std::cout << "Usage: " << argv[0] << " [-pP] [-bN] [-v]
[write data quadlets in hex]" << std::endl; 88 | std::cout << " where P = port number, N = board number" << std::endl 89 | << " can also specify -pfwP, -pethP or -pudp[xx.xx.xx.xx]" << std::endl 90 | << " v specifies verbose mode" << std::endl; 91 | exit(0); 92 | } 93 | 94 | 95 | std::stringstream debugStream(std::stringstream::out|std::stringstream::in); 96 | 97 | BasePort *Port = PortFactory(portDescription.c_str(), debugStream); 98 | if (!Port) { 99 | PrintDebugStream(debugStream); 100 | std::cerr << "Failed to create port using: " << portDescription << std::endl; 101 | return -1; 102 | } 103 | if (!Port->IsOK()) { 104 | PrintDebugStream(debugStream); 105 | std::cerr << "Failed to initialize " << Port->GetPortTypeString() << std::endl; 106 | return -1; 107 | } 108 | else if (verbose) { 109 | PrintDebugStream(debugStream); 110 | } 111 | 112 | AmpIO Board(bid); 113 | Port->AddBoard(&Board); 114 | 115 | // Quadlet R/W 116 | if (isQuad1394 && (args_found == 1)) 117 | { 118 | if (Port->ReadQuadlet(bid, addr, (*data))) 119 | std::cout << "0x" << std::hex << data[0] << "\n"; 120 | else { 121 | PrintDebugStream(debugStream); 122 | std::cerr << "ReadQuadlet Failed \n"; 123 | } 124 | } 125 | else if (isQuad1394 && (args_found == 2)) 126 | { 127 | std::cout << "WriteQuadlet\n"; 128 | if (!Port->WriteQuadlet(bid, addr, (*data) )) { 129 | PrintDebugStream(debugStream); 130 | std::cerr << "WriteQuadlet Failed\n"; 131 | } 132 | } 133 | 134 | // Block R/W 135 | else if (!isQuad1394 && (args_found <= 2)) 136 | { 137 | if (Port->ReadBlock(bid, addr, data, size * 4)) { 138 | for (j=0; jWriteBlock(bid, addr, data, size * 4)) { 149 | PrintDebugStream(debugStream); 150 | std::cerr << "WriteBlock Failed \n"; 151 | } 152 | } 153 | 154 | if (Port->IsOK()) 155 | Port->RemoveBoard(bid); 156 | delete Port; 157 | return 0; 158 | } 159 | -------------------------------------------------------------------------------- /lib/FirewirePort.h: -------------------------------------------------------------------------------- 1 | /* -*- Mode: C++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */ 2 | /* ex: set filetype=cpp softtabstop=4 shiftwidth=4 tabstop=4 cindent expandtab: */ 3 | 4 | /* 5 | Author(s): Zihan Chen, Peter Kazanzides 6 | 7 | (C) Copyright 2011-2019 Johns Hopkins University (JHU), All Rights Reserved. 8 | 9 | --- begin cisst license - do not edit --- 10 | 11 | This software is provided "as is" under an open source license, with 12 | no warranty. The complete license can be found in license.txt and 13 | http://www.cisst.org/cisst/license.txt. 14 | 15 | --- end cisst license --- 16 | */ 17 | 18 | #ifndef __FirewirePort_H__ 19 | #define __FirewirePort_H__ 20 | 21 | #include 22 | #include 23 | #include "BoardIO.h" 24 | #include "BasePort.h" 25 | 26 | // Forward declarations 27 | struct raw1394_handle; 28 | typedef struct raw1394_handle *raw1394handle_t; 29 | typedef int (*bus_reset_handler_t)(raw1394handle_t, unsigned int generation); 30 | 31 | // Maximum Firewire packet size in bytes (at 400 Mbits/sec) 32 | const unsigned int FW_MAX_PACKET_SIZE = 2048; 33 | 34 | // We subtract 24 bytes for the Block Write or Block Read Response header and CRC. 35 | // The Firewire specification will actually allow the full 2048 bytes for the data, 36 | // but this would require a firmware update. 37 | const unsigned int FW_MAX_DATA_SIZE = FW_MAX_PACKET_SIZE-24; 38 | 39 | class FirewirePort : public BasePort { 40 | public: 41 | protected: 42 | 43 | raw1394handle_t handle; // normal read/write handle 44 | nodeid_t baseNodeId; 45 | 46 | // List of all ports instantiated (for use by reset_handler) 47 | typedef std::vector PortListType; 48 | static PortListType PortList; 49 | bus_reset_handler_t old_reset_handler; 50 | // callback for 1394 bus reset event 51 | static int reset_handler(raw1394handle_t hdl, unsigned int gen); 52 | 53 | //! Read quadlet from node (internal method called by ReadQuadlet). 54 | // Parameter "flags" is not used for Firewire. 55 | bool ReadQuadletNode(nodeid_t node, nodeaddr_t addr, quadlet_t &data, unsigned char flags = 0); 56 | 57 | //! Write quadlet to node (internal method called by WriteQuadlet) 58 | // Parameter "flags" is not used for Firewire. 59 | bool WriteQuadletNode(nodeid_t node, nodeaddr_t addr, quadlet_t data, unsigned char flags = 0); 60 | 61 | // Method called by ReadAllBoards/ReadAllBoardsBroadcast if no data read 62 | void OnNoneRead(void); 63 | 64 | // Method called by WriteAllBoards/WriteAllBoardsBroadcast if no data written 65 | void OnNoneWritten(void); 66 | 67 | // Poll for IEEE 1394 events, such as bus reset. 68 | void PollEvents(void); 69 | 70 | // Initialize Firewire port 71 | bool Init(void); 72 | 73 | // Cleanup Firewire port 74 | void Cleanup(void); 75 | 76 | // Initialize nodes on the bus; called by ScanNodes 77 | // \return Maximum number of nodes on bus (0 if error) 78 | nodeid_t InitNodes(void); 79 | 80 | // Write the broadcast packet containing the DAC values and power control 81 | bool WriteBroadcastOutput(quadlet_t *buffer, unsigned int size); 82 | 83 | // Write a block to the specified node. Internal method called by WriteBlock and 84 | // WriteAllBoardsBroadcast. 85 | // Parameter "flags" is not used for Firewire. 86 | bool WriteBlockNode(nodeid_t node, nodeaddr_t addr, quadlet_t *wdata, 87 | unsigned int nbytes, unsigned char flags = 0); 88 | 89 | // Read a block from the specified node. Internal method called by ReadBlock. 90 | // Parameter "flags" is not used for Firewire. 91 | bool ReadBlockNode(nodeid_t node, nodeaddr_t addr, quadlet_t *rdata, 92 | unsigned int nbytes, unsigned char flags = 0); 93 | 94 | public: 95 | // Initialize IEEE-1394 (Firewire) port. 96 | FirewirePort(int portNum, std::ostream &debugStream = std::cerr); 97 | ~FirewirePort(); 98 | 99 | //****************** BasePort pure virtual methods *********************** 100 | 101 | PortType GetPortType(void) const { return PORT_FIREWIRE; } 102 | 103 | // Call lsof to count the number of users, assumes /dev/fw 104 | int NumberOfUsers(void); 105 | 106 | bool IsOK(void) { return (handle != NULL); } 107 | 108 | unsigned int GetBusGeneration(void) const; 109 | 110 | void UpdateBusGeneration(unsigned int gen); 111 | 112 | unsigned int GetPrefixOffset(MsgType) const { return 0; } 113 | unsigned int GetWritePostfixSize(void) const { return 0; } 114 | unsigned int GetReadPrefixSize(void) const { return 0; } 115 | unsigned int GetReadPostfixSize(void) const { return 0; } 116 | 117 | unsigned int GetWriteQuadAlign(void) const { return 0; } 118 | unsigned int GetReadQuadAlign(void) const { return 0; } 119 | 120 | // Get the maximum number of data bytes that can be read 121 | // (via ReadBlock) or written (via WriteBlock). 122 | unsigned int GetMaxReadDataSize(void) const { return FW_MAX_DATA_SIZE; } 123 | unsigned int GetMaxWriteDataSize(void) const { return FW_MAX_DATA_SIZE; } 124 | 125 | // Adds board(s) 126 | bool AddBoard(BoardIO *board); 127 | 128 | // Removes board 129 | bool RemoveBoard(unsigned char boardId); 130 | 131 | /*! 132 | \brief Write the broadcast read request 133 | */ 134 | bool WriteBroadcastReadRequest(unsigned int seq); 135 | 136 | /*! 137 | \brief Wait for broadcast read data to be available 138 | */ 139 | void WaitBroadcastRead(void); 140 | 141 | /*! 142 | \brief Add delay (if needed) for PROM I/O operations 143 | The delay is 0 for FireWire. 144 | */ 145 | void PromDelay(void) const {} 146 | 147 | //****************** FireWire specific methods *********************** 148 | 149 | // Stop Cycle Start Packets 150 | void StopCycleStartPacket(void); 151 | 152 | }; 153 | 154 | #endif // __FirewirePort_H__ 155 | -------------------------------------------------------------------------------- /tests/instrument.cpp: -------------------------------------------------------------------------------- 1 | /* -*- Mode: C++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */ 2 | /* ex: set filetype=cpp softtabstop=4 shiftwidth=4 tabstop=4 cindent expandtab: */ 3 | 4 | /****************************************************************************** 5 | * 6 | * (C) Copyright 2018-2023 Johns Hopkins University (JHU), All Rights Reserved. 7 | * 8 | * This program is used to read the Dallas DS2505 chip inside a da Vinci instrument 9 | * via its 1-wire interface. The 1-wire interface is implemented in the FPGA, 10 | * using either the direct 1-wire interface or an external DS2480B driver chip. 11 | * The direct 1-wire interface uses bi-redirectional digital port DOUT3, 12 | * which is available with QLA Rev 1.4+. The external DS2480B driver chip is 13 | * located either on the dMIB (Rev F+) or on an external dongle. 14 | * This program depends on the AmpIO library (which may depend on libraw1394 15 | * and/or pcap). 16 | * 17 | * Usage: instrument [-pP] 18 | * where P is the Firewire port number (default 0), 19 | * or a string such as ethP and fwP, where P is the port number 20 | * 21 | ******************************************************************************/ 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | #include 29 | #include "PortFactory.h" 30 | #include "AmpIO.h" 31 | 32 | void PrintDebugStream(std::stringstream &debugStream) 33 | { 34 | char line[80]; 35 | while (debugStream.getline(line, sizeof(line))) 36 | std::cerr << line << std::endl; 37 | debugStream.clear(); 38 | debugStream.str(""); 39 | } 40 | 41 | int main(int argc, char** argv) 42 | { 43 | int i; 44 | int board = 0; 45 | 46 | std::string portDescription = BasePort::DefaultPort(); 47 | 48 | if (argc > 1) { 49 | int args_found = 0; 50 | for (i = 1; i < argc; i++) { 51 | if (argv[i][0] == '-') { 52 | if (argv[i][1] == 'p') { 53 | portDescription = argv[i]+2; 54 | } 55 | else { 56 | std::cerr << "Usage: instrument [-pP]" << std::endl 57 | << " where = rotary switch setting (0-15)" << std::endl 58 | << " P = port number (default 0)" << std::endl 59 | << " can also specify -pfwP, -pethP or -pudp" << std::endl; 60 | return 0; 61 | } 62 | } 63 | else { 64 | if (args_found == 0) 65 | board = atoi(argv[i]); 66 | args_found++; 67 | } 68 | } 69 | } 70 | 71 | std::stringstream debugStream(std::stringstream::out|std::stringstream::in); 72 | BasePort *Port = PortFactory(portDescription.c_str(), debugStream); 73 | if (!Port) { 74 | PrintDebugStream(debugStream); 75 | std::cerr << "Failed to create port using: " << portDescription << std::endl; 76 | return -1; 77 | } 78 | if (!Port->IsOK()) { 79 | PrintDebugStream(debugStream); 80 | std::cerr << "Failed to initialize " << Port->GetPortTypeString() << std::endl; 81 | return -1; 82 | } 83 | 84 | AmpIO Board(board); 85 | Port->AddBoard(&Board); 86 | 87 | uint32_t fver = Board.GetFirmwareVersion(); 88 | if (fver < 7) { 89 | std::cerr << "Instrument read requires firmware version 7+ (detected version " << fver << ")" << std::endl; 90 | Port->RemoveBoard(board); 91 | delete Port; 92 | return -1; 93 | } 94 | uint32_t status = Board.ReadStatus(); 95 | 96 | // Now, we try to read the Dallas chip. This will also populate the status field. 97 | unsigned char buffer[2048]; // Buffer for entire contents of DS2505 memory (2 Kbytes) 98 | bool ret = Board.DallasReadMemory(0, (unsigned char *) buffer, sizeof(buffer)); 99 | if (!Board.DallasReadStatus(status)) { 100 | std::cerr << "Failed to read DS2505 status" << std::endl; 101 | Port->RemoveBoard(board); 102 | delete Port; 103 | return -1; 104 | } 105 | // No longer need these 106 | Port->RemoveBoard(board); 107 | delete Port; 108 | if ((status & 0x00000001) != 0x00000001) { 109 | std::cerr << "DS2505 interface not enabled (hardware problem)" << std::endl; 110 | return -1; 111 | } 112 | unsigned char ds_reset = static_cast((status & 0x00000006)>>1); 113 | if (ds_reset != 1) { 114 | std::cerr << "Failed to communicate with DS2505" << std::endl; 115 | if (ds_reset == 2) 116 | std::cerr << " - DOUT3 did not reach high state -- is pullup resistor missing?" << std::endl; 117 | else if (ds_reset == 3) 118 | std::cerr << " - Did not received ACK from DS2505 -- is dMIB signal jumpered?" << std::endl; 119 | return -1; 120 | } 121 | unsigned char family_code = static_cast((status&0xFF000000)>>24); 122 | if (family_code != 0x0B) { 123 | std::cerr << "Unknown device family code: 0x" << std::hex << static_cast(family_code) 124 | << " (DS2505 should be 0x0B)" << std::endl; 125 | return -1; 126 | } 127 | bool useDS2480B = (status & 0x00008000) == 0x00008000; 128 | if (!useDS2480B) { 129 | unsigned char rise_time = static_cast((status&0x00FF0000)>>16); 130 | std::cout << "Measured rise time: " << (rise_time/49.152) << " microseconds" << std::endl; 131 | } 132 | if (!ret) { 133 | std::cerr << "Failed to read instrument memory" << std::endl; 134 | return -1; 135 | } 136 | std::ofstream outFile("instrument.txt", std::ios::binary); 137 | if (outFile.good()) { 138 | outFile.write((char *)buffer, sizeof(buffer)); 139 | std::cout << "Data written to instrument.txt" << std::endl; 140 | } 141 | else { 142 | std::cerr << "Failed to open instrument.txt for writing" << std::endl; 143 | return -1; 144 | } 145 | outFile.close(); 146 | return 0; 147 | } 148 | -------------------------------------------------------------------------------- /lib/BoardIO.h: -------------------------------------------------------------------------------- 1 | /* -*- Mode: C++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */ 2 | /* ex: set filetype=cpp softtabstop=4 shiftwidth=4 tabstop=4 cindent expandtab: */ 3 | 4 | /* 5 | Author(s): Peter Kazanzides, Zihan Chen 6 | 7 | (C) Copyright 2011-2024 Johns Hopkins University (JHU), All Rights Reserved. 8 | 9 | --- begin cisst license - do not edit --- 10 | 11 | This software is provided "as is" under an open source license, with 12 | no warranty. The complete license can be found in license.txt and 13 | http://www.cisst.org/cisst/license.txt. 14 | 15 | --- end cisst license --- 16 | */ 17 | 18 | #ifndef __BOARDIO_H__ 19 | #define __BOARDIO_H__ 20 | 21 | // Base class for custom boards with IEEE-1394 (Firewire) and Ethernet interface. 22 | 23 | #include // for memset 24 | #include 25 | #include "Amp1394Types.h" 26 | 27 | typedef uint32_t quadlet_t; 28 | typedef uint64_t nodeaddr_t; 29 | typedef uint16_t nodeid_t; 30 | 31 | class BasePort; 32 | class FirewirePort; 33 | class EthBasePort; 34 | class EthRawPort; 35 | class EthUdpPort; 36 | 37 | class BoardIO 38 | { 39 | protected: 40 | // Prevent copies 41 | BoardIO(const BoardIO &); 42 | BoardIO& operator=(const BoardIO&); 43 | 44 | unsigned char BoardId; 45 | BasePort *port; 46 | 47 | bool readValid; 48 | bool writeValid; 49 | 50 | unsigned int numReadErrors; 51 | unsigned int numWriteErrors; 52 | 53 | friend class BasePort; 54 | friend class FirewirePort; 55 | friend class EthBasePort; 56 | friend class EthRawPort; 57 | friend class EthUdpPort; 58 | 59 | // InitBoard sets the number of motors and encoders, based on the hardware 60 | // (e.g., QLA or dRA1) and firmware version. It assumes that the port member 61 | // data has already been set. 62 | virtual void InitBoard(void) = 0; 63 | 64 | // For real-time block reads and writes, the board class (i.e., derived classes from BoardIO) 65 | // determines the data size (NumBytes), but the port classes (i.e., derived classes from BasePort) 66 | // allocate the memory. 67 | 68 | // Following methods are for real-time block reads 69 | void SetReadValid(bool flag) 70 | { readValid = flag; if (!readValid) numReadErrors++; } 71 | virtual unsigned int GetReadNumBytes() const = 0; 72 | virtual void SetReadData(const quadlet_t *buf) = 0; 73 | 74 | // Following methods are for real-time block writes 75 | void SetWriteValid(bool flag) 76 | { writeValid = flag; if (!writeValid) numWriteErrors++; } 77 | virtual unsigned int GetWriteNumBytes() const = 0; 78 | virtual bool GetWriteData(quadlet_t *buf, unsigned int offset, unsigned int numQuads, bool doSwap = true) const = 0; 79 | virtual void InitWriteBuffer(void) = 0; 80 | 81 | virtual bool WriteBufferResetsWatchdog(void) const = 0; 82 | virtual void CheckCollectCallback() = 0; 83 | 84 | public: 85 | enum {MAX_BOARDS = 16}; // Maximum number of boards 86 | 87 | // The following registers are required to be supported on all boards because they are used by 88 | // the Port classes when scanning/configuring the bus. 89 | // 90 | // BOARD_STATUS: Bits 27-24 contain the board number (e.g., rotary switch) 91 | // FW_PHY_REQ: Write 0 to this register to initiate a read of PHY register 0 (self-id); 92 | // needed for an unmanaged Firewire bus (e.g., PC not connected to Firewire) 93 | // FW_PHY_RESP: PHY response; not used by Port classes 94 | // HARDWARE_VERSION: Indicates type of connected board (e.g., QLA, dRAC) 95 | // FIRMWARE_VERSION: Version of FPGA firmware 96 | // IP_ADDR: Ethernet IP address (written during Ethernet configuration) 97 | // ETH_STATUS: Ethernet status register (used to distinguish FPGA version) 98 | // GIT_DESC: Git description 99 | // 100 | enum Registers { 101 | BOARD_STATUS = 0, // RW: Board status/control register 102 | FW_PHY_REQ = 1, // WO: Firewire PHY register read/write request 103 | FW_PHY_RESP = 2, // RO: Firewire PHY register read response 104 | HARDWARE_VERSION = 4, // RO: Companion board type (e.g., "QLA1") 105 | FIRMWARE_VERSION = 7, // RO: Firmware version number 106 | IP_ADDR = 11, // RW: Ethernet IP address (Firmware V7+) 107 | ETH_STATUS = 12, // RW: Ethernet status register (Firmware V5+) 108 | GIT_DESC = 15 // RO: Git description (after Firmware V8 release) 109 | }; 110 | 111 | BoardIO(unsigned char board_id) : BoardId(board_id), port(0), readValid(false), writeValid(false), 112 | numReadErrors(0), numWriteErrors(0) {} 113 | virtual ~BoardIO() {} 114 | 115 | inline unsigned char GetBoardId() const { return BoardId; } 116 | 117 | // Returns true if a valid board 118 | inline bool IsValid() const { return (BoardId < MAX_BOARDS); } 119 | 120 | inline bool ValidRead() const { return readValid; } 121 | inline bool ValidWrite() const { return writeValid; } 122 | 123 | inline unsigned int GetReadErrors() const { return numReadErrors; } 124 | inline unsigned int GetWriteErrors() const { return numWriteErrors; } 125 | 126 | inline void ClearReadErrors() { numReadErrors = 0; } 127 | inline void ClearWriteErrors() { numWriteErrors = 0; } 128 | 129 | uint32_t GetFirmwareVersion(void) const; 130 | 131 | // Return FPGA major version number (1, 2, 3) 132 | // Returns 0 if unknown 133 | unsigned int GetFpgaVersionMajor(void) const; 134 | 135 | /*! Get FPGA major version from Ethernet status register 136 | \param status Ethernet status register (input) 137 | \return unsigned int: FPGA major version (1, 2 or 3) 138 | */ 139 | static unsigned int GetFpgaVersionMajorFromStatus(uint32_t status); 140 | 141 | uint32_t GetHardwareVersion(void) const; 142 | std::string GetHardwareVersionString(void) const; 143 | 144 | // Returns FPGA clock period in seconds 145 | virtual double GetFPGAClockPeriod(void) const = 0; 146 | 147 | // ********************** READ Methods *********************************** 148 | // The ReadXXX methods below read data directly from the boards via the 149 | // bus using quadlet reads. 150 | 151 | uint32_t ReadStatus(void) const; 152 | 153 | /*! Read IPv4 address (only relevant for FPGA boards with Ethernet support) 154 | \returns IPv4 address (uint32) or 0 if error 155 | */ 156 | uint32_t ReadIPv4Address(void) const; 157 | 158 | // ********************** WRITE Methods ********************************** 159 | 160 | /*! Write IPv4 address. 161 | \param IP address to write, as uint32 162 | \returns true if successful; false otherwise 163 | */ 164 | bool WriteIPv4Address(uint32_t IPaddr); 165 | }; 166 | 167 | #endif // __BOARDIO_H__ 168 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # 2 | # (C) Copyright 2011-2025 Johns Hopkins University (JHU), All Rights Reserved. 3 | # 4 | # --- begin cisst license - do not edit --- 5 | # 6 | # This software is provided "as is" under an open source license, with 7 | # no warranty. The complete license can be found in license.txt and 8 | # http://www.cisst.org/cisst/license.txt. 9 | # 10 | # --- end cisst license --- 11 | 12 | cmake_minimum_required (VERSION 3.16) 13 | 14 | project (Amp1394 VERSION 2.2.0) 15 | 16 | # Set the version number 17 | set (Amp1394_VERSION "${Amp1394_VERSION_MAJOR}.${Amp1394_VERSION_MINOR}.${Amp1394_VERSION_PATCH}") 18 | 19 | # If LIBRARY_OUTPUT_PATH is not defined, define it here (note that if this 20 | # is built within cisst, then LIBRARY_OUTPUT_PATH is defined by cisst). 21 | if (NOT LIBRARY_OUTPUT_PATH) 22 | set (LIBRARY_OUTPUT_PATH "${CMAKE_CURRENT_BINARY_DIR}/lib") 23 | endif (NOT LIBRARY_OUTPUT_PATH) 24 | 25 | # If EXECUTABLE_OUTPUT_PATH is not defined, define it here (note that if this 26 | # is built within cisst, then EXECUTABLE_OUTPUT_PATH is defined by cisst). 27 | if (NOT EXECUTABLE_OUTPUT_PATH) 28 | set (EXECUTABLE_OUTPUT_PATH "${CMAKE_CURRENT_BINARY_DIR}/bin") 29 | endif (NOT EXECUTABLE_OUTPUT_PATH) 30 | 31 | # Set ARM32 when cross-compiling for Zynq 32 | if (Arch STREQUAL "arm32") 33 | set (ARM32 ON) 34 | else () 35 | set (ARM32 OFF) 36 | endif () 37 | 38 | # TODO: Move include files to Amp1394 subdirectory, i.e., lib/Amp1394 or lib/include/Amp1394. 39 | set (Amp1394_INCLUDE_DIR "${CMAKE_CURRENT_SOURCE_DIR}/lib;${Amp1394_BINARY_DIR}") 40 | set (Amp1394_LIBRARY_DIR ${LIBRARY_OUTPUT_PATH}) 41 | set (Amp1394_LIBRARIES Amp1394) 42 | set (Amp1394Console_LIBRARIES Amp1394Console) 43 | set (Amp1394Console_EXTRA_LIBRARIES "") 44 | 45 | if (WIN32) 46 | 47 | # On Windows, can build with WinPcap or Npcap (Ethernet support) 48 | # Use WinPcap for Windows XP and earlier (no longer maintained) 49 | # Use Npcap for Windows 7 and higher (use Npcap SDK) 50 | # OFF by default because UDP interface is preferred 51 | option (Amp1394_HAS_PCAP "Build Amp1394 with Ethernet support (winpcap or npcap)" OFF) 52 | 53 | if (Amp1394_HAS_PCAP) 54 | # Need to specify where WinPcap Developer's Pack or Npcap SDK is installed 55 | find_path (PCAP_INCLUDE_DIR pcap.h) 56 | # In addition to wpcap, need Packet library for PacketOpenAdapter and PacketReqest 57 | # Make sure to use correct version (32 or 64 bit) 58 | find_path (PCAP_LIBRARY_DIR "wpcap.lib" DOC "Path to WinPcap/Npcap wpcap and Packet libraries") 59 | set (PCAP_LIBRARIES "wpcap" "Packet") 60 | endif (Amp1394_HAS_PCAP) 61 | 62 | # Disable annoying warnings 63 | add_definitions(-D_CRT_SECURE_NO_WARNINGS) 64 | 65 | else () 66 | 67 | # On other platforms (mostly Linux), can build with pcap and/or libraw1394 68 | # On Zynq (embedded Linux), use EMIO interface to FPGA 69 | if (ARM32) 70 | option (Amp1394_HAS_EMIO "Build Amp1394 with Zynq EMIO support" ON) 71 | else () 72 | option (Amp1394_HAS_PCAP "Build Amp1394 with Ethernet support (pcap)" OFF) 73 | if (NOT APPLE) 74 | option (Amp1394_HAS_RAW1394 "Build Amp1394 with FireWire support (libraw1394)" ON) 75 | endif (NOT APPLE) 76 | endif () 77 | 78 | if (Amp1394_HAS_PCAP) 79 | # For now, assume pcap is installed somewhere standard 80 | # To install: sudo apt-get install libpcap-dev 81 | set (PCAP_INCLUDE_DIR "") 82 | set (PCAP_LIBRARIES "pcap") 83 | endif (Amp1394_HAS_PCAP) 84 | 85 | option (Amp1394Console_HAS_CURSES "Build Amp1394 console library with curses (OFF --> VT100 mode)" ON) 86 | 87 | if (Amp1394Console_HAS_CURSES) 88 | find_package (Curses) 89 | if (CURSES_FOUND) 90 | set (Amp1394Console_EXTRA_LIBRARIES ${Amp1394Console_EXTRA_LIBRARIES} ${CURSES_LIBRARIES}) 91 | else (CURSES_FOUND) 92 | message (STATUS "Cannot find Curses library, changing to VT100 mode") 93 | endif (CURSES_FOUND) 94 | endif (Amp1394Console_HAS_CURSES) 95 | 96 | endif () 97 | 98 | # TODO: Determine whether it is necessary to have separate EXTRA variables for LIBRARY_DIR 99 | # and LIBRARIES. Currently, it seems that both are always used together. 100 | # The Amp1394_EXTRA_INCLUDE_DIR should be separate since it is only needed when 101 | # building Amp1394. 102 | set (Amp1394_EXTRA_INCLUDE_DIR "") 103 | set (Amp1394_EXTRA_LIBRARY_DIR "") 104 | set (Amp1394_EXTRA_LIBRARIES "") 105 | if (Amp1394_HAS_RAW1394) 106 | # Assume libraw1394 is installed in standard include/lib directories 107 | set (Amp1394_EXTRA_LIBRARIES ${Amp1394_EXTRA_LIBRARIES} raw1394) 108 | endif (Amp1394_HAS_RAW1394) 109 | if (Amp1394_HAS_PCAP) 110 | set (Amp1394_EXTRA_INCLUDE_DIR ${PCAP_INCLUDE_DIR}) 111 | set (Amp1394_EXTRA_LIBRARY_DIR ${PCAP_LIBRARY_DIR}) 112 | set (Amp1394_EXTRA_LIBRARIES ${Amp1394_EXTRA_LIBRARIES} ${PCAP_LIBRARIES}) 113 | if (NOT WIN32) 114 | # Check if pcap_set_immediate_mode is available. 115 | # This may not be needed on Windows; if needed, would probably have to 116 | # specify PCAP_INCLUDE_DIR and PCAP_LIBRARY_DIR. 117 | include (CheckCXXSourceCompiles) 118 | set (CMAKE_REQUIRED_LIBRARIES ${PCAP_LIBRARIES}) 119 | check_cxx_source_compiles ("#include 120 | int main() 121 | { pcap_set_immediate_mode(0, 0); }" 122 | PCAP_HAS_IMMEDIATE_MODE) 123 | if (PCAP_HAS_IMMEDIATE_MODE) 124 | add_definitions (-DPCAP_HAS_IMMEDIATE_MODE) 125 | endif () 126 | endif () 127 | endif (Amp1394_HAS_PCAP) 128 | if (WIN32) 129 | # for Windows, need WinSock, Iphlpapi (for getting interface info) and Ws2_32 (for WSAIoctl) 130 | set (Amp1394_EXTRA_LIBRARIES ${Amp1394_EXTRA_LIBRARIES} WSOCK32 Iphlpapi Ws2_32) 131 | endif (WIN32) 132 | if (Amp1394_HAS_EMIO) 133 | set (Amp1394_EXTRA_LIBRARIES ${Amp1394_EXTRA_LIBRARIES} "fpgav3") 134 | endif (Amp1394_HAS_EMIO) 135 | 136 | # Generate Amp1394Config.cmake 137 | set (CONF_INCLUDE_DIR ${Amp1394_INCLUDE_DIR}) 138 | set (CONF_LIBRARY_DIR ${Amp1394_LIBRARY_DIR} ${Amp1394_EXTRA_LIBRARY_DIR}) 139 | set (CONF_LIBRARIES ${Amp1394_LIBRARIES} ${Amp1394_EXTRA_LIBRARIES}) 140 | set (CONF_CONSOLE_LIBRARIES ${Amp1394Console_LIBRARIES} ${Amp1394Console_EXTRA_LIBRARIES}) 141 | configure_file (Amp1394Config.cmake.in 142 | "${Amp1394_BINARY_DIR}/Amp1394Config.cmake" @ONLY) 143 | 144 | if (UNIX) 145 | # Add some warnings 146 | include (CheckCXXCompilerFlag) 147 | check_cxx_compiler_flag ("-Wextra" CXX_SUPPORTS_WEXTRA) 148 | if (CXX_SUPPORTS_WEXTRA) 149 | set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wextra") 150 | endif () 151 | 152 | check_cxx_compiler_flag ("-Wall" CXX_SUPPORTS_WALL) 153 | if (CXX_SUPPORTS_WALL) 154 | set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall") 155 | endif () 156 | 157 | check_cxx_compiler_flag ("-fPIC" CXX_SUPPORTS_FPIC) 158 | if (CXX_SUPPORTS_FPIC) 159 | set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIC") 160 | endif () 161 | endif (UNIX) 162 | 163 | # Utilities to test libraw1394 164 | if (Amp1394_HAS_RAW1394) 165 | add_subdirectory (util) 166 | endif (Amp1394_HAS_RAW1394) 167 | 168 | # Code for library Amp1394 169 | add_subdirectory (lib) 170 | 171 | # Test programs 172 | add_subdirectory (tests) 173 | 174 | # Utility to flash/test board 175 | add_subdirectory (programmer) 176 | -------------------------------------------------------------------------------- /programmer/mcsFile.cpp: -------------------------------------------------------------------------------- 1 | /* -*- Mode: C++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */ 2 | /* ex: set filetype=cpp softtabstop=4 shiftwidth=4 tabstop=4 cindent expandtab: */ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include "mcsFile.h" 9 | 10 | mcsFile::mcsFile() : line_num(0), startAddr(0L) 11 | { 12 | } 13 | 14 | mcsFile::~mcsFile() 15 | { 16 | } 17 | 18 | bool mcsFile::ProcessNextLine(RecInfo &rec) 19 | { 20 | // MCS file generated by ISE has line lengths no larger than 43 bytes 21 | const int MCS_LINE_MAX = 64; 22 | char buffer[MCS_LINE_MAX]; 23 | 24 | file.getline(buffer, sizeof(buffer)); 25 | line_num++; 26 | if (file.eof()) return false; 27 | 28 | if (buffer[0] != ':') return false; 29 | int nbytes = static_cast(file.gcount()); 30 | if (nbytes >= MCS_LINE_MAX) { 31 | // If this error occurs, increase MCS_LINE_MAX 32 | std::cerr << "ProcessNextLine: line " << line_num 33 | << " has too many characters, nbytes = " << nbytes << std::endl; 34 | return false; 35 | } 36 | if (!buffer[nbytes-1]) nbytes--; 37 | 38 | unsigned long csum_computed = 0L; 39 | if (!toHex(buffer+1, rec.ndata)) return false; 40 | csum_computed += rec.ndata; 41 | if (rec.ndata > sizeof(rec.data)) { 42 | // If this error occurs, increase RecInfo::DATA_MAX 43 | std::cerr << "ProcessNextLine: line " << line_num 44 | << " has too much data, num bytes = " << rec.ndata << std::endl; 45 | return false; 46 | } 47 | unsigned char addr_high, addr_low; 48 | if (!toHex(buffer+3, addr_high)) return false; 49 | csum_computed += addr_high; 50 | if (!toHex(buffer+5, addr_low)) return false; 51 | csum_computed += addr_low; 52 | rec.addr = (addr_high<<8)+addr_low; 53 | if (!toHex(buffer+7, rec.type)) return false; 54 | csum_computed += rec.type; 55 | 56 | // convert to binary and compute checksum 57 | for (unsigned int i = 0; i < rec.ndata; i++) { 58 | if (!toHex(buffer+9+2*i, rec.data[i])) return false; 59 | csum_computed += rec.data[i]; 60 | } 61 | unsigned char cksum; 62 | if (!toHex(buffer+9+2*rec.ndata, cksum)) return false; 63 | csum_computed += cksum; 64 | 65 | csum_computed &= 0x000000ff; 66 | if (csum_computed) { 67 | std::cerr << "ProcessNextLine: line " << line_num 68 | << " checksum error = " << csum_computed << std::endl; 69 | return false; 70 | } 71 | return true; 72 | } 73 | 74 | bool mcsFile::toHex(const char *p2, unsigned char &result) const 75 | { 76 | const unsigned int NUM_DIGITS = 2; 77 | unsigned char digit[NUM_DIGITS]; 78 | result = 0; 79 | for (unsigned int i = 0; i < NUM_DIGITS; i++) { 80 | digit[i] = p2[i]-'0'; 81 | if (digit[i] > 9) { 82 | digit[i] = toupper(p2[i])-'A'; 83 | if (digit[i] > 5) return false; 84 | digit[i] += 10; 85 | } 86 | result |= digit[i] << (4*(NUM_DIGITS-i-1)); 87 | } 88 | return true; 89 | } 90 | 91 | bool mcsFile::OpenFile(const std::string &fileName) 92 | { 93 | file.open(fileName.c_str()); 94 | if (!file.is_open()) { 95 | std::cerr << "mcsFile: could not open input file " << fileName << std::endl; 96 | return false; 97 | } 98 | line_num = 0; 99 | return true; 100 | } 101 | 102 | bool mcsFile::ReadNextSector() 103 | { 104 | RecInfo rec; 105 | if (!ProcessNextLine(rec)) return false; 106 | if (rec.type != RECORD_EXT_LINEAR) { 107 | std::cerr << "ReadNextSector: line " << line_num << " does not start a sector" << std::endl; 108 | return false; 109 | } 110 | if (rec.ndata != 2) { 111 | std::cerr << "ReadNextSector: line " << line_num << " has invalid data length = " 112 | << rec.ndata << std::endl; 113 | return false; 114 | } 115 | startAddr = ((rec.data[0]<<16)+rec.data[1]) << 16; 116 | numBytes = 0L; 117 | while ((numBytes < sizeof(curSector)) && ProcessNextLine(rec)) { 118 | if (rec.type == RECORD_DATA) { 119 | if (numBytes != rec.addr) { 120 | std::cerr << "ReadNextSector: line " << line_num 121 | << ", expected offset " << numBytes << ", got offset " 122 | << rec.addr << std::endl; 123 | return false; 124 | } 125 | memcpy(curSector+numBytes, rec.data, rec.ndata); 126 | numBytes += rec.ndata; 127 | } 128 | else if (rec.type == RECORD_EOF) 129 | break; 130 | else 131 | std::cerr << "ReadNextSector: line " << line_num 132 | << " ignoring record type " << rec.type << std::endl; 133 | } 134 | // Pad sector with 0xff, just in case 135 | if (numBytes < sizeof(curSector)) { 136 | memset(curSector+numBytes, 0xff, sizeof(curSector)-numBytes); 137 | } 138 | else if (numBytes > sizeof(curSector)) 139 | std::cerr << "ReadNextSector: too many bytes = " << numBytes << std::endl; 140 | return true; 141 | } 142 | 143 | bool mcsFile::VerifySector(const unsigned char *data, unsigned long len) const 144 | { 145 | unsigned int lim = (len < sizeof(curSector)) ? len : sizeof(curSector); 146 | int num = 0; // number of mismatches 147 | unsigned int i; 148 | for (i = 0; i < lim; i++) { 149 | if (curSector[i] != data[i]) { 150 | std::cout << std::hex << "Mismatch at address " << startAddr+i 151 | << ", file = " << (unsigned int)curSector[i] 152 | << ", prom = " << (unsigned int)data[i] << std::endl; 153 | if (++num >= 10) 154 | break; 155 | } 156 | } 157 | return (i == lim) && (num == 0); 158 | } 159 | 160 | void mcsFile::Rewind() 161 | { 162 | file.clear(); 163 | file.seekg(0, std::ios_base::beg); 164 | } 165 | 166 | void mcsFile::CloseFile() 167 | { 168 | std::cout << "Processed " << line_num << " lines" << std::endl; 169 | file.close(); 170 | } 171 | 172 | void mcsFile::WriteSectorHeader(std::ofstream &file, unsigned int num) 173 | { 174 | file << std::hex << std::setfill('0') << std::uppercase; 175 | file << ":" << std::hex << "02000004"; 176 | unsigned long csum_computed = 0x02 + 0x04; 177 | file << std::setw(4) << static_cast(num); 178 | csum_computed += num&0x000000ff; 179 | csum_computed += (num&0x0000ff00)>>8; 180 | csum_computed = (~csum_computed+1)&0x000000ff; 181 | file << std::setw(2) << csum_computed << std::endl; 182 | } 183 | 184 | void mcsFile::WriteDataLine(std::ofstream &file, unsigned long addr, unsigned char *bytes, unsigned int numBytes) 185 | { 186 | if (numBytes == 0) return; 187 | file << ":" << std::setw(2) << numBytes; 188 | unsigned long csum_computed = numBytes; 189 | file << std::setw(4) << static_cast(addr); 190 | csum_computed += addr&0x000000ff; 191 | csum_computed += (addr&0x0000ff00)>>8; 192 | file << "00"; // type = RECORD_DATA 193 | for (unsigned int i = 0; i < numBytes; i++) { 194 | csum_computed += bytes[i]; 195 | file << std::setw(2) << (unsigned int) bytes[i]; 196 | } 197 | csum_computed = (~csum_computed+1)&0x000000ff; 198 | file << std::setw(2) << csum_computed << std::endl; 199 | } 200 | 201 | void mcsFile::WriteEOF(std::ofstream &file) 202 | { 203 | file << ":00000001FF" << std::endl; 204 | } 205 | -------------------------------------------------------------------------------- /lib/EncoderVelocity.h: -------------------------------------------------------------------------------- 1 | /* -*- Mode: C++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */ 2 | /* ex: set filetype=cpp softtabstop=4 shiftwidth=4 tabstop=4 cindent expandtab: */ 3 | 4 | /* 5 | Author(s): Peter Kazanzides, Jie Ying Wu, Zihan Chen 6 | 7 | (C) Copyright 2021-2022 Johns Hopkins University (JHU), All Rights Reserved. 8 | 9 | --- begin cisst license - do not edit --- 10 | 11 | This software is provided "as is" under an open source license, with 12 | no warranty. The complete license can be found in license.txt and 13 | http://www.cisst.org/cisst/license.txt. 14 | 15 | --- end cisst license --- 16 | */ 17 | 18 | #ifndef __ENCODER_VELOCITY_H__ 19 | #define __ENCODER_VELOCITY_H__ 20 | 21 | #include "Amp1394Types.h" 22 | 23 | // Methods for computing encoder velocity (and acceleration) based on FPGA measured periods 24 | // between encoder edges. This class handles the differences between firmware versions. 25 | // The approach is documented in the following paper: 26 | // 27 | // J. Y. Wu, Z. Chen, A. Deguet, and P. Kazanzides, "FPGA-based velocity estimation for 28 | // control of robots with low-resolution encoders", IEEE/RSJ Intl. Conf. on Intelligent 29 | // Robots and Systems (IROS), Oct 2018. 30 | // 31 | // The most accurate velocity estimate will be returned by GetEncoderVelocityPredicted, which 32 | // estimates the acceleration and uses it to compensate for the estimated delay. 33 | 34 | class EncoderVelocity { 35 | protected: 36 | double clkPeriod; // Clock period, in seconds 37 | uint32_t velPeriod; // Encoder full-cycle period (for velocity) 38 | bool velOverflow; // Velocity (period) overflow 39 | uint32_t velPeriodMax; // Maximum possible velocity period 40 | bool velDir; // Velocity direction (true -> positive direction) 41 | bool dirChange; // Direction change during last velocity period, V7+ 42 | bool encError; // Encoder error detected, V7+ 43 | bool partialCycle; // No full cycle yet, V7+ 44 | uint32_t qtr1Period; // Encoder quarter-cycle period 1 (for accel), V6+ 45 | bool qtr1Overflow; // Qtr1 overflow, V7+ 46 | bool qtr1Dir; // Qtr1 direction (true -> positive direction), V7+ 47 | unsigned char qtr1Edges; // Qtr1 edge mask (A-up, B-up, A-down, B-down), V7+ 48 | uint32_t qtr5Period; // Encoder quarter-cycle period 5 (for accel), V6+ 49 | bool qtr5Overflow; // Qtr5 overflow, V7+ 50 | bool qtr5Dir; // Qtr5 direction (true -> positive direction), V7+ 51 | unsigned char qtr5Edges; // Qtr5 edge mask (A-up, B-up, A-down, B-down), V7+ 52 | uint32_t qtrPeriodMax; // Maximum Qtr1 or Qtr5 period 53 | uint32_t runPeriod; // Time since last encoder edge, Firmware V4,5,7+ 54 | bool runOverflow; // Running counter overflow, V7+ 55 | 56 | public: 57 | EncoderVelocity() { Init(); } 58 | ~EncoderVelocity() {} 59 | 60 | void Init(); 61 | 62 | // SetData for Firmware Rev 7+ 63 | // Set isESPM true for dVRK-Si 64 | void SetData(uint32_t rawPeriod, uint32_t rawQtr1, uint32_t rawQtr5, uint32_t rawRun, 65 | bool isESPM = false); 66 | // SetData for Firmware Rev 6 67 | void SetDataRev6(uint32_t rawPeriod, uint32_t rawQtr); 68 | // SetData for Firmware <= 5; useRunCounter should be true for Firmware Rev 4-5 69 | void SetDataOld(uint32_t rawPeriod, bool useRunCounter); 70 | 71 | /*! Returns the encoder velocity, in counts per second, based on the FPGA measurement 72 | of the encoder period (i.e., time between two consecutive edges). Specifically, the 73 | velocity is given by 4/(period*clk) counts/sec, where clk is the period of the clock used to measure 74 | the encoder period. The numerator is 4 because an encoder period is equal to 4 counts (quadrature). 75 | If the counter overflows, the velocity is set to 0. This method has improved performance starting 76 | with Firmware V6. For a better velocity estimate, see the GetEncoderVelocityPredicted method. 77 | */ 78 | double GetEncoderVelocity() const; 79 | 80 | /*! Returns the predicted encoder velocity, in counts per second, based on the FPGA measurement of the 81 | encoder period (i.e., time between two consecutive edges), with compensation for the measurement delay. 82 | For Firmware Rev 6+, the predicted encoder velocity uses the estimated acceleration to predict the 83 | velocity at the current time. For Rev 7+, the prediction also uses the encoder running counter (time 84 | since last edge). There are two limits enforced: 85 | 1) The predicted velocity will not change sign (i.e., be in the opposite direction from the measured velocity). 86 | 2) The predicted velocity will not be larger than the velocity that would have caused one encoder count to 87 | occur during the time measured by the running counter. 88 | For an explanation of percent_threshold, see GetEncoderAcceleration. 89 | */ 90 | double GetEncoderVelocityPredicted(double percent_threshold = 1.0) const; 91 | 92 | /*! Returns the encoder acceleration in counts per second**2, based on the scaled difference 93 | between the most recent full cycle and the previous full cycle. If the encoder counter overflowed 94 | (i.e., velocity was very slow or zero), the acceleration is set to 0. Since velocity is averaged 95 | over an entire cycle, acceleration can be applied over half the cycle to reduce delays. 96 | The percent_threshold is between 0 and 1 and essentially controls the maximum velocity for 97 | which acceleration is estimated. Specifically, if T is the most recent quarter-cycle period, 98 | then the acceleration is set to 0 if the magnitude of 1/T is greater than percent_threshold. 99 | This can avoid noisy acceleration estimates in those cases. Setting percent_threshold to 1.0 100 | effectively disables this feature. */ 101 | double GetEncoderAcceleration(double percent_threshold = 1.0) const; 102 | 103 | /*! Get the encoder running counter, in seconds. This is primarily used for Firmware Rev 7+, but 104 | also supports the running counter in Firmware Rev 4-5. 105 | */ 106 | double GetEncoderRunningCounterSeconds() const; 107 | 108 | /* Returns true if an encoder error was detected (V7+) */ 109 | bool IsEncoderError() { return encError; } 110 | 111 | //*********** Following methods used by qladisp and enctest ************/ 112 | 113 | // Returns the raw encoder velocity period 114 | uint32_t GetEncoderVelocityPeriod() const 115 | { return velPeriod; } 116 | 117 | // Returns the raw encoder quarter1 period 118 | uint32_t GetEncoderQuarter1Period() const 119 | { return qtr1Period; } 120 | 121 | // Returns the raw encoder quarter5 period 122 | uint32_t GetEncoderQuarter5Period() const 123 | { return qtr5Period; } 124 | 125 | // Indicates whether running counter has overflowed 126 | bool IsRunningCounterOverflow() const {return runOverflow; } 127 | 128 | enum EdgeMask { A_UP = 0x08, B_UP = 0x04, A_DN = 0x02, B_DN = 0x01 }; 129 | 130 | // Returns the quarter 1 edge mask 131 | // Normally, only one edge should be detected (see EdgeMask) 132 | unsigned char GetEncoderQuarter1Edges() const { return qtr1Edges; } 133 | 134 | // Returns the quarter 5 edge mask 135 | // Normally, only one edge should be detected (see EdgeMask) 136 | unsigned char GetEncoderQuarter5Edges() const { return qtr5Edges; } 137 | 138 | // Returns whether there was a direction change 139 | bool GetEncoderDirChange() const { return dirChange; } 140 | }; 141 | 142 | #endif // __ENCODER_VELOCITY_H__ 143 | -------------------------------------------------------------------------------- /python/test_eth1394.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import Amp1394Python as amp1394 4 | import time 5 | import numpy as np 6 | import argparse 7 | from random import randint 8 | from Amp1394Python import bswap32, bswap16 9 | 10 | def TestWriteQuadletBroadcast(fw): 11 | """Write Quadlet Broadcast""" 12 | wval = randint(1, 1000) 13 | fw.WriteQuadletBroadcast(0xffffff001000, 0x03, wval) 14 | nnodes = fw.GetNumOfNodes() 15 | 16 | 17 | def TestQRead(eth, fw, bid): 18 | wval = randint(1, 1000) 19 | fw.WriteQuadlet(bid, 0x03, wval) 20 | ret, rval = eth.ReadQuadlet(bid, 0x03) 21 | print "wval = " + str(wval) + " rval = " + str(rval) 22 | if (wval == rval): 23 | return True 24 | else: 25 | return False 26 | 27 | def TestQWrite(eth, fw, bid): 28 | wval = randint(1, 1000) 29 | eth.WriteQuadlet(bid, 0x03, wval) 30 | ret, rval = fw.ReadQuadlet(bid, 0x03) 31 | print "wval = " + str(wval) + " rval = " + str(rval) 32 | if (wval == rval): 33 | return True 34 | else: 35 | return False 36 | 37 | def TestBRead(eth, fw, bid): 38 | wval = [randint(1,1000), randint(1,1000), randint(1,1000)] 39 | fw.WriteBlock(bid, 0x1000, wval) 40 | ret, rval = eth.ReadBlock(bid, 0x1000, len(wval)) 41 | if not ret: 42 | return ret 43 | print "wval = " + str(wval) + " rval = " + str(rval) 44 | ispass = True 45 | for i in range(0, len(wval)): 46 | if (wval[i] != rval[i]): 47 | ispass = False 48 | return ispass 49 | 50 | def TestBWrite(eth, fw, bid): 51 | wval = [randint(1,1000), randint(1,1000), randint(1,1000)] 52 | eth.WriteBlock(bid, 0x1000, wval) 53 | ret, rval = eth.ReadBlock(bid, 0x1000, len(wval)) 54 | if not ret: return ret 55 | print "wval = " + str(wval) + " rval = " + str(rval) 56 | ispass = True 57 | for i in range(0, len(wval)): 58 | if (wval[i] != rval[i]): 59 | ispass = False 60 | return ispass 61 | 62 | 63 | def TestContinuousRead(eth, bid=3, len=100): 64 | print "Continuous read" 65 | 66 | hver = 0x514c4131 67 | success = 0 68 | readFailures = 0 69 | compareFailures = 0 70 | start = time.time() 71 | for i in range(1,len+1): 72 | ret = False 73 | rval = 0 74 | [ret, rval] = eth.ReadQuadlet(bid, 0x04) 75 | time.sleep(0.001) 76 | if not ret: 77 | print "failed to read quadlet" 78 | readFailures = readFailures + 1 79 | elif (rval != hver): 80 | print "Not a QLA1 board" 81 | compareFailures = compareFailures + 1 82 | else: 83 | success = success + 1 84 | 85 | # if (i%100 == 0): 86 | # print "i = " + str(i) + " success = " + str(success) + \ 87 | # " read failures = " + str(readFailures) + \ 88 | # " compare failures = " + str(compareFailures) 89 | 90 | print "Time elapsed for " + str(len) + " read = " + \ 91 | str( time.time() - start ) + " sec" 92 | 93 | print "Done Total = " + str(len) + " success = " + str(success) + \ 94 | " read failures = " + str(readFailures) + \ 95 | " compare failures = " + str(compareFailures) 96 | 97 | 98 | def TestContinuousWrite(eth, bid=3, len=100): 99 | print "Continuous Write" 100 | 101 | start = time.time() 102 | # write 103 | for i in range(0,len): 104 | eth.WriteQuadlet(bid, 0x03, i) 105 | 106 | print "Time elapsed for " + str(len) + " write = " + \ 107 | str( time.time() - start ) + " sec" 108 | 109 | # read 110 | [ret, rval] = eth.ReadQuadlet(bid, 0x03) 111 | print "i = " + str(i) + " rval = " + str(rval) 112 | 113 | def TestContinuous(eth, bid=0, len=100): 114 | print "Continuous Write/Read" 115 | start = time.time() 116 | for i in range(0,len): 117 | twait = 0.00001 118 | wval = i % 65535 119 | wret = eth.WriteQuadlet(bid, 0x03, wval) 120 | # time.sleep(twait) 121 | [rret, val] = eth.ReadQuadlet(bid, 0x03) 122 | # time.sleep(twait) 123 | if not wret or not rret: 124 | print "write = " + str(wval) + " read = " + str(val) 125 | elif wval != val: 126 | print "wval = " + str(wval) + " rval = " + str(val) 127 | 128 | print "Time elapsed for " + str(len) + " W/R = " + \ 129 | str( time.time() - start ) + " sec" 130 | 131 | def ReceivePacket(bd): 132 | # bd: firewire board 133 | _, RXFCTR = bd.ReadKSZ8851Reg(0x9C) 134 | _, RXFHSR = bd.ReadKSZ8851Reg(0x7C) 135 | _, RXFHBCR = bd.ReadKSZ8851Reg(0x7E) 136 | bd.WriteKSZ8851Reg(0x86, 0x5000) 137 | _, RXQCR = bd.ReadKSZ8851Reg(0x82) 138 | bd.WriteKSZ8851Reg(0x82, RXQCR|0x0008) 139 | 140 | numWords = (((RXFHBCR&0x0FFF) + 3) & 0xFFFC) >> 1 141 | print "num bytes = " + str(RXFHBCR) 142 | print "num words = " + str(numWords) 143 | 144 | # skip 145 | dummy = bd.ReadKSZ8851DMA() 146 | status = bd.ReadKSZ8851DMA() 147 | length = bd.ReadKSZ8851DMA() 148 | 149 | dataStr = '' 150 | for i in range(0, numWords): 151 | _, data = bd.ReadKSZ8851DMA() 152 | dataStr = dataStr + format(bswap16(data), '04X') + ' ' 153 | if i == 3 or i == 6: 154 | dataStr = dataStr + '\n' 155 | if (i > 7) and (i+2)%4==0: 156 | dataStr = dataStr + '\n' 157 | print dataStr 158 | 159 | _, RXQCR = bd.ReadKSZ8851Reg(0x82) 160 | bd.WriteKSZ8851Reg(0x82, RXQCR&0xFFF7) 161 | 162 | 163 | 164 | # Test setup 165 | # - connect two boards (bid = 0 & 3) 166 | # - test quadlet read/write 167 | # - on directly connected board 168 | # - on firewire indirectly connected board 169 | # - test block read/write 170 | 171 | def PrintEthDebug(fwbd): 172 | status = fwbd.ReadKSZ8851Status() 173 | dbgstr = "" 174 | if (status&0x4000): dbgstr = dbgstr + "error " 175 | if (status&0x2000): dbgstr = dbgstr + "initOK " 176 | if (status&0x1000): dbgstr = dbgstr + "initReq " 177 | if (status&0x0800): dbgstr = dbgstr + "ethIoErr " 178 | if (status&0x0400): dbgstr = dbgstr + "PacketErr " 179 | if (status&0x0200): dbgstr = dbgstr + "DestErr " 180 | if (status&0x0100): dbgstr = dbgstr + "qRead " 181 | if (status&0x0080): dbgstr = dbgstr + "qWrite " 182 | if (status&0x0040): dbgstr = dbgstr + "bRead " 183 | if (status&0x0020): dbgstr = dbgstr + "bWrite " 184 | if (status&0x0010): dbgstr = dbgstr + "multicast " 185 | if (status&0x0008): dbgstr = dbgstr + "KSZ-idle " 186 | if (status&0x0004): dbgstr = dbgstr + "ETH-idle " 187 | print "Eth dbg = " + dbgstr 188 | 189 | 190 | 191 | if __name__ == '__main__': 192 | 193 | parser = argparse.ArgumentParser() 194 | parser.add_argument('-b', '--board', default=0, type=int, 195 | help="board id, default = 0") 196 | parser.add_argument('-f', '--firewire', default=False, help="firewire only mode") 197 | parser.add_argument('-t', '--test', default=False, help="enable basic test") 198 | args = parser.parse_args() 199 | 200 | print "====================" 201 | print "bid = " + str(args.board) 202 | bid = args.board 203 | 204 | eth = None 205 | if args.firewire: 206 | fw = amp1394.FirewirePort(0) 207 | bd1 = amp1394.AmpIO(bid) 208 | fw.AddBoard(bd1) 209 | else: 210 | fw = amp1394.FirewirePort(0) 211 | bd1 = amp1394.AmpIO(bid) 212 | fw.AddBoard(bd1) 213 | 214 | eth = amp1394.EthUdpPort(0) 215 | bd2 = amp1394.AmpIO(bid) 216 | eth.AddBoard(bd2) 217 | 218 | if args.test: 219 | TestQRead(eth, fw, bid) 220 | TestQWrite(eth, fw, bid) 221 | TestBRead(eth, fw, bid) 222 | TestBWrite(eth, fw, bid) 223 | 224 | fw.RemoveBoard(bid) 225 | if eth: 226 | eth.RemoveBoard(bid) 227 | 228 | #try: 229 | # import ipdb; ipdb.set_trace() 230 | #except ImportError: 231 | # print 'Failed to import ipdb' 232 | -------------------------------------------------------------------------------- /util/block1394.c: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * 3 | * This is a minimal Linux command line program that reads/writes blocks of 4 | * data from/to the 1394-based controller. As a special case, if the program 5 | * name (argv[0]) contains quad1394, it reads/write one quadlet (32-bit value). 6 | * 7 | * Compile: gcc -Wall -lraw1394 -o 8 | * Usage: [-pP] [-nN] address [size] [value1, ....] 9 | * P - IEEE-1394 port 10 | * N - address of node to talk to (1394 node ID, not board switch) 11 | * size - number of quadlets to read or write (1=quadlet transfer) 12 | * address - address, in hex, to read from or write to (quadlets only) 13 | * valuen - one or more quadlet values to write, in hex 14 | * read operation performed if none specified 15 | * values autofilled if the number of values < size argument 16 | * Returns: for read operation, list of quadlet values read, one per line 17 | * 18 | * Notes: 19 | * - Block reads/writes apply only to hardwired real-time data registers 20 | * - Quadlet reads/writes can access any address, though some are read-only 21 | * - Node IDs are assigned automatically: n slaves get IDs 0 to n-1, PC ID=n 22 | * 23 | ******************************************************************************/ 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include // strerror 30 | #include // errno message 31 | 32 | // libraw1394 33 | #include 34 | #include //1394 CSR constants 35 | 36 | raw1394handle_t handle; 37 | 38 | /* bus reset handler updates the bus generation */ 39 | int reset_handler(raw1394handle_t hdl, unsigned int gen) { 40 | int id = raw1394_get_local_id(hdl); 41 | printf("Bus reset to gen %d, local id %d\n", gen, id); 42 | raw1394_update_generation(hdl, gen); 43 | return 0; 44 | } 45 | 46 | /* signal handler cleans up and exits the program */ 47 | void signal_handler(int sig) { 48 | signal(SIGINT, SIG_DFL); 49 | raw1394_destroy_handle(handle); 50 | exit(0); 51 | } 52 | 53 | /******************************************************************************* 54 | * main program 55 | */ 56 | int main(int argc, char** argv) 57 | { 58 | int i, j, rc, args_found; 59 | int nports; 60 | int port; 61 | nodeid_t node; 62 | nodeaddr_t addr; 63 | int size; 64 | quadlet_t data1; 65 | quadlet_t *data = &data1; 66 | 67 | signal(SIGINT, signal_handler); 68 | 69 | int isQuad1394 = (strstr(argv[0], "quad1394") != 0); 70 | int isBroadcast = 0; 71 | int isDebug = 0; 72 | 73 | port = 0; 74 | node = 0; 75 | size = 1; 76 | j = 0; 77 | args_found = 0; 78 | for (i = 1; i < argc; i++) { 79 | if (argv[i][0] == '-') { 80 | if (argv[i][1] == 'p') { 81 | port = atoi(argv[i]+2); 82 | printf("Selecting port %d\n", port); 83 | } 84 | else if (argv[i][1] == 'n') { 85 | node = atoi(argv[i]+2); 86 | printf("Selecting node %d\n", node); 87 | } 88 | else if (argv[i][1] == 'd') { 89 | isDebug = 1; 90 | } 91 | } 92 | else { 93 | if (args_found == 0) 94 | addr = strtoull(argv[i], 0, 16); 95 | else if ((args_found == 1) && (isQuad1394)) 96 | data1 = bswap_32(strtoul(argv[i], 0, 16)); 97 | else if ((args_found == 1) && (!isQuad1394)) { 98 | size = strtoul(argv[i], 0, 10); 99 | /* Allocate data array, initializing contents to 0 */ 100 | data = (quadlet_t *) calloc(sizeof(quadlet_t), size); 101 | if (!data) { 102 | fprintf(stderr, "Failed to allocate memory for %d quadlets", size); 103 | exit(-1); 104 | } 105 | } 106 | else if (!isQuad1394 && (j < size)) 107 | data[j++] = bswap_32(strtoul(argv[i], 0, 16)); 108 | else 109 | fprintf(stderr, "Warning: extra parameter: %s\n", argv[i]); 110 | 111 | args_found++; 112 | } 113 | } 114 | 115 | if (args_found < 1) { 116 | if (isQuad1394) 117 | printf("Usage: %s [-pP] [-nN]
[value to write in hex]\n", argv[0]); 118 | else 119 | printf("Usage: %s [-pP] [-nN]
[write data quadlets in hex]\n", argv[0]); 120 | printf(" where P = port number, N = node number\n"); 121 | exit(0); 122 | } 123 | 124 | /* get handle to device and check for errors */ 125 | handle = raw1394_new_handle(); 126 | if (handle == NULL) { 127 | fprintf(stderr, "**** Error: could not open 1394 handle\n"); 128 | exit(-1); 129 | } 130 | 131 | /* set the bus reset handler */ 132 | raw1394_set_bus_reset_handler(handle, reset_handler); 133 | 134 | /* get number of ports and check for errors */ 135 | nports = raw1394_get_port_info(handle, NULL, 0); 136 | if (nports < 0) { 137 | fprintf(stderr, "**** Error: could not get port info\n"); 138 | exit(-1); 139 | } 140 | if ((port < 0) || (port >= nports)) { 141 | fprintf(stderr, "**** Error: port %d does not exist (num ports = %d)\n", port, nports); 142 | exit(-1); 143 | } 144 | 145 | /* set handle to current port and skip errors */ 146 | rc = raw1394_set_port(handle, port); 147 | if (rc) { 148 | fprintf(stderr, "**** Error setting port %d\n", port); 149 | exit(-1); 150 | } 151 | 152 | // get local id and printf libraw1394 version & local_node_id for debugging 153 | nodeid_t id = raw1394_get_local_id(handle); 154 | if (isDebug) { 155 | printf("libraw1394_version = %s local_node_id = %x\n", raw1394_get_libversion(), id); 156 | } 157 | 158 | /* get number of nodes connected to current port/handle */ 159 | int nnodes = raw1394_get_nodecount(handle); 160 | // bool 161 | if (node == 63) { 162 | isBroadcast = 1; 163 | printf("**** Warning: Broadcasting message \n"); 164 | printf(" Address should be larger than CSR_CONFIG_ROM_END(0x%llx)\n", CSR_CONFIG_ROM_END + CSR_REGISTER_BASE); 165 | } else if ((node < 0) || (node >= nnodes)) { 166 | fprintf(stderr, "**** Error: node %d does not exist (num nodes = %d)\n", node, nnodes); 167 | exit(-1); 168 | } 169 | nodeid_t target_node = (id & 0xFFC0)+node; 170 | 171 | /* determine whether to read or write based on args_found */ 172 | if ((isQuad1394 && (args_found == 1)) || 173 | (!isQuad1394 && (args_found <= 2))) 174 | { 175 | if (isBroadcast) { 176 | fprintf(stderr, "**** Error: broadcast read NOT supported on node %d (num nodes = %d)\n", node, nnodes); 177 | } 178 | 179 | /* read the data block and print out the values */ 180 | rc = raw1394_read(handle, target_node, addr, size*4, data); 181 | if (!rc) { 182 | for (j=0; j 5 not yet released) 137 | * Can be compiled on Windows for future ethernet support 138 | * Updated Python wrappers 139 | * Bug fixes: 140 | * qladisp and qlatest: fixed streaming of long error messages 141 | 142 | 1.2.1 (2016-08-31) 143 | ================== 144 | * API changes: 145 | * None 146 | * New features: 147 | * Compilation: use -fPIC when available 148 | * Bug fixes: 149 | * None 150 | 151 | 1.2.0 (2015-10-18) 152 | ================== 153 | * API changes: 154 | * New features: 155 | * Added utility qlacloserelays to close all safety relays on controllers connected 156 | * When qladisp is started w/o board numbers, display results of port query 157 | * Added GetFPGASerialNumber and GetQLASerialNumber methods 158 | * Added code to read/write digital outputs 159 | * Added code to support PWM (requires firmware 5+) 160 | * Bug fixes: 161 | * Reversed order of digital outputs (fixed in firmware 5+) 162 | 163 | 1.1.0 (2015-04-28) 164 | ================== 165 | 166 | * API changes: 167 | * Encoder API now uses signed integers, assumes all values are related to mid range (for setters and getters) 168 | * Default protocol is now broadcast write if all boards have firmware version 4.0 or higher 169 | * Deprecated features: 170 | * `SetUseBroadcastFlag` has been replaced by `SetProtocol` 171 | * New features: 172 | * Added revision number, new header file `AmpIORevision.h` needs to be included 173 | * Added method to get encoder channel A/B 174 | * Added `ProtocolType` to select between firewire no broadcast, write only broadcast or read/write broadcast 175 | * Added method to get encoder overflow bit 176 | * Bug fixes: 177 | * None 178 | 179 | 1.0.0 (2014-01-24) 180 | ================== 181 | 182 | * No change log file, initial release. 183 | -------------------------------------------------------------------------------- /lib/code/EncoderVelocity.cpp: -------------------------------------------------------------------------------- 1 | /* -*- Mode: C++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */ 2 | /* ex: set filetype=cpp softtabstop=4 shiftwidth=4 tabstop=4 cindent expandtab: */ 3 | 4 | /* 5 | Author(s): Peter Kazanzides, Jie Ying Wu, Zihan Chen 6 | 7 | (C) Copyright 2011-2022 Johns Hopkins University (JHU), All Rights Reserved. 8 | 9 | --- begin cisst license - do not edit --- 10 | 11 | This software is provided "as is" under an open source license, with 12 | no warranty. The complete license can be found in license.txt and 13 | http://www.cisst.org/cisst/license.txt. 14 | 15 | --- end cisst license --- 16 | */ 17 | 18 | #include "EncoderVelocity.h" 19 | 20 | const uint32_t ENC_VEL_MASK_16 = 0x0000ffff; /*!< Mask for encoder velocity (period) bits, Firmware Version <= 5 (16 bits) */ 21 | const uint32_t ENC_VEL_MASK_22 = 0x003fffff; /*!< Mask for encoder velocity (period) bits, Firmware Version == 6 (22 bits) */ 22 | const uint32_t ENC_VEL_MASK_26 = 0x03ffffff; /*!< Mask for encoder velocity (period) bits, Firmware Version >= 7 (26 bits) */ 23 | 24 | // The following masks read the most recent quarter-cycle period and the previous one of the same type, which are used 25 | // for estimating acceleration in Firmware Rev 6+. 26 | // Following are for Firmware Rev 6, which split the bits among different fields due to packet size limitations. 27 | const uint32_t ENC_ACC_REC_MS_MASK = 0xfff00000; /*!< Mask (into encoder freq/acc) for upper 12 bits of most recent quarter-cycle period */ 28 | const uint32_t ENC_ACC_REC_LS_MASK = 0x3fc00000; /*!< Mask (into encoder period) for lower 8 bits of most recent quarter-cycle period */ 29 | const uint32_t ENC_ACC_PREV_MASK = 0x000fffff; /*!< Mask (into encoder period) for all 20 bits of previous quarter-cycle period */ 30 | // Following are for Firmware Rev 7+, which increased the block read packet size, allowing all bits to be grouped together 31 | const uint32_t ENC_VEL_QTR_MASK = 0x03ffffff; /*!< Mask (into encoder QTR1/QTR5) for all 26 bits of quarter cycle period */ 32 | 33 | // Following offsets are for FPGA Firmware Version 6 (22 bits) and 7+ (26 bits) 34 | // (Note that older versions of software assumed that Firmware Version 6 would have different bit assignments) 35 | const uint32_t ENC_VEL_OVER_MASK = 0x80000000; /*!< Mask for encoder velocity (period) overflow bit */ 36 | const uint32_t ENC_DIR_MASK = 0x40000000; /*!< Mask for encoder velocity (period) direction bit */ 37 | const uint32_t ENC_DIR_CHANGE_MASK = 0x20000000; /*!< Mask for encoder velocity (period) direction change (V7+) */ 38 | 39 | const double VEL_PERD_ESPM = 1.0/80000000; /* Clock period for ESPM velocity measurements (dVRK Si) */ 40 | const double VEL_PERD = 1.0/49152000; /* Clock period for velocity measurements (Rev 7+ firmware) */ 41 | const double VEL_PERD_REV6 = 1.0/3072000; /* Slower clock for velocity measurements (Rev 6 firmware) */ 42 | const double VEL_PERD_OLD = 1.0/768000; /* Slower clock for velocity measurements (prior to Rev 6 firmware) */ 43 | 44 | void EncoderVelocity::Init() 45 | { 46 | clkPeriod = 1.0; 47 | velPeriod = 0; 48 | velOverflow = false; 49 | velPeriodMax = 0; 50 | velDir = false; 51 | dirChange = false; 52 | encError = false; 53 | partialCycle = true; 54 | qtr1Period = 0; 55 | qtr1Overflow = false; 56 | qtr1Dir = false; 57 | qtr1Edges = 0; 58 | qtr5Period = 0; 59 | qtr5Overflow = false; 60 | qtr5Dir = false; 61 | qtr5Edges = 0; 62 | qtrPeriodMax = 0; 63 | runPeriod = 0; 64 | runOverflow = false; 65 | } 66 | 67 | // SetData for Firmware V7+ 68 | 69 | void EncoderVelocity::SetData(uint32_t rawPeriod, uint32_t rawQtr1, uint32_t rawQtr5, uint32_t rawRun, 70 | bool isESPM) 71 | { 72 | Init(); 73 | clkPeriod = isESPM ? VEL_PERD_ESPM : VEL_PERD; 74 | velPeriodMax = ENC_VEL_MASK_26; 75 | qtrPeriodMax = ENC_VEL_QTR_MASK; // 26 bits 76 | velPeriod = rawPeriod & ENC_VEL_MASK_26; 77 | velOverflow = rawPeriod & ENC_VEL_OVER_MASK; 78 | velDir = rawPeriod & ENC_DIR_MASK; 79 | dirChange = rawPeriod & 0x20000000; 80 | encError = rawPeriod & 0x10000000; 81 | partialCycle = rawPeriod & 0x08000000; 82 | qtr1Period = rawQtr1 & ENC_VEL_QTR_MASK; 83 | qtr1Overflow = rawQtr1 & ENC_VEL_OVER_MASK; 84 | qtr1Dir = rawQtr1 & ENC_DIR_MASK; 85 | qtr1Edges = (rawQtr1>>26)&0x0f; 86 | qtr5Period = rawQtr5 & ENC_VEL_QTR_MASK; 87 | qtr5Overflow = rawQtr5 & ENC_VEL_OVER_MASK; 88 | qtr5Dir = rawQtr5 & ENC_DIR_MASK; 89 | qtr5Edges = (rawQtr5>>26)&0x0f; 90 | runPeriod = rawRun & ENC_VEL_QTR_MASK; 91 | runOverflow = rawRun & ENC_VEL_OVER_MASK; 92 | } 93 | 94 | // SetData for Firmware Rev 6 95 | 96 | void EncoderVelocity::SetDataRev6(uint32_t rawPeriod, uint32_t rawQtr) 97 | { 98 | Init(); 99 | clkPeriod = VEL_PERD_REV6; 100 | velPeriodMax = ENC_VEL_MASK_22; 101 | qtrPeriodMax = ENC_ACC_PREV_MASK; // 20 bits 102 | // Firmware 6 has bits stuffed in different places: 103 | // Q1: lower 8 bits in velPeriod[29:22] and upper 12 bits in accQtr1[31:20] 104 | // Q5: all 20 bits in accQtr1[19:0] 105 | velPeriod = rawPeriod & ENC_VEL_MASK_22; 106 | qtr1Period = (((rawQtr & ENC_ACC_REC_MS_MASK)>>12) | 107 | ((rawPeriod & ENC_ACC_REC_LS_MASK) >> 22)) & ENC_ACC_PREV_MASK; 108 | qtr5Period = rawQtr & ENC_ACC_PREV_MASK; 109 | velOverflow = rawPeriod & ENC_VEL_OVER_MASK; 110 | velDir = rawPeriod & ENC_DIR_MASK; 111 | // Qtr1 and Qtr5 overflow at 0x000fffff (no overflow bit is set by firmware) 112 | if (qtr1Period == qtrPeriodMax) 113 | qtr1Overflow = true; 114 | if (qtr5Period == qtrPeriodMax) 115 | qtr5Overflow = true; 116 | // Qtr1 and Qtr5 direction are not recorded, so set them same as velDir; 117 | // i.e., assume that there hasn't been a direction change. 118 | qtr1Dir = velDir; 119 | qtr5Dir = velDir; 120 | } 121 | 122 | // SetData for older firmware (Rev 1-5) 123 | 124 | void EncoderVelocity::SetDataOld(uint32_t rawPeriod, bool useRunCounter) 125 | { 126 | Init(); 127 | // Prior to Firmware Version 6, the latched counter value is returned 128 | // as the lower 16 bits. Starting with Firmware Version 4, the upper 16 bits are 129 | // the free-running counter, which was not used. 130 | // Note that the counter values are signed, so we convert to unsigned and set a direction bit 131 | // to be consistent with later versions of firmware. 132 | clkPeriod = VEL_PERD_OLD; 133 | velPeriodMax = ENC_VEL_MASK_16; 134 | velPeriod = static_cast(rawPeriod & ENC_VEL_MASK_16); 135 | // Convert from signed count to unsigned count and direction 136 | if (velPeriod == 0x8000) { // if overflow 137 | velPeriod = 0x00007fff; 138 | velOverflow = true; 139 | // Firmware also sets velPeriod to overflow when direction change occurred, so could potentially 140 | // set dirChange = true. 141 | } 142 | else if (velPeriod & 0x8000) { // if negative 143 | velPeriod = ~velPeriod; // ones complement (16-bits) 144 | velPeriod = velPeriod + 1; // twos complement (32-bits) 145 | velDir = false; 146 | } 147 | else { 148 | velPeriod = velPeriod; 149 | velDir = true; 150 | } 151 | if (useRunCounter) { 152 | uint16_t runCtr = static_cast((rawPeriod>>16) & ENC_VEL_MASK_16); 153 | // Convert from signed count to unsigned count and direction 154 | if (runCtr == 0x8000) { // if overflow 155 | runOverflow = true; 156 | runCtr = 0x7fff; 157 | } 158 | else if (runCtr & 0x8000) { // if negative 159 | runCtr = ~runCtr; // ones complement (16-bits) 160 | runCtr += 1; // twos complement (16-bits) 161 | } 162 | runPeriod = static_cast(runCtr); 163 | } 164 | } 165 | 166 | // Returns encoder velocity in counts/sec -> 4/period 167 | double EncoderVelocity::GetEncoderVelocity() const 168 | { 169 | uint32_t delta = 0; 170 | // Avoid divide by 0 (should never happen) 171 | if (velPeriod == 0) delta = 1; 172 | 173 | double vel = 0.0; 174 | if (!velOverflow && !dirChange) { 175 | vel = 4.0/((velPeriod+delta)*clkPeriod); 176 | if (!velDir) 177 | vel = -vel; 178 | } 179 | 180 | return vel; 181 | } 182 | 183 | // Returns predicted encoder velocity in counts/sec, taking into account 184 | // acceleration and running counter. 185 | double EncoderVelocity::GetEncoderVelocityPredicted(double percent_threshold) const 186 | { 187 | double encVel = GetEncoderVelocity(); 188 | double encAcc = GetEncoderAcceleration(percent_threshold); 189 | // The encoder measurement delay is half the measured period, based on the assumption that measuring the 190 | // period over a full cycle (4 quadrature counts) estimates the velocity in the middle of that cycle. 191 | double encDelay = velPeriod*clkPeriod/2.0; 192 | double encRun = GetEncoderRunningCounterSeconds(); 193 | double deltaVel = encAcc*(encDelay+encRun); 194 | double predVel = encVel+deltaVel; 195 | if (encVel < 0) { 196 | // Do not change velocity direction 197 | if (predVel > 0.0) 198 | predVel = 0.0; 199 | // Maximum velocity limited by 1 count (i.e., we know 200 | // that a count has not happened for encRun seconds) 201 | if (predVel*encRun < -1.0) 202 | predVel = -1.0/encRun; 203 | } 204 | else if (encVel > 0.0) { 205 | // Do not change velocity direction 206 | if (predVel < 0.0) 207 | predVel = 0.0; 208 | // Maximum velocity limited by 1 count (i.e., we know 209 | // that a count has not happened for encRun seconds) 210 | if (predVel*encRun > 1.0) 211 | predVel = 1.0/encRun; 212 | } 213 | else { 214 | // If not moving, do not attempt to predict 215 | predVel = 0.0; 216 | } 217 | return predVel; 218 | } 219 | 220 | // Estimate acceleration from two quarters of the same type; units are counts/second**2 221 | // Valid for firmware version 6+. 222 | double EncoderVelocity::GetEncoderAcceleration(double percent_threshold) const 223 | { 224 | if (velOverflow) 225 | return 0.0; 226 | 227 | uint32_t velPeriodPrev = velPeriod - qtr1Period + qtr5Period; // Previous full-cycle period 228 | if (qtr5Overflow) 229 | velPeriodPrev = velPeriodMax; 230 | 231 | // Should never happen 232 | if ((qtr1Period == 0) || (qtr5Period == 0) || (velPeriod == 0) || (velPeriodPrev == 0)) 233 | return 0.0; 234 | 235 | if (qtr1Edges != qtr5Edges) 236 | return 0.0; 237 | 238 | if ((qtr1Dir != qtr5Dir) || (qtr1Dir != velDir)) 239 | return 0.0; 240 | 241 | double acc = 0.0; 242 | if (1.0/qtr1Period <= percent_threshold) { 243 | double qtrDiff = static_cast(qtr5Period) - static_cast(qtr1Period); 244 | double qtrSum = static_cast(qtr5Period + qtr1Period); 245 | double velProd = static_cast(velPeriod)*static_cast(velPeriodPrev)*clkPeriod*clkPeriod; 246 | acc = (8.0*qtrDiff)/(velProd*qtrSum); 247 | if (!velDir) 248 | acc = -acc; 249 | } 250 | return acc; 251 | } 252 | 253 | // Get the encoder running counter, in seconds. This is primarily used for Firmware Rev 7+, but 254 | // also supports the running counter in Firmware Rev 4-5. 255 | double EncoderVelocity::GetEncoderRunningCounterSeconds() const 256 | { 257 | return runPeriod*clkPeriod; 258 | } 259 | -------------------------------------------------------------------------------- /lib/code/FirewirePort.cpp: -------------------------------------------------------------------------------- 1 | /* -*- Mode: C++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */ 2 | /* ex: set filetype=cpp softtabstop=4 shiftwidth=4 tabstop=4 cindent expandtab: */ 3 | 4 | /* 5 | Author(s): Zihan Chen, Peter Kazanzides 6 | 7 | (C) Copyright 2011-2022 Johns Hopkins University (JHU), All Rights Reserved. 8 | 9 | --- begin cisst license - do not edit --- 10 | 11 | This software is provided "as is" under an open source license, with 12 | no warranty. The complete license can be found in license.txt and 13 | http://www.cisst.org/cisst/license.txt. 14 | 15 | --- end cisst license --- 16 | */ 17 | 18 | // system 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include // errno 27 | 28 | // firewire 29 | #include "FirewirePort.h" 30 | #include "Amp1394Time.h" 31 | #include 32 | #include 33 | 34 | FirewirePort::PortListType FirewirePort::PortList; 35 | 36 | FirewirePort::FirewirePort(int portNum, std::ostream &debugStream): 37 | BasePort(portNum, debugStream) 38 | { 39 | Init(); 40 | } 41 | 42 | bool FirewirePort::Init(void) 43 | { 44 | // create firewire port handle 45 | handle = raw1394_new_handle(); 46 | if (!handle) { 47 | outStr << "FirewirePort::Init: error, could not create handle" << std::endl; 48 | return false; 49 | } 50 | 51 | PortListType::const_iterator it; 52 | for (it = PortList.begin(); it != PortList.end(); it++) { 53 | if ((*it)->PortNum == PortNum) 54 | outStr << "FirewirePort::Init: warning, port " << PortNum 55 | << " is already used (be careful about thread safety)" << std::endl; 56 | if ((*it)->handle == handle) // should never happen 57 | outStr << "FirewirePort::Init: warning, handle is already used" << std::endl; 58 | } 59 | PortList.push_back(this); 60 | 61 | memset(Node2Board, BoardIO::MAX_BOARDS, sizeof(Node2Board)); 62 | 63 | // set the bus reset handler 64 | old_reset_handler = raw1394_set_bus_reset_handler(handle, reset_handler); 65 | 66 | // get number of ports 67 | int nports = raw1394_get_port_info(handle, NULL, 0); 68 | outStr << "FirewirePort::Init: number of ports = " << nports << std::endl; 69 | if (nports > 0) { 70 | struct raw1394_portinfo *pinfo = new raw1394_portinfo[nports]; 71 | raw1394_get_port_info(handle, pinfo, nports); 72 | for (int i = 0; i < nports; i++) 73 | outStr << " Port " << i << ": " << pinfo[i].name << ", " << pinfo[i].nodes << " nodes" << std::endl; 74 | delete [] pinfo; 75 | } 76 | if (nports < PortNum) { 77 | outStr << "FirewirePort::Init: port " << PortNum << " does not exist" << std::endl; 78 | raw1394_destroy_handle(handle); 79 | handle = NULL; 80 | return false; 81 | } 82 | 83 | if (raw1394_set_port(handle, PortNum)) { 84 | outStr << "FirewirePort::Init: error setting port to " << PortNum << std::endl; 85 | raw1394_destroy_handle(handle); 86 | handle = NULL; 87 | return false; 88 | } 89 | outStr << "FirewirePort::Init: successfully initialized port " << PortNum << std::endl; 90 | outStr << "Using libraw1394 version " << raw1394_get_libversion() << std::endl; 91 | 92 | // stop cycle start packet 93 | StopCycleStartPacket(); 94 | 95 | bool ret = ScanNodes(); 96 | if (ret) 97 | SetDefaultProtocol(); 98 | return ret; 99 | } 100 | 101 | void FirewirePort::StopCycleStartPacket(void) 102 | { 103 | // IMPORTANT: Disable Cycle Start Packet, no isochronous 104 | int rc = 0; // return code 105 | quadlet_t data_stop_cmc = bswap_32(0x100); 106 | rc = raw1394_write(handle, 107 | raw1394_get_local_id(handle), 108 | CSR_REGISTER_BASE + CSR_STATE_CLEAR, 109 | 4, 110 | &data_stop_cmc); 111 | if (rc) { 112 | outStr << "FirewirePort::Init: error, can NOT disable cycle start packet" << std::endl; 113 | } else { 114 | outStr << "FirewirePort::Init: successfully disabled cycle start packet" << std::endl; 115 | } 116 | } 117 | 118 | 119 | FirewirePort::~FirewirePort() 120 | { 121 | Cleanup(); 122 | } 123 | 124 | int FirewirePort::NumberOfUsers(void) 125 | { 126 | // try to count of many users on the handle 127 | char command[256]; 128 | sprintf(command, "lsof -t /dev/fw%d 2> /dev/null", PortNum); 129 | FILE * fp; 130 | int status; 131 | char path[512]; 132 | fp = popen(command, "r"); 133 | if (fp == NULL) { 134 | outStr << "FirewirePort::NumberOfUsers: unable to start lsof to count number of users on port " << PortNum << std::endl; 135 | } 136 | std::string result; 137 | int numberOfLines = 0; 138 | while (fgets(path, 512, fp) != NULL) { 139 | result.append(path); 140 | numberOfLines++; 141 | } 142 | status = pclose(fp); 143 | 144 | if (status == -1) { 145 | outStr << "FirewirePort::NumberOfUsers: error in pclose after lsof call" << std::endl; 146 | } 147 | int numberOfUsers = numberOfLines - 1; // lsof itself 148 | if (numberOfUsers > 1) { 149 | sprintf(command, "lsof -R /dev/fw%d 2> /dev/null", PortNum); 150 | fp = popen(command, "r"); 151 | if (fp == NULL) { 152 | outStr << "FirewirePort::NumberOfUsers: unable to start lsof to count number of users on port " << PortNum << std::endl; 153 | } 154 | result = ""; // clear result 155 | while (fgets(path, 512, fp) != NULL) { 156 | result.append(path); 157 | } 158 | status = pclose(fp); 159 | 160 | outStr << "FirewirePort::NumberOfUsers: found " << numberOfUsers << " users" << std::endl 161 | << "Full lsof result:" << std::endl 162 | << result << std::endl; 163 | } 164 | return numberOfUsers; 165 | } 166 | 167 | void FirewirePort::Cleanup(void) 168 | { 169 | PortListType::iterator it = std::find(PortList.begin(), PortList.end(), this); 170 | if (it == PortList.end()) 171 | outStr << "FirewirePort::Cleanup: could not find entry for port " << PortNum << std::endl; 172 | else 173 | PortList.erase(it); 174 | raw1394_destroy_handle(handle); 175 | handle = NULL; 176 | } 177 | 178 | unsigned int FirewirePort::GetBusGeneration(void) const 179 | { 180 | if (FwBusGeneration != raw1394_get_generation(handle)) 181 | outStr << "FirewirePort::GetBusGeneration mismatch: " << FwBusGeneration 182 | << ", " << raw1394_get_generation(handle) << std::endl; 183 | return FwBusGeneration; 184 | } 185 | 186 | void FirewirePort::UpdateBusGeneration(unsigned int gen) 187 | { 188 | raw1394_update_generation(handle, gen); 189 | FwBusGeneration = gen; 190 | } 191 | 192 | int FirewirePort::reset_handler(raw1394handle_t hdl, uint gen) 193 | { 194 | int ret = 0; 195 | PortListType::iterator it; 196 | for (it = PortList.begin(); it != PortList.end(); it++) { 197 | if ((*it)->handle == hdl) { 198 | (*it)->outStr << "FirewirePort::reset_handler: generation = " << gen << std::endl; 199 | // Call default handler, which calls raw1394_update_generation 200 | // ret = (*it)->old_reset_handler(hdl, gen); 201 | (*it)->newFwBusGeneration = gen; 202 | break; 203 | } 204 | } 205 | if (it == PortList.end()) 206 | std::cerr << "FirewirePort::reset_handler: could not find port" << std::endl; 207 | return ret; 208 | } 209 | 210 | // This function checks if there is a pending read on the IEEE-1394 bus, 211 | // which could include a bus reset 212 | void FirewirePort::PollEvents(void) 213 | { 214 | fd_set fds; 215 | struct timeval timeout = {0, 0}; 216 | 217 | int fd = raw1394_get_fd(handle); 218 | FD_ZERO(&fds); 219 | FD_SET(fd, &fds); 220 | int ret = select(fd+1, &fds, 0, 0, &timeout); 221 | if (ret > 0) 222 | raw1394_loop_iterate(handle); 223 | } 224 | 225 | nodeid_t FirewirePort::InitNodes(void) 226 | { 227 | // Get base node id (zero out 6 lsb) 228 | baseNodeId = raw1394_get_local_id(handle) & 0xFFC0; 229 | outStr << "FirewirePort::InitNodes: base node id = " << std::hex << baseNodeId << std::dec << std::endl; 230 | 231 | // Get Firewire bus generation 232 | FwBusGeneration = raw1394_get_generation(handle); 233 | newFwBusGeneration = FwBusGeneration; 234 | 235 | // Get total number of nodes on bus 236 | int numNodes = raw1394_get_nodecount(handle); 237 | // Subtract 1 for PC which, as bus master, is always the highest number 238 | return (numNodes-1); 239 | } 240 | 241 | 242 | bool FirewirePort::AddBoard(BoardIO *board) 243 | { 244 | bool ret = BasePort::AddBoard(board); 245 | if (ret) { 246 | unsigned int id = board->BoardId; 247 | HubBoard = id; // last added board would be hub board 248 | } 249 | return ret; 250 | } 251 | 252 | bool FirewirePort::RemoveBoard(unsigned char boardId) 253 | { 254 | return BasePort::RemoveBoard(boardId); 255 | } 256 | 257 | bool FirewirePort::WriteBroadcastOutput(quadlet_t *buffer, unsigned int size) 258 | { 259 | #if 1 260 | return WriteBlockNode(0, 0xffffffff0000, buffer, size); 261 | #else 262 | return WriteBlockNode(FW_NODE_BROADCAST, 0xffffff000000, // now the address is hardcoded 263 | buffer, size); 264 | #endif 265 | } 266 | 267 | bool FirewirePort::WriteBroadcastReadRequest(unsigned int seq) 268 | { 269 | quadlet_t bcReqData = (seq << 16); 270 | if (IsAllBoardsRev4_5_ || IsBroadcastShorterWait()) 271 | bcReqData += BoardInUseMask_; 272 | else 273 | // For a mixed set of boards, disable the shorter wait capability by "tricking" the system into thinking 274 | // that all nodes are used in this configuration. Otherwise, boards with the shorter wait capability may 275 | // attempt to transmit at the same time as boards that do not have this capability. The FireWire bus 276 | // arbitration protocol should prevent disaster, but this could lead to lower efficiency. 277 | bcReqData += ((1 << NumOfNodes_)-1); 278 | 279 | nodeaddr_t bcReqAddr; 280 | if (IsAllBoardsRev4_6_) 281 | bcReqAddr = 0xffffffff000f; // special address to trigger broadcast read 282 | else 283 | bcReqAddr = 0x1800; // special address to trigger broadcast read 284 | 285 | #if 1 286 | WriteQuadletNode(0, bcReqAddr, bcReqData); 287 | // WriteQuadletNode returns false because the "fake" broadcast packet to node 0 does not 288 | // return an ACK. 289 | return true; 290 | #else 291 | return WriteQuadlet(FW_NODE_BROADCAST, bcReqAddr, bcReqData); 292 | #endif 293 | } 294 | 295 | void FirewirePort::WaitBroadcastRead(void) 296 | { 297 | // Wait for all boards to respond with data 298 | // Shorter wait: 10 + 5 * Nb us, where Nb is number of boards used in this configuration 299 | // Standard wait: 5 + 5 * Nn us, where Nn is the total number of nodes on the FireWire bus 300 | double waitTime_uS = IsBroadcastShorterWait() ? (10.0 + 5.0*NumOfBoards_) : (5.0 + 5.0*NumOfNodes_); 301 | Amp1394_Sleep(waitTime_uS*1e-6); 302 | } 303 | 304 | void FirewirePort::OnNoneRead(void) 305 | { 306 | PollEvents(); 307 | } 308 | 309 | void FirewirePort::OnNoneWritten(void) 310 | { 311 | PollEvents(); 312 | } 313 | 314 | bool FirewirePort::ReadQuadletNode(nodeid_t node, nodeaddr_t addr, quadlet_t &data, unsigned char) 315 | { 316 | if (!CheckFwBusGeneration("FirewirePort::ReadQuadlet")) 317 | return false; 318 | 319 | bool ret = !raw1394_read(handle, baseNodeId+node, addr, 4, &data); 320 | if (ret) 321 | data = bswap_32(data); 322 | return ret; 323 | } 324 | 325 | bool FirewirePort::WriteQuadletNode(nodeid_t node, nodeaddr_t addr, quadlet_t data, unsigned char) 326 | { 327 | if (!CheckFwBusGeneration("FirewirePort::WriteQuadlet")) 328 | return false; 329 | 330 | data = bswap_32(data); 331 | bool ret = !raw1394_write(handle, baseNodeId+node, addr, 4, &data); 332 | 333 | // Workaround for Firewire broadcast, which is not currently working due to an issue with 334 | // either libraw1394 or the Firewire JuJu driver. 335 | if (!ret && (node == FW_NODE_BROADCAST)) { 336 | ret = true; 337 | for (nodeid_t curNode = 0; curNode < NumOfNodes_; curNode++) { 338 | if (raw1394_write(handle, baseNodeId+curNode, addr, 4, &data)) 339 | ret = false; 340 | } 341 | } 342 | 343 | return ret; 344 | } 345 | 346 | bool FirewirePort::ReadBlockNode(nodeid_t node, nodeaddr_t addr, quadlet_t *rdata, 347 | unsigned int nbytes, unsigned char) 348 | { 349 | if (!CheckFwBusGeneration("FirewirePort::ReadBlock")) 350 | return false; 351 | 352 | rtRead = true; // for debugging 353 | return !raw1394_read(handle, baseNodeId+node, addr, nbytes, rdata); 354 | } 355 | 356 | bool FirewirePort::WriteBlockNode(nodeid_t node, nodeaddr_t addr, quadlet_t *wdata, 357 | unsigned int nbytes, unsigned char) 358 | { 359 | if (!CheckFwBusGeneration("FirewirePort::WriteBlock")) 360 | return false; 361 | 362 | rtWrite = true; // for debugging 363 | return !raw1394_write(handle, baseNodeId+node, addr, nbytes, wdata); 364 | } 365 | -------------------------------------------------------------------------------- /lib/EthBasePort.h: -------------------------------------------------------------------------------- 1 | /* -*- Mode: C++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */ 2 | /* ex: set filetype=cpp softtabstop=4 shiftwidth=4 tabstop=4 cindent expandtab: */ 3 | 4 | /* 5 | Author(s): Zihan Chen, Peter Kazanzides 6 | 7 | (C) Copyright 2014-2024 Johns Hopkins University (JHU), All Rights Reserved. 8 | 9 | --- begin cisst license - do not edit --- 10 | 11 | This software is provided "as is" under an open source license, with 12 | no warranty. The complete license can be found in license.txt and 13 | http://www.cisst.org/cisst/license.txt. 14 | 15 | --- end cisst license --- 16 | */ 17 | 18 | 19 | #ifndef __EthBasePort_H__ 20 | #define __EthBasePort_H__ 21 | 22 | #include 23 | #include "BasePort.h" 24 | 25 | // Some useful constants related to the FireWire protocol 26 | const unsigned int FW_QREAD_SIZE = 16; // Number of bytes in Firewire quadlet read request packet 27 | const unsigned int FW_QWRITE_SIZE = 20; // Number of bytes in Firewire quadlet write packet 28 | const unsigned int FW_QRESPONSE_SIZE = 20; // Number of bytes in Firewire quadlet read response packet 29 | const unsigned int FW_BREAD_SIZE = 20; // Number of bytes in Firewire block read request 30 | const unsigned int FW_BRESPONSE_HEADER_SIZE = 20; // Number of bytes in Firewire block read response header (including header CRC) 31 | const unsigned int FW_BWRITE_HEADER_SIZE = 20; // Number of bytes in Firewire block write header (including header CRC) 32 | const unsigned int FW_CRC_SIZE = 4; // Number of bytes in Firewire CRC 33 | 34 | // Firewire control word -- specific to our implementation. 35 | // First byte is FW_CTRL_FLAGS, second byte is Firewire bus generation 36 | const unsigned int FW_CTRL_SIZE = 2; // Number of bytes in Firewire control word 37 | 38 | // Firewire extra data, received after the quadlet and block responses. This is specific to our implementation. 39 | const unsigned int FW_EXTRA_SIZE = 8; // Number of extra bytes (4 words) 40 | 41 | // FW_CTRL_FLAGS 42 | const unsigned char FW_CTRL_NOFORWARD = 0x01; // Prevent forwarding by Ethernet/Firewire bridge 43 | 44 | class EthBasePort : public BasePort 45 | { 46 | public: 47 | //! FireWire tcode 48 | enum TCODE { 49 | QWRITE = 0, 50 | BWRITE = 1, 51 | QREAD = 4, 52 | BREAD = 5, 53 | QRESPONSE = 6, 54 | BRESPONSE = 7 55 | }; 56 | 57 | struct FPGA_Status { 58 | bool FwBusReset; 59 | bool FwPacketDropped; 60 | bool EthInternalError; 61 | bool EthSummaryError; 62 | bool noForwardFlag; 63 | unsigned int srcPort; 64 | unsigned int numStateInvalid; // Not used, except in debug builds of firmware 65 | unsigned int numPacketError; 66 | FPGA_Status() : FwBusReset(false), FwPacketDropped(false), EthInternalError(false), EthSummaryError(false), 67 | noForwardFlag(false), srcPort(0), numStateInvalid(0), numPacketError(0) {} 68 | ~FPGA_Status() {} 69 | }; 70 | 71 | typedef bool (*EthCallbackType)(EthBasePort &port, unsigned char boardId, std::ostream &debugStream); 72 | 73 | protected: 74 | 75 | // Whether to use Ethernet/Firewire bridge (if false, use Ethernet only) 76 | bool useFwBridge; 77 | 78 | uint8_t fw_tl; // FireWire transaction label (6 bits) 79 | 80 | EthCallbackType eth_read_callback; 81 | double ReceiveTimeout; // Ethernet receive timeout (seconds) 82 | 83 | enum FPGA_FLAGS { 84 | FwBusReset = 0x01, // Firewire bus reset is active 85 | FwPacketDropped = 0x02, // Firewire packet dropped 86 | EthInternalError = 0x04, // Internal FPGA error (bus access or invalid state) 87 | EthSummaryError = 0x08, // Summary of Ethernet protocol errors (see Status) 88 | noForwardFlag = 0x10, // 1 -> Ethernet only (no forward to Firewire) 89 | srcPortMask = 0x60, // Mask for srcPort bits 90 | srcPortShift = 5 // Shift for srcPort bits 91 | }; 92 | 93 | FPGA_Status FpgaStatus; // FPGA status from extra data returned 94 | 95 | double FPGA_RecvTime; // Time for FPGA to receive Ethernet packet (seconds) 96 | double FPGA_TotalTime; // Total time for FPGA to receive packet and respond (seconds) 97 | 98 | //! Read quadlet from node (internal method called by ReadQuadlet) 99 | bool ReadQuadletNode(nodeid_t node, nodeaddr_t addr, quadlet_t &data, unsigned char flags = 0); 100 | 101 | //! Write quadlet to node (internal method called by WriteQuadlet) 102 | bool WriteQuadletNode(nodeid_t node, nodeaddr_t addr, quadlet_t data, unsigned char flags = 0); 103 | 104 | // Read a block from the specified node (also calls ReceiveResponseNode). Internal method called by ReadBlock. 105 | bool ReadBlockNode(nodeid_t node, nodeaddr_t addr, quadlet_t *rdata, unsigned int nbytes, unsigned char flags = 0); 106 | 107 | // Receive the block data. Internal method called by ReadBlockNode. 108 | bool ReceiveResponseNode(nodeid_t node, quadlet_t *rdata, unsigned int nbytes, uint8_t fw_tl, nodeid_t *src_node = 0); 109 | 110 | // Write a block to the specified node. Internal method called by WriteBlock and 111 | // WriteAllBoardsBroadcast. 112 | bool WriteBlockNode(nodeid_t node, nodeaddr_t addr, quadlet_t *wdata, unsigned int nbytes, unsigned char flags = 0); 113 | 114 | // Send packet 115 | virtual bool PacketSend(nodeid_t node, unsigned char *packet, size_t nbytes, bool useEthernetBroadcast) = 0; 116 | 117 | // Receive packet 118 | virtual int PacketReceive(unsigned char *packet, size_t nbytes) = 0; 119 | 120 | // Flush all packets in receive buffer 121 | virtual int PacketFlushAll(void) = 0; 122 | 123 | // Method called by ReadAllBoards/ReadAllBoardsBroadcast if no data read 124 | void OnNoneRead(void); 125 | 126 | // Method called by WriteAllBoards/WriteAllBoardsBroadcast if no data written 127 | void OnNoneWritten(void); 128 | 129 | // Method called when Firewire bus reset has caused the Firewire generation number on the FPGA 130 | // to be different than the one on the PC. 131 | virtual void OnFwBusReset(unsigned int FwBusGeneration_FPGA); 132 | 133 | // Make Ethernet header (only needed for raw Ethernet) 134 | virtual void make_ethernet_header(unsigned char *packet, unsigned int numBytes, nodeid_t node, unsigned char flags); 135 | 136 | // Make control word 137 | void make_write_header(unsigned char *packet, unsigned int nBytes, unsigned char flags); 138 | 139 | void make_1394_header(quadlet_t *packet, nodeid_t node, nodeaddr_t addr, unsigned int tcode, unsigned int tl); 140 | 141 | void make_qread_packet(quadlet_t *packet, nodeid_t node, nodeaddr_t addr, unsigned int tl); 142 | void make_qwrite_packet(quadlet_t *packet, nodeid_t node, nodeaddr_t addr, quadlet_t data, unsigned int tl); 143 | void make_bread_packet(quadlet_t *packet, nodeid_t node, nodeaddr_t addr, unsigned int nBytes, unsigned int tl); 144 | void make_bwrite_packet(quadlet_t *packet, nodeid_t node, nodeaddr_t addr, quadlet_t *data, unsigned int nBytes, unsigned int tl); 145 | 146 | // Optimize Firewire gap count if needed 147 | bool OptimizeFirewireGapCount(); 148 | 149 | public: 150 | 151 | EthBasePort(int portNum, bool forceFwBridge, std::ostream &debugStream = std::cerr, EthCallbackType cb = 0); 152 | 153 | ~EthBasePort(); 154 | 155 | void SetEthCallback(EthCallbackType callback) 156 | { eth_read_callback = callback; } 157 | 158 | double GetReceiveTimeout(void) const { return ReceiveTimeout; } 159 | 160 | void SetReceiveTimeout(double timeSec) { ReceiveTimeout = timeSec; } 161 | 162 | // Return FPGA status related to Ethernet interface 163 | void GetFpgaStatus(FPGA_Status &status) const { status = FpgaStatus; } 164 | 165 | // Return time required to receive Ethernet packet on FPGA, in seconds. 166 | double GetFpgaReceiveTime(void) const { return FPGA_RecvTime; } 167 | 168 | // Return time required to receive and respond to Ethernet packet on FPGA, in seconds. 169 | // This is the last data value placed in the response packet, but it is a little 170 | // lower (by about 0.7 microseconds) because it does not include the time to write this 171 | // value to the KSZ8851 or to queue the packet into the transmit buffer. 172 | double GetFpgaTotalTime(void) const { return FPGA_TotalTime; } 173 | 174 | //****************** Virtual methods *************************** 175 | // Implementations of pure virtual methods from BasePort 176 | 177 | // For now, always returns 1 178 | int NumberOfUsers(void) { return 1; } 179 | 180 | unsigned int GetBusGeneration(void) const { return FwBusGeneration; } 181 | 182 | void UpdateBusGeneration(unsigned int gen) { FwBusGeneration = gen; } 183 | 184 | // virtual method in BasePort 185 | bool CheckFwBusGeneration(const std::string &caller, bool doScan = false); 186 | 187 | /*! 188 | \brief Write the broadcast packet containing the DAC values and power control 189 | */ 190 | bool WriteBroadcastOutput(quadlet_t *buffer, unsigned int size); 191 | 192 | /*! 193 | \brief Write the broadcast read request 194 | */ 195 | bool WriteBroadcastReadRequest(unsigned int seq); 196 | 197 | /*! 198 | \brief Wait for broadcast read data to be available 199 | */ 200 | void WaitBroadcastRead(void); 201 | 202 | bool isBroadcastReadOrdered(void) const; 203 | 204 | // Return clock period used for broadcast read timing measurements 205 | // 125 MHz for FPGA V3 Ethernet-only; otherwise 49.152 MHz 206 | double GetBroadcastReadClockPeriod(void) const; 207 | 208 | /*! 209 | \brief Receive the broadcast read response. This is usually a block read from 210 | address 0x1000 (Hub memory); in the case of Ethernet-only, it receives 211 | a response from an FPGA board some time after WriteBroadcastReadRequest 212 | is issued. 213 | */ 214 | bool ReceiveBroadcastReadResponse(quadlet_t *rdata, unsigned int nbytes); 215 | 216 | /*! 217 | \brief Add delay (if needed) for PROM I/O operations 218 | The delay is non-zero for Ethernet. 219 | */ 220 | void PromDelay(void) const; 221 | 222 | //****************** Static methods *************************** 223 | 224 | // Reverse the bits in the input 225 | static uint32_t BitReverse32(uint32_t input); 226 | 227 | // Returns the destination MAC address (6 bytes) 228 | // The first 3 characters are FA:61:OE, which is the CID assigned to LCSR by IEEE 229 | // The next 2 characters are 13:94 230 | // The last character is 0 (set it to the board address) 231 | static void GetDestMacAddr(unsigned char *macAddr); 232 | 233 | // Returns the destination multicast MAC address (6 bytes) 234 | // The first 3 characters are FB:61:OE, which is the CID assigned to LCSR by IEEE, with the multicast bit set 235 | // The last 3 characters are 13:94:FF 236 | static void GetDestMulticastMacAddr(unsigned char *macAddr); 237 | 238 | // Print MAC address 239 | static void PrintMAC(std::ostream &outStr, const char* name, const uint8_t *addr, bool swap16 = false); 240 | 241 | // Print IP address 242 | static void PrintIP(std::ostream &outStr, const char* name, const uint8_t *addr, bool swap16 = false); 243 | 244 | // Check Ethernet header (only for EthRawPort) 245 | virtual bool CheckEthernetHeader(const unsigned char *packet, bool useEthernetBroadcast); 246 | 247 | // Byteswap quadlets received packet (quadlet or block read response) to make it easier to work with, 248 | // and to be consistent with CheckFirewirePacket and PrintFirewirePacket. This is typically used to 249 | // byteswap the Firewire header quadlets. 250 | void ByteswapQuadlets(unsigned char *packet, unsigned int nbytes); 251 | 252 | // Get information from Firewire packet header 253 | // Can specify 0 (null pointer) for fields to ignore 254 | static void GetFirewireHeaderInfo(const unsigned char *packet, nodeid_t *src_node, unsigned int *tcode, 255 | unsigned int *tl); 256 | 257 | // Check if FireWire packet valid 258 | // length: length of data section (for BRESPONSE) 259 | // node: expected source node 260 | // tcode: expected tcode (e.g., QRESPONSE or BRESPONSE) 261 | // tl: transaction label 262 | bool CheckFirewirePacket(const unsigned char *packet, size_t length, nodeid_t node, unsigned int tcode, 263 | unsigned int tl); 264 | 265 | // Process extra data received from FPGA 266 | void ProcessExtraData(const unsigned char *packet); 267 | 268 | static bool checkCRC(const unsigned char *packet); 269 | 270 | // Print FireWire packet 271 | static void PrintFirewirePacket(std::ostream &out, const quadlet_t *packet, unsigned int max_quads); 272 | 273 | static void PrintEthernetPacket(std::ostream &out, const quadlet_t *packet, unsigned int max_quads); 274 | 275 | static void PrintStatus(std::ostream &debugStream, uint32_t status); 276 | 277 | // Print Ethernet debug data; clockPeriod is in seconds 278 | // CheckDebugHeader looks for debug header string ("DBGx", where x is the version) 279 | // PrintDebugData from higher-level module (EthernetIO) 280 | // PrintDebugDataKSZ from lower-level module on FPGA V2 (KSZ8851) 281 | // PrintDebugDataRTI from lower-level module on FPGA V3 (EthRtInterface) 282 | static bool CheckDebugHeader(std::ostream &debugStream, const std::string &caller, const char *header, 283 | unsigned int version); 284 | static void PrintDebugData(std::ostream &debugStream, const quadlet_t *data, double clockPeriod); 285 | static void PrintDebugDataKSZ(std::ostream &debugStream, const quadlet_t *data, double clockPeriod); 286 | static void PrintDebugDataRTL(std::ostream &debugStream, const quadlet_t *data, const char *portName); 287 | static void PrintDebugDataRTI(std::ostream &debugStream, const quadlet_t *data, double clockPeriod); 288 | }; 289 | 290 | #endif // __EthBasePort_H__ 291 | -------------------------------------------------------------------------------- /tests/ethswitch.cpp: -------------------------------------------------------------------------------- 1 | /* -*- Mode: C++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */ 2 | /* ex: set filetype=cpp softtabstop=4 shiftwidth=4 tabstop=4 cindent expandtab: */ 3 | 4 | /* 5 | Author(s): Peter Kazanzides 6 | 7 | (C) Copyright 2024 Johns Hopkins University (JHU), All Rights Reserved. 8 | 9 | --- begin cisst license - do not edit --- 10 | 11 | This software is provided "as is" under an open source license, with 12 | no warranty. The complete license can be found in license.txt and 13 | http://www.cisst.org/cisst/license.txt. 14 | 15 | --- end cisst license --- 16 | */ 17 | 18 | /****************************************************************************** 19 | * 20 | * This program continuously displays the status of the FPGA V3 Ethernet switch 21 | * It relies on the Amp1394 library (which can depend on libraw1394 and/or pcap) 22 | * and on the Amp1394Console library (which may depend on curses). 23 | * 24 | * Usage: ethswitch [-pP] [] 25 | * where P is the port number (default 0), 26 | * or a string such as ethP and fwP, where P is the port number 27 | * -br or -bw specify to use a broadcast protocol 28 | * 29 | ******************************************************************************/ 30 | 31 | #include 32 | #include 33 | #include 34 | 35 | #include 36 | #include "PortFactory.h" 37 | #include "EthUdpPort.h" 38 | #include "AmpIO.h" 39 | #include "Amp1394Time.h" 40 | #include "Amp1394Console.h" 41 | #include "Amp1394BSwap.h" 42 | 43 | const unsigned int NUM_PORTS = 4; 44 | 45 | // Must match SwitchData in EthSwitch.v 46 | struct EthSwitchData { 47 | char headerString[4]; 48 | uint16_t PortConfig; 49 | uint16_t unused1; 50 | uint32_t MacAddrPrimary0High; 51 | uint32_t MacAddrPrimary01; 52 | uint32_t MacAddrPrimary1Low; 53 | uint16_t PortForwardFpga[2]; 54 | uint32_t UdpMulticastFpga; 55 | uint16_t PortAttr; 56 | uint16_t unused7; 57 | uint16_t numPacketRecv[4]; 58 | uint16_t numPacketSent[4]; 59 | uint8_t TxInfoReg[4]; 60 | uint8_t numCrcErrorIn[4]; 61 | uint8_t numCrcErrorOut[4]; 62 | uint8_t numIPv4ErrorIn[4]; 63 | uint16_t fifo_active; 64 | uint16_t fifo_underflow; 65 | uint16_t fifo_empty; 66 | uint16_t fifo_full; 67 | uint16_t unused18; 68 | uint16_t info_not_empty; 69 | uint16_t packet_dropped; 70 | uint16_t packet_truncated; 71 | uint8_t numPacketFwd[4][4]; 72 | uint32_t unused24_31[8]; 73 | }; 74 | 75 | bool GetBit(uint16_t value, uint16_t inPort, uint16_t outPort) 76 | { 77 | uint16_t offset = NUM_PORTS*inPort + outPort; 78 | return value & (1 << offset); 79 | } 80 | 81 | // Convert bitmask to list of board numbers 82 | const char *PrintFpgaList(uint16_t fpgaList) 83 | { 84 | static char buffer[128]; 85 | char *p = buffer; 86 | for (size_t i = 0; i < 16; i++) { 87 | if (fpgaList & (1< BoardList; 120 | 121 | std::string portDescription = BasePort::DefaultPort(); 122 | 123 | for (i = 1; i < (unsigned int)argc; i++) { 124 | if (argv[i][0] == '-') { 125 | if (argv[i][1] == 'p') { 126 | portDescription = argv[i]+2; 127 | } 128 | } 129 | else { 130 | int bnum = atoi(argv[i]); 131 | if ((bnum >= 0) && (bnum < BoardIO::MAX_BOARDS)) { 132 | BoardList.push_back(new AmpIO(bnum)); 133 | std::cerr << "Selecting board " << bnum << std::endl; 134 | } 135 | else 136 | std::cerr << "Invalid board number: " << argv[i] << std::endl; 137 | } 138 | } 139 | 140 | if (BoardList.size() < 1) { 141 | // usage 142 | std::cerr << "Usage: ethswitch [] [-pP]" << std::endl 143 | << " where P = port number (default 0)" << std::endl 144 | << " can also specify -pfw[:P], -peth:P or -pudp[:xx.xx.xx.xx]" << std::endl 145 | << std::endl; 146 | 147 | return 0; 148 | } 149 | 150 | std::stringstream debugStream(std::stringstream::out|std::stringstream::in); 151 | BasePort *Port = PortFactory(portDescription.c_str(), debugStream); 152 | if (!Port) { 153 | PrintDebugStream(debugStream); 154 | std::cerr << "Failed to create port using: " << portDescription << std::endl; 155 | return -1; 156 | } 157 | if (!Port->IsOK()) { 158 | PrintDebugStream(debugStream); 159 | std::cerr << "Failed to initialize " << Port->GetPortTypeString() << std::endl; 160 | return -1; 161 | } 162 | 163 | if (Port->GetNumOfNodes() == 0) { 164 | PrintDebugStream(debugStream); 165 | std::cerr << "Failed to find any boards" << std::endl; 166 | return -1; 167 | } 168 | 169 | // TODO: support multiple boards 170 | #if 0 171 | for (i = 0; i < BoardList.size(); i++) { 172 | Port->AddBoard(BoardList[i]); 173 | } 174 | #else 175 | Port->AddBoard(BoardList[0]); 176 | #endif 177 | 178 | AmpIO *board = BoardList[0]; 179 | 180 | uint32_t debugString; 181 | Port->ReadQuadlet(board->GetBoardId(), 0x40a0, debugString); 182 | if (strncmp(reinterpret_cast(&debugString), "ESW0", 4) != 0) { 183 | std::cerr << "Firmware does not support Ethernet switch data" << std::endl; 184 | return -1; 185 | } 186 | 187 | uint64_t MacAddrPort[2]; 188 | MacAddrPort[0] = 0; 189 | MacAddrPort[1] = 0; 190 | 191 | // IP address used for UDP Multicast (RT) 192 | uint32_t UdpMulticastFpga = 0; 193 | 194 | Amp1394Console console; 195 | console.Init(); 196 | if (!console.IsOK()) { 197 | std::cerr << "Failed to initialize console" << std::endl; 198 | return -1; 199 | } 200 | 201 | console.Print(1, lm, "Ethernet Switch status for Board %d", board->GetBoardId()); 202 | console.Print(2, lm, "Press ESC to quit, c to clear errors, e to show/hide CRC errors"); 203 | 204 | const unsigned int NL = 8; // Number of lines per port 205 | 206 | for (i = 0; i < NUM_PORTS; i++) { 207 | console.Print(4+NL*i, lm, "Port %d: %s", i, PortName[i]); 208 | console.Print(5+NL*i, lm+2, "NumRecv:"); 209 | console.Print(6+NL*i, lm+2, "NumSent:"); 210 | if (show_errors) { 211 | console.Print(5+NL*i, lm+26, "CRC/IPv4:"); 212 | console.Print(6+NL*i, lm+26, "CRC:"); 213 | } 214 | console.Print(7+NL*i, lm+2, "FIFOs -->"); 215 | console.Print(8+NL*i, lm+2, "FIFOs <--"); 216 | console.Print(9+NL*i, lm+2, "TxInfo:"); 217 | console.Print(10+NL*i, lm+2, "NumFwd:"); 218 | } 219 | console.Print(36, lm, "MAC Addr:"); 220 | console.Print(37, lm, "FPGA Fwd:"); 221 | console.Refresh(); 222 | 223 | // control loop 224 | while ((c = console.GetChar()) != ESC_CHAR) 225 | { 226 | uint32_t buffer[32]; 227 | 228 | if (c == 'c') { 229 | board->WriteEthernetClearErrors(); 230 | } 231 | else if (c == 'e') { 232 | show_errors = !show_errors; 233 | if (show_errors) { 234 | for (i = 0; i < NUM_PORTS; i++) { 235 | console.Print(5+NL*i, lm+26, "CRC/IPv4:"); 236 | console.Print(6+NL*i, lm+26, "CRC:"); 237 | } 238 | } 239 | else { 240 | for (i = 0; i < NUM_PORTS; i++) { 241 | console.Print(5+NL*i, lm+26, " "); 242 | console.Print(6+NL*i, lm+26, " "); 243 | console.Print(5+NL*i, lm+36, " "); 244 | console.Print(5+NL*i, lm+44, " "); 245 | console.Print(6+NL*i, lm+36, " "); 246 | } 247 | } 248 | } 249 | 250 | if (board->ReadEthernetData(buffer, 0x00a0, 24)) { 251 | EthSwitchData *data = reinterpret_cast(buffer); 252 | for (i = 0; i < NUM_PORTS; i++) { 253 | bool portActive = data->PortConfig & (0x0001 << i); 254 | bool portFast = data->PortConfig & (0x0010 << i); 255 | bool recvReady = data->PortAttr & (0x0001 << i); 256 | bool dataReady = data->PortAttr & (0x0010 << i); 257 | console.Print(4+NL*i, lm+18, portActive ? "on " : "off"); 258 | console.Print(4+NL*i, lm+23, !portActive ? " " : portFast ? "fast" : "slow"); 259 | console.Print(4+NL*i, lm+30, portActive&recvReady ? "rr" : " "); 260 | console.Print(4+NL*i, lm+34, portActive&dataReady ? "dr" : " "); 261 | console.Print(5+NL*i, lm+14, "%6d", data->numPacketRecv[i]); 262 | console.Print(6+NL*i, lm+14, "%6d", data->numPacketSent[i]); 263 | if (show_errors) { 264 | console.Print(5+NL*i, lm+36, "%6d", data->numCrcErrorIn[i]); 265 | console.Print(5+NL*i, lm+44, "%6d", data->numIPv4ErrorIn[i]); 266 | console.Print(6+NL*i, lm+36, "%6d", data->numCrcErrorOut[i]); 267 | } 268 | for (j = 0; j < NUM_PORTS; j++) { // out ports 269 | bool bit, ebit, fbit; 270 | ebit = GetBit(data->fifo_empty, i, j); 271 | fbit = GetBit(data->fifo_full, i, j); 272 | if (ebit && fbit) 273 | console.Print(7+NL*i, lm+16+16*j, "?"); 274 | else if (ebit) 275 | console.Print(7+NL*i, lm+16+16*j, "e"); 276 | else if (fbit) 277 | console.Print(7+NL*i, lm+16+16*j, "f"); 278 | else 279 | console.Print(7+NL*i, lm+16+16*j, " "); 280 | ebit = GetBit(data->fifo_empty, j, i); 281 | fbit = GetBit(data->fifo_full, j, i); 282 | if (ebit && fbit) 283 | console.Print(8+NL*i, lm+16+16*j, "?"); 284 | else if (ebit) 285 | console.Print(8+NL*i, lm+16+16*j, "e"); 286 | else if (fbit) 287 | console.Print(8+NL*i, lm+16+16*j, "f"); 288 | else 289 | console.Print(8+NL*i, lm+16+16*j, " "); 290 | bit = GetBit(data->info_not_empty, i, j); 291 | console.Print(7+NL*i, lm+18+16*j, bit ? "i" : " "); 292 | bit = GetBit(data->info_not_empty, j, i); 293 | console.Print(8+NL*i, lm+18+16*j, bit ? "i" : " "); 294 | bit = GetBit(data->fifo_active, i, j); 295 | console.Print(7+NL*i, lm+20+16*j, bit ? "a" : " "); 296 | bit = GetBit(data->fifo_active, j, i); 297 | console.Print(8+NL*i, lm+20+16*j, bit ? "a" : " "); 298 | bit = GetBit(data->fifo_underflow, i, j); 299 | console.Print(7+NL*i, lm+22+16*j, bit ? "u" : " "); 300 | bit = GetBit(data->fifo_underflow, j, i); 301 | console.Print(8+NL*i, lm+22+16*j, bit ? "u" : " "); 302 | bit = GetBit(data->packet_dropped, i, j); 303 | console.Print(7+NL*i, lm+24+16*j, bit ? "d" : " "); 304 | bit = GetBit(data->packet_dropped, j, i); 305 | console.Print(8+NL*i, lm+24+16*j, bit ? "d" : " "); 306 | bit = GetBit(data->packet_truncated, i, j); 307 | console.Print(7+NL*i, lm+28+16*j, bit ? "t" : " "); 308 | bit = GetBit(data->packet_truncated, j, i); 309 | console.Print(8+NL*i, lm+28+16*j, bit ? "t" : " "); 310 | uint8_t TxSt = (data->TxInfoReg[j]&0xc0)>>6; 311 | uint8_t curIn = (data->TxInfoReg[j]&0x30)>>4; 312 | uint8_t errBits = data->TxInfoReg[j]&0x0f; 313 | console.Print(9+NL*i, lm+16+16*j, "[%1x %1d %1x]", TxSt, curIn, errBits); 314 | console.Print(10+NL*i, lm+16+16*j, "%4d", data->numPacketFwd[i][j]); 315 | } 316 | } 317 | uint64_t newMacAddr[2]; 318 | newMacAddr[0] = data->MacAddrPrimary0High; 319 | newMacAddr[0] <<= 16; 320 | newMacAddr[0] |= (data->MacAddrPrimary01&0xffff0000)>>16; 321 | newMacAddr[1] = data->MacAddrPrimary01&0x0000ffff; 322 | newMacAddr[1] <<= 32; 323 | newMacAddr[1] |= data->MacAddrPrimary1Low; 324 | if (newMacAddr[0] != MacAddrPort[0]) { 325 | MacAddrPort[0] = newMacAddr[0]; 326 | console.Print(36, lm+16, "%llx", MacAddrPort[0]); 327 | } 328 | if (newMacAddr[1] != MacAddrPort[1]) { 329 | MacAddrPort[1] = newMacAddr[1]; 330 | console.Print(36, lm+32, "%llx", MacAddrPort[1]); 331 | } 332 | if (data->UdpMulticastFpga != UdpMulticastFpga) { 333 | UdpMulticastFpga = data->UdpMulticastFpga; 334 | console.Print(36, lm+64, EthUdpPort::IP_String(bswap_32(UdpMulticastFpga)).c_str()); 335 | } 336 | console.Print(37, lm+16, PrintFpgaList(data->PortForwardFpga[0])); 337 | console.Print(37, lm+32, PrintFpgaList(data->PortForwardFpga[1])); 338 | } 339 | } 340 | 341 | console.End(); 342 | 343 | #if 0 344 | for (j = 0; j < BoardList.size(); j++) 345 | Port->RemoveBoard(BoardList[j]->GetBoardId()); 346 | #else 347 | Port->RemoveBoard(BoardList[0]->GetBoardId()); 348 | #endif 349 | 350 | delete Port; 351 | return 0; 352 | } 353 | -------------------------------------------------------------------------------- /lib/FpgaIO.h: -------------------------------------------------------------------------------- 1 | /* -*- Mode: C++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */ 2 | /* ex: set filetype=cpp softtabstop=4 shiftwidth=4 tabstop=4 cindent expandtab: */ 3 | 4 | /* 5 | Author(s): Zihan Chen, Peter Kazanzides, Jie Ying Wu 6 | 7 | (C) Copyright 2011-2024 Johns Hopkins University (JHU), All Rights Reserved. 8 | 9 | --- begin cisst license - do not edit --- 10 | 11 | This software is provided "as is" under an open source license, with 12 | no warranty. The complete license can be found in license.txt and 13 | http://www.cisst.org/cisst/license.txt. 14 | 15 | --- end cisst license --- 16 | */ 17 | 18 | #ifndef __Fpga_IO_H__ 19 | #define __Fpga_IO_H__ 20 | 21 | #include 22 | 23 | #include "BoardIO.h" 24 | 25 | // Methods specific to FPGA board (Rev 1.x, Rev 2.x or Rev 3.x). 26 | // Rev 1.x and 2.x use a Spartan6 FPGA, whereas Rev 3.x uses a Zynq FPGA. 27 | // 28 | // Supports: 29 | // - Read/Write FPGA PROM (M25P16) to reprogram firmware or FPGA serial number (Rev 1.x, 2.x) 30 | // - Read/Write external PROM (25AA128); although this PROM is not on the FPGA board, 31 | // it is suggested that all companion boards (e.g., QLA) include a 25AA128 32 | // (or compatible) PROM connected to pins IO1-1 through IO1-4. 33 | // - Reboot FPGA via software command (Firmware V7+) 34 | // - Turn on/off LED on FPGA board; note that for some hardware (e.g., QLA), 35 | // this would also enable board power 36 | // - Query whether FPGA board has Ethernet interface 37 | // - Query FPGA clock period 38 | // - Read Ethernet/Firewire buffers from FPGA board (for debugging) 39 | // - Read/Write Firewire PHY register 40 | // 41 | // This class also contains a firmwareTime member (and accessor methods), which is 42 | // updated by the derived class to keep a running timer based on the FPGA clock. 43 | 44 | class FpgaIO : public BoardIO 45 | { 46 | public: 47 | 48 | FpgaIO(uint8_t board_id); 49 | ~FpgaIO(); 50 | 51 | // Return FPGA serial number (empty string if not found) 52 | // This method actually performs a read (should have been called ReadFPGASerialNumber) 53 | std::string GetFPGASerialNumber(void); 54 | 55 | // Returns FPGA clock period in seconds 56 | double GetFPGAClockPeriod(void) const; 57 | 58 | // Returns true if FPGA has Ethernet (Rev 2.0+) 59 | bool HasEthernet(void) const; 60 | 61 | // Get elapsed time, in seconds, based on FPGA clock. This is computed by accumulating 62 | // the timestamp values in the real-time read packet; thus, it is only accurate when 63 | // there are periodic calls to ReadAllBoards or ReadAllBoardsBroadcast. 64 | double GetFirmwareTime(void) const { return firmwareTime; } 65 | 66 | // Set firmware time 67 | void SetFirmwareTime(double newTime = 0.0) { firmwareTime = newTime; } 68 | 69 | // ********************** WRITE Methods ********************************** 70 | 71 | // Reboot FPGA 72 | bool WriteReboot(void); 73 | 74 | // Turn FPGA LED on/off (for QLA, same bit as pwr_enable) 75 | bool WriteLED(bool status); 76 | 77 | // **************** Static WRITE Methods (for broadcast) ******************** 78 | 79 | static bool WriteRebootAll(BasePort *port); 80 | 81 | static bool ResetEthernetAll(BasePort *port); 82 | 83 | // ********************** PROM Methods *********************************** 84 | // Methods for reading or programming 85 | // 1 - the FPGA configuration PROM (M25P16) 86 | // 2 - Hardware (QLA) PROM (25AA128) 87 | // 25AA128_1 and 25AA128_2 are used to support multiple PROMs (DQLA) 88 | // NOTE: 89 | // - M25P16 & 25AA128 Have exact same command table, except M25P16 has 90 | // a few extra commands (e.g. ReadID, DeepSleep) 91 | // - address length 92 | // - M25P16: 24-bit 93 | // - 25AA128: 16-bit (2 MSB are ignored) 94 | // - 25AA128_1: same as 25AA128 95 | // - 25AA128_2: same as 25AA128 96 | enum PromType{ 97 | PROM_M25P16 = 1, 98 | PROM_25AA128 = 2, 99 | PROM_25AA128_1 = 3, 100 | PROM_25AA128_2 = 4 101 | }; 102 | 103 | /*! 104 | \brief Get PROM read/write address (1394 space) based on prom type 105 | 106 | \param type PROM type e.g. M25P16/25AA128 107 | \param isWrite write address or read address 108 | \return nodeaddr_t return address in FPGA 1394 address space 109 | */ 110 | nodeaddr_t GetPromAddress(PromType type = PROM_M25P16, bool isWrite = true); 111 | 112 | // User-supplied callback, called when software needs to wait for an 113 | // action, or when an error occurs. In the latter case, the error message 114 | // is passed as a parameter (NULL if no error). If the callback returns 115 | // false, the current wait operation is aborted. 116 | typedef bool (*ProgressCallback)(const char *msg); 117 | 118 | // Returns the PROM ID (M25P16 ONLY) 119 | // should be 0x00202015 for the M25P16 120 | // returns 0 on error 121 | uint32_t PromGetId(void); 122 | 123 | // PromGetStatus (General) 124 | // returns the status register from the M25P16 PROM 125 | // (this is different than the interface status read from address offset 8) 126 | // The following masks are for the useful status register bits. 127 | enum { MASK_WIP = 1, MASK_WEL = 2 }; // status register bit masks 128 | bool PromGetStatus(uint32_t &status, PromType type = PROM_M25P16); 129 | 130 | // PromGetResult (General) 131 | // returns the result (if any) from the last command sent 132 | bool PromGetResult(uint32_t &result, PromType type = PROM_M25P16); 133 | 134 | // Returns nbytes data read from the specified address 135 | bool PromReadData(uint32_t addr, uint8_t *data, 136 | unsigned int nbytes); 137 | 138 | // Enable programming commands (erase and program page) (General) 139 | // This sets the WEL bit in the status register. 140 | // This mode is automatically cleared after a programming command is executed. 141 | bool PromWriteEnable(PromType type = PROM_M25P16); 142 | 143 | // Disable programming commands (General) 144 | // This clears the WEL bit in the status register. 145 | bool PromWriteDisable(PromType type = PROM_M25P16); 146 | 147 | // Erase a sector (64K) at the specified address. (M25P16 ONLY) 148 | // This command calls PromWriteEnable. 149 | // If non-zero, the callback (cb) is called while the software is waiting 150 | // for the PROM to be erased, or if there is an error. 151 | bool PromSectorErase(uint32_t addr, const ProgressCallback cb = 0); 152 | 153 | // Program a page (up to 256 bytes) at the specified address. 154 | // This command calls PromWriteEnable. 155 | // nbytes must be a multiple of 4. 156 | // If non-zero, the callback (cb) is called while the software is waiting 157 | // for the PROM to be programmed, or if there is an error. 158 | // Returns the number of bytes programmed (-1 if error). 159 | int PromProgramPage(uint32_t addr, const uint8_t *bytes, 160 | unsigned int nbytes, const ProgressCallback cb = 0); 161 | 162 | 163 | // ******************* Hardware (QLA) PROM ONLY Methods *************************** 164 | // Parameter "chan" is used to distinguish between multiple PROMs. Set to 0 for QLA 165 | // and set to 1 or 2 for DQLA. These get converted to PROM_25AA128, PROM_25AA128_1 166 | // and PROM_25AA128_2 internally. 167 | bool PromReadByte25AA128(uint16_t addr, uint8_t &data, 168 | unsigned char chan = 0); 169 | bool PromWriteByte25AA128(uint16_t addr, const uint8_t &data, 170 | unsigned char chan = 0); 171 | bool PromReadBlock25AA128(uint16_t addr, quadlet_t* data, unsigned int nquads, 172 | unsigned char chan = 0); 173 | bool PromWriteBlock25AA128(uint16_t addr, quadlet_t* data, unsigned int nquads, 174 | unsigned char chan = 0); 175 | 176 | // ********************** KSZ8851 Ethernet MAC/PHY Methods ************************ 177 | // Following functions enable access to the KSZ8851 Ethernet controller on the 178 | // FPGA V2 board via FireWire. They are provided for testing/debugging. 179 | // Note that both 8-bit and 16-bit transactions are supported. 180 | bool WriteKSZ8851Reg(uint8_t addr, const uint8_t &data); 181 | bool WriteKSZ8851Reg(uint8_t addr, const uint16_t &data); 182 | bool ReadKSZ8851Reg(uint8_t addr, uint8_t &rdata); 183 | bool ReadKSZ8851Reg(uint8_t addr, uint16_t &rdata); 184 | // Following are for DMA access (assumes chip has been placed in DMA mode) 185 | bool WriteKSZ8851DMA(const uint16_t &data); 186 | bool ReadKSZ8851DMA(uint16_t &rdata); 187 | // Read Chip ID from register 0xC0 188 | uint16_t ReadKSZ8851ChipID(); 189 | // Read KSZ8851 status; format is: VALID(1) ERROR(1) initOK(1) ... 190 | // VALID=1 indicates that Ethernet is present 191 | // ERROR=1 indicates that last command had an error (i.e., state machine was not idle) 192 | // InitOK=1 indicates that the KSZ8851 has been initialized 193 | // For other bits, see EthBasePort::PrintStatus 194 | // Returns 0 on error (i.e., if Ethernet not present, or read fails) 195 | uint16_t ReadKSZ8851Status(); 196 | 197 | // *********************** RTL8211F Ethernet PHY Methods ************************** 198 | // The following functions enable access to both of the RTL8211F Ethernet PHYs on 199 | // the FPGA V3 board via FireWire. They are provided for testing/debugging. 200 | 201 | // The RTL8211F registers are provided via multiple pages. Use the RTL8211F_PAGSR 202 | // (page select register) to select the desired page. The default page is 0xa42. 203 | // Version 1.4 of the datasheet specifies the page for each register, though it 204 | // appears that many of the registers are accessible on multiple pages (clearly PAGSR 205 | // must be accessible on all pages). The datasheet indicates that registers 0-15 206 | // (IEEE Standard Registers) can be accessed from page 0 or 0xa42. 207 | // It appears that in some cases, it may be possible to read a register on multiple 208 | // pages, but only possible to write to the register from a specific page. 209 | // The following enum defines some of the more useful registers, and indicates the 210 | // corresponding page that is specified in the Version 1.4 datasheet. 211 | enum RTL8211F_Pages { 212 | RTL8211F_PAGE_IEEE = 0, // IEEE Standard Registers 213 | RTL8211F_PAGE_DEFAULT = 0x0a42, // Default page (includes IEEE registers) 214 | RTL8211F_PAGE_LED = 0x0d04 // Page for LED configuration 215 | }; 216 | 217 | enum RTL8211F_Regs { 218 | RTL8211F_BMCR = 0, // Basic Mode Control Register, page 0 219 | RTL8211F_BMSR = 1, // Basic Mode Status Register, page 0 220 | RTL8211F_PHYID1 = 2, // PHY Identifier Register 1, page 0 221 | RTL8211F_PHYID2 = 3, // PHY Identifier Register 2, page 0 222 | RTL8211F_INER = 18, // Interrupt Enable Register, page 0xa42 223 | RTL8211F_PHYSR = 26, // PHY Specific Status Register, page 0xa43 224 | RTL8211F_INSR = 29, // Interrupt Status Register, page 0xa43 225 | RTL8211F_PAGSR = 31 // Page Select Register, 0xa43 226 | }; 227 | 228 | // Some useful Ethernet PHY addresses: 229 | // FPGAV3 is designed so that the RTL8211F PHY has a default address of 1. 230 | // The GMII to RGMII core uses the default PHY address of 8. 231 | enum PHY_ADDR { PHY_BROADCAST = 0, PHY_RTL8211F = 1, PHY_GMII_CORE = 8 }; 232 | 233 | // Wait for FPGA mdio interface to return to idle state, using specified number of loops 234 | // (loop_cnt) as timeout. This method is necessary for the Zynq EMIO mmap interface, 235 | // which is faster than the FPGA mdio state machine. 236 | // callerName name of calling routine (used if printing timeout message) 237 | // chan 1 or 2 for PHY1 or PHY2 238 | // loop_cnt number of read loops for checking mdio state 239 | bool WaitRTL8211F_Idle(const char *callerName, unsigned int chan, unsigned int loop_cnt); 240 | 241 | // Read PHY register 242 | // chan 1 or 2 for PHY1 or PHY2 243 | // phyAddr PHY address (see PHY_ADDR) 244 | // regAddr PHY register to read (see RTL8211F_Regs) 245 | // data data read from PHY register 246 | // Returns true if read was successful 247 | bool ReadRTL8211F_Register(unsigned int chan, unsigned int phyAddr, unsigned int regAddr, uint16_t &data); 248 | 249 | // Write PHY register 250 | // chan 1 or 2 for PHY1 or PHY2 251 | // phyAddr PHY address (see PHY_ADDR) 252 | // regAddr PHY register to write (see RTL8211F_Regs) 253 | // data data to write to PHY register 254 | // Returns true if write was successful 255 | bool WriteRTL8211F_Register(unsigned int chan, unsigned int phyAddr, unsigned int regAddr, uint16_t data); 256 | 257 | // ************************ Ethernet Methods ************************************* 258 | 259 | // Read Ethernet status 260 | // Reads the version-specific Ethernet status register. The first two most significant 261 | // bits indicate the FPGA version: 262 | // 00 FPGA V1 (no Ethernet) 263 | // 1x FPGA V2 (one Ethernet port, with KSZ8851 PHY) 264 | // 01 FPGA V3 (two Ethernet ports, with RTL8211F PHYs) 265 | // The following enums define the bits (see also EthBasePort::PrintStatus) 266 | enum EthStatus { 267 | ETH_STAT_PRESENT_V2 = 0x80000000, // Indicates FPGA V2 268 | ETH_STAT_REQ_ERR_V2 = 0x40000000, // V2: Request error (V3: 1) 269 | ETH_STAT_INIT_OK_V2 = 0x20000000, // V2: Init OK 270 | ETH_STAT_FRAME_ERR = 0x10000000, // Unsupported Ethernet frame (not Raw, IPv4, or ARP) 271 | ETH_STAT_IPV4_ERR = 0x08000000, // IPv4 header error (e.g., not UDP or ICMP) 272 | ETH_STAT_UDP_ERR = 0x04000000, // Unsupported UDP port (not 1394) 273 | ETH_STAT_DEST_ERR = 0x02000000, // Incorrect Firewire packet destination address 274 | ETH_STAT_ACCESS_ERR = 0x01000000, // Internal bus access error (0 for Rev 9+) 275 | ETH_STAT_STATE_ERR_V2 = 0x00800000, // V2: KSZ8851 state machine error 276 | ETH_STAT_CLK125_OK_V3 = 0x00800000, // V3: 125 MHz clock ok 277 | ETH_STAT_ETHST_ERR = 0x00400000, // EthernetIO state machine error 278 | ETH_STAT_CLK200_OK_V3 = 0x00200000, // V3: 200 MHz clock ok (V2: 0) 279 | ETH_STAT_UDP = 0x00100000, // 1 -> UDP mode, 0 -> Raw Ethernet 280 | ETH_STAT_LINK_STAT_V2 = 0x00080000, // V2: link status (1 -> On) 281 | ETH_STAT_IDLE_V2 = 0x00040000, // V2: KSZ8851 state machine is idle 282 | ETH_STAT_WAIT_MASK_V2 = 0x00030000, // V2: Mask for waitInfo 283 | ETH_STAT_PSETH_EN_V3 = 0x00010000, // V3: PS Ethernet enabled (Rev 9+) 284 | ETH_STAT_DATA_MASK_V2 = 0x0000ffff, // V2: Mask for register read result 285 | ETH_STAT_PORT2_MASK_V3 = 0x0000ff00, // V3: Mask for Eth2 status (see EthPortV3Status) 286 | ETH_STAT_PORT1_MASK_V3 = 0x000000ff // V3: Mask for Eth1 status (see EthPortV3Status) 287 | }; 288 | bool ReadEthernetStatus(uint32_t &status); 289 | 290 | // Get Ethernet port status (for FPGA V3) from specified status value 291 | // status Status value from ReadEthernetStatus 292 | // chan Ethernet port number (1 or 2) 293 | // Returns the 8-bit port status 294 | enum EthPortV3Status { 295 | ETH_PORT_STAT_INIT_OK = 0x80, 296 | ETH_PORT_STAT_HAS_IRQ = 0x40, 297 | ETH_PORT_STAT_LINK_STAT = 0x20, 298 | ETH_PORT_STAT_SPEED_MASK = 0x18, 299 | ETH_PORT_STAT_RECV_ERR = 0x04, // Rev 8 only 300 | ETH_PORT_STAT_SEND_OVF = 0x02, // Rev 8 only 301 | ETH_PORT_STAT_PS_ETH = 0x01 // Rev 8 only 302 | }; 303 | static uint8_t GetEthernetPortStatusV3(uint32_t status, unsigned int chan); 304 | 305 | // Write Ethernet control register 306 | enum EthControl { 307 | ETH_CTRL_CLR_ERR_SW_V3 = 0x40000000, // V3: Clear switch errors 308 | ETH_CTRL_CLR_ERR_NET = 0x20000000, // Clear network layer errors 309 | ETH_CTRL_CLR_ERR_LINK_V2 = 0x10000000, // V2: Clear link layer errors 310 | ETH_CTRL_IRQ_GEN_V3 = 0x10000000, // V3: Mask for generating software IRQ (MaskL) 311 | ETH_CTRL_DMA_V2 = 0x08000000, // V2: DMA register access 312 | ETH_CTRL_IRQ_DIS_V3 = 0x08000000, // V3: Mask for disabling interrupts 313 | ETH_CTRL_RESET_PHY = 0x04000000, // Reset PHY 314 | ETH_CTRL_WR_V2 = 0x02000000, // V2: Register read(0) or write(1) 315 | ETH_CTRL_EN_PS_ETH_V3 = 0x02000000, // V3: Mask for enabling PS ethernet 316 | ETH_CTRL_WB_V2 = 0x01000000, // V2: Byte(0) or word(1) access 317 | ETH_CTRL_ADDR_MASK_V2 = 0x00ff0000, // V2: Register address (8 bits) 318 | ETH_CTRL_PS_ETH_V3 = 0x00010000, // V3: Enable PS ethernet 319 | ETH_CTRL_DATA_MASK_V2 = 0x0000ffff // V2: Register data (16 bits) 320 | }; 321 | // Offsets for Ethernet port control (FPGA V3) 322 | // For Eth1, use these bit masks 323 | // For Eth2, shift left by 8 324 | enum EthPortV3Control { 325 | ETH_PORT_CTRL_RESET_PHY = 0x80, // Reset RTL8211F PHY (if ETH_CTRL_RESET_PHY) 326 | ETH_PORT_CTRL_IRQ_DIS = 0x40, // Disable interrupts 327 | ETH_PORT_CTRL_IRQ_GEN = 0x20 // Generate interrupt from software (if ETH_CTRL_IRQ_GEN_V3) 328 | }; 329 | bool WriteEthernetControl(uint32_t write_data); 330 | 331 | // Reset the Ethernet PHY chip (KSZ8851 for FPGA V2, RTL8211F for FPGA V3) 332 | // chan FPGA V2: 0 (requires ~60 msec) 333 | // FPGA V3: 1 -> PHY1, 2 -> PHY2, 3-> both 334 | bool WriteEthernetPhyReset(unsigned int chan = 0); 335 | 336 | // Clear Ethernet errors 337 | // For V2, clears EthernetIO and KSZ8851 errors 338 | // For V3, clears EthernetIO and EthSwitch errors 339 | bool WriteEthernetClearErrors(); 340 | 341 | // Read Ethernet data 342 | // buffer buffer for storing data 343 | // offset address offset (in quadlets) 344 | // nquads number of quadlets to read (not more than 64) 345 | bool ReadEthernetData(quadlet_t *buffer, unsigned int offset, unsigned int nquads); 346 | 347 | // ************************ FireWire Methods ************************************* 348 | // Read FireWire data 349 | // buffer buffer for storing data 350 | // offset address offset (in quadlets) 351 | // nquads number of quadlets to read (not more than 64) 352 | bool ReadFirewireData(quadlet_t *buffer, unsigned int offset, unsigned int nquads); 353 | 354 | // Read FireWire PHY register 355 | // addr FireWire PHY register number (0-15) 356 | // data Data read from register 357 | bool ReadFirewirePhy(unsigned char addr, unsigned char &data); 358 | 359 | // Write FireWire PHY register 360 | // addr FireWire PHY register number (0-15) 361 | // data Data to write to register 362 | bool WriteFirewirePhy(unsigned char addr, unsigned char data); 363 | 364 | // Print FireWire debug data 365 | static void PrintFirewireDebug(std::ostream &debugStream, const quadlet_t *data); 366 | 367 | protected: 368 | 369 | // Accumulated firmware time 370 | double firmwareTime; 371 | 372 | }; 373 | 374 | #endif // __FpgaIO_H__ 375 | --------------------------------------------------------------------------------