static int module_do_callback_(const struct cbmod_st* cbmod, struct jbxm_callback_if_st* cbif)
{
FUNC_ENTER
JBX_ASSERT(cbmod);
struct type2cb_st t2cb = { 0 };
syserr = type2cb(cbif->type, cbmod->loadlib_ref->callbacks, &t2cb);
if (syserr)
{
FIRE("type2cb");
}
if (! t2cb.func)
{
FINISH();
}
cbif->process_no = mgd.runv.ss.nt_boottime;
TRCLOG(
"enter user-function\n"
"--->>\n"
"\t dll path = %s\n"
"\t func name = %s (%p)\n"
"\t next events = 0x%08x%s"
, cbmod->iconf->dllpath
, t2cb.name
, t2cb.func
, cbif->flow ? (cbif->flow->next_events) : 0
, cbif->flow ? (cbif->flow->next_events ? " *" : "") : ""
);
syserr = t2cb.func(cbmod->iconf, cbif);
if (syserr)
{
FIRE("callback(type=%d)", cbif->type);
}
TRCLOG(
"leave user-function\n"
"\t dll path = %s\n"
"\t func name = %s (%p)\n"
"\t next events = 0x%08x%s\n"
"<<---"
, cbmod->iconf->dllpath
, t2cb.name
, t2cb.func
, cbif->flow ? (cbif->flow->next_events) : 0
, cbif->flow ? (cbif->flow->next_events ? " *" : "") : ""
);
FUNC_CHECKPOINT
FUNC_LEAVE
return FUNC_RC_INT();
}
static ssize_t udp_Recvfrom(struct udp_data_st* ud, struct sockaddr_in* sa)
{
FUNC_ENTER
socklen_t sa_len = sizeof *sa;
ssize_t nr = recvfrom(mgd.hop.sockUfd, ud, sizeof *ud, 0, sa, &sa_len);
if (nr == -1)
{
FIRE("recvfrom");
}
#if UDP_DEBUG_PRINT
char adstr[INET_ADDRSTRLEN] = { 0 };
inet_ntop(AF_INET, &sa->sin_addr.s_addr, adstr, sizeof adstr);
fprintf(stderr,
"* recvfrom '%s' <--\n"
"\t===========================================\n"
""
, adstr
);
print_udp_data(ud);
#endif
FUNC_CHECKPOINT
FUNC_LEAVE
return nr;
}
static int collect_port_(void* val, void* param)
{
FUNC_ENTER
const struct fkv_st* fkv = val;
struct sortedll_st* ports = param;
const char* const blockstr = "/listen-modules/";
size_t blockstr_len = strlen(blockstr);
if (strncmp(fkv->family, blockstr, blockstr_len) == 0)
{
const char* s_pos = &fkv->family[blockstr_len];
if (STR_IS_PRESENT(s_pos))
{
//printf("port=[%s]\n", s_pos);
char* e = NULL;
long port = strtol(s_pos, &e, 10);
if (*e)
{
FIRE("%s: port=[%s]", e, s_pos);
}
if (port < 0 || UINT16_MAX <= port)
{
FIRE("port(%ld) is not in range", port);
}
syserr = sll_add(ports, (void*)(intptr_t)port);
if (syserr < 0)
{
// 1 is duplicate-value ... OK
FIRE("sll_add");
}
}
}
FUNC_CHECKPOINT
FUNC_LEAVE
return FUNC_RC_INT();
}
int main(void) {
m_disableJTAG();
m_clockdivide(2);
setup_pins();
if (debug_fire|| RF_debug) {setupUSB();}
setup_timer_1();
setup_timer_3();
m_bus_init();
m_rf_open(chan,RX_add,p_length);
int timer_3_cnt = 0;
//sei();
set_motors(0,0);
while (1){
if (check(TIFR3,OCF3A)){
set(TIFR3, OCF3A);
timer_3_cnt++;
if(fired){
since_fired++;
if (debug_fire){
m_usb_tx_string(" its been\t");
m_usb_tx_int(since_fired);
m_usb_tx_string(" milisec\n\r");
}
}
if (since_fired>10){
clear(PORTF,5);
since_fired=0;
fired = false;
if (debug_fire){m_usb_tx_string(" its been 100 sec\n\r");}
}
if ( fire && check(PINB,3)){
fire=false;
fired=true;
since_fired=0;
if (debug_fire){m_usb_tx_string(" portb 3 is high\n\r");}
}
// m_rf_open(chan,RX_add,p_length);
m_green(TOGGLE);
m_rf_init();
if ( timer_3_cnt == 10){
m_red(2);
timer_3_cnt=0;
m_rf_open(chan,RX_add,p_length);
}
}
if(new){ turretDrive();
if(RF_debug){ debug_rf(); }
if ((receive_buffer[0] == 1 || receive_buffer[1]==1) && !fire){FIRE();}
}
//TODO fill in timer code for the firing mechanism
}
}
struct lnklst_st* new_lstnmods(struct lnklst_st* fkvs)
{
FUNC_ENTER
struct sortedll_st* ports = NULL;
struct lnklst_st* lstnmods = NULL; // ports がユニークなので lnklst_t でよい
ports = sll_init(NULL, cmp_as_long_, SLLOPT_DISALLOW_DUPVAL);
if (! ports)
{
FIRE("sll_init");
}
syserr = ll_foreach(fkvs, collect_port_, ports);
if (syserr)
{
FIRE("ll_foreach");
}
lstnmods = new_lstnmods_(ports, fkvs);
if (! lstnmods)
{
FIRE("new_lstnmods_");
}
FUNC_CHECKPOINT
if (HAS_ERROR())
{
ll_free(lstnmods);
lstnmods = NULL;
}
sll_free(ports);
ports = NULL;
FUNC_LEAVE
return lstnmods;
}
static int add_lsnr_to_epoll_(void* val, void* param)
{
FUNC_ENTER
struct listener_st* lsnr = val;
if (mgd.tsopt.reuse_port)
{
#if 0
int listening = -1;
socklen_t len = sizeof(val);
syserr = getsockopt(lsnr->sockfd, SOL_SOCKET, SO_ACCEPTCONN, &listening, &len);
if (syserr)
{
FIRE("getsockopt");
}
if (listening)
{
FIRE("already listen socket fd=%d", lsnr->sockfd);
}
#endif
DBGLOG("listen(B) tcp-socket [fd=%d] backlog=%u", lsnr->sockfd, mgd.stav.somaxconn);
syserr = listen(lsnr->sockfd, mgd.stav.somaxconn);
if (syserr)
{
FIRE("listen [fd=%d]", lsnr->sockfd);
}
}
DBGLOG("TCP-Listener [fd=%d] add to epoll [fd=%d]", lsnr->sockfd, cgd.epfd);
struct epoll_event ev = { .data={ .ptr=lsnr }, .events=EPOLLIN };
int udp_Sendto(struct udp_data_st* ud, struct in_addr ia)
{
FUNC_ENTER
mgd.runv.ss.seqno++;
ud->ss = mgd.runv.ss;
ud->version = sizeof(struct udp_data_st);
ud->nt_sendtime = to_nettime(NOW());
struct sockaddr_in sa =
{
.sin_family=AF_INET,
.sin_addr.s_addr=ia.s_addr,
.sin_port=htons(mgd.stav.bcUport),
};
#if UDP_DEBUG_PRINT
char adstr[INET_ADDRSTRLEN] = { 0 };
inet_ntop(AF_INET, &sa.sin_addr.s_addr, adstr, sizeof adstr);
fprintf(stderr,
"* sendto '%s' -->\n"
"\t===========================================\n"
, adstr
);
print_udp_data(ud);
#endif
ssize_t nw = sendto(mgd.hop.sockUfd, ud, sizeof *ud, 0, &sa, sizeof sa);
if (nw == -1)
{
FIRE("sendto");
}
FUNC_CHECKPOINT
FUNC_LEAVE
return FUNC_RC_INT();
}
static int udp_send_limited_broadcast(struct udp_data_st* ud)
{
return udp_Sendto(ud, (struct in_addr){ .s_addr=INADDR_BROADCAST});
}
void ZeroCrossSM()
{
static float B=0, R=0;
static int C=0;
static t_states state=Init;
switch(state)
{
case Init:
R = Rp;
B = Bp;
C = 0;
iBaseCount = 0;
FIRE();
state = RlB;
break;
case RlB:
if(R < B)
{
R+=Rp;
}
else
{
R+=Rp;
B+=Bp;
FIRE();
state = RgB;
}
if(C == (REFERENCE-1))
{
R = Rp;
B = Bp;
C = 0;
iBaseCount = 0;
FIRE();
state = Init;
}
C++;
break;
case RgB:
if(R < B)
{
R+=Rp;
state = RlB;
}
else
{
R+=Rp;
B+=Bp;
FIRE();
}
if(C == (REFERENCE-1))
{
R = Rp;
B = Bp;
C = 0;
iBaseCount = 0;
FIRE();
state = Init;
}
C++;
break;
default:
break;
}
printf("B=%f; R=%f\n", B, R);
}
static struct lnklst_st* new_lstnmods_(struct sortedll_st* ports, struct lnklst_st* fkvs)
{
FUNC_ENTER
struct lnklst_st* lstnmods = NULL;
struct cbmod_st* defset = NULL;
struct cbmod_st* cbmod = NULL;
defset = new_default_cbmod(fkvs, "/listen-module-default");
if (! defset)
{
FIRE("new_default_cbmod_");
}
defset->iconf->cbmt = JBXM_CBMT_LISTEN;
//puts("# defset -->");
//print_cbmod_(defset, NULL);
//puts("# defset --<");
lstnmods = ll_init(free_cbmod);
if (! lstnmods)
{
FIRE("ll_init");
}
char family[32] = "";
for (struct lnkelm_st* le=sll_first(ports); le->next; le=le->next)
{
// ポート番号を数値に変換
intptr_t n_port = (intptr_t)le->val;
// デフォルト値をコピーする (family_, name, port 以外)
cbmod = new_copy_cbmod_attr(defset);
if (! cbmod)
{
FIRE("new_copy_cbmod");
}
snprintf(family, sizeof family, "/listen-modules/%zd", n_port);
// family_ をコピー
cbmod->family_ = strdup(family);
if (! cbmod->family_)
{
FIRE("strdup");
}
// family_ の値を元に conf から値を設定する
syserr = ll_foreach(fkvs, set_cbmod_by_family, cbmod);
if (syserr)
{
FIRE("ll_foreach");
}
if (! is_valid_cbmod(cbmod))
{
FIRE("is_valid_cbmod port=%ld", n_port);
}
cbmod->iconf->port = (u_short)n_port;
if (cbmod->enable)
{
print_cbmod(cbmod, NULL);
void* ok = ll_add_tail(lstnmods, cbmod);
if (! ok)
{
FIRE("ll_add");
}
cbmod = NULL;
}
else
{
DBGLOG("port(%u) disable", cbmod->iconf->port);
free_cbmod(cbmod);
cbmod = NULL;
}
}
FUNC_CHECKPOINT
if (HAS_ERROR())
{
free_cbmod(cbmod);
cbmod = NULL;
ll_free(lstnmods);
lstnmods = NULL;
}
free_cbmod(defset);
defset = NULL;
FUNC_LEAVE
return lstnmods;
}
int cmdargs_setup(int argc, char** argv)
{
FUNC_ENTER
struct option lopts[] =
{
{ "config", required_argument, NULL, 'c' },
{ 0, 0, 0, 0 },
};
int opt = 0;
int optidx = 0L;
while ((opt = getopt_long(argc, argv, "c:", lopts, &optidx)) != -1)
{
if (opt == -1)
{
break;
}
switch (opt)
{
case 0:
{
break;
}
case 'c':
{
cmdargs.confpath = strdup(optarg);
if (! cmdargs.confpath)
{
FIRE("strdup");
}
break;
}
/*
case 'k':
{
cmdargs.kill_process = true;
break;
}
*/
default:
{
FIRE("unknown argument '%c'", opt);
break;
}
}
}
if (! cmdargs.confpath)
{
cmdargs.confpath = strdup("/etc/jbxd.conf");
if (! cmdargs.confpath)
{
FIRE("strdup");
}
}
cmdargs.argc = argc;
cmdargs.argv = argv;
FUNC_CHECKPOINT
FUNC_LEAVE
return FUNC_RC_INT();
}
