pxt-ev3/libs/core/linux.cpp

542 lines
12 KiB
C++
Raw Normal View History

2017-06-29 19:41:38 +02:00
#include "pxt.h"
#include <stdlib.h>
#include <stdio.h>
#include <sys/time.h>
#include <time.h>
2017-07-01 12:43:10 +02:00
#include <cstdarg>
2017-07-03 13:57:34 +02:00
#include <pthread.h>
2017-07-05 18:53:22 +02:00
#include <unistd.h>
#include <dirent.h>
#include <signal.h>
#include <sys/types.h>
#include <sys/stat.h>
2017-07-09 11:19:14 +02:00
#include <errno.h>
#include <fcntl.h>
#include <malloc.h>
2017-07-01 12:43:10 +02:00
#define THREAD_DBG(...)
#define MALLOC_LIMIT (8 * 1024 * 1024)
#define MALLOC_CHECK_PERIOD (1024 * 1024)
void *xmalloc(size_t sz) {
static size_t allocBytes = 0;
allocBytes += sz;
if (allocBytes >= MALLOC_CHECK_PERIOD) {
allocBytes = 0;
auto info = mallinfo();
DMESG("malloc used: %d kb", info.uordblks / 1024);
if (info.uordblks > MALLOC_LIMIT) {
target_panic(904);
}
}
auto r = malloc(sz);
if (r == NULL)
target_panic(905); // shouldn't happen
return r;
}
2017-07-04 16:11:54 +02:00
void *operator new(size_t size) {
return xmalloc(size);
2017-07-04 16:11:54 +02:00
}
void *operator new[](size_t size) {
return xmalloc(size);
2017-07-04 16:11:54 +02:00
}
void operator delete(void *p) {
free(p);
}
void operator delete[](void *p) {
free(p);
}
2017-07-03 13:57:34 +02:00
namespace pxt {
2017-07-02 13:32:17 +02:00
static int startTime;
2017-07-03 13:57:34 +02:00
static pthread_mutex_t execMutex;
2017-07-05 02:17:26 +02:00
static pthread_mutex_t eventMutex;
2017-07-03 13:57:34 +02:00
static pthread_cond_t newEventBroadcast;
2017-07-02 13:32:17 +02:00
struct Thread {
struct Thread *next;
Action act;
TValue arg0;
2017-07-03 13:57:34 +02:00
pthread_t pid;
pthread_cond_t waitCond;
2017-07-02 13:32:17 +02:00
int waitSource;
int waitValue;
2017-07-08 12:16:39 +02:00
TValue data0;
TValue data1;
2017-07-02 13:32:17 +02:00
};
static struct Thread *allThreads;
2017-07-03 13:57:34 +02:00
static struct Event *eventHead, *eventTail;
static int usbFD;
static int dmesgPtr;
2017-07-10 13:47:00 +02:00
static int dmesgSerialPtr;
static char dmesgBuf[4096];
2017-07-02 13:32:17 +02:00
struct Event {
2017-07-03 13:57:34 +02:00
struct Event *next;
2017-07-02 13:32:17 +02:00
int source;
int value;
};
2017-07-03 13:57:34 +02:00
Event lastEvent;
2017-07-02 13:32:17 +02:00
Event *mkEvent(int source, int value) {
auto res = new Event();
memset(res, 0, sizeof(Event));
res->source = source;
res->value = value;
return res;
}
#define USB_MAGIC 0x3d3f
#define USB_SERIAL 1
2017-07-09 11:19:14 +02:00
#define USB_RESTART 2
#define USB_DMESG 3
struct UsbPacket {
uint16_t size;
uint16_t msgcount;
uint16_t magic;
uint16_t code;
2017-07-09 11:19:14 +02:00
char buf[1024 - 8];
};
2017-07-09 11:19:14 +02:00
void *usbThread(void *) {
UsbPacket pkt;
UsbPacket resp;
while (true) {
int len = read(usbFD, &pkt, sizeof(pkt));
if (len <= 4) {
sleep_core_us(20000);
continue;
}
resp.msgcount = pkt.msgcount;
if (pkt.magic == USB_MAGIC) {
if (pkt.code == USB_RESTART) {
target_reset();
} else if (pkt.code == USB_DMESG) {
dumpDmesg();
}
/*
resp.magic = pkt.magic;
resp.code = pkt.code;
resp.size = 8;
write(usbFD, &resp, sizeof(resp));
*/
} else {
resp.magic = 0xffff;
resp.size = 4;
write(usbFD, &resp, sizeof(resp));
}
sleep_core_us(1000);
}
}
static void startUsb() {
usbFD = open("/dev/lms_usbdev", O_RDWR, 0666);
pthread_t pid;
pthread_create(&pid, NULL, usbThread, NULL);
pthread_detach(pid);
}
2017-07-09 11:19:14 +02:00
void sendUsb(uint16_t code, const char *data, int len) {
while (len > 0) {
int sz = len;
if (sz > 1000)
sz = 1000;
UsbPacket pkt = {(uint16_t)(6 + sz), 0, USB_MAGIC, code, {}};
memcpy(pkt.buf, data, sz);
write(usbFD, &pkt, sizeof(pkt));
len -= sz;
data += sz;
2017-07-05 18:53:22 +02:00
}
}
2017-07-05 18:53:22 +02:00
void sendSerial(const char *data, int len) {
sendUsb(USB_SERIAL, data, len);
2017-06-29 19:41:38 +02:00
}
volatile bool paniced;
extern "C" void drawPanic(int code);
2017-06-29 19:41:38 +02:00
extern "C" void target_panic(int error_code) {
char buf[50];
paniced = true;
pthread_mutex_trylock(&execMutex);
2017-06-29 19:41:38 +02:00
snprintf(buf, sizeof(buf), "\nPANIC %d\n", error_code);
drawPanic(error_code);
DMESG("PANIC %d", error_code);
for (int i = 0; i < 10; ++i) {
sendSerial(buf, strlen(buf));
sleep_core_us(500 * 1000);
}
target_reset();
2017-06-29 19:41:38 +02:00
}
2017-07-03 13:57:34 +02:00
void startUser() {
pthread_mutex_lock(&execMutex);
}
void stopUser() {
pthread_mutex_unlock(&execMutex);
}
2017-07-04 11:53:49 +02:00
void sleep_core_us(uint64_t us) {
2017-07-03 13:57:34 +02:00
struct timespec ts;
2017-07-04 11:53:49 +02:00
ts.tv_sec = us / 1000000;
ts.tv_nsec = (us % 1000000) * 1000;
2017-07-03 13:57:34 +02:00
while (nanosleep(&ts, &ts))
;
2017-07-03 14:02:03 +02:00
}
2017-07-03 13:57:34 +02:00
2017-07-03 14:02:03 +02:00
void sleep_ms(uint32_t ms) {
2017-07-04 11:53:49 +02:00
stopUser();
sleep_core_us(ms * 1000);
startUser();
2017-07-02 13:32:17 +02:00
}
2017-06-29 19:41:38 +02:00
void sleep_us(uint64_t us) {
2017-07-04 11:53:49 +02:00
if (us > 50000) {
2017-07-02 13:32:17 +02:00
sleep_ms(us / 1000);
}
2017-07-04 11:53:49 +02:00
sleep_core_us(us);
2017-06-29 19:41:38 +02:00
}
uint64_t currTime() {
struct timeval tv;
gettimeofday(&tv, NULL);
return tv.tv_sec * 1000 + tv.tv_usec / 1000;
}
int current_time_ms() {
return currTime() - startTime;
}
2017-07-02 13:32:17 +02:00
void disposeThread(Thread *t) {
if (allThreads == t) {
allThreads = t->next;
} else {
for (auto tt = allThreads; tt; tt = tt->next) {
if (tt->next == t) {
tt->next = t->next;
break;
}
}
}
decr(t->act);
2017-07-08 12:16:39 +02:00
decr(t->arg0);
decr(t->data0);
decr(t->data1);
2017-07-03 13:57:34 +02:00
pthread_cond_destroy(&t->waitCond);
2017-07-02 13:32:17 +02:00
delete t;
}
static void runAct(Thread *thr) {
2017-07-03 13:57:34 +02:00
startUser();
2017-07-02 13:32:17 +02:00
pxt::runAction1(thr->act, thr->arg0);
2017-07-03 13:57:34 +02:00
stopUser();
2017-07-02 13:32:17 +02:00
disposeThread(thr);
}
static void mainThread(Thread *) {}
2017-07-08 12:16:39 +02:00
void setupThread(Action a, TValue arg = 0, void (*runner)(Thread *) = NULL, TValue d0 = 0,
TValue d1 = 0) {
2017-07-03 13:57:34 +02:00
if (runner == NULL)
runner = runAct;
2017-07-02 13:32:17 +02:00
auto thr = new Thread();
memset(thr, 0, sizeof(Thread));
thr->next = allThreads;
allThreads = thr;
2017-07-08 12:16:39 +02:00
thr->act = incr(a);
thr->arg0 = incr(arg);
thr->data0 = incr(d0);
thr->data1 = incr(d1);
2017-07-03 13:57:34 +02:00
pthread_cond_init(&thr->waitCond, NULL);
if (runner == mainThread) {
thr->pid = pthread_self();
} else {
pthread_create(&thr->pid, NULL, (void *(*)(void *))runner, thr);
THREAD_DBG("setup thread: %p (pid %p)", thr, thr->pid);
pthread_detach(thr->pid);
}
2017-06-29 19:41:38 +02:00
}
2017-07-02 13:32:17 +02:00
2017-06-29 19:41:38 +02:00
void runInBackground(Action a) {
2017-07-02 13:32:17 +02:00
setupThread(a);
2017-06-29 19:41:38 +02:00
}
2017-07-02 13:32:17 +02:00
static void runFor(Thread *t) {
2017-07-03 13:57:34 +02:00
startUser();
2017-07-02 13:32:17 +02:00
while (true) {
pxt::runAction0(t->act);
sleep_ms(20);
}
}
2017-06-29 19:41:38 +02:00
void runForever(Action a) {
2017-07-02 13:32:17 +02:00
setupThread(a, 0, runFor);
2017-06-29 19:41:38 +02:00
}
2017-07-02 13:32:17 +02:00
2017-07-03 13:57:34 +02:00
void waitForEvent(int source, int value) {
THREAD_DBG("waitForEv: %d %d", source, value);
2017-07-03 13:57:34 +02:00
auto self = pthread_self();
for (auto t = allThreads; t; t = t->next) {
THREAD_DBG("t: %p", t);
2017-07-03 13:57:34 +02:00
if (t->pid == self) {
2017-07-05 02:17:26 +02:00
pthread_mutex_lock(&eventMutex);
2017-07-03 13:57:34 +02:00
t->waitSource = source;
t->waitValue = value;
stopUser();
2017-07-03 13:57:34 +02:00
// spourious wake ups may occur they say
while (t->waitSource) {
2017-07-05 02:17:26 +02:00
pthread_cond_wait(&t->waitCond, &eventMutex);
2017-07-03 13:57:34 +02:00
}
2017-07-05 02:17:26 +02:00
pthread_mutex_unlock(&eventMutex);
startUser();
2017-07-03 13:57:34 +02:00
return;
}
}
DMESG("current thread not registered!");
target_panic(901);
2017-06-29 19:41:38 +02:00
}
2017-07-02 13:32:17 +02:00
static void dispatchEvent(Event &e) {
2017-07-03 13:57:34 +02:00
lastEvent = e;
2017-07-02 13:32:17 +02:00
2017-07-04 16:11:54 +02:00
auto curr = findBinding(e.source, e.value);
2017-07-02 13:32:17 +02:00
if (curr)
2017-07-04 16:11:54 +02:00
setupThread(curr->action, fromInt(e.value));
2017-07-02 13:32:17 +02:00
2017-07-04 16:11:54 +02:00
curr = findBinding(e.source, DEVICE_EVT_ANY);
2017-07-02 13:32:17 +02:00
if (curr)
2017-07-04 16:11:54 +02:00
setupThread(curr->action, fromInt(e.value));
2017-07-02 13:32:17 +02:00
}
static void *evtDispatcher(void *dummy) {
2017-07-05 02:17:26 +02:00
pthread_mutex_lock(&eventMutex);
2017-07-02 13:32:17 +02:00
while (true) {
2017-07-05 02:17:26 +02:00
pthread_cond_wait(&newEventBroadcast, &eventMutex);
2017-07-03 13:57:34 +02:00
while (eventHead != NULL) {
if (paniced)
return 0;
2017-07-03 13:57:34 +02:00
Event *ev = eventHead;
eventHead = ev->next;
if (eventHead == NULL)
eventTail = NULL;
2017-07-02 13:32:17 +02:00
for (auto thr = allThreads; thr; thr = thr->next) {
if (paniced)
return 0;
2017-07-07 12:49:13 +02:00
if (thr->waitSource == 0)
continue;
if (thr->waitValue != ev->value && thr->waitValue != DEVICE_EVT_ANY)
continue;
if (thr->waitSource == ev->source) {
2017-07-02 13:32:17 +02:00
thr->waitSource = 0; // once!
2017-07-03 13:57:34 +02:00
pthread_cond_broadcast(&thr->waitCond);
2017-07-07 12:49:13 +02:00
} else if (thr->waitSource == DEVICE_ID_NOTIFY &&
ev->source == DEVICE_ID_NOTIFY_ONE) {
thr->waitSource = 0; // once!
pthread_cond_broadcast(&thr->waitCond);
break; // do not wake up any other threads
2017-07-02 13:32:17 +02:00
}
}
2017-07-03 13:57:34 +02:00
2017-07-02 13:32:17 +02:00
dispatchEvent(*ev);
delete ev;
}
}
}
2017-07-07 12:44:34 +02:00
int allocateNotifyEvent() {
static volatile int notifyId;
pthread_mutex_lock(&eventMutex);
int res = ++notifyId;
pthread_mutex_unlock(&eventMutex);
return res;
}
2017-07-02 13:32:17 +02:00
void raiseEvent(int id, int event) {
auto e = mkEvent(id, event);
2017-07-05 02:17:26 +02:00
pthread_mutex_lock(&eventMutex);
2017-07-03 13:57:34 +02:00
if (eventTail == NULL) {
if (eventHead != NULL)
target_panic(902);
2017-07-03 13:57:34 +02:00
eventHead = eventTail = e;
} else {
eventTail->next = e;
eventTail = e;
}
pthread_cond_broadcast(&newEventBroadcast);
2017-07-05 02:17:26 +02:00
pthread_mutex_unlock(&eventMutex);
2017-07-02 13:32:17 +02:00
}
2017-11-09 19:24:12 +01:00
void registerWithDal(int id, int event, Action a, int flags) {
// TODO support flags
2017-07-04 16:11:54 +02:00
setBinding(id, event, a);
2017-07-02 13:32:17 +02:00
}
2017-07-08 12:16:39 +02:00
static void runPoller(Thread *thr) {
Action query = thr->data0;
auto us = (uint64_t)toInt(thr->data1) * 1000;
// note that this is run without the user mutex held - it should not modify any state!
TValue prev = pxt::runAction0(query);
while (true) {
sleep_core_us(us);
if (paniced)
break;
2017-07-08 12:16:39 +02:00
TValue curr = pxt::runAction0(query);
if (curr != prev) {
startUser();
pxt::runAction2(thr->act, prev, curr);
stopUser();
if (paniced)
break;
2017-07-08 12:16:39 +02:00
decr(prev);
prev = curr;
}
}
// disposeThread(thr);
2017-07-08 12:16:39 +02:00
}
//%
void unsafePollForChanges(int ms, Action query, Action handler) {
setupThread(handler, 0, runPoller, query, fromInt(ms));
}
2017-06-29 19:41:38 +02:00
uint32_t afterProgramPage() {
return 0;
}
void dumpDmesg() {
2017-07-10 13:47:00 +02:00
auto len = dmesgPtr - dmesgSerialPtr;
if (len == 0)
return;
sendSerial(dmesgBuf + dmesgSerialPtr, len);
dmesgSerialPtr = dmesgPtr;
}
int lmsPid;
void stopLMS() {
struct dirent *ent;
DIR *dir;
dir = opendir("/proc");
if (dir == NULL)
return;
while ((ent = readdir(dir)) != NULL) {
int pid = atoi(ent->d_name);
if (!pid)
continue;
char namebuf[100];
snprintf(namebuf, 1000, "/proc/%d/cmdline", pid);
FILE *f = fopen(namebuf, "r");
if (f) {
fread(namebuf, 1, 99, f);
if (strcmp(namebuf, "./lms2012") == 0) {
lmsPid = pid;
}
fclose(f);
if (lmsPid)
break;
}
}
closedir(dir);
lmsPid = 0; // disable SIGSTOP for now - rethink if problems with I2C (runs on a thread)
if (lmsPid) {
DMESG("SIGSTOP to lmsPID=%d", lmsPid);
if (kill(lmsPid, SIGSTOP))
DMESG("SIGSTOP failed");
}
}
void runLMS() {
DMESG("re-starting LMS2012");
kill(lmsPid, SIGCONT);
sleep_core_us(200000);
exit(0);
/*
chdir("/home/root/lms2012/sys");
for (int fd = 3; fd < 9999; ++fd)
close(fd);
execl("lms2012", "./lms2012");
exit(100); // should not be reached
*/
}
extern "C" void target_reset() {
if (lmsPid)
runLMS();
else
exit(0);
2017-06-29 19:41:38 +02:00
}
2017-07-02 13:32:17 +02:00
void screen_init();
2017-07-02 13:32:17 +02:00
void initRuntime() {
// daemon(1, 1);
2017-07-02 13:32:17 +02:00
startTime = currTime();
2017-07-05 18:53:22 +02:00
DMESG("runtime starting...");
stopLMS();
2017-07-09 11:19:14 +02:00
startUsb();
2017-07-03 13:57:34 +02:00
pthread_t disp;
pthread_create(&disp, NULL, evtDispatcher, NULL);
pthread_detach(disp);
setupThread(0, 0, mainThread);
2017-07-05 19:35:05 +02:00
target_init();
screen_init();
2017-07-04 11:53:49 +02:00
startUser();
2017-07-02 13:32:17 +02:00
}
2017-07-05 18:53:22 +02:00
static FILE *dmesgFile;
2017-07-05 18:53:22 +02:00
void dmesgRaw(const char *buf, uint32_t len) {
2017-07-05 18:53:22 +02:00
if (!dmesgFile) {
dmesgFile = fopen("/tmp/dmesg.txt", "w");
if (!dmesgFile)
dmesgFile = stderr;
}
if (len > sizeof(dmesgBuf) / 2)
return;
if (dmesgPtr + len > sizeof(dmesgBuf)) {
dmesgPtr = 0;
2017-07-10 13:47:00 +02:00
dmesgSerialPtr = 0;
}
memcpy(dmesgBuf + dmesgPtr, buf, len);
dmesgPtr += len;
fwrite(buf, 1, len, dmesgFile);
}
void dmesg(const char *format, ...) {
char buf[500];
snprintf(buf, sizeof(buf), "[%8d] ", current_time_ms());
dmesgRaw(buf, strlen(buf));
2017-07-05 18:53:22 +02:00
va_list arg;
va_start(arg, format);
vsnprintf(buf, sizeof(buf), format, arg);
va_end(arg);
dmesgRaw(buf, strlen(buf));
dmesgRaw("\n", 1);
2017-07-05 18:53:22 +02:00
fflush(dmesgFile);
fdatasync(fileno(dmesgFile));
}
} // namespace pxt