void
if_loop_input_task (intptr_t exinf)
{
T_NET_BUF *input;
ID tskid;
get_tid(&tskid);
syslog(LOG_NOTICE, "[LOOP INPUT:%d] started.", tskid);
while (true) {
if (rcv_dtq(DTQ_LOOP_INPUT, (intptr_t)&input) == E_OK) {
NET_COUNT_LOOP(net_count_loop.in_octets, input->len);
NET_COUNT_LOOP(net_count_loop.in_packets, 1);
#if defined(SUPPORT_INET4)
/* IPv4 入力関数を呼び出す */
if (IP4_VHL_V(GET_IP4_HDR(input)->vhl) == IPV4_VERSION)
ip_input(input);
#endif /* of #if defined(SUPPORT_INET4) */
#if defined(SUPPORT_INET6)
/* IPv6 入力関数を呼び出す */
if (IP6_VCF_V(ntohl(GET_IP6_HDR(input)->vcf)) == IPV6_VERSION)
ip6_input(input);
#endif /* of #if defined(SUPPORT_INET6) */
}
}
}
void RTOS::rcvMessage(void** rv_message, uint16 rv_mbx_id){
// 非同期メッセージ受信.
// OS のデータキューを利用.
ER at_err = rcv_dtq(rv_mbx_id, reinterpret_cast<intptr_t *>(rv_message));
if(at_err != E_OK) {
// 受信エラー発生時.
}
else {
// Don't care...
}
}
void
if_loop_output_task (intptr_t exinf)
{
T_NET_BUF *output;
ER error;
ID tskid;
get_tid(&tskid);
syslog(LOG_NOTICE, "[LOOP OUTPUT:%d] started.", tskid);
while (true) {
if (rcv_dtq(DTQ_LOOP_OUTPUT, (intptr_t*)&output) == E_OK) {
NET_COUNT_LOOP(net_count_loop.in_octets, output->len);
NET_COUNT_LOOP(net_count_loop.in_packets, 1);
if ((error = snd_dtq(DTQ_LOOP_INPUT, output)) != E_OK)
syslog(LOG_NOTICE, "[LOOP OUTPUT] drop error: %s", itron_strerror(error));
}
}
}
void
ppp_output_task(intptr_t exinf)
{
T_NET_BUF *output;
ID tskid;
get_tid(&tskid);
syslog(LOG_NOTICE, "[PPP OUTPUT:%d] started.", tskid);
while (true) {
while (rcv_dtq(DTQ_PPP_OUTPUT, (intptr_t*)&output) == E_OK) {
NET_COUNT_PPP(net_count_ppp.out_octets, output->len);
NET_COUNT_PPP(net_count_ppp.out_packets, 1);
syscall(HDLC_write(output));
syscall(rel_net_buf(output));
}
}
}
/*---------------------------------------------------------------------------*
* Routine: sys_arch_mbox_fetch
*---------------------------------------------------------------------------*
* Description:
* Blocks the thread until a message arrives in the mailbox, but does
* not block the thread longer than "timeout" milliseconds (similar to
* the sys_arch_sem_wait() function). The "msg" argument is a result
* parameter that is set by the function (i.e., by doing "*msg =
* ptr"). The "msg" parameter maybe NULL to indicate that the message
* should be dropped.
*
* The return values are the same as for the sys_arch_sem_wait() function:
* Number of milliseconds spent waiting or SYS_ARCH_TIMEOUT if there was a
* timeout.
*
* Note that a function with a similar name, sys_mbox_fetch(), is
* implemented by lwIP.
* Inputs:
* sys_mbox_t mbox -- Handle of mailbox
* void **msg -- Pointer to pointer to msg received
* u32_t timeout -- Number of milliseconds until timeout
* Outputs:
* u32_t -- SYS_ARCH_TIMEOUT if timeout, else number
* of milliseconds until received.
*---------------------------------------------------------------------------*/
u32_t sys_arch_mbox_fetch(sys_mbox_t *mbox, void **msg, u32_t timeout) {
u32_t start = us_ticker_read();
// osEvent event = osMessageGet(mbox->id, (timeout != 0)?(timeout):(osWaitForever));
//if (event.status != osEventMessage)
if (timeout != 0) {
if (trcv_dtq(mbox->id, (intptr_t *)msg, timeout) == E_TMOUT) {
return SYS_ARCH_TIMEOUT;
}
} else {
// if timeout == 0 then wait forever.
ER ercd = rcv_dtq(mbox->id, (intptr_t *)msg);
if (ercd != E_OK) {
error("rcv_dtq returned with error= %d\n", ercd);
}
}
// *msg = (void *)event.value.v;
return (us_ticker_read() - start) / 1000;
}
void task_led(intptr_t exinf)
{
uint16_t msg;
led_init();
while(1)
{
if (rcv_dtq(DTQ_LED, (intptr_t *)&msg) == E_OK) {
switch (MSG_TARGET(msg)) {
case DBLED0:
led_debug_write(0, MSG_CONTROL(msg));
break;
case DBLED1:
led_debug_write(1, MSG_CONTROL(msg));
break;
case DBLED2:
led_debug_write(2, MSG_CONTROL(msg));
break;
case DBLED3:
led_debug_write(3, MSG_CONTROL(msg));
break;
case SWLED0:
led_switch_write(0, MSG_CONTROL(msg));
break;
case SWLED1:
led_switch_write(1, MSG_CONTROL(msg));
break;
case SWLED2:
led_switch_write(2, MSG_CONTROL(msg));
break;
case SWLED3:
led_switch_write(3, MSG_CONTROL(msg));
break;
}
}
tslp_tsk(10);
}
}
void
udp_echo_cli_task (intptr_t exinf)
{
static char msg[sizeof(MESSAGE_FORMAT)] = MESSAGE_FORMAT;
T_IN_ADDR addr;
ID tskid, cepid;
ER error = E_OK;
char *line, *p;
int_t rep;
uint32_t count, msgno;
uint16_t portno;
#ifdef USE_UDP_EXTENTIONS
T_UDP_CCEP ccep;
#endif /* of #ifdef USE_UDP_EXTENTIONS */
get_tid(&tskid);
syslog(LOG_NOTICE, "[UDP ECHO CLI:%d,%d] started.", tskid, (ID)exinf);
while (true) {
if (rcv_dtq(DTQ_UDP_ECHO_CLI, (intptr_t*)&line) == E_OK) {
#ifdef USE_UDP_EXTENTIONS
ccep.cepatr = UINT_C(0);
ccep.myaddr.portno = UDP_PORTANY;
#if defined(SUPPORT_INET4)
ccep.myaddr.ipaddr = IPV4_ADDRANY;
#endif
#if defined(SUPPORT_INET6)
memcpy(&ccep.myaddr.ipaddr, &ipv6_addrany, sizeof(T_IN6_ADDR));
#endif
#ifdef USE_UDP_NON_BLOCKING
ccep.callback = (FP)callback_nblk_udp_echo_cli;
#else
ccep.callback = NULL;
#endif
if ((error = alloc_udp_cep(&cepid, tskid, &ccep)) != E_OK) {
syslog(LOG_NOTICE, "[UEC:%02d TSK] CEP create error: %s", cepid, itron_strerror(error));
continue;
}
#else /* of #ifdef USE_UDP_EXTENTIONS */
cepid = (ID)exinf;
#endif /* of #ifdef USE_UDP_EXTENTIONS */
line = skip_blanks(GET_IPADDR(&addr, skip_blanks(line))); /* IP Address */
if ('0' <= *line && *line <= '9') { /* Port No */
line = get_int(&rep, line);
portno = (uint16_t)rep;
}
else {
line ++;
portno = ECHO_SRV_PORTNO;
}
line = skip_blanks(line);
if ('0' <= *line && *line <= '9') { /* Repeat */
line = get_int(&rep, line);
if (rep > 1000000)
rep = 1000000;
udp_echo_cli_valid = true;
for (count = 0; count ++ < rep; ) {
if (!udp_echo_cli_valid) {
syslog(LOG_NOTICE, "[UEC:%02d TSK] canceled.", cepid);
break;
}
p = &msg[sizeof(MESSAGE_FORMAT)] - 1;
for (msgno = count; msgno > 0; msgno /= 10)
*(-- p) = msgno % 10 + '0';
while (*(p - 2) != 'G')
*(-- p) = ' ';
if ((error = send_udp_echo(cepid, &addr, portno, msg)) != E_OK)
break;
dly_tsk(10 * SYSTIM_HZ + net_rand() % (10 * SYSTIM_HZ));
}
}
else /* Single Message */
error = send_udp_echo(cepid, &addr, portno, line);
if (error != E_OK)
syslog(LOG_NOTICE, "[UEC:%02d TSK] error: %s", cepid, itron_strerror(error));
#ifdef USE_UDP_EXTENTIONS
if ((error = free_udp_cep(cepid, !(error == E_NOEXS || error == E_DLT))) != E_OK)
syslog(LOG_NOTICE, "[UEC:%02d TSK] CEP delete error: %s", cepid, itron_strerror(error));
#endif /* of #ifdef USE_UDP_EXTENTIONS */
}
}
}
void main_task(intptr_t unused) {
ev3_lcd_set_font(EV3_FONT_MEDIUM);
ev3_font_get_size(EV3_FONT_MEDIUM, &fontw, &fonth);
memfile_t memfile;
ev3_memfile_load("/test.bmp", &memfile);
image_t image;
ev3_image_load(&memfile, &image);
ev3_lcd_draw_image(&image, 0, fonth * 2);
//ev3_sta_cyc(TEST_EV3_CYC1);
// Enable TEST_EV3_CYC2 for 5 seconds
ev3_sta_cyc(TEST_EV3_CYC2);
tslp_tsk(5000);
ev3_stp_cyc(TEST_EV3_CYC2);
syslog(LOG_NOTICE, "TEST DTQ");
intptr_t data = 0xdeadbeef;
assert(snd_dtq(DTQ1, data) == E_OK);
data = 0;
assert(rcv_dtq(DTQ1, &data) == E_OK);
assert(data == 0xdeadbeef);
syslog(LOG_NOTICE, "TEST PDQ");
data = 0xdeadbeef;
assert(snd_pdq(PDQ1, data, 1) == E_OK);
data = 0xdeadbee2;
assert(snd_pdq(PDQ1, data, 2) == E_OK);
PRI datapri;
assert(rcv_pdq(PDQ1, &data, &datapri) == E_OK);
assert(data == 0xdeadbeef && datapri == 1);
assert(rcv_pdq(PDQ1, &data, &datapri) == E_OK);
assert(data == 0xdeadbee2 && datapri == 2);
syslog(LOG_NOTICE, "TEST MTX");
assert(loc_mtx(MTX1) == E_OK);
assert(unl_mtx(MTX1) == E_OK);
syslog(LOG_NOTICE, "TEST DONE");
#if 0
// Register button handlers
ev3_button_set_on_clicked(BACK_BUTTON, button_clicked_handler, BACK_BUTTON);
ev3_button_set_on_clicked(ENTER_BUTTON, button_clicked_handler, ENTER_BUTTON);
ev3_button_set_on_clicked(LEFT_BUTTON, button_clicked_handler, LEFT_BUTTON);
// Configure sensors
ev3_sensor_config(gyro_sensor, GYRO_SENSOR);
// Configure motors
ev3_motor_config(left_motor, LARGE_MOTOR);
ev3_motor_config(right_motor, LARGE_MOTOR);
// Start task for self-balancing
act_tsk(BALANCE_TASK);
// Open Bluetooth file
bt = ev3_serial_open_file(EV3_SERIAL_BT);
assert(bt != NULL);
// Start task for printing message while idle
act_tsk(IDLE_TASK);
while(1) {
uint8_t c = fgetc(bt);
sus_tsk(IDLE_TASK);
switch(c) {
case 'w':
if(motor_control_drive < 0)
motor_control_drive = 0;
else
motor_control_drive += 10;
fprintf(bt, "motor_control_drive: %d\n", motor_control_drive);
break;
case 's':
if(motor_control_drive > 0)
motor_control_drive = 0;
else
motor_control_drive -= 10;
fprintf(bt, "motor_control_drive: %d\n", motor_control_drive);
break;
case 'a':
if(motor_control_steer < 0)
motor_control_steer = 0;
else
motor_control_steer += 10;
fprintf(bt, "motor_control_steer: %d\n", motor_control_steer);
break;
case 'd':
if(motor_control_steer > 0)
motor_control_steer = 0;
else
motor_control_steer -= 10;
fprintf(bt, "motor_control_steer: %d\n", motor_control_steer);
break;
case 'h':
fprintf(bt, "==========================\n");
fprintf(bt, "Usage:\n");
fprintf(bt, "Press 'w' to speed up\n");
fprintf(bt, "Press 's' to speed down\n");
fprintf(bt, "Press 'a' to turn left\n");
fprintf(bt, "Press 'd' to turn right\n");
fprintf(bt, "Press 'i' for idle task\n");
fprintf(bt, "Press 'h' for this message\n");
fprintf(bt, "==========================\n");
break;
case 'i':
fprintf(bt, "Idle task started.\n");
rsm_tsk(IDLE_TASK);
break;
default:
fprintf(bt, "Unknown key '%c' pressed.\n", c);
}
}
#endif
}
void
tcp_echo_srv_snd_task(intptr_t exinf)
{
ID tskid, cepid;
ER error;
ER_UINT slen;
SYSTIM now;
uint16_t blen, last;
char head, tail;
get_tid(&tskid);
syslog(LOG_NOTICE, "[TCP ECHO SRV SND:%d] started.", tskid);
while (true) {
if ((error = rcv_dtq(DTQ_TCP_ECHO_SRV_SND, (intptr_t*)&cepid)) != E_OK) {
syslog(LOG_NOTICE, "[TES:%02d SND] sync error: %s",
cepid, itron_strerror(error));
continue;
}
while (true) {
/* バッファに文字が入るまで待つ。*/
syscall(wai_sem(SEM_TCP_ECHO_SRV_SND_READY));
/* 受信完了で、バッファに文字がなければ終了する。*/
if (!ena_rcv && chars == 0)
break;
/* 送信サイズを計算する。*/
syscall(wai_sem(SEM_TCP_ECHO_SRV_LOCK));
if (rptr > sptr)
blen = rptr - sptr;
else
blen = sizeof(buffer) - (sptr - buffer);
syscall(sig_sem(SEM_TCP_ECHO_SRV_LOCK));
if ((slen = tcp_snd_dat(cepid, sptr, blen, TMO_FEVR)) <= 0) {
syslog(LOG_NOTICE, "[TES:%02d SND] send error = %s", cepid, itron_strerror(slen));
break;
}
head = *sptr;
tail = *(sptr + slen - 1);
#ifdef SHOW_RCV_RANGE
syslog(LOG_NOTICE, "[TES:%02d SND] len: %4d, data %02x -> %02x",
cepid, (uint16_t)slen, head, tail);
#endif /* of #ifdef SHOW_RCV_RANGE */
snd_count ++;
snd_total += slen;
syscall(wai_sem(SEM_TCP_ECHO_SRV_LOCK));
/* 送信ポインタを進める。*/
if (sptr - buffer + slen >= sizeof(buffer))
sptr -= sizeof(buffer) - slen;
else
sptr += slen;
/* バッファ内の文字数を減らす。*/
last = chars;
chars -= (uint16_t)slen;
/* 受信完了で、バッファに文字がなければ終了する。*/
if (!ena_rcv && chars == 0) {
syscall(sig_sem(SEM_TCP_ECHO_SRV_LOCK));
break;
}
/* バッファ内に文字があれば、送信タスクを起床する。*/
if (chars > 0)
sig_sem(SEM_TCP_ECHO_SRV_SND_READY);
/* 受信可能で、バッファ内に空きができれば、受信タスクを起床する。*/
if (ena_rcv && last == sizeof(buffer) && sizeof(buffer) - chars > 0)
sig_sem(SEM_TCP_ECHO_SRV_RCV_READY);
syscall(sig_sem(SEM_TCP_ECHO_SRV_LOCK));
}
/* 受信バッファに空ができるを待っている受信タスクを起床する。*/
if (chars == sizeof(buffer))
sig_sem(SEM_TCP_ECHO_SRV_SND_READY);
#ifdef USE_TCP_SHT_CEP
if ((error = tcp_sht_cep(cepid)) != E_OK)
syslog(LOG_NOTICE, "[TES:%02d SND] shtudown error = %s", cepid, itron_strerror(error));
#endif /* of #ifdef USE_TCP_SHT_CEP */
if ((error = tcp_cls_cep(cepid, TMO_FEVR)) != E_OK)
syslog(LOG_NOTICE, "[TES:%02d SND] close error = %s", cepid, itron_strerror(error));
get_tim(&now);
syslog(LOG_NOTICE, "[TES:%02d SND] finished: %6ld, snd: %4d, rcv: %4d, len: %ld",
cepid, now / SYSTIM_HZ, snd_count, rcv_count, snd_total);
sig_sem(SEM_TCP_ECHO_SRV_CLS_READY);
pol_sem(SEM_TCP_ECHO_SRV_SND_READY);
pol_sem(SEM_TCP_ECHO_SRV_RCV_READY);
}
}
void
tcp_echo_srv_snd_task(intptr_t exinf)
{
ER error;
ID tskid, cepid;
uint16_t blen;
get_tid(&tskid);
syslog(LOG_NOTICE, "[TCP ECHO SRV (NBLK) SND:%d] started.", tskid);
while (true) {
if ((error = rcv_dtq(DTQ_TCP_ECHO_SRV_SND, (intptr_t*)&cepid)) != E_OK) {
syslog(LOG_NOTICE, "[TES:%02d SND] sync error: %s",
cepid, itron_strerror(error));
continue;
}
while (true) {
/* バッファに文字が入るまで待つ。*/
syscall(wai_sem(SEM_TCP_ECHO_SRV_SND_READY));
/* 待っている間にエラーになったら終了する。*/
if (snd_error <= 0)
break;
/* 受信完了で、バッファに文字がなければ終了する。*/
if (!ena_rcv && chars == 0)
break;
/* 送信サイズを計算する。*/
syscall(wai_sem(SEM_TCP_ECHO_SRV_LOCK));
if (rptr > sptr)
blen = rptr - sptr;
else
blen = sizeof(buffer) - (sptr - buffer);
syscall(sig_sem(SEM_TCP_ECHO_SRV_LOCK));
/* 送信する。*/
if ((error = tcp_snd_dat(cepid, sptr, blen, TMO_NBLK)) != E_WBLK) {
syslog(LOG_NOTICE, "[TES:%02d SND] send error = %s", cepid, itron_strerror(error));
break;
}
}
/* 受信バッファに空ができるを待っている受信タスクを起床する。*/
if (chars == sizeof(buffer))
sig_sem(SEM_TCP_ECHO_SRV_SND_READY);
#ifdef USE_TCP_SHT_CEP
if ((error = tcp_sht_cep(cepid)) != E_OK)
syslog(LOG_NOTICE, "[TES:%02d SND] shutdown error = %s", cepid, itron_strerror(error));
#endif /* of #ifdef USE_TCP_SHT_CEP */
if ((error = tcp_cls_cep(cepid, TMO_NBLK)) != E_WBLK) {
syslog(LOG_NOTICE, "[TES:%02d SND] close error = %s", cepid, itron_strerror(error));
}
pol_sem(SEM_TCP_ECHO_SRV_SND_READY);
pol_sem(SEM_TCP_ECHO_SRV_RCV_READY);
}
}
