From 5b906aa3ea8ac95e7872e9bc92d54710557d4102 Mon Sep 17 00:00:00 2001 From: Steffen Vogel Date: Tue, 21 Aug 2018 13:24:17 +0200 Subject: [PATCH] move more common code from VILLASfpga repo --- common/include/villas/config.h | 62 ++ common/include/villas/kernel/kernel.h | 98 ++++ common/include/villas/kernel/pci.h | 84 +++ common/include/villas/kernel/vfio.hpp | 161 ++++++ common/include/villas/list.h | 120 ++++ common/include/villas/log.h | 194 +++++++ common/include/villas/log_config.h | 37 ++ common/include/villas/utils.h | 284 +++++++++ common/lib/CMakeLists.txt | 15 + common/lib/kernel/kernel.c | 283 +++++++++ common/lib/kernel/pci.c | 388 +++++++++++++ common/lib/kernel/vfio.cpp | 803 ++++++++++++++++++++++++++ common/lib/list.c | 203 +++++++ common/lib/log.c | 310 ++++++++++ common/lib/log_config.c | 85 +++ common/lib/log_helper.c | 138 +++++ common/lib/utils.c | 401 +++++++++++++ 17 files changed, 3666 insertions(+) create mode 100644 common/include/villas/config.h create mode 100644 common/include/villas/kernel/kernel.h create mode 100644 common/include/villas/kernel/pci.h create mode 100644 common/include/villas/kernel/vfio.hpp create mode 100644 common/include/villas/list.h create mode 100644 common/include/villas/log.h create mode 100644 common/include/villas/log_config.h create mode 100644 common/include/villas/utils.h create mode 100644 common/lib/kernel/kernel.c create mode 100644 common/lib/kernel/pci.c create mode 100644 common/lib/kernel/vfio.cpp create mode 100644 common/lib/list.c create mode 100644 common/lib/log.c create mode 100644 common/lib/log_config.c create mode 100644 common/lib/log_helper.c create mode 100644 common/lib/utils.c diff --git a/common/include/villas/config.h b/common/include/villas/config.h new file mode 100644 index 000000000..6982778c4 --- /dev/null +++ b/common/include/villas/config.h @@ -0,0 +1,62 @@ +/** Compile time configuration + * + * This file contains some compiled-in settings. + * This settings are not part of the configuration file. + * + * @file + * @author Steffen Vogel + * @copyright 2017-2018, Institute for Automation of Complex Power Systems, EONERC + * @license GNU General Public License (version 3) + * + * VILLASfpga + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + *********************************************************************************/ + +#pragma once + +#ifndef V + #define V 2 +#endif + +/* Paths */ +#define PLUGIN_PATH PREFIX "/share/villas/node/plugins" +#define WEB_PATH PREFIX "/share/villas/node/web" +#define SYSFS_PATH "/sys" +#define PROCFS_PATH "/proc" + +/** Default number of values in a sample */ +#define DEFAULT_SAMPLELEN 64 +#define DEFAULT_QUEUELEN 1024 + +/** Number of hugepages which are requested from the the kernel. + * @see https://www.kernel.org/doc/Documentation/vm/hugetlbpage.txt */ +#define DEFAULT_NR_HUGEPAGES 100 + +/** Width of log output in characters */ +#define LOG_WIDTH 80 +#define LOG_HEIGHT 25 + +/** Socket priority */ +#define SOCKET_PRIO 7 + +/* Protocol numbers */ +#define IPPROTO_VILLAS 137 +#define ETH_P_VILLAS 0xBABE + +#define USER_AGENT "VILLASfpga (" BUILDID ")" + +/* Required kernel version */ +#define KERNEL_VERSION_MAJ 3 +#define KERNEL_VERSION_MIN 6 diff --git a/common/include/villas/kernel/kernel.h b/common/include/villas/kernel/kernel.h new file mode 100644 index 000000000..d5528acb5 --- /dev/null +++ b/common/include/villas/kernel/kernel.h @@ -0,0 +1,98 @@ +/** Linux kernel related functions. + * + * @file + * @author Steffen Vogel + * @copyright 2017-2018, Institute for Automation of Complex Power Systems, EONERC + * @license GNU General Public License (version 3) + * + * VILLASfpga + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + *********************************************************************************/ + +/** @addtogroup fpga Kernel @{ */ + +#pragma once + +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/* Forward declarations */ +struct version; + +//#include + +/** Check if current process has capability \p cap. + * + * @retval 0 If capabilty is present. + * @retval <0 If capability is not present. + */ +//int kernel_check_cap(cap_value_t cap); + +/** Get number of reserved hugepages. */ +int kernel_get_nr_hugepages(); + +/** Set number of reserved hugepages. */ +int kernel_set_nr_hugepages(int nr); + +/** Get kernel cmdline parameter + * + * See https://www.kernel.org/doc/Documentation/kernel-parameters.txt + * + * @param param The cmdline parameter to look for. + * @param buf The string buffer to which the parameter value will be copied to. + * @param len The length of the buffer \p value + * @retval 0 Parameter \p key was found and value was copied to \p value + * @reval <>0 Kernel was not booted with parameter \p key + */ +int kernel_get_cmdline_param(const char *param, char *buf, size_t len); + +/** Get the version of the kernel. */ +int kernel_get_version(struct version *v); + +/** Checks if a kernel module is loaded + * + * @param module the name of the module + * @retval 0 Module is loaded. + * @reval <>0 Module is not loaded. + */ +int kernel_module_loaded(const char *module); + +/** Load kernel module via modprobe */ +int kernel_module_load(const char *module); + +/** Set parameter of loaded kernel module */ +int kernel_module_set_param(const char *module, const char *param, const char *value); + +/** Get cacheline size in bytes */ +int kernel_get_cacheline_size(); + +/** Get the size of a standard page in bytes. */ +int kernel_get_page_size(); + +/** Get the size of a huge page in bytes. */ +int kernel_get_hugepage_size(); + +/** Set SMP affinity of IRQ */ +int kernel_irq_setaffinity(unsigned irq, uintmax_t affinity, uintmax_t *old); + +#ifdef __cplusplus +} +#endif + +/** @} */ diff --git a/common/include/villas/kernel/pci.h b/common/include/villas/kernel/pci.h new file mode 100644 index 000000000..8f253306b --- /dev/null +++ b/common/include/villas/kernel/pci.h @@ -0,0 +1,84 @@ +/** Linux PCI helpers + * + * @file + * @author Steffen Vogel + * @copyright 2017-2018, Steffen Vogel + **********************************************************************************/ + +/** @addtogroup fpga Kernel @{ */ + +#pragma once + +#include +#include +#include + +#define PCI_SLOT(devfn) (((devfn) >> 3) & 0x1f) +#define PCI_FUNC(devfn) ((devfn) & 0x07) + +#ifdef __cplusplus +extern "C" { +#endif + +struct pci_device { + struct { + int vendor; + int device; + int class_code; + } id; + + struct { + int domain; + int bus; + int device; + int function; + } slot; /**< Bus, Device, Function (BDF) */ +}; + +struct pci_region { + int num; + uintptr_t start; + uintptr_t end; + unsigned long long flags; +}; + +struct pci { + struct list devices; /**< List of available PCI devices in the system (struct pci_device) */ +}; + +/** Initialize Linux PCI handle. + * + * This search for all available PCI devices under /sys/bus/pci + * + * @retval 0 Success. Everything went well. + * @retval <0 Error. Something went wrong. + */ +int pci_init(struct pci *p); + +/** Destroy handle. */ +int pci_destroy(struct pci *p); + +int pci_device_parse_slot(struct pci_device *f, const char *str, const char **error); + +int pci_device_parse_id(struct pci_device *f, const char *str, const char **error); + +int pci_device_compare(const struct pci_device *d, const struct pci_device *f); + +struct pci_device * pci_lookup_device(struct pci *p, struct pci_device *filter); + +/** Get currently loaded driver for device */ +int pci_get_driver(const struct pci_device *d, char *buf, size_t buflen); + +/** Bind a new LKM to the PCI device */ +int pci_attach_driver(const struct pci_device *d, const char *driver); + +/** Return the IOMMU group of this PCI device or -1 if the device is not in a group. */ +int pci_get_iommu_group(const struct pci_device *d); + +size_t pci_get_regions(const struct pci_device *d, struct pci_region** regions); + +#ifdef __cplusplus +} +#endif + +/** @} */ diff --git a/common/include/villas/kernel/vfio.hpp b/common/include/villas/kernel/vfio.hpp new file mode 100644 index 000000000..69cfb8c41 --- /dev/null +++ b/common/include/villas/kernel/vfio.hpp @@ -0,0 +1,161 @@ +/** Virtual Function IO wrapper around kernel API + * + * @file + * @author Steffen Vogel + * @author Daniel Krebs + * @copyright 2017-2018, Steffen Vogel + * @copyright 2018, Daniel Krebs + *********************************************************************************/ + +/** @addtogroup fpga Kernel @{ */ + +#pragma once + +#include +#include +#include + +#include +#include + +#define VFIO_PATH "/dev/vfio/" +#define VFIO_DEV VFIO_PATH "vfio" + +/* Forward declarations */ +struct pci_device; + +namespace villas { + +class VfioContainer; +class VfioGroup; + + +class VfioDevice { + friend class VfioContainer; +public: + VfioDevice(const std::string& name, VfioGroup& group) : + name(name), group(group) {} + + ~VfioDevice(); + + bool reset(); + + /** Map a device memory region to the application address space (e.g. PCI BARs) */ + void* regionMap(size_t index); + + /** munmap() a region which has been mapped by vfio_map_region() */ + bool regionUnmap(size_t index); + + /** Get the size of a device memory region */ + size_t regionGetSize(size_t index); + + + /** Enable memory accesses and bus mastering for PCI device */ + bool pciEnable(); + + bool pciHotReset(); + int pciMsiInit(int efds[32]); + int pciMsiDeinit(int efds[32]); + bool pciMsiFind(int nos[32]); + + bool isVfioPciDevice() const; + +private: + /// Name of the device as listed under + /// /sys/kernel/iommu_groups/[vfio_group::index]/devices/ + std::string name; + + /// VFIO device file descriptor + int fd; + + struct vfio_device_info info; + + std::vector irqs; + std::vector regions; + std::vector mappings; + + /**< libpci handle of the device */ + const struct pci_device *pci_device; + + VfioGroup& group; /**< The VFIO group this device belongs to */ +}; + + + +class VfioGroup { + friend class VfioContainer; + friend VfioDevice; +private: + VfioGroup(int index) : fd(-1), index(index) {} +public: + ~VfioGroup(); + + static std::unique_ptr + attach(VfioContainer& container, int groupIndex); + +private: + /// VFIO group file descriptor + int fd; + + /// Index of the IOMMU group as listed under /sys/kernel/iommu_groups/ + int index; + + /// Status of group + struct vfio_group_status status; + + /// All devices owned by this group + std::list> devices; + + VfioContainer* container; /**< The VFIO container to which this group is belonging */ +}; + + +class VfioContainer { +private: + VfioContainer(); +public: + ~VfioContainer(); + + static std::shared_ptr + create(); + + void dump(); + + VfioDevice& attachDevice(const char *name, int groupIndex); + VfioDevice& attachDevice(const struct pci_device *pdev); + + /** + * @brief Map VM to an IOVA, which is accessible by devices in the container + * @param virt virtual address of memory + * @param phys IOVA where to map @p virt, -1 to use VFIO internal allocator + * @param length size of memory region in bytes + * @return IOVA address, UINTPTR_MAX on failure + */ + uintptr_t memoryMap(uintptr_t virt, uintptr_t phys, size_t length); + + /** munmap() a region which has been mapped by vfio_map_region() */ + bool memoryUnmap(uintptr_t phys, size_t length); + + bool isIommuEnabled() const + { return this->hasIommu; } + + const int& getFd() const + { return fd; } + +private: + VfioGroup& getOrAttachGroup(int index); + +private: + int fd; + int version; + int extensions; + uint64_t iova_next; /**< Next free IOVA address */ + bool hasIommu; + + /// All groups bound to this container + std::list> groups; +}; + +/** @} */ + +} // namespace villas diff --git a/common/include/villas/list.h b/common/include/villas/list.h new file mode 100644 index 000000000..b206da037 --- /dev/null +++ b/common/include/villas/list.h @@ -0,0 +1,120 @@ +/** A generic list implementation. + * + * This is a generic implementation of a list which can store void pointers to + * arbitrary data. The data itself is not stored or managed by the list. + * + * Internally, an array of pointers is used to store the pointers. + * If needed, this array will grow by realloc(). + * + * @file + * @author Steffen Vogel + * @copyright 2017-2018, Institute for Automation of Complex Power Systems, EONERC + *********************************************************************************/ + +#pragma once + +#include +#include + +#include + +#define LIST_CHUNKSIZE 16 + +/** Static list initialization */ +#define LIST_INIT() { \ + .array = NULL, \ + .length = 0, \ + .capacity = 0, \ + .lock = PTHREAD_MUTEX_INITIALIZER, \ + .state = STATE_INITIALIZED \ +} + +#define LIST_INIT_STATIC(l) \ +__attribute__((constructor(105))) static void UNIQUE(__ctor)() {\ + if ((l)->state == STATE_DESTROYED) \ + list_init(l); \ +} \ +__attribute__((destructor(105))) static void UNIQUE(__dtor)() { \ + list_destroy(l, NULL, false); \ +} + +#define list_length(list) ((list)->length) +#define list_at_safe(list, index) ((list)->length > index ? (list)->array[index] : NULL) +#define list_at(list, index) ((list)->array[index]) + +#define list_first(list) list_at(list, 0) +#define list_last(list) list_at(list, (list)->length-1) + +#ifdef __cplusplus +extern "C" { +#endif + +/** Callback to destroy list elements. + * + * @param data A pointer to the data which should be freed. + */ +typedef int (*dtor_cb_t)(void *); + +/** Callback to search or sort a list. */ +typedef int (*cmp_cb_t)(const void *, const void *); + +/* The list data structure. */ +struct list { + void **array; /**< Array of pointers to list elements */ + size_t capacity; /**< Size of list::array in elements */ + size_t length; /**< Number of elements of list::array which are in use */ + pthread_mutex_t lock; /**< A mutex to allow thread-safe accesses */ + enum state state; /**< The state of this list. */ +}; + +/** Initialize a list. + * + * @param l A pointer to the list data structure. + */ +int list_init(struct list *l); + +/** Destroy a list and call destructors for all list elements + * + * @param free free() all list members during when calling list_destroy() + * @param dtor A function pointer to a desctructor which will be called for every list item when the list is destroyed. + * @param l A pointer to the list data structure. + */ +int list_destroy(struct list *l, dtor_cb_t dtor, bool free); + +/** Append an element to the end of the list */ +void list_push(struct list *l, void *p); + +/** Remove all occurences of a list item */ +void list_remove(struct list *l, void *p); + +/** Return the first list element which is identified by a string in its first member variable. + * + * List elements are pointers to structures of the following form: + * + * struct obj { + * char *name; + * // more members + * } + * + * @see Only possible because of §1424 of http://c0x.coding-guidelines.com/6.7.2.1.html + */ +void * list_lookup(struct list *l, const char *name); + +/** Return the first element of the list for which cmp returns zero */ +void * list_search(struct list *l, cmp_cb_t cmp, void *ctx); + +/** Returns the number of occurences for which cmp returns zero when called on all list elements. */ +int list_count(struct list *l, cmp_cb_t cmp, void *ctx); + +/** Return 0 if list contains pointer p */ +int list_contains(struct list *l, void *p); + +/** Sort the list using the quicksort algorithm of libc */ +void list_sort(struct list *l, cmp_cb_t cmp); + +/** Set single element in list */ +int list_set(struct list *l, int index, void *value); + +#ifdef __cplusplus +} +#endif diff --git a/common/include/villas/log.h b/common/include/villas/log.h new file mode 100644 index 000000000..98d266c3c --- /dev/null +++ b/common/include/villas/log.h @@ -0,0 +1,194 @@ +/** Logging and debugging routines + * + * @file + * @author Steffen Vogel + * @copyright 2017-2018, Institute for Automation of Complex Power Systems, EONERC + * @license GNU General Public License (version 3) + * + * VILLASfpga + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + *********************************************************************************/ + +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include + +#include +#include + +#ifdef __GNUC__ + #define INDENT int __attribute__ ((__cleanup__(log_outdent), unused)) _old_indent = log_indent(1); + #define NOINDENT int __attribute__ ((__cleanup__(log_outdent), unused)) _old_indent = log_noindent(); +#else + #define INDENT ; + #define NOINDENT ; +#endif + +/* The log level which is passed as first argument to print() */ +#define LOG_LVL_DEBUG CLR_GRY("Debug") +#define LOG_LVL_INFO CLR_WHT("Info ") +#define LOG_LVL_WARN CLR_YEL("Warn ") +#define LOG_LVL_ERROR CLR_RED("Error") +#define LOG_LVL_STATS CLR_MAG("Stats") + +/** Debug facilities. + * + * To be or-ed with the debug level + */ +enum log_facilities { + LOG_POOL = (1L << 8), + LOG_QUEUE = (1L << 9), + LOG_CONFIG = (1L << 10), + LOG_HOOK = (1L << 11), + LOG_PATH = (1L << 12), + LOG_NODE = (1L << 13), + LOG_MEM = (1L << 14), + LOG_WEB = (1L << 15), + LOG_API = (1L << 16), + LOG_LOG = (1L << 17), + LOG_VFIO = (1L << 18), + LOG_PCI = (1L << 19), + LOG_XIL = (1L << 20), + LOG_TC = (1L << 21), + LOG_IF = (1L << 22), + LOG_ADVIO = (1L << 23), + + /* Node-types */ + LOG_SOCKET = (1L << 24), + LOG_FILE = (1L << 25), + LOG_FPGA = (1L << 26), + LOG_NGSI = (1L << 27), + LOG_WEBSOCKET = (1L << 28), + LOG_OPAL = (1L << 30), + + /* Classes */ + LOG_NODES = LOG_NODE | LOG_SOCKET | LOG_FILE | LOG_FPGA | LOG_NGSI | LOG_WEBSOCKET | LOG_OPAL, + LOG_KERNEL = LOG_VFIO | LOG_PCI | LOG_TC | LOG_IF, + LOG_ALL = ~0xFF +}; + +struct log { + enum state state; + + struct timespec epoch; /**< A global clock used to prefix the log messages. */ + + struct winsize window; /**< Size of the terminal window. */ + int width; /**< The real usable log output width which fits into one line. */ + + /** Debug level used by the debug() macro. + * It defaults to V (defined by the Makefile) and can be + * overwritten by the 'debug' setting in the configuration file. */ + int level; + long facilities; /**< Debug facilities used by the debug() macro. */ + const char *path; /**< Path of the log file. */ + char *prefix; /**< Prefix each line with this string. */ + int syslog; /**< Whether or not to log to syslogd. */ + + FILE *file; /**< Send all log output to this file / stdout / stderr. */ +}; + +/** The global log instance. */ +extern struct log *global_log; +extern struct log default_log; + +/** Initialize log object */ +int log_init(struct log *l, int level, long faciltities); + +int log_start(struct log *l); + +int log_stop(struct log *l); + +/** Destroy log object */ +int log_destroy(struct log *l); + +/** Change log indention for current thread. + * + * The argument level can be negative! + */ +int log_indent(int levels); + +/** Disable log indention of current thread. */ +int log_noindent(); + +/** A helper function the restore the previous log indention level. + * + * This function is usually called by a __cleanup__ handler (GCC C Extension). + * See INDENT macro. + */ +void log_outdent(int *); + +/** Set logging facilities based on expression. + * + * Currently we support two types of expressions: + * 1. A comma seperated list of logging facilities + * 2. A comma seperated list of logging facilities which is prefixes with an exclamation mark '!' + * + * The first case enables only faciltities which are in the list. + * The second case enables all faciltities with exception of those which are in the list. + * + * @param expression The expression + * @return The new facilties mask (see enum log_faciltities) + */ +int log_set_facility_expression(struct log *l, const char *expression); + +/** Logs variadic messages to stdout. + * + * @param lvl The log level + * @param fmt The format string (printf alike) + */ +void log_print(struct log *l, const char *lvl, const char *fmt, ...) + __attribute__ ((format(printf, 3, 4))); + +/** Logs variadic messages to stdout. + * + * @param lvl The log level + * @param fmt The format string (printf alike) + * @param va The variadic argument list (see stdarg.h) + */ +void log_vprint(struct log *l, const char *lvl, const char *fmt, va_list va); + +/** Printf alike debug message with level. */ +void debug(long lvl, const char *fmt, ...) + __attribute__ ((format(printf, 2, 3))); + +/** Printf alike info message. */ +void info(const char *fmt, ...) + __attribute__ ((format(printf, 1, 2))); + +/** Printf alike warning message. */ +void warn(const char *fmt, ...) + __attribute__ ((format(printf, 1, 2))); + +/** Printf alike statistics message. */ +void stats(const char *fmt, ...) + __attribute__ ((format(printf, 1, 2))); + +/** Print error and exit. */ +void error(const char *fmt, ...) + __attribute__ ((format(printf, 1, 2))); + +/** Print error and strerror(errno). */ +void serror(const char *fmt, ...) + __attribute__ ((format(printf, 1, 2))); + +#ifdef __cplusplus +} +#endif diff --git a/common/include/villas/log_config.h b/common/include/villas/log_config.h new file mode 100644 index 000000000..e16bb724e --- /dev/null +++ b/common/include/villas/log_config.h @@ -0,0 +1,37 @@ +/** Logging routines that depend on jansson. + * + * @file + * @author Steffen Vogel + * @copyright 2017-2018, Institute for Automation of Complex Power Systems, EONERC + * @license GNU General Public License (version 3) + * + * VILLASfpga + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + *********************************************************************************/ + +#pragma once + +struct log; + +#include + +#include + +/** Parse logging configuration. */ +int log_parse(struct log *l, json_t *cfg); + +/** Print configuration error and exit. */ +void jerror(json_error_t *err, const char *fmt, ...) + __attribute__ ((format(printf, 2, 3))); diff --git a/common/include/villas/utils.h b/common/include/villas/utils.h new file mode 100644 index 000000000..ea06b660e --- /dev/null +++ b/common/include/villas/utils.h @@ -0,0 +1,284 @@ +/** Various helper functions. + * + * @file + * @author Steffen Vogel + * @copyright 2017-2018, Institute for Automation of Complex Power Systems, EONERC + * @license GNU General Public License (version 3) + * + * VILLASfpga + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + *********************************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include +#include + +#include + +#ifdef __cplusplus +extern "C" { +#endif + +#ifdef __GNUC__ + #define LIKELY(x) __builtin_expect((x),1) + #define UNLIKELY(x) __builtin_expect((x),0) +#else + #define LIKELY(x) (x) + #define UNLIKELY(x) (x) +#endif + +/* Some color escape codes for pretty log messages */ +#define CLR(clr, str) "\e[" XSTR(clr) "m" str "\e[0m" +#define CLR_GRY(str) CLR(30, str) /**< Print str in gray */ +#define CLR_RED(str) CLR(31, str) /**< Print str in red */ +#define CLR_GRN(str) CLR(32, str) /**< Print str in green */ +#define CLR_YEL(str) CLR(33, str) /**< Print str in yellow */ +#define CLR_BLU(str) CLR(34, str) /**< Print str in blue */ +#define CLR_MAG(str) CLR(35, str) /**< Print str in magenta */ +#define CLR_CYN(str) CLR(36, str) /**< Print str in cyan */ +#define CLR_WHT(str) CLR(37, str) /**< Print str in white */ +#define CLR_BLD(str) CLR( 1, str) /**< Print str in bold */ + +/* Alternate character set + * + * The suffixed of the BOX_ macro a constructed by + * combining the following letters in the written order: + * - U for a line facing upwards + * - D for a line facing downwards + * - L for a line facing leftwards + * - R for a line facing rightwards + * + * E.g. a cross can be constructed by combining all line fragments: + * BOX_UDLR + */ +#define BOX(chr) "\e(0" chr "\e(B" +#define BOX_LR BOX("\x71") /**< Boxdrawing: ─ */ +#define BOX_UD BOX("\x78") /**< Boxdrawing: │ */ +#define BOX_UDR BOX("\x74") /**< Boxdrawing: ├ */ +#define BOX_UDLR BOX("\x6E") /**< Boxdrawing: ┼ */ +#define BOX_UDL BOX("\x75") /**< Boxdrawing: ┤ */ +#define BOX_ULR BOX("\x76") /**< Boxdrawing: ┴ */ +#define BOX_UL BOX("\x6A") /**< Boxdrawing: ┘ */ +#define BOX_DLR BOX("\x77") /**< Boxdrawing: ┘ */ +#define BOX_DL BOX("\x6B") /**< Boxdrawing: ┘ */ + +/* CPP stringification */ +#define XSTR(x) STR(x) +#define STR(x) #x + +#define CONCAT_DETAIL(x, y) x##y +#define CONCAT(x, y) CONCAT_DETAIL(x, y) +#define UNIQUE(x) CONCAT(x, __COUNTER__) + +#define ALIGN(x, a) ALIGN_MASK(x, (uintptr_t) (a) - 1) +#define ALIGN_MASK(x, m) (((uintptr_t) (x) + (m)) & ~(m)) +#define IS_ALIGNED(x, a) (ALIGN(x, a) == (uintptr_t) x) + +#define SWAP(x,y) do { \ + __auto_type _x = x; \ + __auto_type _y = y; \ + x = _y; \ + y = _x; \ +} while(0) + +/** Round-up integer division */ +#define CEIL(x, y) (((x) + (y) - 1) / (y)) + +/** Get nearest up-rounded power of 2 */ +#define LOG2_CEIL(x) (1 << (log2i((x) - 1) + 1)) + +/** Check if the number is a power of 2 */ +#define IS_POW2(x) (((x) != 0) && !((x) & ((x) - 1))) + +/** Calculate the number of elements in an array. */ +#define ARRAY_LEN(a) ( sizeof (a) / sizeof (a)[0] ) + +/* Return the bigger value */ +#define MAX(a, b) ({ __typeof__ (a) _a = (a); \ + __typeof__ (b) _b = (b); \ + _a > _b ? _a : _b; }) + +/* Return the smaller value */ +#define MIN(a, b) ({ __typeof__ (a) _a = (a); \ + __typeof__ (b) _b = (b); \ + _a < _b ? _a : _b; }) + +#ifndef offsetof + #define offsetof(type, member) __builtin_offsetof(type, member) +#endif + +#ifndef container_of + #define container_of(ptr, type, member) ({ const typeof( ((type *) 0)->member ) *__mptr = (ptr); \ + (type *) ( (char *) __mptr - offsetof(type, member) ); \ + }) +#endif + +#define BITS_PER_LONGLONG (sizeof(long long) * 8) + +/* Some helper macros */ +#define BITMASK(h, l) (((~0ULL) << (l)) & (~0ULL >> (BITS_PER_LONGLONG - 1 - (h)))) +#define BIT(nr) (1UL << (nr)) + +/* Forward declarations */ +struct timespec; + +/** Print copyright message to stdout. */ +void print_copyright(); + +/** Print version to stdout. */ +void print_version(); + +/** Normal random variate generator using the Box-Muller method + * + * @param m Mean + * @param s Standard deviation + * @return Normal variate random variable (Gaussian) + */ +double box_muller(float m, float s); + +/** Double precission uniform random variable */ +double randf(); + +/** Concat formatted string to an existing string. + * + * This function uses realloc() to resize the destination. + * Please make sure to only on dynamic allocated destionations!!! + * + * @param dest A pointer to a malloc() allocated memory region + * @param fmt A format string like for printf() + * @param ... Optional parameters like for printf() + * @retval The the new value of the dest buffer. + */ +char * strcatf(char **dest, const char *fmt, ...) + __attribute__ ((format(printf, 2, 3))); + +/** Variadic version of strcatf() */ +char * vstrcatf(char **dest, const char *fmt, va_list va) + __attribute__ ((format(printf, 2, 0))); + +/** Format string like strcatf() just starting with empty string */ +#define strf(fmt, ...) strcatf(&(char *) { NULL }, fmt, ##__VA_ARGS__) +#define vstrf(fmt, va) vstrcatf(&(char *) { NULL }, fmt, va) + +#ifdef __linux__ +/** Convert integer to cpu_set_t. + * + * @param set An integer number which is used as the mask + * @param cset A pointer to the cpu_set_t datastructure + */ +void cpuset_from_integer(uintmax_t set, cpu_set_t *cset); + +/** Convert cpu_set_t to an integer. */ +void cpuset_to_integer(cpu_set_t *cset, uintmax_t *set); + +/** Parses string with list of CPU ranges. + * + * From: https://github.com/mmalecki/util-linux/blob/master/lib/cpuset.c + * + * @retval 0 On success. + * @retval 1 On error. + * @retval 2 If fail is set and a cpu number passed in the list doesn't fit + * into the cpu_set. If fail is not set cpu numbers that do not fit are + * ignored and 0 is returned instead. + */ +int cpulist_parse(const char *str, cpu_set_t *set, int fail); + +/** Returns human readable representation of the cpuset. + * + * From: https://github.com/mmalecki/util-linux/blob/master/lib/cpuset.c + * + * The output format is a list of CPUs with ranges (for example, "0,1,3-9"). + */ +char * cpulist_create(char *str, size_t len, cpu_set_t *set); +#endif + +/** Allocate and initialize memory. */ +void * alloc(size_t bytes); + +/** Allocate and copy memory. */ +void * memdup(const void *src, size_t bytes); + +/** Call quit() in the main thread. */ +void die(); + +/** Used by version_parse(), version_compare() */ +struct version { + int major; + int minor; +}; + +/** Compare two versions. */ +int version_cmp(struct version *a, struct version *b); + +/** Parse a dotted version string. */ +int version_parse(const char *s, struct version *v); + +/** Check assertion and exit if failed. */ +#ifndef assert + #define assert(exp) do { \ + if (!EXPECT(exp, 0)) \ + error("Assertion failed: '%s' in %s(), %s:%d", \ + XSTR(exp), __FUNCTION__, __BASE_FILE__, __LINE__); \ + } while (0) +#endif + +/** Fill buffer with random data */ +size_t read_random(char *buf, size_t len); + +/** Get CPU timestep counter */ +__attribute__((always_inline)) static inline uint64_t rdtsc() +{ + uint64_t tsc; + + __asm__ ("rdtsc;" + "shl $32, %%rdx;" + "or %%rdx,%%rax" + : "=a" (tsc) + : + : "%rcx", "%rdx", "memory"); + + return tsc; +} + +/** Get log2 of long long integers */ +static inline int log2i(long long x) { + if (x == 0) + return 1; + + return sizeof(x) * 8 - __builtin_clzll(x) - 1; +} + +/** Sleep with rdtsc */ +void rdtsc_sleep(uint64_t nanosecs, uint64_t start); + +/** Register a exit callback for program termination: SIGINT, SIGKILL & SIGALRM. */ +int signals_init(void (*cb)(int signal, siginfo_t *sinfo, void *ctx)); + +/** Send signal \p sig to main thread. */ +void killme(int sig); + +pid_t spawn(const char *name, char *const argv[]); + +/** Determines the string length as printed on the screen (ignores escable sequences). */ +size_t strlenp(const char *str); + +#ifdef __cplusplus +} +#endif diff --git a/common/lib/CMakeLists.txt b/common/lib/CMakeLists.txt index 14448b356..3fffb6281 100644 --- a/common/lib/CMakeLists.txt +++ b/common/lib/CMakeLists.txt @@ -25,6 +25,16 @@ add_library(villas-common SHARED utils.cpp memory.cpp memory_manager.cpp + + kernel/kernel.c + kernel/pci.c + kernel/vfio.cpp + + utils.c + list.c + log.c + log_config.c + log_helper.c ) target_include_directories(villas-common PUBLIC @@ -37,3 +47,8 @@ target_link_libraries(villas-common PUBLIC ${JANSSON_LIBRARIES} ${CMAKE_DL_LIBS} ) + +target_compile_definitions(villas-common PRIVATE + BUILDID=\"abc\" + _GNU_SOURCE +) diff --git a/common/lib/kernel/kernel.c b/common/lib/kernel/kernel.c new file mode 100644 index 000000000..ed69b1225 --- /dev/null +++ b/common/lib/kernel/kernel.c @@ -0,0 +1,283 @@ +/** Linux kernel related functions. + * + * @author Steffen Vogel + * @copyright 2017-2018, Institute for Automation of Complex Power Systems, EONERC + * @license GNU General Public License (version 3) + * + * VILLASfpga + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + *********************************************************************************/ + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + +int kernel_get_cacheline_size() +{ +#ifdef __linux__ + return sysconf(_SC_LEVEL1_ICACHE_LINESIZE); +#else + return 64; /** @todo fixme */ +#endif +} + +#ifdef __linux__ + +int kernel_module_set_param(const char *module, const char *param, const char *value) +{ + FILE *f; + char fn[256]; + + snprintf(fn, sizeof(fn), "%s/module/%s/parameters/%s", SYSFS_PATH, module, param); + f = fopen(fn, "w"); + if (!f) + serror("Failed set parameter %s for kernel module %s to %s", module, param, value); + + debug(LOG_KERNEL | 5, "Set parameter %s of kernel module %s to %s", module, param, value); + fprintf(f, "%s", value); + fclose(f); + + return 0; +} + +int kernel_module_load(const char *module) +{ + int ret; + + ret = kernel_module_loaded(module); + if (!ret) { + debug(LOG_KERNEL | 5, "Kernel module %s already loaded...", module); + return 0; + } + + pid_t pid = fork(); + switch (pid) { + case -1: // error + return -1; + + case 0: // child + execlp("modprobe", "modprobe", module, (char *) 0); + exit(EXIT_FAILURE); // exec never returns + + default: + wait(&ret); + + return kernel_module_loaded(module); + } +} + +int kernel_module_loaded(const char *module) +{ + FILE *f; + int ret = -1; + char *line = NULL; + size_t len = 0; + + f = fopen(PROCFS_PATH "/modules", "r"); + if (!f) + return -1; + + while (getline(&line, &len, f) >= 0) { + if (strstr(line, module) == line) { + ret = 0; + break; + } + } + + free(line); + fclose(f); + + return ret; +} + +int kernel_get_version(struct version *v) +{ + struct utsname uts; + + if (uname(&uts) < 0) + return -1; + + if (version_parse(uts.release, v)) + return -1; + + return 0; +} + +int kernel_get_cmdline_param(const char *param, char *buf, size_t len) +{ + int ret; + char cmdline[512]; + + FILE *f = fopen(PROCFS_PATH "/cmdline", "r"); + if (!f) + return -1; + + if (!fgets(cmdline, sizeof(cmdline), f)) + goto out; + + char *tok = strtok(cmdline, " \t"); + do { + char key[128], value[128]; + + ret = sscanf(tok, "%127[^=]=%127s", key, value); + if (ret >= 1) { + if (ret >= 2) + debug(30, "Found kernel param: %s=%s", key, value); + else + debug(30, "Found kernel param: %s", key); + + if (strcmp(param, key) == 0) { + if (ret >= 2 && buf) + strncpy(buf, value, len); + + return 0; /* found */ + } + } + } while((tok = strtok(NULL, " \t"))); + +out: + fclose(f); + + return -1; /* not found or error */ +} + +int kernel_get_page_size() +{ + return sysconf(_SC_PAGESIZE); +} + +/* There is no sysconf interface to get the hugepage size */ +int kernel_get_hugepage_size() +{ + char *key, *value, *unit, *line = NULL; + int sz = -1; + size_t len = 0; + FILE *f; + + f = fopen(PROCFS_PATH "/meminfo", "r"); + if (!f) + return -1; + + while (getline(&line, &len, f) != -1) { + key = strtok(line, ": "); + value = strtok(NULL, " "); + unit = strtok(NULL, "\n"); + + if (!strcmp(key, "Hugepagesize") && !strcmp(unit, "kB")) { + sz = strtoul(value, NULL, 10) * 1024; + break; + } + } + + free(line); + fclose(f); + + return sz; +} + +int kernel_get_nr_hugepages() +{ + FILE *f; + int nr, ret; + + f = fopen(PROCFS_PATH "/sys/vm/nr_hugepages", "r"); + if (!f) + serror("Failed to open %s", PROCFS_PATH "/sys/vm/nr_hugepages"); + + ret = fscanf(f, "%d", &nr); + if (ret != 1) + nr = -1; + + fclose(f); + + return nr; +} + +int kernel_set_nr_hugepages(int nr) +{ + FILE *f; + + f = fopen(PROCFS_PATH "/sys/vm/nr_hugepages", "w"); + if (!f) + serror("Failed to open %s", PROCFS_PATH "/sys/vm/nr_hugepages"); + + fprintf(f, "%d\n", nr); + fclose(f); + + return 0; +} + +#if 0 +int kernel_has_cap(cap_value_t cap) +{ + int ret; + + cap_t caps; + cap_flag_value_t value; + + caps = cap_get_proc(); + if (caps == NULL) + return -1; + + ret = cap_get_proc(caps); + if (ret == -1) + return -1; + + ret = cap_get_flag(caps, cap, CAP_EFFECTIVE, &value); + if (ret == -1) + return -1; + + ret = cap_free(caps); + if (ret) + return -1; + + return value == CAP_SET ? 0 : -1; +} +#endif + +int kernel_irq_setaffinity(unsigned irq, uintmax_t affinity, uintmax_t *old) +{ + char fn[64]; + FILE *f; + int ret = 0; + + snprintf(fn, sizeof(fn), "/proc/irq/%u/smp_affinity", irq); + + f = fopen(fn, "w+"); + if (!f) + return errno; + + if (old) + ret = fscanf(f, "%jx", old); + + fprintf(f, "%jx", affinity); + fclose(f); + + return ret; +} + +#endif /* __linux__ */ diff --git a/common/lib/kernel/pci.c b/common/lib/kernel/pci.c new file mode 100644 index 000000000..2a7dcdf45 --- /dev/null +++ b/common/lib/kernel/pci.c @@ -0,0 +1,388 @@ +/** Linux PCI helpers + * + * @author Steffen Vogel + * @copyright 2017-2018, Steffen Vogel + * @license GNU General Public License (version 3) + * + * VILLASfpga + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + *********************************************************************************/ + +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +int pci_init(struct pci *p) +{ + struct dirent *e; + DIR *dp; + FILE *f; + char path[PATH_MAX]; + int ret; + + list_init(&p->devices); + + snprintf(path, sizeof(path), "%s/bus/pci/devices", SYSFS_PATH); + + dp = opendir(path); + if (dp == NULL) { + serror("Failed to detect PCI devices"); + return -1; + } + + while ((e = readdir(dp))) { + + // ignore special entries + if ((strcmp(e->d_name, ".") == 0) || + (strcmp(e->d_name, "..") == 0) ) + continue; + + struct pci_device *d = (struct pci_device *) alloc(sizeof(struct pci_device)); + + struct { const char *s; int *p; } map[] = { + { "vendor", &d->id.vendor }, + { "device", &d->id.device } + }; + + /* Read vendor & device id */ + for (int i = 0; i < 2; i++) { + snprintf(path, sizeof(path), "%s/bus/pci/devices/%s/%s", SYSFS_PATH, e->d_name, map[i].s); + + f = fopen(path, "r"); + if (!f) + serror("Failed to open '%s'", path); + + ret = fscanf(f, "%x", map[i].p); + if (ret != 1) + error("Failed to parse %s ID from: %s", map[i].s, path); + + fclose(f); + } + + /* Get slot id */ + ret = sscanf(e->d_name, "%4x:%2x:%2x.%u", &d->slot.domain, &d->slot.bus, &d->slot.device, &d->slot.function); + if (ret != 4) + error("Failed to parse PCI slot number: %s", e->d_name); + + list_push(&p->devices, d); + } + + closedir(dp); + + return 0; +} + +int pci_destroy(struct pci *p) +{ + list_destroy(&p->devices, NULL, true); + + return 0; +} + +int pci_device_parse_slot(struct pci_device *f, const char *s, const char **error) +{ + char *str = strdup(s); + char *colon = strrchr(str, ':'); + char *dot = strchr((colon ? colon + 1 : str), '.'); + char *mid = str; + char *e, *bus, *colon2; + + if (colon) { + *colon++ = 0; + mid = colon; + + colon2 = strchr(str, ':'); + if (colon2) { + *colon2++ = 0; + bus = colon2; + + if (str[0] && strcmp(str, "*")) { + long int x = strtol(str, &e, 16); + if ((e && *e) || (x < 0 || x > 0x7fffffff)) { + *error = "Invalid domain number"; + goto fail; + } + + f->slot.domain = x; + } + } + else + bus = str; + + if (bus[0] && strcmp(bus, "*")) { + long int x = strtol(bus, &e, 16); + if ((e && *e) || (x < 0 || x > 0xff)) { + *error = "Invalid bus number"; + goto fail; + } + + f->slot.bus = x; + } + } + + if (dot) + *dot++ = 0; + + if (mid[0] && strcmp(mid, "*")) { + long int x = strtol(mid, &e, 16); + + if ((e && *e) || (x < 0 || x > 0x1f)) { + *error = "Invalid slot number"; + goto fail; + } + + f->slot.device = x; + } + + if (dot && dot[0] && strcmp(dot, "*")) { + long int x = strtol(dot, &e, 16); + + if ((e && *e) || (x < 0 || x > 7)) { + *error = "Invalid function number"; + goto fail; + } + + f->slot.function = x; + } + + free(str); + return 0; + +fail: + free(str); + return -1; +} + +/* ID filter syntax: [vendor]:[device][:class] */ +int pci_device_parse_id(struct pci_device *f, const char *str, const char **error) +{ + char *s, *c, *e; + + if (!*str) + return 0; + + s = strchr(str, ':'); + if (!s) { + *error = "':' expected"; + goto fail; + } + + *s++ = 0; + if (str[0] && strcmp(str, "*")) { + long int x = strtol(str, &e, 16); + + if ((e && *e) || (x < 0 || x > 0xffff)) { + *error = "Invalid vendor ID"; + goto fail; + } + + f->id.vendor = x; + } + + c = strchr(s, ':'); + if (c) + *c++ = 0; + + if (s[0] && strcmp(s, "*")) { + long int x = strtol(s, &e, 16); + if ((e && *e) || (x < 0 || x > 0xffff)) { + *error = "Invalid device ID"; + goto fail; + } + + f->id.device = x; + } + + if (c && c[0] && strcmp(s, "*")) { + long int x = strtol(c, &e, 16); + + if ((e && *e) || (x < 0 || x > 0xffff)) { + *error = "Invalid class code"; + goto fail; + } + + f->id.class_code = x; + } + + return 0; + +fail: + return -1; +} + +int pci_device_compare(const struct pci_device *d, const struct pci_device *f) +{ + if ((f->slot.domain != 0 && f->slot.domain != d->slot.domain) || + (f->slot.bus != 0 && f->slot.bus != d->slot.bus) || + (f->slot.device != 0 && f->slot.device != d->slot.device) || + (f->slot.function != 0 && f->slot.function != d->slot.function)) + return 1; + + if ((f->id.device != 0 && f->id.device != d->id.device) || + (f->id.vendor != 0 && f->id.vendor != d->id.vendor)) + return 1; + + if ((f->id.class_code != 0) || (f->id.class_code != d->id.class_code)) + return 1; + + // found + return 0; +} + +struct pci_device * pci_lookup_device(struct pci *p, struct pci_device *f) +{ + return list_search(&p->devices, (cmp_cb_t) pci_device_compare, (void *) f); +} + +size_t pci_get_regions(const struct pci_device *d, struct pci_region** regions) +{ + FILE* f; + char sysfs[1024]; + + assert(regions != NULL); + + snprintf(sysfs, sizeof(sysfs), "%s/bus/pci/devices/%04x:%02x:%02x.%x/resource", + SYSFS_PATH, d->slot.domain, d->slot.bus, d->slot.device, d->slot.function); + + f = fopen(sysfs, "r"); + if (!f) + serror("Failed to open resource mapping %s", sysfs); + + struct pci_region _regions[8]; + struct pci_region* cur_region = _regions; + size_t valid_regions = 0; + + ssize_t bytesRead; + char* line = NULL; + size_t len = 0; + + int region = 0; + // cap to 8 regions, just because we don't know how many may exist + while(region < 8 && (bytesRead = getline(&line, &len, f)) != -1) { + unsigned long long tokens[3]; + char* s = line; + for(int i = 0; i < 3; i++) { + char* end; + tokens[i] = strtoull(s, &end, 16); + if(s == end) { + printf("Error parsing line %d of %s\n", region + 1, sysfs); + tokens[0] = tokens[1] = 0; // mark invalid + break; + } + s = end; + } + + free(line); + + // required for getline() to allocate a new buffer on the next iteration + line = NULL; + len = 0; + + if(tokens[0] != tokens[1]) { + // this is a valid region + cur_region->num = region; + cur_region->start = tokens[0]; + cur_region->end = tokens[1]; + cur_region->flags = tokens[2]; + cur_region++; + valid_regions++; + } + + region++; + } + + if(valid_regions > 0) { + const size_t len = valid_regions * sizeof (struct pci_region); + *regions = malloc(len); + memcpy(*regions, _regions, len); + } + + return valid_regions; +} + + +int pci_get_driver(const struct pci_device *d, char *buf, size_t buflen) +{ + int ret; + char sysfs[1024], syml[1024]; + memset(syml, 0, sizeof(syml)); + + snprintf(sysfs, sizeof(sysfs), "%s/bus/pci/devices/%04x:%02x:%02x.%x/driver", SYSFS_PATH, + d->slot.domain, d->slot.bus, d->slot.device, d->slot.function); + + ret = readlink(sysfs, syml, sizeof(syml)); + if (ret < 0) + return ret; + + char *driver = basename(syml); + + strncpy(buf, driver, buflen); + + return 0; +} + +int pci_attach_driver(const struct pci_device *d, const char *driver) +{ + FILE *f; + char fn[1024]; + + /* Add new ID to driver */ + snprintf(fn, sizeof(fn), "%s/bus/pci/drivers/%s/new_id", SYSFS_PATH, driver); + f = fopen(fn, "w"); + if (!f) + serror("Failed to add PCI id to %s driver (%s)", driver, fn); + + info("Adding ID to %s module: %04x %04x", driver, d->id.vendor, d->id.device); + fprintf(f, "%04x %04x", d->id.vendor, d->id.device); + fclose(f); + + /* Bind to driver */ + snprintf(fn, sizeof(fn), "%s/bus/pci/drivers/%s/bind", SYSFS_PATH, driver); + f = fopen(fn, "w"); + if (!f) + serror("Failed to bind PCI device to %s driver (%s)", driver, fn); + + info("Bind device to %s driver", driver); + fprintf(f, "%04x:%02x:%02x.%x\n", d->slot.domain, d->slot.bus, d->slot.device, d->slot.function); + fclose(f); + + return 0; +} + +int pci_get_iommu_group(const struct pci_device *d) +{ + int ret; + char *group, link[1024], sysfs[1024]; + memset(link, 0, sizeof(link)); + + snprintf(sysfs, sizeof(sysfs), "%s/bus/pci/devices/%04x:%02x:%02x.%x/iommu_group", SYSFS_PATH, + d->slot.domain, d->slot.bus, d->slot.device, d->slot.function); + + ret = readlink(sysfs, link, sizeof(link)); + if (ret < 0) + return -1; + + group = basename(link); + + return atoi(group); +} diff --git a/common/lib/kernel/vfio.cpp b/common/lib/kernel/vfio.cpp new file mode 100644 index 000000000..822cf4b6c --- /dev/null +++ b/common/lib/kernel/vfio.cpp @@ -0,0 +1,803 @@ +/** Virtual Function IO wrapper around kernel API + * + * @author Steffen Vogel + * @author Daniel Krebs + * @copyright 2017-2018, Steffen Vogel + * @copyright 2018, Daniel Krebs + * @license GNU General Public License (version 3) + * + * VILLASfpga + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + *********************************************************************************/ + +#define _DEFAULT_SOURCE + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +static auto logger = loggerGetOrCreate("Vfio"); + +static const char *vfio_pci_region_names[] = { + "PCI_BAR0", // VFIO_PCI_BAR0_REGION_INDEX, + "PCI_BAR1", // VFIO_PCI_BAR1_REGION_INDEX, + "PCI_BAR2", // VFIO_PCI_BAR2_REGION_INDEX, + "PCI_BAR3", // VFIO_PCI_BAR3_REGION_INDEX, + "PCI_BAR4", // VFIO_PCI_BAR4_REGION_INDEX, + "PCI_BAR5", // VFIO_PCI_BAR5_REGION_INDEX, + "PCI_ROM", // VFIO_PCI_ROM_REGION_INDEX, + "PCI_CONFIG", // VFIO_PCI_CONFIG_REGION_INDEX, + "PCI_VGA" // VFIO_PCI_INTX_IRQ_INDEX, +}; + +static const char *vfio_pci_irq_names[] = { + "PCI_INTX", // VFIO_PCI_INTX_IRQ_INDEX, + "PCI_MSI", // VFIO_PCI_MSI_IRQ_INDEX, + "PCI_MSIX", // VFIO_PCI_MSIX_IRQ_INDEX, + "PCI_ERR", // VFIO_PCI_ERR_IRQ_INDEX, + "PCI_REQ" // VFIO_PCI_REQ_IRQ_INDEX, +}; + +namespace villas { + + +VfioContainer::VfioContainer() + : iova_next(0) +{ + + static constexpr const char* requiredKernelModules[] = { + "vfio", "vfio_pci", "vfio_iommu_type1" + }; + + for(const char* module : requiredKernelModules) { + if(kernel_module_loaded(module) != 0) { + logger->error("Kernel module '{}' required but not loaded. " + "Please load manually!", module); + throw std::exception(); + } + } + + /* Open VFIO API */ + fd = open(VFIO_DEV, O_RDWR); + if (fd < 0) { + logger->error("Failed to open VFIO container"); + throw std::exception(); + } + + /* Check VFIO API version */ + version = ioctl(fd, VFIO_GET_API_VERSION); + if (version < 0 || version != VFIO_API_VERSION) { + logger->error("Failed to get VFIO version"); + throw std::exception(); + } + + /* Check available VFIO extensions (IOMMU types) */ + extensions = 0; + for (unsigned int i = VFIO_TYPE1_IOMMU; i <= VFIO_NOIOMMU_IOMMU; i++) { + int ret = ioctl(fd, VFIO_CHECK_EXTENSION, i); + if (ret < 0) { + logger->error("Failed to get VFIO extensions"); + throw std::exception(); + } + else if (ret > 0) { + extensions |= (1 << i); + } + } + + hasIommu = false; + + if(not (extensions & (1 << VFIO_NOIOMMU_IOMMU))) { + if(not (extensions & (1 << VFIO_TYPE1_IOMMU))) { + logger->error("No supported IOMMU extension found"); + throw std::exception(); + } else { + hasIommu = true; + } + } + + logger->debug("Version: {:#x}", version); + logger->debug("Extensions: {:#x}", extensions); + logger->debug("IOMMU: {}", hasIommu ? "yes" : "no"); +} + + +VfioContainer::~VfioContainer() +{ + logger->debug("Clean up container with fd {}", fd); + + /* Release memory and close fds */ + groups.clear(); + + /* Close container */ + int ret = close(fd); + if (ret < 0) { + logger->error("Cannot close vfio container fd {}", fd); + } +} + + +std::shared_ptr +VfioContainer::create() +{ + std::shared_ptr container { new VfioContainer }; + return container; +} + + +void +VfioContainer::dump() +{ + logger->info("File descriptor: {}", fd); + logger->info("Version: {}", version); + logger->info("Extensions: 0x{:x}", extensions); + + for(auto& group : groups) { + logger->info("VFIO Group {}, viable={}, container={}", + group->index, + (group->status.flags & VFIO_GROUP_FLAGS_VIABLE) > 0, + (group->status.flags & VFIO_GROUP_FLAGS_CONTAINER_SET) > 0 + ); + + for(auto& device : group->devices) { + logger->info("Device {}: regions={}, irqs={}, flags={}", + device->name, + device->info.num_regions, + device->info.num_irqs, + device->info.flags + ); + + for (size_t i = 0; i < device->info.num_regions && i < 8; i++) { + struct vfio_region_info *region = &device->regions[i]; + + if (region->size > 0) + logger->info("Region {} {}: size={}, offset={}, flags={}", + region->index, + (device->info.flags & VFIO_DEVICE_FLAGS_PCI) ? + vfio_pci_region_names[i] : "", + region->size, + region->offset, + region->flags + ); + } + + for (size_t i = 0; i < device->info.num_irqs; i++) { + struct vfio_irq_info *irq = &device->irqs[i]; + + if (irq->count > 0) + logger->info("IRQ {} {}: count={}, flags={}", + irq->index, + (device->info.flags & VFIO_DEVICE_FLAGS_PCI ) ? + vfio_pci_irq_names[i] : "", + irq->count, + irq->flags + ); + } + } + } +} + + +VfioDevice& +VfioContainer::attachDevice(const char* name, int index) +{ + VfioGroup& group = getOrAttachGroup(index); + auto device = std::make_unique(name, group); + + /* Open device fd */ + device->fd = ioctl(group.fd, VFIO_GROUP_GET_DEVICE_FD, name); + if (device->fd < 0) { + logger->error("Failed to open VFIO device: {}", device->name); + throw std::exception(); + } + + /* Get device info */ + device->info.argsz = sizeof(device->info); + + int ret = ioctl(device->fd, VFIO_DEVICE_GET_INFO, &device->info); + if (ret < 0) { + logger->error("Failed to get VFIO device info for: {}", device->name); + throw std::exception(); + } + + logger->debug("Device has {} regions", device->info.num_regions); + logger->debug("Device has {} IRQs", device->info.num_irqs); + + // reserve slots already so that we can use the []-operator for access + device->irqs.resize(device->info.num_irqs); + device->regions.resize(device->info.num_regions); + device->mappings.resize(device->info.num_regions); + + /* Get device regions */ + for (size_t i = 0; i < device->info.num_regions && i < 8; i++) { + struct vfio_region_info region; + memset(®ion, 0, sizeof (region)); + + region.argsz = sizeof(region); + region.index = i; + + ret = ioctl(device->fd, VFIO_DEVICE_GET_REGION_INFO, ®ion); + if (ret < 0) { + logger->error("Failed to get region of VFIO device: {}", device->name); + throw std::exception(); + } + + device->regions[i] = region; + } + + + /* Get device irqs */ + for (size_t i = 0; i < device->info.num_irqs; i++) { + struct vfio_irq_info irq; + memset(&irq, 0, sizeof (irq)); + + irq.argsz = sizeof(irq); + irq.index = i; + + ret = ioctl(device->fd, VFIO_DEVICE_GET_IRQ_INFO, &irq); + if (ret < 0) { + logger->error("Failed to get IRQs of VFIO device: {}", device->name); + throw std::exception(); + } + + device->irqs[i] = irq; + } + + group.devices.push_back(std::move(device)); + + return *group.devices.back().get(); +} + + +VfioDevice& +VfioContainer::attachDevice(const pci_device* pdev) +{ + int ret; + char name[32]; + static constexpr const char* kernelDriver = "vfio-pci"; + + /* Load PCI bus driver for VFIO */ + if (kernel_module_load("vfio_pci")) { + logger->error("Failed to load kernel driver: vfio_pci"); + throw std::exception(); + } + + /* Bind PCI card to vfio-pci driver if not already bound */ + ret = pci_get_driver(pdev, name, sizeof(name)); + if (ret || strcmp(name, kernelDriver)) { + logger->debug("Bind PCI card to kernel driver '{}'", kernelDriver); + ret = pci_attach_driver(pdev, kernelDriver); + if (ret) { + logger->error("Failed to attach device to driver"); + throw std::exception(); + } + } + + /* Get IOMMU group of device */ + int index = isIommuEnabled() ? pci_get_iommu_group(pdev) : 0; + if (index < 0) { + logger->error("Failed to get IOMMU group of device"); + throw std::exception(); + } + + /* VFIO device name consists of PCI BDF */ + snprintf(name, sizeof(name), "%04x:%02x:%02x.%x", pdev->slot.domain, + pdev->slot.bus, pdev->slot.device, pdev->slot.function); + + logger->info("Attach to device {} with index {}", std::string(name), index); + auto& device = attachDevice(name, index); + + device.pci_device = pdev; + + /* Check if this is really a vfio-pci device */ + if(not device.isVfioPciDevice()) { + logger->error("Device is not a vfio-pci device"); + throw std::exception(); + } + + return device; +} + + +uintptr_t +VfioContainer::memoryMap(uintptr_t virt, uintptr_t phys, size_t length) +{ + int ret; + + if(not hasIommu) { + logger->error("DMA mapping not supported without IOMMU"); + return UINTPTR_MAX; + } + + if (length & 0xFFF) { + length += 0x1000; + length &= ~0xFFF; + } + + /* Super stupid allocator */ + size_t iovaIncrement = 0; + if (phys == UINTPTR_MAX) { + phys = this->iova_next; + iovaIncrement = length; + } + + struct vfio_iommu_type1_dma_map dmaMap; + memset(&dmaMap, 0, sizeof(dmaMap)); + + dmaMap.argsz = sizeof(dmaMap); + dmaMap.vaddr = virt; + dmaMap.iova = phys; + dmaMap.size = length; + dmaMap.flags = VFIO_DMA_MAP_FLAG_READ | VFIO_DMA_MAP_FLAG_WRITE; + + ret = ioctl(this->fd, VFIO_IOMMU_MAP_DMA, &dmaMap); + if (ret) { + logger->error("Failed to create DMA mapping: {}", ret); + return UINTPTR_MAX; + } + + logger->debug("DMA map size={:#x}, iova={:#x}, vaddr={:#x}", + dmaMap.size, dmaMap.iova, dmaMap.vaddr); + + // mapping successful, advance IOVA allocator + this->iova_next += iovaIncrement; + + // we intentionally don't return the actual mapped length, the users are + // only guaranteed to have their demanded memory mapped correctly + return dmaMap.iova; +} + + +bool +VfioContainer::memoryUnmap(uintptr_t phys, size_t length) +{ + int ret; + + if(not hasIommu) { + return true; + } + + struct vfio_iommu_type1_dma_unmap dmaUnmap; + dmaUnmap.argsz = sizeof(struct vfio_iommu_type1_dma_unmap); + dmaUnmap.flags = 0; + dmaUnmap.iova = phys; + dmaUnmap.size = length; + + ret = ioctl(this->fd, VFIO_IOMMU_UNMAP_DMA, &dmaUnmap); + if (ret) { + logger->error("Failed to unmap DMA mapping"); + return false; + } + + return true; +} + + +VfioGroup& +VfioContainer::getOrAttachGroup(int index) +{ + // search if group with index already exists + for(auto& group : groups) { + if(group->index == index) { + return *group; + } + } + + // group not yet part of this container, so acquire ownership + auto group = VfioGroup::attach(*this, index); + if(not group) { + logger->error("Failed to attach to IOMMU group: {}", index); + throw std::exception(); + } else { + logger->debug("Attached new group {} to VFIO container", index); + } + + // push to our list + groups.push_back(std::move(group)); + + return *groups.back(); +} + + +VfioDevice::~VfioDevice() +{ + logger->debug("Clean up device {} with fd {}", this->name, this->fd); + + for(auto& region : regions) { + regionUnmap(region.index); + } + + int ret = close(fd); + if (ret != 0) { + logger->error("Closing device fd {} failed", fd); + } +} + + +bool +VfioDevice::reset() +{ + if (this->info.flags & VFIO_DEVICE_FLAGS_RESET) + return ioctl(this->fd, VFIO_DEVICE_RESET) == 0; + else + return false; /* not supported by this device */ +} + + +void* +VfioDevice::regionMap(size_t index) +{ + struct vfio_region_info *r = ®ions[index]; + + if (!(r->flags & VFIO_REGION_INFO_FLAG_MMAP)) + return MAP_FAILED; + + mappings[index] = mmap(nullptr, r->size, + PROT_READ | PROT_WRITE, + MAP_SHARED | MAP_32BIT, + fd, r->offset); + + return mappings[index]; +} + + +bool +VfioDevice::regionUnmap(size_t index) +{ + int ret; + struct vfio_region_info *r = ®ions[index]; + + if (!mappings[index]) + return false; /* was not mapped */ + + logger->debug("Unmap region {} from device {}", index, name); + + ret = munmap(mappings[index], r->size); + if (ret) + return false; + + mappings[index] = nullptr; + + return true; +} + + +size_t +VfioDevice::regionGetSize(size_t index) +{ + if(index >= regions.size()) { + logger->error("Index out of range: {} >= {}", index, regions.size()); + throw std::out_of_range("Index out of range"); + } + + return regions[index].size; +} + + +bool +VfioDevice::pciEnable() +{ + int ret; + uint32_t reg; + const off_t offset = PCI_COMMAND + + (static_cast(VFIO_PCI_CONFIG_REGION_INDEX) << 40); + + /* Check if this is really a vfio-pci device */ + if (!(this->info.flags & VFIO_DEVICE_FLAGS_PCI)) + return false; + + ret = pread(this->fd, ®, sizeof(reg), offset); + if (ret != sizeof(reg)) + return false; + + /* Enable memory access and PCI bus mastering which is required for DMA */ + reg |= PCI_COMMAND_MEMORY | PCI_COMMAND_MASTER; + + ret = pwrite(this->fd, ®, sizeof(reg), offset); + if (ret != sizeof(reg)) + return false; + + return true; +} + + +bool +VfioDevice::pciHotReset() +{ + /* Check if this is really a vfio-pci device */ + if (not isVfioPciDevice()) + return false; + + const size_t reset_infolen = sizeof(struct vfio_pci_hot_reset_info) + + sizeof(struct vfio_pci_dependent_device) * 64; + + auto reset_info = reinterpret_cast + (calloc(1, reset_infolen)); + + reset_info->argsz = reset_infolen; + + + if (ioctl(this->fd, VFIO_DEVICE_GET_PCI_HOT_RESET_INFO, reset_info) != 0) { + free(reset_info); + return false; + } + + logger->debug("Dependent devices for hot-reset:"); + for (size_t i = 0; i < reset_info->count; i++) { + struct vfio_pci_dependent_device *dd = &reset_info->devices[i]; + logger->debug(" {:04x}:{:02x}:{:02x}.{:01x}: iommu_group={}", + dd->segment, dd->bus, + PCI_SLOT(dd->devfn), PCI_FUNC(dd->devfn), dd->group_id); + + if (static_cast(dd->group_id) != this->group.index) { + free(reset_info); + return false; + } + } + + free(reset_info); + + const size_t resetlen = sizeof(struct vfio_pci_hot_reset) + + sizeof(int32_t) * 1; + auto reset = reinterpret_cast + (calloc(1, resetlen)); + + reset->argsz = resetlen; + reset->count = 1; + reset->group_fds[0] = this->group.fd; + + int ret = ioctl(this->fd, VFIO_DEVICE_PCI_HOT_RESET, reset); + const bool success = (ret == 0); + + free(reset); + + if(not success and not group.container->isIommuEnabled()) { + logger->info("PCI hot reset failed, but this is expected without IOMMU"); + return true; + } + + return success; +} + + +int +VfioDevice::pciMsiInit(int efds[]) +{ + /* Check if this is really a vfio-pci device */ + if(not isVfioPciDevice()) + return -1; + + const size_t irqCount = irqs[VFIO_PCI_MSI_IRQ_INDEX].count; + const size_t irqSetSize = sizeof(struct vfio_irq_set) + + sizeof(int) * irqCount; + + auto irqSet = reinterpret_cast(calloc(1, irqSetSize)); + if(irqSet == nullptr) + return -1; + + irqSet->argsz = irqSetSize; + irqSet->flags = VFIO_IRQ_SET_DATA_EVENTFD | VFIO_IRQ_SET_ACTION_TRIGGER; + irqSet->index = VFIO_PCI_MSI_IRQ_INDEX; + irqSet->start = 0; + irqSet->count = irqCount; + + /* Now set the new eventfds */ + for (size_t i = 0; i < irqCount; i++) { + efds[i] = eventfd(0, 0); + if (efds[i] < 0) { + free(irqSet); + return -1; + } + } + + memcpy(irqSet->data, efds, sizeof(int) * irqCount); + + if(ioctl(fd, VFIO_DEVICE_SET_IRQS, irqSet) != 0) { + free(irqSet); + return -1; + } + + free(irqSet); + + return irqCount; +} + + +int +VfioDevice::pciMsiDeinit(int efds[]) +{ + /* Check if this is really a vfio-pci device */ + if(not isVfioPciDevice()) + return -1; + + const size_t irqCount = irqs[VFIO_PCI_MSI_IRQ_INDEX].count; + const size_t irqSetSize = sizeof(struct vfio_irq_set) + + sizeof(int) * irqCount; + + auto irqSet = reinterpret_cast(calloc(1, irqSetSize)); + if(irqSet == nullptr) + return -1; + + irqSet->argsz = irqSetSize; + irqSet->flags = VFIO_IRQ_SET_DATA_EVENTFD | VFIO_IRQ_SET_ACTION_TRIGGER; + irqSet->index = VFIO_PCI_MSI_IRQ_INDEX; + irqSet->count = irqCount; + irqSet->start = 0; + + for (size_t i = 0; i < irqCount; i++) { + close(efds[i]); + efds[i] = -1; + } + + memcpy(irqSet->data, efds, sizeof(int) * irqCount); + + if (ioctl(fd, VFIO_DEVICE_SET_IRQS, irqSet) != 0) { + free(irqSet); + return -1; + } + + free(irqSet); + + return irqCount; +} + + +bool +VfioDevice::pciMsiFind(int nos[]) +{ + int ret, idx, irq; + char *end, *col, *last, line[1024], name[13]; + FILE *f; + + f = fopen("/proc/interrupts", "r"); + if (!f) + return false; + + for (int i = 0; i < 32; i++) + nos[i] = -1; + + /* For each line in /proc/interrupts */ + while (fgets(line, sizeof(line), f)) { + col = strtok(line, " "); + + /* IRQ number is in first column */ + irq = strtol(col, &end, 10); + if (col == end) + continue; + + /* Find last column of line */ + do { + last = col; + } while ((col = strtok(nullptr, " "))); + + + ret = sscanf(last, "vfio-msi[%u](%12[0-9:])", &idx, name); + if (ret == 2) { + if (strstr(this->name.c_str(), name) == this->name.c_str()) + nos[idx] = irq; + } + } + + fclose(f); + + return true; +} + + +bool +VfioDevice::isVfioPciDevice() const +{ + return info.flags & VFIO_DEVICE_FLAGS_PCI; +} + + +VfioGroup::~VfioGroup() +{ + logger->debug("Clean up group {} with fd {}", this->index, this->fd); + + /* Release memory and close fds */ + devices.clear(); + + if(fd < 0) { + logger->debug("Destructing group that has not been attached"); + } else { + int ret = ioctl(fd, VFIO_GROUP_UNSET_CONTAINER); + if (ret != 0) { + logger->error("Cannot unset container for group fd {}", fd); + } + + ret = close(fd); + if (ret != 0) { + logger->error("Cannot close group fd {}", fd); + } + } +} + + +std::unique_ptr +VfioGroup::attach(VfioContainer& container, int groupIndex) +{ + std::unique_ptr group { new VfioGroup(groupIndex) }; + + group->container = &container; + + /* Open group fd */ + std::stringstream groupPath; + groupPath << VFIO_PATH + << (container.isIommuEnabled() ? "" : "noiommu-") + << groupIndex; + + group->fd = open(groupPath.str().c_str(), O_RDWR); + if (group->fd < 0) { + logger->error("Failed to open VFIO group {}", group->index); + return nullptr; + } + + logger->debug("VFIO group {} (fd {}) has path {}", + groupIndex, group->fd, groupPath.str()); + + /* Claim group ownership */ + int ret = ioctl(group->fd, VFIO_GROUP_SET_CONTAINER, &container.getFd()); + if (ret < 0) { + logger->error("Failed to attach VFIO group {} to container fd {} (error {})", + group->index, container.getFd(), ret); + return nullptr; + } + + /* Set IOMMU type */ + int iommu_type = container.isIommuEnabled() ? + VFIO_TYPE1_IOMMU : + VFIO_NOIOMMU_IOMMU; + + ret = ioctl(container.getFd(), VFIO_SET_IOMMU, iommu_type); + if (ret < 0) { + logger->error("Failed to set IOMMU type of container: {}", ret); + return nullptr; + } + + /* Check group viability and features */ + group->status.argsz = sizeof(group->status); + + ret = ioctl(group->fd, VFIO_GROUP_GET_STATUS, &group->status); + if (ret < 0) { + logger->error("Failed to get VFIO group status"); + return nullptr; + } + + if (!(group->status.flags & VFIO_GROUP_FLAGS_VIABLE)) { + logger->error("VFIO group is not available: bind all devices to the VFIO driver!"); + return nullptr; + } + + return group; +} + +} // namespace villas + diff --git a/common/lib/list.c b/common/lib/list.c new file mode 100644 index 000000000..5a21a5df3 --- /dev/null +++ b/common/lib/list.c @@ -0,0 +1,203 @@ +/** A generic linked list + * + * Linked lists a used for several data structures in the code. + * + * @author Steffen Vogel + * @copyright 2017-2018, Institute for Automation of Complex Power Systems, EONERC + * @license GNU General Public License (version 3) + * + * VILLASfpga + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + *********************************************************************************/ + +#include +#include + +#include +#include + +/* Compare functions */ +static int cmp_lookup(const void *a, const void *b) { + const struct { + char *name; + } *obj = a; + + return strcmp(obj->name, b); +} + +static int cmp_contains(const void *a, const void *b) { + return a == b ? 0 : 1; +} + +static int cmp_sort(const void *a, const void *b, void *thunk) { + cmp_cb_t cmp = (cmp_cb_t) thunk; + + return cmp(*(void **) a, *(void **) b); +} + +int list_init(struct list *l) +{ + assert(l->state == STATE_DESTROYED); + + pthread_mutex_init(&l->lock, NULL); + + l->length = 0; + l->capacity = 0; + l->array = NULL; + + l->state = STATE_INITIALIZED; + + return 0; +} + +int list_destroy(struct list *l, dtor_cb_t destructor, bool release) +{ + pthread_mutex_lock(&l->lock); + + assert(l->state != STATE_DESTROYED); + + for (size_t i = 0; i < list_length(l); i++) { + void *p = list_at(l, i); + + if (destructor) + destructor(p); + if (release) + free(p); + } + + free(l->array); + + l->array = NULL; + + l->length = -1; + l->capacity = 0; + + pthread_mutex_unlock(&l->lock); + pthread_mutex_destroy(&l->lock); + + l->state = STATE_DESTROYED; + + return 0; +} + +void list_push(struct list *l, void *p) +{ + pthread_mutex_lock(&l->lock); + + assert(l->state == STATE_INITIALIZED); + + /* Resize array if out of capacity */ + if (l->length >= l->capacity) { + l->capacity += LIST_CHUNKSIZE; + l->array = realloc(l->array, l->capacity * sizeof(void *)); + } + + l->array[l->length] = p; + l->length++; + + pthread_mutex_unlock(&l->lock); +} + +void list_remove(struct list *l, void *p) +{ + int removed = 0; + + pthread_mutex_lock(&l->lock); + + assert(l->state == STATE_INITIALIZED); + + for (size_t i = 0; i < list_length(l); i++) { + if (l->array[i] == p) + removed++; + else + l->array[i - removed] = l->array[i]; + } + + l->length -= removed; + + pthread_mutex_unlock(&l->lock); +} + +void * list_lookup(struct list *l, const char *name) +{ + return list_search(l, cmp_lookup, (void *) name); +} + +int list_contains(struct list *l, void *p) +{ + return list_count(l, cmp_contains, p); +} + +int list_count(struct list *l, cmp_cb_t cmp, void *ctx) +{ + int c = 0; + + pthread_mutex_lock(&l->lock); + + assert(l->state == STATE_INITIALIZED); + + for (size_t i = 0; i < list_length(l); i++) { + void *e = list_at(l, i); + + if (cmp(e, ctx) == 0) + c++; + } + + pthread_mutex_unlock(&l->lock); + + return c; +} + +void * list_search(struct list *l, cmp_cb_t cmp, void *ctx) +{ + void *e; + + pthread_mutex_lock(&l->lock); + + assert(l->state == STATE_INITIALIZED); + + for (size_t i = 0; i < list_length(l); i++) { + e = list_at(l, i); + if (cmp(e, ctx) == 0) + goto out; + } + + e = NULL; /* not found */ + +out: pthread_mutex_unlock(&l->lock); + + return e; +} + +void list_sort(struct list *l, cmp_cb_t cmp) +{ + pthread_mutex_lock(&l->lock); + + assert(l->state == STATE_INITIALIZED); + + qsort_r(l->array, l->length, sizeof(void *), cmp_sort, (void *) cmp); + + pthread_mutex_unlock(&l->lock); +} + +int list_set(struct list *l, int index, void *value) +{ + if (index >= l->length) + return -1; + + l->array[index] = value; + + return 0; +} diff --git a/common/lib/log.c b/common/lib/log.c new file mode 100644 index 000000000..06d418252 --- /dev/null +++ b/common/lib/log.c @@ -0,0 +1,310 @@ +/** Logging and debugging routines + * + * @author Steffen Vogel + * @copyright 2017-2018, Institute for Automation of Complex Power Systems, EONERC + * @license GNU General Public License (version 3) + * + * VILLASfpga + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + *********************************************************************************/ + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#ifdef ENABLE_OPAL_ASYNC +/* Define RTLAB before including OpalPrint.h for messages to be sent + * to the OpalDisplay. Otherwise stdout will be used. */ + #define RTLAB + #include "OpalPrint.h" +#endif + +struct log *global_log; +struct log default_log; + +/* We register a default log instance */ +__attribute__((constructor)) +void register_default_log() +{ + int ret; + static struct log default_log; + + ret = log_init(&default_log, V, LOG_ALL); + if (ret) + error("Failed to initalize log"); + + ret = log_start(&default_log); + if (ret) + error("Failed to start log"); +} + +/** List of debug facilities as strings */ +static const char *facilities_strs[] = { + "pool", /* LOG_POOL */ + "queue", /* LOG_QUEUE */ + "config", /* LOG_CONFIG */ + "hook", /* LOG_HOOK */ + "path", /* LOG_PATH */ + "node", /* LOG_NODE */ + "mem", /* LOG_MEM */ + "web", /* LOG_WEB */ + "api", /* LOG_API */ + "log", /* LOG_LOG */ + "vfio", /* LOG_VFIO */ + "pci", /* LOG_PCI */ + "xil", /* LOG_XIL */ + "tc", /* LOG_TC */ + "if", /* LOG_IF */ + "advio", /* LOG_ADVIO */ + + /* Node-types */ + "socket", /* LOG_SOCKET */ + "file", /* LOG_FILE */ + "fpga", /* LOG_FPGA */ + "ngsi", /* LOG_NGSI */ + "websocket", /* LOG_WEBSOCKET */ + "opal", /* LOG_OPAL */ +}; + +#ifdef __GNUC__ +/** The current log indention level (per thread!). */ +static __thread int indent = 0; + +int log_indent(int levels) +{ + int old = indent; + indent += levels; + return old; +} + +int log_noindent() +{ + int old = indent; + indent = 0; + return old; +} + +void log_outdent(int *old) +{ + indent = *old; +} +#endif + +static void log_resize(int signal, siginfo_t *sinfo, void *ctx) +{ + int ret; + + ret = ioctl(STDOUT_FILENO, TIOCGWINSZ, &global_log->window); + if (ret) + return; + + global_log->width = global_log->window.ws_col - 25; + if (global_log->prefix) + global_log->width -= strlenp(global_log->prefix); + + debug(LOG_LOG | 15, "New terminal size: %dx%x", global_log->window.ws_row, global_log->window.ws_col); +} + +int log_init(struct log *l, int level, long facilitites) +{ + int ret; + + /* Register this log instance globally */ + global_log = l; + + l->level = level; + l->syslog = 0; + l->facilities = facilitites; + l->file = stderr; + l->path = NULL; + + l->prefix = getenv("VILLAS_LOG_PREFIX"); + + /* Register signal handler which is called whenever the + * terminal size changes. */ + if (l->file == stderr) { + struct sigaction sa_resize = { + .sa_flags = SA_SIGINFO, + .sa_sigaction = log_resize + }; + + sigemptyset(&sa_resize.sa_mask); + + ret = sigaction(SIGWINCH, &sa_resize, NULL); + if (ret) + return ret; + + /* Try to get initial window size */ + ioctl(STDERR_FILENO, TIOCGWINSZ, &global_log->window); + } + else { + l->window.ws_col = LOG_WIDTH; + l->window.ws_row = LOG_HEIGHT; + } + + l->width = l->window.ws_col - 25; + if (l->prefix) + l->width -= strlenp(l->prefix); + + l->state = STATE_INITIALIZED; + + return 0; +} + +int log_start(struct log *l) +{ + if (l->path) { + l->file = fopen(l->path, "a+");; + if (!l->file) { + l->file = stderr; + error("Failed to open log file '%s'", l->path); + } + } + else + l->file = stderr; + + l->state = STATE_STARTED; + + if (l->syslog) { + openlog(NULL, LOG_PID, LOG_DAEMON); + } + + debug(LOG_LOG | 5, "Log sub-system started: level=%d, faciltities=%#lx, path=%s", l->level, l->facilities, l->path); + + return 0; +} + +int log_stop(struct log *l) +{ + if (l->state != STATE_STARTED) + return 0; + + if (l->file != stderr && l->file != stdout) { + fclose(l->file); + } + + if (l->syslog) { + closelog(); + } + + l->state = STATE_STOPPED; + + return 0; +} + +int log_destroy(struct log *l) +{ + default_log.epoch = l->epoch; + + global_log = &default_log; + + l->state = STATE_DESTROYED; + + return 0; +} + +int log_set_facility_expression(struct log *l, const char *expression) +{ + bool negate; + char *copy, *token; + long mask = 0, facilities = 0; + + copy = strdup(expression); + token = strtok(copy, ","); + + while (token != NULL) { + if (token[0] == '!') { + token++; + negate = true; + } + else + negate = false; + + /* Check for some classes */ + if (!strcmp(token, "all")) + mask = LOG_ALL; + else if (!strcmp(token, "nodes")) + mask = LOG_NODES; + else if (!strcmp(token, "kernel")) + mask = LOG_KERNEL; + else { + for (int ind = 0; ind < ARRAY_LEN(facilities_strs); ind++) { + if (!strcmp(token, facilities_strs[ind])) { + mask = (1 << (ind+8)); + goto found; + } + } + + error("Invalid log class '%s'", token); + } + +found: if (negate) + facilities &= ~mask; + else + facilities |= mask; + + token = strtok(NULL, ","); + } + + l->facilities = facilities; + + free(copy); + + return l->facilities; +} + +void log_print(struct log *l, const char *lvl, const char *fmt, ...) +{ + va_list ap; + + va_start(ap, fmt); + log_vprint(l, lvl, fmt, ap); + va_end(ap); +} + +void log_vprint(struct log *l, const char *lvl, const char *fmt, va_list ap) +{ + char *buf = alloc(512); + + /* Optional prefix */ + if (l->prefix) + strcatf(&buf, "%s", l->prefix); + + /* Indention */ +#ifdef __GNUC__ + for (int i = 0; i < indent; i++) + strcatf(&buf, "%s ", BOX_UD); + + strcatf(&buf, "%s ", BOX_UDR); +#endif + + /* Format String */ + vstrcatf(&buf, fmt, ap); + + /* Output */ +#ifdef ENABLE_OPAL_ASYNC + OpalPrint("VILLASfpga: %s\n", buf); +#endif + fprintf(l->file ? l->file : stderr, "%s\n", buf); + + free(buf); +} diff --git a/common/lib/log_config.c b/common/lib/log_config.c new file mode 100644 index 000000000..ed1c182c6 --- /dev/null +++ b/common/lib/log_config.c @@ -0,0 +1,85 @@ +/** Logging routines that depend on jansson. + * + * @author Steffen Vogel + * @copyright 2017-2018, Institute for Automation of Complex Power Systems, EONERC + * @license GNU General Public License (version 3) + * + * VILLASfpga + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + *********************************************************************************/ + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +int log_parse(struct log *l, json_t *cfg) +{ + const char *facilities = NULL; + const char *path = NULL; + int ret; + + json_error_t err; + + ret = json_unpack_ex(cfg, &err, 0, "{ s?: i, s?: s, s?: s, s?: b }", + "level", &l->level, + "file", &path, + "facilities", &facilities, + "syslog", &l->syslog + ); + if (ret) + jerror(&err, "Failed to parse logging configuration"); + + if (path) + l->path = strdup(path); + + if (facilities) + log_set_facility_expression(l, facilities); + + l->state = STATE_PARSED; + + return 0; +} + +void jerror(json_error_t *err, const char *fmt, ...) +{ + va_list ap; + char *buf = NULL; + + struct log *l = global_log ? global_log : &default_log; + + va_start(ap, fmt); + vstrcatf(&buf, fmt, ap); + va_end(ap); + + log_print(l, LOG_LVL_ERROR, "%s:", buf); + { INDENT + log_print(l, LOG_LVL_ERROR, "%s in %s:%d:%d", err->text, err->source, err->line, err->column); + + if (l->syslog) + syslog(LOG_ERR, "%s in %s:%d:%d", err->text, err->source, err->line, err->column); + } + + free(buf); + + killme(SIGABRT); + pause(); +} diff --git a/common/lib/log_helper.c b/common/lib/log_helper.c new file mode 100644 index 000000000..cc050894a --- /dev/null +++ b/common/lib/log_helper.c @@ -0,0 +1,138 @@ +/** Logging and debugging routines + * + * @author Steffen Vogel + * @copyright 2017-2018, Institute for Automation of Complex Power Systems, EONERC + * @license GNU General Public License (version 3) + * + * VILLASfpga + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + *********************************************************************************/ + +#include +#include +#include + +#include +#include + +void debug(long class, const char *fmt, ...) +{ + va_list ap; + + struct log *l = global_log; + + int lvl = class & 0xFF; + int fac = class & ~0xFF; + + if (((fac == 0) || (fac & l->facilities)) && (lvl <= l->level)) { + va_start(ap, fmt); + + log_vprint(l, LOG_LVL_DEBUG, fmt, ap); + + if (l->syslog) + syslog(LOG_DEBUG, fmt, ap); + + va_end(ap); + } +} + +void info(const char *fmt, ...) +{ + va_list ap; + + struct log *l = global_log; + + va_start(ap, fmt); + + log_vprint(l, LOG_LVL_INFO, fmt, ap); + + if (l->syslog) + syslog(LOG_INFO, fmt, ap); + + va_end(ap); +} + +void warn(const char *fmt, ...) +{ + va_list ap; + + struct log *l = global_log; + + va_start(ap, fmt); + + log_vprint(l, LOG_LVL_WARN, fmt, ap); + + if (l->syslog) + syslog(LOG_WARNING, fmt, ap); + + va_end(ap); +} + +void stats(const char *fmt, ...) +{ + va_list ap; + + struct log *l = global_log; + + va_start(ap, fmt); + + log_vprint(l, LOG_LVL_STATS, fmt, ap); + + if (l->syslog) + syslog(LOG_INFO, fmt, ap); + + va_end(ap); +} + +void error(const char *fmt, ...) +{ + va_list ap; + + struct log *l = global_log; + + va_start(ap, fmt); + + log_vprint(l, LOG_LVL_ERROR, fmt, ap); + + if (l->syslog) + syslog(LOG_ERR, fmt, ap); + + va_end(ap); + + killme(SIGABRT); + pause(); +} + +void serror(const char *fmt, ...) +{ + va_list ap; + char *buf = NULL; + + struct log *l = global_log; + + va_start(ap, fmt); + vstrcatf(&buf, fmt, ap); + va_end(ap); + + log_print(l, LOG_LVL_ERROR, "%s: %m (%u)", buf, errno); + + if (l->syslog) + syslog(LOG_ERR, "%s: %m (%u)", buf, errno); + + free(buf); + + killme(SIGABRT); + pause(); +} diff --git a/common/lib/utils.c b/common/lib/utils.c new file mode 100644 index 000000000..9764f4496 --- /dev/null +++ b/common/lib/utils.c @@ -0,0 +1,401 @@ +/** General purpose helper functions. + * + * @author Steffen Vogel + * @copyright 2017-2018, Institute for Automation of Complex Power Systems, EONERC + * @license GNU General Public License (version 3) + * + * VILLASfpga + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + *********************************************************************************/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +pthread_t main_thread; + +void print_copyright() +{ + printf("VILLASfpga %s (built on %s %s)\n", + CLR_BLU(BUILDID), CLR_MAG(__DATE__), CLR_MAG(__TIME__)); + printf(" Copyright 2014-2017, Institute for Automation of Complex Power Systems, EONERC\n"); + printf(" Steffen Vogel \n"); +} + +void print_version() +{ + printf("%s\n", BUILDID); +} + +int version_parse(const char *s, struct version *v) +{ + return sscanf(s, "%u.%u", &v->major, &v->minor) != 2; +} + +int version_cmp(struct version *a, struct version *b) { + int major = a->major - b->major; + int minor = a->minor - b->minor; + + return major ? major : minor; +} + +double box_muller(float m, float s) +{ + double x1, x2, y1; + static double y2; + static int use_last = 0; + + if (use_last) { /* use value from previous call */ + y1 = y2; + use_last = 0; + } + else { + double w; + do { + x1 = 2.0 * randf() - 1.0; + x2 = 2.0 * randf() - 1.0; + w = x1*x1 + x2*x2; + } while (w >= 1.0); + + w = sqrt(-2.0 * log(w) / w); + y1 = x1 * w; + y2 = x2 * w; + use_last = 1; + } + + return m + y1 * s; +} + +double randf() +{ + return (double) random() / RAND_MAX; +} + +char * strcatf(char **dest, const char *fmt, ...) +{ + va_list ap; + va_start(ap, fmt); + vstrcatf(dest, fmt, ap); + va_end(ap); + + return *dest; +} + +char * vstrcatf(char **dest, const char *fmt, va_list ap) +{ + char *tmp; + int n = *dest ? strlen(*dest) : 0; + int i = vasprintf(&tmp, fmt, ap); + + *dest = (char *)(realloc(*dest, n + i + 1)); + if (*dest != NULL) + strncpy(*dest+n, tmp, i + 1); + + free(tmp); + + return *dest; +} + +#ifdef __linux__ + +void cpuset_to_integer(cpu_set_t *cset, uintmax_t *set) +{ + *set = 0; + for (int i = 0; i < MIN(sizeof(*set) * 8, CPU_SETSIZE); i++) { + if (CPU_ISSET(i, cset)) + *set |= 1ULL << i; + } +} + +void cpuset_from_integer(uintmax_t set, cpu_set_t *cset) +{ + CPU_ZERO(cset); + for (int i = 0; i < MIN(sizeof(set) * 8, CPU_SETSIZE); i++) { + if (set & (1L << i)) + CPU_SET(i, cset); + } +} + +/* From: https://github.com/mmalecki/util-linux/blob/master/lib/cpuset.c */ +static const char *nexttoken(const char *q, int sep) +{ + if (q) + q = strchr(q, sep); + if (q) + q++; + return q; +} + +int cpulist_parse(const char *str, cpu_set_t *set, int fail) +{ + const char *p, *q; + int r = 0; + + q = str; + CPU_ZERO(set); + + while (p = q, q = nexttoken(q, ','), p) { + unsigned int a; /* beginning of range */ + unsigned int b; /* end of range */ + unsigned int s; /* stride */ + const char *c1, *c2; + char c; + + if ((r = sscanf(p, "%u%c", &a, &c)) < 1) + return 1; + b = a; + s = 1; + + c1 = nexttoken(p, '-'); + c2 = nexttoken(p, ','); + if (c1 != NULL && (c2 == NULL || c1 < c2)) { + if ((r = sscanf(c1, "%u%c", &b, &c)) < 1) + return 1; + c1 = nexttoken(c1, ':'); + if (c1 != NULL && (c2 == NULL || c1 < c2)) { + if ((r = sscanf(c1, "%u%c", &s, &c)) < 1) + return 1; + if (s == 0) + return 1; + } + } + + if (!(a <= b)) + return 1; + while (a <= b) { + if (fail && (a >= CPU_SETSIZE)) + return 2; + CPU_SET(a, set); + a += s; + } + } + + if (r == 2) + return 1; + + return 0; +} + +char *cpulist_create(char *str, size_t len, cpu_set_t *set) +{ + size_t i; + char *ptr = str; + int entry_made = 0; + + for (i = 0; i < CPU_SETSIZE; i++) { + if (CPU_ISSET(i, set)) { + int rlen; + size_t j, run = 0; + entry_made = 1; + for (j = i + 1; j < CPU_SETSIZE; j++) { + if (CPU_ISSET(j, set)) + run++; + else + break; + } + if (!run) + rlen = snprintf(ptr, len, "%zd,", i); + else if (run == 1) { + rlen = snprintf(ptr, len, "%zd,%zd,", i, i + 1); + i++; + } else { + rlen = snprintf(ptr, len, "%zd-%zd,", i, i + run); + i += run; + } + if (rlen < 0 || (size_t) rlen + 1 > len) + return NULL; + ptr += rlen; + if (rlen > 0 && len > (size_t) rlen) + len -= rlen; + else + len = 0; + } + } + ptr -= entry_made; + *ptr = '\0'; + + return str; +} +#endif /* __linux__ */ + +void * alloc(size_t bytes) +{ + void *p = malloc(bytes); + if (!p) + error("Failed to allocate memory"); + + memset(p, 0, bytes); + + return p; +} + +void * memdup(const void *src, size_t bytes) +{ + void *dst = alloc(bytes); + + memcpy(dst, src, bytes); + + return dst; +} + +size_t read_random(char *buf, size_t len) +{ + int fd; + ssize_t bytes, total; + + fd = open("/dev/urandom", O_RDONLY); + if (fd < 0) + return 0; + + bytes = 0; + total = 0; + while (total < len) { + bytes = read(fd, buf + total, len - total); + if (bytes < 0) + break; + + total += bytes; + } + + close(fd); + + return bytes; +} + +void rdtsc_sleep(uint64_t nanosecs, uint64_t start) +{ + uint64_t cycles; + + /** @todo Replace the hard coded CPU clock frequency */ + cycles = (double) nanosecs / (1e9 / 3392389000); + + if (start == 0) + start = rdtsc(); + + do { + __asm__("nop"); + } while (rdtsc() - start < cycles); +} + +/* Setup exit handler */ +int signals_init(void (*cb)(int signal, siginfo_t *sinfo, void *ctx)) +{ + int ret; + + info("Initialize signals"); + + struct sigaction sa_quit = { + .sa_flags = SA_SIGINFO | SA_NODEFER, + .sa_sigaction = cb + }; + + struct sigaction sa_chld = { + .sa_flags = 0, + .sa_handler = SIG_IGN + }; + + main_thread = pthread_self(); + + sigemptyset(&sa_quit.sa_mask); + + ret = sigaction(SIGINT, &sa_quit, NULL); + if (ret) + return ret; + + ret = sigaction(SIGTERM, &sa_quit, NULL); + if (ret) + return ret; + + ret = sigaction(SIGALRM, &sa_quit, NULL); + if (ret) + return ret; + + ret = sigaction(SIGCHLD, &sa_chld, NULL); + if (ret) + return ret; + + return 0; +} + +void killme(int sig) +{ + /* Send only to main thread in case the ID was initilized by signals_init() */ + if (main_thread) + pthread_kill(main_thread, sig); + else + kill(0, sig); +} + +pid_t spawn(const char* name, char *const argv[]) +{ + pid_t pid; + + pid = fork(); + switch (pid) { + case -1: return -1; + case 0: return execvp(name, (char * const*) argv); + } + + return pid; +} + +size_t strlenp(const char *str) +{ + size_t sz = 0; + + for (const char *d = str; *d; d++) { + const unsigned char *c = (const unsigned char *) d; + + if (isprint(*c)) + sz++; + else if (c[0] == '\b') + sz--; + else if (c[0] == '\t') + sz += 4; /* tab width == 4 */ + /* CSI sequence */ + else if (c[0] == '\e' && c[1] == '[') { + c += 2; + while (*c && *c != 'm') + c++; + } + /* UTF-8 */ + else if (c[0] >= 0xc2 && c[0] <= 0xdf) { + sz++; + c += 1; + } + else if (c[0] >= 0xe0 && c[0] <= 0xef) { + sz++; + c += 2; + } + else if (c[0] >= 0xf0 && c[0] <= 0xf4) { + sz++; + c += 3; + } + + d = (const char *) c; + } + + return sz; +}