/*
|
* Copyright (C) 2013 Philippe Gerum <rpm@xenomai.org>.
|
*
|
* This library is free software; you can redistribute it and/or
|
* modify it under the terms of the GNU Lesser General Public
|
* License as published by the Free Software Foundation; either
|
* version 2 of the License, or (at your option) any later version.
|
*
|
* This library 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
|
* Lesser General Public License for more details.
|
|
* You should have received a copy of the GNU Lesser General Public
|
* License along with this library; if not, write to the Free Software
|
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
|
*/
|
#include <sys/types.h>
|
#include <sys/syscall.h>
|
#include <stdio.h>
|
#include <time.h>
|
#include <stdlib.h>
|
#include <stdarg.h>
|
#include <pthread.h>
|
#include <sched.h>
|
#include <unistd.h>
|
#include <ctype.h>
|
#include <string.h>
|
#include <errno.h>
|
#include <limits.h>
|
#include <malloc.h>
|
#include "boilerplate/atomic.h"
|
#include "boilerplate/lock.h"
|
#include "boilerplate/time.h"
|
#include "boilerplate/scope.h"
|
#include "boilerplate/setup.h"
|
#include "boilerplate/debug.h"
|
#include "boilerplate/ancillaries.h"
|
#include "boilerplate/signal.h"
|
#include "boilerplate/namegen.h"
|
#include "xenomai/init.h"
|
|
pthread_mutex_t __printlock;
|
|
static struct timespec init_date;
|
|
static int init_done;
|
|
static void __do_printout(const char *name, const char *header,
|
unsigned int ms, unsigned int us,
|
const char *fmt, va_list ap)
|
{
|
FILE *fp = stderr;
|
|
__RT(fprintf(fp, "%4u\"%.3u.%.3u| ", ms / 1000, ms % 1000, us));
|
|
if (header)
|
__RT(fputs(header, fp));
|
|
__RT(fprintf(fp, "[%s] ", name ?: "main"));
|
__RT(vfprintf(fp, fmt, ap));
|
__RT(fputc('\n', fp));
|
}
|
|
static void __do_early_printout(const char *name, const char *header,
|
unsigned int ms, unsigned int us,
|
const char *fmt, va_list ap)
|
{
|
FILE *fp = stderr;
|
|
__STD(fprintf(fp, "%4u\"%.3u.%.3u| ", ms / 1000, ms % 1000, us));
|
|
if (header)
|
__STD(fputs(header, fp));
|
|
__STD(fprintf(fp, "[%s] ", name ?: "main"));
|
__STD(vfprintf(fp, fmt, ap));
|
__STD(fputc('\n', fp));
|
fflush(fp);
|
}
|
|
void __printout(const char *name, const char *header,
|
const char *fmt, va_list ap)
|
{
|
struct timespec now, delta;
|
unsigned long long ns;
|
unsigned int ms, us;
|
|
/*
|
* Catch early printouts, when the init sequence is not
|
* completed yet. In such event, we don't care for serializing
|
* output, since we must be running over the main thread
|
* uncontended.
|
*/
|
if (!init_done) {
|
__do_early_printout(name, header, 0, 0, fmt, ap);
|
return;
|
}
|
|
__RT(clock_gettime(CLOCK_MONOTONIC, &now));
|
timespec_sub(&delta, &now, &init_date);
|
ns = delta.tv_sec * 1000000000ULL;
|
ns += delta.tv_nsec;
|
ms = ns / 1000000ULL;
|
us = (ns % 1000000ULL) / 1000ULL;
|
SIGSAFE_LOCK_ENTRY(&__printlock);
|
__do_printout(name, header, ms, us, fmt, ap);
|
SIGSAFE_LOCK_EXIT(&__printlock);
|
}
|
|
void __warning(const char *name, const char *fmt, va_list ap)
|
{
|
__printout(name, "WARNING: ", fmt, ap);
|
}
|
|
void __notice(const char *name, const char *fmt, va_list ap)
|
{
|
__printout(name, NULL, fmt, ap);
|
}
|
|
void ___panic(const char *fn, const char *name,
|
const char *fmt, va_list ap)
|
{
|
char *p;
|
|
if (asprintf(&p, "BUG in %s(): ", fn) < 0)
|
p = "BUG: ";
|
__printout(name, p, fmt, ap);
|
#ifdef CONFIG_XENO_COBALT
|
rt_print_flush_buffers();
|
#endif /* CONFIG_XENO_COBALT */
|
exit(1);
|
}
|
|
__weak void error_hook(struct error_frame *ef) /* NULL in non-debug mode */
|
{
|
}
|
|
#define __esym_def(e) [e] = #e
|
|
static const char *__esym_map[] = {
|
[0] = "OK",
|
__esym_def(EPERM),
|
__esym_def(ENOENT),
|
__esym_def(ESRCH),
|
__esym_def(EINTR),
|
__esym_def(EIO),
|
__esym_def(ENXIO),
|
__esym_def(E2BIG),
|
__esym_def(ENOEXEC),
|
__esym_def(EBADF),
|
__esym_def(ECHILD),
|
__esym_def(EAGAIN),
|
__esym_def(ENOMEM),
|
__esym_def(EACCES),
|
__esym_def(EFAULT),
|
__esym_def(ENOTBLK),
|
__esym_def(EBUSY),
|
__esym_def(EEXIST),
|
__esym_def(EXDEV),
|
__esym_def(ENODEV),
|
__esym_def(ENOTDIR),
|
__esym_def(EISDIR),
|
__esym_def(EINVAL),
|
__esym_def(ENFILE),
|
__esym_def(EMFILE),
|
__esym_def(ENOTTY),
|
__esym_def(ETXTBSY),
|
__esym_def(EFBIG),
|
__esym_def(ENOSPC),
|
__esym_def(ESPIPE),
|
__esym_def(EROFS),
|
__esym_def(EMLINK),
|
__esym_def(EPIPE),
|
__esym_def(EDOM),
|
__esym_def(ERANGE),
|
__esym_def(ENOSYS),
|
__esym_def(ETIMEDOUT),
|
__esym_def(ENOMSG),
|
__esym_def(EIDRM),
|
__esym_def(EADDRINUSE),
|
__esym_def(EPROTO),
|
};
|
|
#define __esym_max (sizeof(__esym_map) / sizeof(__esym_map[0]))
|
|
const char *symerror(int errnum)
|
{
|
int v = -errnum;
|
size_t ebufsz;
|
char *ebuf;
|
|
if (v < 0 || v >= (int)__esym_max || __esym_map[v] == NULL) {
|
/* Catch missing codes in the error map. */
|
ebuf = __get_error_buf(&ebufsz);
|
snprintf(ebuf, ebufsz, "%d?", errnum);
|
return ebuf;
|
}
|
|
return __esym_map[v];
|
}
|
|
void __run_cleanup_block(struct cleanup_block *cb)
|
{
|
__RT(pthread_mutex_unlock(cb->lock));
|
cb->handler(cb->arg);
|
}
|
|
void __early_panic(const char *fn, const char *fmt, ...)
|
{
|
va_list ap;
|
|
va_start(ap, fmt);
|
___panic(fn, NULL, fmt, ap);
|
va_end(ap);
|
}
|
|
void __panic(const char *fn, const char *fmt, ...)
|
__attribute__((alias("__early_panic"), weak));
|
|
void early_warning(const char *fmt, ...)
|
{
|
va_list ap;
|
|
va_start(ap, fmt);
|
__warning(NULL, fmt, ap);
|
va_end(ap);
|
}
|
|
void warning(const char *fmt, ...)
|
__attribute__((alias("early_warning"), weak));
|
|
void early_notice(const char *fmt, ...)
|
{
|
va_list ap;
|
|
va_start(ap, fmt);
|
__notice(NULL, fmt, ap);
|
va_end(ap);
|
}
|
|
void notice(const char *fmt, ...)
|
__attribute__((alias("early_notice"), weak));
|
|
char *generate_name(char *buf, const char *radix,
|
struct name_generator *ngen)
|
{
|
int len = ngen->length - 1, tag;
|
|
if (radix && *radix) {
|
strncpy(buf, radix, len);
|
buf[len] = '\0';
|
} else {
|
tag = atomic_add_fetch(&ngen->serial, 1);
|
#ifdef CONFIG_XENO_PSHARED
|
snprintf(buf, len, "%s@%d[%d]", ngen->radix, tag, __node_id);
|
#else
|
snprintf(buf, len, "%s@%d", ngen->radix, tag);
|
#endif
|
}
|
|
return buf;
|
}
|
|
#ifdef CONFIG_XENO_PSHARED
|
|
/*
|
* Client libraries may override these symbols for implementing heap
|
* pointer validation in their own context (e.g. copperplate).
|
*/
|
|
__weak int pshared_check(void *heap, void *addr)
|
{
|
return 1;
|
}
|
|
__weak void *__main_heap = NULL;
|
|
#endif /* !CONFIG_XENO_PSHARED */
|
|
#ifdef CONFIG_XENO_DEBUG
|
|
int __check_cancel_type(const char *locktype)
|
{
|
int oldtype;
|
|
pthread_setcanceltype(PTHREAD_CANCEL_DEFERRED, &oldtype);
|
if (oldtype == PTHREAD_CANCEL_DEFERRED)
|
return 0;
|
|
warning("%s() section is NOT cancel-safe", locktype);
|
abort();
|
|
return __bt(-EINVAL);
|
}
|
|
#endif /* CONFIG_XENO_DEBUG */
|
|
int get_static_cpu_count(void)
|
{
|
char buf[BUFSIZ];
|
int count = 0;
|
FILE *fp;
|
|
/*
|
* We want the maximum # of CPU the running kernel was
|
* configured for, not the current online/present/possible
|
* count of CPU devices.
|
*/
|
fp = fopen("/sys/devices/system/cpu/kernel_max", "r");
|
if (fp == NULL)
|
return -1;
|
|
if (fgets(buf, sizeof(buf), fp))
|
count = atoi(buf);
|
|
fclose(fp);
|
|
return count;
|
}
|
|
static int __get_online_cpus_from_proc(cpu_set_t *cpuset)
|
{
|
char buf[BUFSIZ];
|
FILE *fp;
|
int cpu;
|
|
/* If no hotplug support, fall back to reading /proc/stat. */
|
|
fp = fopen("/proc/stat", "r");
|
if (fp == NULL)
|
return -ENOENT;
|
|
while (fgets(buf, sizeof(buf), fp)) {
|
/*
|
* Like the glibc, assume cpu* entries are at the
|
* front of /proc/stat and will stay this way.
|
*/
|
if (strncmp(buf, "cpu", 3))
|
break;
|
if (!isdigit(buf[3]))
|
continue;
|
cpu = atoi(buf + 3);
|
if (cpu >= 0 && cpu < CPU_SETSIZE)
|
CPU_SET(cpu, cpuset);
|
}
|
|
fclose(fp);
|
|
return 0;
|
}
|
|
int get_online_cpu_set(cpu_set_t *cpuset)
|
{
|
char buf[BUFSIZ], *range, *save_range, *save_bound,
|
*lo_bound, *hi_bound, *p;
|
int cpu_lo, cpu_hi;
|
FILE *fp;
|
|
CPU_ZERO(cpuset);
|
|
fp = fopen("/sys/devices/system/cpu/online", "r");
|
if (fp == NULL)
|
return __get_online_cpus_from_proc(cpuset);
|
|
if (fgets(buf, sizeof(buf), fp) == NULL) {
|
fclose(fp);
|
return -EBADF;
|
}
|
|
p = buf;
|
for (;;) {
|
range = strtok_r(p, " \t", &save_range);
|
if (range == NULL)
|
break;
|
lo_bound = strtok_r(range, "-", &save_bound);
|
if (lo_bound) {
|
cpu_lo = atoi(lo_bound);
|
hi_bound = strtok_r(NULL, "-", &save_bound);
|
if (hi_bound) {
|
cpu_hi = atoi(hi_bound);
|
do {
|
CPU_SET(cpu_lo, cpuset);
|
} while (cpu_lo++ < cpu_hi);
|
} else
|
CPU_SET(cpu_lo, cpuset);
|
}
|
p = NULL;
|
}
|
|
fclose(fp);
|
|
return 0;
|
}
|
|
#ifdef CONFIG_XENO_COBALT
|
|
#include <sys/cobalt.h>
|
|
int get_realtime_cpu_set(cpu_set_t *cpuset)
|
{
|
unsigned long long cpumask;
|
char buf[BUFSIZ], *p;
|
FILE *fp;
|
int cpu;
|
|
fp = fopen("/sys/module/xenomai/parameters/supported_cpus", "r");
|
if (fp == NULL)
|
return -ENOENT;
|
|
p = fgets(buf, sizeof(buf), fp);
|
fclose(fp);
|
if (p == NULL)
|
return -EBADF;
|
|
errno = 0;
|
cpumask = strtoll(p, NULL, 10);
|
if (cpumask == LLONG_MAX && errno == ERANGE)
|
cpumask = ULLONG_MAX;
|
for (cpu = 0; cpumask != 0; cpu++, cpumask >>= 1) {
|
if (cpumask & 1ULL)
|
CPU_SET(cpu, cpuset);
|
}
|
|
return 0;
|
}
|
|
int get_current_cpu(void) /* No mode migration */
|
{
|
struct cobalt_threadstat stat;
|
int ret;
|
|
ret = cobalt_thread_stat(0, &stat);
|
if (ret)
|
return ret;
|
|
return stat.cpu;
|
}
|
|
#else /* CONFIG_XENO_MERCURY */
|
|
int get_realtime_cpu_set(cpu_set_t *cpuset)
|
{
|
return get_online_cpu_set(cpuset);
|
}
|
|
int get_current_cpu(void)
|
{
|
int cpu = sched_getcpu();
|
|
if (cpu < 0)
|
return -errno;
|
|
return cpu;
|
}
|
|
#endif /* CONFIG_XENO_MERCURY */
|
|
pid_t get_thread_pid(void)
|
{
|
return syscall(__NR_gettid);
|
}
|
|
char *lookup_command(const char *cmd)
|
{
|
const char *dirs[] = {
|
"/bin",
|
"/sbin",
|
"/usr/bin",
|
"/usr/sbin",
|
};
|
char *path;
|
int n, ret;
|
|
for (n = 0; n < sizeof(dirs) / sizeof(dirs[0]); n++) {
|
ret = asprintf(&path, "%s/%s", dirs[n], cmd);
|
if (ret < 0)
|
return NULL;
|
if (access(path, X_OK) == 0)
|
return path;
|
free(path);
|
}
|
|
return NULL;
|
}
|
|
size_t get_mem_size(const char *arg)
|
{
|
size_t size;
|
char *p;
|
|
size = strtol(arg, &p, 0);
|
if (size == LONG_MIN || size == LONG_MAX)
|
return 0;
|
|
if (*p == '\0')
|
return size;
|
|
switch (tolower(*p)) {
|
case 'k':
|
size *= 1024;
|
break;
|
case 'm':
|
size *= (1024 * 1024);
|
break;
|
case 'g':
|
size *= (1024 * 1024 * 1024);
|
break;
|
default:
|
size = 0;
|
}
|
|
return size;
|
}
|
|
const char *config_strings[] = {
|
#include "config-dump.h"
|
NULL,
|
};
|
|
void __boilerplate_init(void)
|
{
|
__RT(clock_gettime(CLOCK_MONOTONIC, &init_date));
|
__RT(pthread_mutex_init(&__printlock, NULL));
|
debug_init();
|
init_done = 1;
|
}
|
|
static int boilerplate_init(void)
|
{
|
pthread_atfork(NULL, NULL, __boilerplate_init);
|
__boilerplate_init();
|
|
return 0;
|
}
|
|
static struct setup_descriptor boilerplate_interface = {
|
.name = "boilerplate",
|
.init = boilerplate_init,
|
};
|
|
boilerplate_setup_call(boilerplate_interface);
|
