void __attribute__((noreturn)) supervisor_task(void)
{
struct trace_entry trace;
uint32_t clk;
INF("<%d> started...", thinkos_thread_self());
trace_tail(&trace);
clk = thinkos_clock();
for (;;) {
struct timeval tv;
char s[80];
/* 8Hz periodic task */
clk += 125;
thinkos_alarm(clk);
while (trace_getnext(&trace, s, sizeof(s)) >= 0) {
trace_ts2timeval(&tv, trace.dt);
printf("%s %2d.%06d: %s\n", trace_lvl_tab[trace.ref->lvl],
(int)tv.tv_sec, (int)tv.tv_usec, s);
}
trace_flush(&trace);
}
}
void supervisor_init(void)
{
static uint32_t supervisor_stack[256];
static const struct thinkos_thread_inf supervisor_inf = {
.stack_ptr = supervisor_stack,
.stack_size = sizeof(supervisor_stack),
.priority = 8,
.thread_id = 2,
.paused = false,
.tag = "SUPV"
};
trace_init();
thinkos_thread_create_inf((void *)supervisor_task, (void *)NULL,
&supervisor_inf);
}
/* -------------------------------------------------------------------------
* MS/TP
* ------------------------------------------------------------------------- */
int mstp_lnk_task(void * arg)
{
struct mstp_lnk * mstp = (struct mstp_lnk *)arg;
printf("MS/TP link task started...\n");
mstp_lnk_loop(mstp);
return 0;
}
struct net_stats {
struct {
unsigned int octet_cnt;
unsigned int bcast_cnt;
unsigned int unicast_cnt;
} rx;
struct {
unsigned int octet_cnt;
unsigned int bcast_cnt;
unsigned int unicast_cnt;
} tx;
};
struct {
struct mstp_lnk * mstp;
struct net_stats stats;
} net;
int net_recv_task(void * arg)
{
struct mstp_lnk * mstp = (struct mstp_lnk *)arg;
struct mstp_frame_inf inf;
uint8_t pdu[502];
int len;
unsigned int seconds = 10;
uint32_t clk;
INF("MS/TP receive task started...");
clk = thinkos_clock() + seconds * 1000;
for (;;) {
len = mstp_lnk_recv(mstp, pdu, sizeof(pdu), &inf);
net.stats.rx.octet_cnt += len;
if (inf.daddr == MSTP_ADDR_BCAST)
net.stats.rx.bcast_cnt++;
else
net.stats.rx.unicast_cnt++;
// printf("RCV: %d->%d (%d) \"%s\"\n", inf.saddr, inf.daddr,
// len, (char*)pdu);
// DBG("RCV: %d->%d (%d)", inf.saddr, inf.daddr, len);
/* Log summary each 5 seconds */
if ((int32_t)(clk - thinkos_clock()) <= 0) {
unsigned int rate;
rate = net.stats.rx.octet_cnt / seconds;
INF("Receive: %d bcast, %d unicast, %d octets, %d.%03d KBps.",
net.stats.rx.bcast_cnt, net.stats.rx.unicast_cnt, net.stats.rx.octet_cnt,
rate / 1000, rate % 1000);
net.stats.rx.bcast_cnt = 0;
net.stats.rx.unicast_cnt = 0;
net.stats.rx.octet_cnt = 0;
seconds = 10;
clk += seconds * 1000;
}
}
return 0;
}
uint32_t mstp_lnk_stack[512] __attribute__((section (".ccm")));
const struct thinkos_thread_inf mstp_lnk_inf = {
.stack_ptr = mstp_lnk_stack,
.stack_size = sizeof(mstp_lnk_stack),
.priority = 1,
.thread_id = 1,
.paused = 0,
.tag = "MS/TP"
};
uint32_t net_recv_stack[512];
const struct thinkos_thread_inf net_recv_inf = {
.stack_ptr = net_recv_stack,
.stack_size = sizeof(net_recv_stack),
.priority = 32,
.thread_id = 3,
.paused = 0,
.tag = "NET RCV"
};
struct mstp_lnk * mstp_start(int addr)
{
struct serial_dev * ser;
struct mstp_lnk * mstp;
INF("1. serial port init");
ser = stm32f_uart1_serial_dma_init(500000, SERIAL_8N1);
INF("2. mstp_lnk_alloc()");
mstp = mstp_lnk_alloc();
INF("3. MS/TP link addr: %d", addr);
mstp_lnk_init(mstp, "MS/TP1", addr, ser);
INF("4. thinkos_thread_create_inf()");
thinkos_thread_create_inf(mstp_lnk_task, mstp, &mstp_lnk_inf);
thinkos_thread_create_inf(net_recv_task, mstp, &net_recv_inf);
thinkos_sleep(100);
printf("5. mstp_lnk_resume()");
mstp_lnk_resume(mstp);
return mstp;
}
/* -------------------------------------------------------------------------
* Test
* ------------------------------------------------------------------------- */
int test_mode = 0;
int mstp_test_task(void * arg)
{
struct mstp_lnk * mstp = (struct mstp_lnk *)arg;
struct mstp_frame_inf inf;
uint8_t pdu[502];
uint32_t clk;
uint32_t tmo;
unsigned int itval = 5;
int len;
int i;
INF("MS/TP test task started...\n");
clk = thinkos_clock();
tmo = clk + itval * 1000;
for (i = 0;; ++i) {
if (test_mode != 0) {
inf.daddr = MSTP_ADDR_BCAST;
inf.type = FRM_DATA_NO_REPLY;
len = snprintf((char *)pdu, 501, "%6d"
" The quick brown fox jumps over the lazy dog."
" The quick brown fox jumps over the lazy dog."
" The quick brown fox jumps over the lazy dog."
" The quick brown fox jumps over the lazy dog."
" The quick brown fox jumps over the lazy dog."
" The quick brown fox jumps over the lazy dog."
" The quick brown fox jumps over the lazy dog."
" The quick brown fox jumps over the lazy dog."
" The quick brown fox jumps over the lazy dog."
" The quick brown fox jumps over the lazy dog."
" The quick brown fox jumps over the lazy dog.",
i);
if (mstp_lnk_send(mstp, pdu, len, &inf) < 0) {
ERR("mstp_lnk_send() failed!");
}
net.stats.tx.octet_cnt += len;
net.stats.tx.bcast_cnt++;
/* Log summary each 10 seconds */
if ((int32_t)(tmo - clk) <= 0) {
unsigned int rate;
rate = net.stats.tx.octet_cnt / itval;
DBG("Send: %d bcasts, %d octets, %d.%03d KBps.",
net.stats.tx.bcast_cnt, net.stats.tx.octet_cnt,
rate / 1000, rate % 1000);
net.stats.tx.octet_cnt = 0;
net.stats.tx.bcast_cnt = 0;
itval = 10;
tmo += itval * 1000;
}
if (test_mode > 1) {
thinkos_alarm(clk + test_mode * 100);
}
} else {
thinkos_alarm(clk + 100);
}
clk = thinkos_clock();
}
return 0;
}
uint32_t test_stack[512];
const struct thinkos_thread_inf test_inf = {
.stack_ptr = test_stack,
.stack_size = sizeof(test_stack),
.priority = 32,
.thread_id = 32,
.paused = 0,
.tag = "TEST"
};
void mstp_test_start(struct mstp_lnk * mstp)
{
thinkos_thread_create_inf(mstp_test_task, mstp, &test_inf);
}
struct board_cfg {
uint32_t magic;
uint32_t mstp_addr;
uint32_t ip_addr;
};
#define CFG_MAGIC 0x01020304
#define CFG_ADDR 0x0800ff00
void show_netmap(struct mstp_lnk * mstp)
{
uint8_t map[16];
unsigned int cnt;
int i;
cnt = mstp_lnk_getnetmap(mstp, map, sizeof(map));
printf("\n---- Network Map ----\n");
for (i = 0; i < cnt; ++i) {
printf("%2d - Node %2d\n", i, map[i]);
}
printf("\n");
}
void show_menu(void)
{
printf("\n===== MS/TP test =====\n");
printf("[s] stop sending\n");
printf("[1..9] set sending mode\n");
printf("[n] show network map\n");
printf("\n");
}
void motd(void)
{
printf("\n--------- Test Application -------\n");
printf("\n");
}
int cmd_ping(FILE * f, int argc, char ** argv)
{
struct raw_pcb * raw;
uint8_t buf[BUF_LEN];
in_addr_t ip_addr;
struct sockaddr_in sin;
struct iphdr * ip;
struct icmphdr * icmp;
uint8_t * data;
int iphdrlen;
int datalen;
char s[16];
int len;
int id;
int seq;
int32_t dt;
uint32_t ts;
uint32_t now;
int i;
int n;
int ret = 0;
if (argc < 2) {
fprintf(f, msg_ping_usage);
return SHELL_ERR_ARG_MISSING;
}
if (argc > 3) {
fprintf(f, msg_ping_usage);
return SHELL_ERR_EXTRA_ARGS;
}
if (strcmp(argv[1], "help") == 0) {
fprintf(f, "ping - send ICMP ECHO_REQUEST to network host\n");
fprintf(f, msg_ping_usage);
return 0;
}
if (inet_aton(argv[1], (struct in_addr *)&ip_addr) == 0) {
fprintf(f, "ip address invalid.\n");
return SHELL_ERR_ARG_INVALID;
}
if (argc > 2) {
n = strtol(argv[2], NULL, 0);
} else {
n = 5;
}
raw = raw_pcb_new(IPPROTO_ICMP);
id = thinkos_thread_self();
datalen = DATA_LEN;
fprintf(f, "PING %s: %d octets data.\n",
inet_ntop(AF_INET, (void *)&ip_addr, s, 16), datalen);
for (seq = 1; seq <= n; seq++) {
icmp = (struct icmphdr *)(void *)buf;
icmp->type = ICMP_ECHO;
icmp->un.echo.id = id;
icmp->un.echo.sequence = seq;
data = buf + sizeof(struct icmphdr);
for (i = 0; i < datalen; i++) {
data[i] = i;
}
len = datalen + sizeof(struct icmphdr);
sin.sin_addr.s_addr = ip_addr;
sin.sin_family = AF_INET;
ts = thinkos_clock();
// gettimeofday(&tv, NULL);
memcpy(data, &ts, sizeof(uint32_t));
icmp->chksum = 0;
icmp->chksum = ~in_chksum(0, icmp, len);
raw_sendto(raw, buf, len, (struct sockaddr_in *)&sin);
len = raw_recvfrom_tmo(raw, buf, BUF_LEN,
(struct sockaddr_in *)&sin, 1000);
if (len < 0) {
if (len != -ETIMEDOUT) {
ret = -1;
break;
}
fprintf(f, "timed out.\n");
continue;
}
now = thinkos_clock();
ip = (struct iphdr *)buf;
iphdrlen = ip->hlen * 4;
icmp = (struct icmphdr *)(buf + iphdrlen);
if ((icmp->type == ICMP_ECHOREPLY) &&
(icmp->un.echo.id == id)) {
memcpy(&ts, buf + iphdrlen + sizeof(struct icmphdr),
sizeof(uint32_t));
len -= iphdrlen + sizeof(struct icmphdr);
dt = (int32_t)(now - ts);
fprintf(f, "%d octets from %s: icmp_seq=%d "
"ttl=%d time=%d ms\n",
len, inet_ntop(AF_INET, (void *)&sin.sin_addr, s, 16),
icmp->un.echo.sequence, ip->ttl, dt);
} else {
fprintf(f, "icmp: %d\n", icmp->type);
}
thinkos_sleep(250);
}
raw_close(raw);
return ret;
}
void __attribute__((noreturn)) supervisor_task(void)
{
struct trace_entry trace;
uint32_t clk;
uint32_t dump_tmo;
int n;
INF("<%d> started...", thinkos_thread_self());
/* set the supervisor stream to stdout */
trace_file = stdout;
/* enable auto flush */
trace_auto_flush = true;
trace_tail(&trace);
#if 0
struct sockaddr_in sin;
struct udp_pcb * udp;
bool udp_enabled;
if ((udp = udp_alloc()) == NULL) {
abort();
}
if (!inet_aton(trace_host, &sin.sin_addr)) {
abort();
}
sin.sin_port = htons(trace_port);
if (udp_bind(udp, INADDR_ANY, htons(10)) < 0) {
abort();
}
// if (udp_connect(udp, sin.sin_addr.s_addr, htons(sin.sin_port)) < 0) {
// abort();
// }
#endif
clk = thinkos_clock();
dump_tmo = clk + 15000;
for (;;) {
char msg[80];
/* 8Hz periodic task */
clk += 125;
thinkos_alarm(clk);
// udp_enabled = trace_udp_enabled;
while (trace_getnext(&trace, msg, sizeof(msg)) >= 0) {
struct timeval tv;
char s[128];
trace_ts2timeval(&tv, trace.dt);
if (trace.ref->lvl <= TRACE_LVL_WARN)
n = sprintf(s, "%s %2d.%06d: %s,%d: %s\n",
trace_lvl_nm[trace.ref->lvl],
(int)tv.tv_sec, (int)tv.tv_usec,
trace.ref->func, trace.ref->line, msg);
else
n = sprintf(s, "%s %2d.%06d: %s\n",
trace_lvl_nm[trace.ref->lvl],
(int)tv.tv_sec, (int)tv.tv_usec, msg);
/* write log to local console */
fwrite(s, n, 1, trace_file);
#if 0
if (udp_enabled) {
/* sent log to remote station */
if (udp_sendto(udp, s, n, &sin) < 0) {
udp_enabled = false;
}
}
#endif
}
if (trace_auto_flush)
trace_flush(&trace);
if ((int32_t)(clk - dump_tmo) >= 0) {
const struct thinkos_thread_inf * infbuf[33];
uint32_t cycbuf[33];
uint32_t cycsum = 0;
uint32_t cycbusy;
uint32_t cycidle;
uint32_t cycdiv;
uint32_t idle;
uint32_t busy;
int n;
int i;
thinkos_critical_enter();
thinkos_thread_inf(infbuf);
n = thinkos_cyccnt(cycbuf);
thinkos_critical_exit();
cycsum = 0;
for (i = 0; i < n; ++i)
cycsum += cycbuf[i];
cycidle = cycbuf[n - 1]; /* The last item is IDLE */
cycbusy = cycsum - cycidle;
cycdiv = (cycsum + 5000) / 10000;
busy = (cycbusy + (cycdiv / 2)) / cycdiv;
idle = 1000 - busy;
printf("CPU usage: %d.%02d%% busy, %d.%02d%% idle\n",
busy / 100, busy % 100, idle / 100, idle % 100);
for (i = 0; i < n; ++i) {
const struct thinkos_thread_inf * inf;
if (((inf = infbuf[i]) != NULL) && (cycbuf[i] != 0)) {
uint32_t usage;
usage = (cycbuf[i] + cycdiv / 2) / cycdiv;
printf("%2d %7s %3d.%02d%%\n", i, inf->tag,
usage / 100, usage % 100);
}
}
dump_tmo = clk + 15000;
}
}
}
