int client_output(struct client *client)
{
int ret;
i_assert(!client->destroyed);
client->last_output = ioloop_time;
timeout_reset(client->to_idle);
if (client->to_idle_output != NULL)
timeout_reset(client->to_idle_output);
o_stream_cork(client->output);
if ((ret = o_stream_flush(client->output)) < 0) {
client_destroy(client, NULL);
return 1;
}
client_output_commands(client);
(void)cmd_sync_delayed(client);
o_stream_uncork(client->output);
imap_refresh_proctitle();
if (client->disconnected)
client_destroy(client, NULL);
else
client_continue_pending_input(client);
return ret;
}
static void proxy_client_worker_input(struct proxy_client_dsync_worker *worker)
{
const char *line;
int ret;
if (worker->to_input != NULL)
timeout_remove(&worker->to_input);
if (worker->worker.input_callback != NULL) {
worker->worker.input_callback(worker->worker.input_context);
timeout_reset(worker->to);
return;
}
while ((ret = proxy_client_worker_read_line(worker, &line)) > 0) {
if (!proxy_client_worker_next_reply(worker, line))
break;
}
if (ret < 0) {
/* try to continue */
proxy_client_worker_next_reply(worker, "");
}
if (worker->to_input != NULL) {
/* input stream's destroy callback was already called.
don't get back here. */
timeout_remove(&worker->to_input);
}
timeout_reset(worker->to);
}
int connection_input_read(struct connection *conn)
{
conn->last_input = ioloop_time;
if (conn->to != NULL)
timeout_reset(conn->to);
switch (i_stream_read(conn->input)) {
case -2:
/* buffer full */
switch (conn->list->set.input_full_behavior) {
case CONNECTION_BEHAVIOR_DESTROY:
conn->disconnect_reason =
CONNECTION_DISCONNECT_BUFFER_FULL;
conn->list->v.destroy(conn);
return -1;
case CONNECTION_BEHAVIOR_ALLOW:
return -2;
}
case -1:
/* disconnected */
conn->disconnect_reason =
CONNECTION_DISCONNECT_CONN_CLOSED;
conn->list->v.destroy(conn);
return -1;
case 0:
/* nothing new read */
return 0;
default:
/* something was read */
return 1;
}
}
static struct event *
win32iocr_process (struct event_queue *evq, struct win32overlapped *ov,
struct event *ev_ready, const msec_t now)
{
struct win32overlapped *ov_next;
for (; ov; ov = ov_next) {
struct event *ev;
const DWORD err = ov->il.err;
const int cancelled = (err == STATUS_CANCELLED) || !ov->e.ev;
ov_next = ov->o.ov_next;
win32iocr_overlapped_del(evq, ov);
if (cancelled)
continue;
ev = ov->e.ev;
ev->w.ov = NULL;
ev->flags &= ~EVENT_AIO_PENDING; /* have to set AIO request */
ev->flags |= err ? EVENT_EOF_RES
: ((ev->flags & EVENT_READ) ? EVENT_READ_RES : EVENT_WRITE_RES);
if (!(ev->flags & EVENT_ACTIVE)) {
ev->flags |= EVENT_ACTIVE;
if (ev->flags & EVENT_ONESHOT)
evq_del(ev, 1);
else if (ev->tq && !(ev->flags & EVENT_TIMEOUT_MANUAL))
timeout_reset(ev, now);
ev->next_ready = ev_ready;
ev_ready = ev;
}
}
return ev_ready;
}
static int client_output(struct client *client)
{
if (o_stream_flush(client->output) < 0) {
if (client->ctrl_output != NULL)
(void)o_stream_send_str(client->ctrl_output, "DISCONNECTED\n");
client_destroy(client);
return 1;
}
timeout_reset(client->to_idle);
if (client->url != NULL) {
if (client_run_url(client) < 0) {
client_destroy(client);
return 1;
}
if (client->url == NULL && client->waiting_input) {
if (!client_handle_input(client)) {
/* client got destroyed */
return 1;
}
}
}
if (client->url != NULL) {
/* url not finished yet */
return 0;
} else if (client->io == NULL) {
/* data still in output buffer, get back here to add IO */
return 0;
} else {
return 1;
}
}
void client_command_free(struct client_command_context **_cmd)
{
struct client_command_context *cmd = *_cmd;
struct client *client = cmd->client;
enum client_command_state state = cmd->state;
*_cmd = NULL;
i_assert(client->output_cmd_lock == NULL);
/* reset input idle time because command output might have taken a
long time and we don't want to disconnect client immediately then */
client->last_input = ioloop_time;
timeout_reset(client->to_idle);
if (cmd->cancel) {
cmd->cancel = FALSE;
client_send_tagline(cmd, "NO Command cancelled.");
}
if (!cmd->param_error)
client->bad_counter = 0;
if (client->input_lock == cmd)
client->input_lock = NULL;
if (client->mailbox_change_lock == cmd)
client->mailbox_change_lock = NULL;
if (client->free_parser == NULL) {
imap_parser_reset(cmd->parser);
client->free_parser = cmd->parser;
} else if (cmd->parser != NULL) {
imap_parser_unref(&cmd->parser);
}
client->command_queue_size--;
DLLIST_REMOVE(&client->command_queue, cmd);
cmd = NULL;
if (client->command_queue == NULL) {
/* no commands left in the queue, we can clear the pool */
p_clear(client->command_pool);
if (client->to_idle_output != NULL)
timeout_remove(&client->to_idle_output);
}
imap_client_notify_command_freed(client);
imap_refresh_proctitle();
/* if command finished from external event, check input for more
unhandled commands since we may not be executing from client_input
or client_output. */
if (state == CLIENT_COMMAND_STATE_WAIT_EXTERNAL &&
!client->disconnected) {
client_add_missing_io(client);
if (client->to_delayed_input == NULL) {
client->to_delayed_input =
timeout_add(0, client_input, client);
}
}
}
bool index_mailbox_want_full_sync(struct mailbox *box,
enum mailbox_sync_flags flags)
{
struct index_mailbox_context *ibox = INDEX_STORAGE_CONTEXT(box);
if ((flags & MAILBOX_SYNC_FLAG_FAST) != 0 &&
ioloop_time < ibox->sync_last_check + MAILBOX_FULL_SYNC_INTERVAL)
return FALSE;
if ((flags & MAILBOX_SYNC_FLAG_FAST) != 0 &&
(box->flags & MAILBOX_FLAG_SAVEONLY) != 0) {
/* lib-lda is syncing the mailbox after saving a mail.
it only wants to find the new mail for potentially copying
to other mailboxes. that's mainly an optimization, and since
the mail was most likely already added to index we don't
need to do a full sync to find it. the main benefit here is
to avoid a very costly sync with a large Maildir/new/ */
return FALSE;
}
if (ibox->notify_to != NULL)
timeout_reset(ibox->notify_to);
ibox->sync_last_check = ioloop_time;
return TRUE;
}
static void idle_client_input_more(struct cmd_idle_context *ctx)
{
struct client *client = ctx->client;
client->last_input = ioloop_time;
timeout_reset(client->to_idle);
switch (i_stream_read(client->input)) {
case -1:
/* disconnected */
client_disconnect(client, "Disconnected in IDLE");
return;
case -2:
client->input_skip_line = TRUE;
idle_finish(ctx, FALSE, TRUE);
client_continue_pending_input(client);
return;
}
if (ctx->sync_ctx != NULL) {
/* we're still sending output to client. wait until it's all
sent so we don't lose any changes. */
io_remove(&client->io);
return;
}
if (idle_client_handle_input(ctx, TRUE)) {
if (!client->disconnected)
client_continue_pending_input(client);
}
}
static void client_input_append(struct client_command_context *cmd)
{
struct cmd_append_context *ctx = cmd->context;
struct client *client = cmd->client;
const char *reason;
bool finished;
uoff_t lit_offset;
i_assert(!client->destroyed);
client->last_input = ioloop_time;
timeout_reset(client->to_idle);
switch (i_stream_read(client->input)) {
case -1:
/* disconnected */
lit_offset = ctx->litinput == NULL ? 0 :
ctx->litinput->v_offset;
reason = get_disconnect_reason(ctx, lit_offset);
cmd_append_finish(cmd->context);
/* Reset command so that client_destroy() doesn't try to call
cmd_append_continue_message() anymore. */
client_command_free(&cmd);
client_destroy(client, reason);
return;
case -2:
if (ctx->message_input) {
/* message data, this is handled internally by
mailbox_save_continue() */
break;
}
cmd_append_finish(cmd->context);
/* parameter word is longer than max. input buffer size.
this is most likely an error, so skip the new data
until newline is found. */
client->input_skip_line = TRUE;
if (!ctx->failed)
client_send_command_error(cmd, "Too long argument.");
cmd->param_error = TRUE;
client_command_free(&cmd);
return;
}
o_stream_cork(client->output);
finished = command_exec(cmd);
if (!finished)
(void)client_handle_unfinished_cmd(cmd);
else
client_command_free(&cmd);
cmd_sync_delayed(client);
o_stream_uncork(client->output);
if (client->disconnected)
client_destroy(client, NULL);
else
client_continue_pending_input(client);
}
static int proxy_client_worker_output(struct proxy_client_dsync_worker *worker)
{
int ret;
ret = proxy_client_worker_output_real(worker);
timeout_reset(worker->to);
return ret;
}
static bool client_handle_input(struct client *client)
{
const char *line, *cmd, *error;
int ret;
if (client->url != NULL || client->to_delay != NULL) {
/* we're still processing a URL. wait until it's
finished. */
io_remove(&client->io);
client->io = NULL;
client->waiting_input = TRUE;
return TRUE;
}
if (client->io == NULL) {
client->io = io_add(client->fd_in, IO_READ,
client_input, client);
}
client->waiting_input = FALSE;
timeout_reset(client->to_idle);
switch (i_stream_read(client->input)) {
case -1:
/* disconnected */
if (client->ctrl_output != NULL)
(void)o_stream_send_str(client->ctrl_output, "DISCONNECTED\n");
client_destroy(client);
return FALSE;
case -2:
/* line too long, kill it */
client_abort(client, "Session aborted: Input line too long");
return FALSE;
}
while ((line = i_stream_next_line(client->input)) != NULL) {
const char *const *args = t_strsplit_tabescaped(line);
if (args[0] == NULL)
continue;
cmd = args[0]; args++;
if (client->mail_user == NULL)
ret = client_handle_user_command(client, cmd, args, &error);
else
ret = client_handle_command(client, cmd, args, &error);
if (ret <= 0) {
if (ret == 0)
break;
i_error("Client input error: %s", error);
client_abort(client, "Session aborted: Unexpected input");
return FALSE;
}
}
return TRUE;
}
EVQ_API int
win32iocp_set (struct event *ev, const unsigned int rw_flags)
{
static WSABUF buf = {0, 0};
struct event_queue *evq = ev->wth->evq;
const sd_t sd = (sd_t) ev->fd;
if ((rw_flags & EVENT_READ) && !ev->w.iocp.rov) {
struct win32overlapped *ov = win32iocp_new_overlapped(evq);
DWORD flags = 0;
if (!ov) return -1;
if (!WSARecv(sd, &buf, 1, NULL, &flags, (OVERLAPPED *) ov, NULL)) {
if (ev->flags & EVENT_AIO_SKIP) {
win32iocp_del_overlapped(evq, ov);
ev->flags |= EVENT_READ_RES | EVENT_ACTIVE;
goto ready;
}
} else if (WSAGetLastError() != WSA_IO_PENDING) {
win32iocp_del_overlapped(evq, ov);
return -1;
}
ov->ev = ev;
ev->w.iocp.rov = ov;
ev->flags |= EVENT_RPENDING; /* IOCP read request is installed */
}
if ((rw_flags & EVENT_WRITE) && !ev->w.iocp.wov) {
struct win32overlapped *ov = win32iocp_new_overlapped(evq);
if (!ov) return -1;
if (!WSASend(sd, &buf, 1, NULL, 0, (OVERLAPPED *) ov, NULL)) {
if (ev->flags & EVENT_AIO_SKIP) {
win32iocp_del_overlapped(evq, ov);
ev->flags |= EVENT_WRITE_RES | EVENT_ACTIVE;
goto ready;
}
} else if (WSAGetLastError() != WSA_IO_PENDING) {
win32iocp_del_overlapped(evq, ov);
return -1;
}
ov->ev = ev;
ev->w.iocp.wov = ov;
ev->flags |= EVENT_WPENDING; /* IOCP write request is installed */
}
return 0;
ready:
if (ev->flags & EVENT_ONESHOT)
evq_del(ev, 1);
else if (ev->tq && !(ev->flags & EVENT_TIMEOUT_MANUAL))
timeout_reset(ev, evq->now);
ev->next_ready = evq->ev_ready;
evq->ev_ready = ev;
return 0;
}
static void notify_callback(struct mailbox *box)
{
struct index_mailbox_context *ibox = INDEX_STORAGE_CONTEXT(box);
timeout_reset(ibox->notify_to);
if (ibox->notify_delay_to == NULL) {
ibox->notify_delay_to =
timeout_add_short(NOTIFY_DELAY_MSECS,
notify_delay_callback, box);
}
}
static void
prv_redraw_parameters (void)
{
if (redraw_timeout)
{
timeout_reset (redraw_timeout, redraw_delay);
}
else
{
timeout_set (redraw_delay, prv_redraw_parameters_callback, 0);
}
}
static void client_input_putscript(struct client *client)
{
struct client_command_context *cmd = &client->cmd;
i_assert(!client->destroyed);
client->last_input = ioloop_time;
timeout_reset(client->to_idle);
switch (i_stream_read(client->input)) {
case -1:
/* disconnected */
cmd_putscript_finish(cmd->context);
/* Reset command so that client_destroy() doesn't try to call
cmd_putscript_continue_script() anymore. */
_client_reset_command(client);
client_destroy(client, "Disconnected in PUTSCRIPT/CHECKSCRIPT");
return;
case -2:
cmd_putscript_finish(cmd->context);
if (client->command_pending) {
/* uploaded script data, this is handled internally by
mailbox_save_continue() */
break;
}
/* parameter word is longer than max. input buffer size.
this is most likely an error, so skip the new data
until newline is found. */
client->input_skip_line = TRUE;
client_send_command_error(cmd, "Too long argument.");
cmd->param_error = TRUE;
_client_reset_command(client);
return;
}
if (cmd->func(cmd)) {
/* command execution was finished. Note that if cmd_sync()
didn't finish, we didn't get here but the input handler
has already been moved. So don't do anything important
here..
reset command once again to reset cmd_sync()'s changes. */
_client_reset_command(client);
if (client->input_pending)
client_input(client);
}
}
static int lmtp_client_output(struct lmtp_client *client)
{
int ret;
lmtp_client_ref(client);
o_stream_cork(client->output);
if ((ret = o_stream_flush(client->output)) < 0)
lmtp_client_fail(client, ERRSTR_TEMP_REMOTE_FAILURE
" (disconnected in output)");
else if (client->input_state == LMTP_INPUT_STATE_DATA)
(void)lmtp_client_send_data(client);
o_stream_uncork(client->output);
if (client->to != NULL)
timeout_reset(client->to);
lmtp_client_unref(&client);
return ret;
}
static void
proxy_client_worker_read_to_eof(struct proxy_client_dsync_worker *worker)
{
struct istream *input = worker->msg_get_data.input;
const unsigned char *data;
size_t size;
int ret;
while ((ret = i_stream_read_data(input, &data, &size, 0)) > 0)
i_stream_skip(input, size);
if (ret == -1) {
i_stream_unref(&input);
io_remove(&worker->io);
proxy_client_worker_msg_get_finish(worker);
}
timeout_reset(worker->to);
}
void mail_session_refresh(struct mail_session *session,
const struct stats *diff_stats)
{
timeout_reset(session->to_idle);
if (diff_stats != NULL)
stats_add(session->stats, diff_stats);
session->last_update = ioloop_timeval;
DLLIST2_REMOVE_FULL(&mail_sessions_head, &mail_sessions_tail, session,
sorted_prev, sorted_next);
DLLIST2_APPEND_FULL(&mail_sessions_head, &mail_sessions_tail, session,
sorted_prev, sorted_next);
mail_user_refresh(session->user, diff_stats);
if (session->ip != NULL)
mail_ip_refresh(session->ip, diff_stats);
}
static void pop3c_client_input_reply(struct pop3c_client *client)
{
i_assert(client->state == POP3C_CLIENT_STATE_DONE);
if (client->to != NULL)
timeout_reset(client->to);
client->input_line = i_stream_read_next_line(client->input);
if (client->input_line != NULL)
io_loop_stop(current_ioloop);
else if (client->input->closed || client->input->eof ||
client->input->stream_errno != 0) {
/* disconnected */
i_error("pop3c(%s): Server disconnected unexpectedly",
client->set.host);
pop3c_client_disconnect(client);
io_loop_stop(current_ioloop);
}
}
void client_input(struct client *client)
{
struct client_command_context *cmd;
struct ostream *output = client->output;
ssize_t bytes;
i_assert(client->io != NULL);
client->last_input = ioloop_time;
timeout_reset(client->to_idle);
if (client->to_delayed_input != NULL)
timeout_remove(&client->to_delayed_input);
bytes = i_stream_read(client->input);
if (bytes == -1) {
/* disconnected */
client_destroy(client, NULL);
return;
}
o_stream_ref(output);
o_stream_cork(output);
if (!client_handle_input(client) && bytes == -2) {
/* parameter word is longer than max. input buffer size.
this is most likely an error, so skip the new data
until newline is found. */
client->input_skip_line = TRUE;
cmd = client->input_lock != NULL ? client->input_lock :
client_command_new(client);
cmd->param_error = TRUE;
client_send_command_error(cmd, "Too long argument.");
client_command_free(&cmd);
}
o_stream_uncork(output);
o_stream_unref(&output);
imap_refresh_proctitle();
if (client->disconnected)
client_destroy(client, NULL);
else
client_continue_pending_input(client);
}
EVQ_API int
evq_set_timeout (struct event *ev, const msec_t msec)
{
struct win32thr *wth = ev->wth;
struct event_queue *evq = wth->evq;
if (!(ev->flags & EVENT_ACTIVE))
win32thr_sleep(wth);
if (ev->tq) {
if (ev->tq->msec == msec) {
timeout_reset(ev, evq->now);
return 0;
}
timeout_del(ev);
}
return (msec == TIMEOUT_INFINITE) ? 0
: timeout_add(ev, msec, evq->now);
}
static void pop3c_client_dot_input(struct pop3c_client *client)
{
ssize_t ret;
if (client->to != NULL)
timeout_reset(client->to);
while ((ret = i_stream_read(client->dot_input)) > 0 || ret == -2) {
i_stream_skip(client->dot_input,
i_stream_get_data_size(client->dot_input));
}
if (ret != 0) {
i_assert(ret == -1);
if (client->dot_input->stream_errno != 0) {
i_error("pop3c(%s): Server disconnected unexpectedly",
client->set.host);
pop3c_client_disconnect(client);
}
if (client->running)
io_loop_stop(current_ioloop);
}
}
static
void ldap_connection_read_more(struct ldap_connection *conn)
{
struct timeval tv = {
.tv_sec = 0,
.tv_usec = 0
};
LDAPMessage *message;
int ret;
/* try get a message */
ret = ldap_result(conn->conn, LDAP_RES_ANY, 0, &tv, &message);
if (ret > 0)
ret = ldap_connection_handle_message(conn, message);
if (ret == -1) {
if (ldap_get_option(conn->conn, LDAP_OPT_RESULT_CODE, &ret) != LDAP_SUCCESS)
i_unreached();
if (ret != LDAP_SERVER_DOWN)
i_error("ldap_result() failed: %s", ldap_err2string(ret));
else
i_error("Connection lost to LDAP server, reconnecting");
/* kill me */
ldap_connection_kill(conn);
} else if (ret != 0) {
ldap_connection_send_next(conn);
}
/* reset timeout */
if (conn->to_disconnect != NULL)
timeout_reset(conn->to_disconnect);
}
bool ldap_result_has_failed(struct ldap_result *result)
{
i_assert((result->openldap_ret == LDAP_SUCCESS) == (result->error_string == NULL));
return result->openldap_ret != LDAP_SUCCESS;
}
static THD_FUNCTION(gurgalof_thread, arg) {
(void)arg;
chRegSetThreadName("APP_GURGALOF");
for(;;) {
#define MIN_PWR 0.2
float pwr = (float)ADC_Value[ADC_IND_EXT];
pwr /= 4095.0;
pwr /= (1.0 - MIN_PWR);
pwr -= MIN_PWR;
if (pwr < 0.0) {
mc_interface_set_current(0.0);
} else {
mc_interface_set_duty(pwr);
// mc_interface_set_current(pwr * mc_interface_get_configuration()->l_current_max);
}
timeout_reset();
chThdSleepMilliseconds(10);
}
}
static void keepalive_timeout(struct cmd_idle_context *ctx)
{
if (ctx->client->output_cmd_lock != NULL) {
/* it's busy sending output */
return;
}
if (o_stream_get_buffer_used_size(ctx->client->output) == 0) {
/* Sending this keeps NATs/stateful firewalls alive.
Sending this also catches dead connections. Don't send
anything if there is already data waiting in output
buffer. */
o_stream_cork(ctx->client->output);
client_send_line(ctx->client, "* OK Still here");
o_stream_uncork(ctx->client->output);
}
/* Make sure idling connections don't get disconnected. There are
several clients that really want to IDLE forever and there's not
much harm in letting them do so. */
timeout_reset(ctx->client->to_idle);
/* recalculate time for the next keepalive timeout */
idle_add_keepalive_timeout(ctx);
}
static void lmtp_client_input(struct lmtp_client *client)
{
const char *line;
int ret;
lmtp_client_ref(client);
o_stream_cork(client->output);
while ((line = i_stream_read_next_line(client->input)) != NULL) {
T_BEGIN {
ret = lmtp_client_input_line(client, line);
} T_END;
if (ret < 0) {
o_stream_uncork(client->output);
lmtp_client_unref(&client);
return;
}
if (ret > 0)
str_truncate(client->input_multiline, 0);
}
if (client->input->stream_errno == ENOBUFS) {
lmtp_client_fail(client,
"501 5.5.4 Command reply line too long");
} else if (client->input->stream_errno != 0) {
i_error("lmtp client: read() failed: %s",
i_stream_get_error(client->input));
lmtp_client_fail(client, ERRSTR_TEMP_REMOTE_FAILURE
" (read failure)");
} else if (client->input->eof) {
lmtp_client_fail(client, ERRSTR_TEMP_REMOTE_FAILURE
" (disconnected in input)");
}
o_stream_uncork(client->output);
if (client->to != NULL)
timeout_reset(client->to);
lmtp_client_unref(&client);
}
EVQ_API int
evq_wait (struct event_queue *evq, msec_t timeout)
{
struct event *ev_ready;
struct kevent *kev = evq->kev_list;
struct timespec ts, *tsp;
int nready;
if (timeout != 0L) {
timeout = timeout_get(evq->tq, timeout, evq->now);
if (timeout == 0L) {
ev_ready = timeout_process(evq->tq, NULL, evq->now);
goto end;
}
}
if (timeout == TIMEOUT_INFINITE)
tsp = NULL;
else {
ts.tv_sec = timeout / 1000;
ts.tv_nsec = (timeout % 1000) * 1000000;
tsp = &ts;
}
sys_vm_leave();
nready = kevent(evq->kqueue_fd, kev, evq->nchanges, kev, NEVENT, tsp);
sys_vm_enter();
evq->nchanges = 0;
evq->now = sys_milliseconds();
if (nready == -1)
return (errno == EINTR) ? 0 : EVQ_FAILED;
if (tsp) {
if (!nready) {
ev_ready = !evq->tq ? NULL
: timeout_process(evq->tq, NULL, evq->now);
if (ev_ready) goto end;
return EVQ_TIMEOUT;
}
timeout = evq->now;
}
ev_ready = NULL;
for (; nready--; ++kev) {
struct event *ev;
const int flags = kev->flags;
const int filter = kev->filter;
if (flags & EV_ERROR)
continue;
if (filter == EVFILT_SIGNAL) {
ev_ready = signal_process_actives(evq, kev->ident,
ev_ready, timeout);
continue;
}
ev = (struct event *) kev->udata;
if (!ev) {
ev_ready = signal_process_interrupt(evq, ev_ready, timeout);
continue;
}
ev->flags |= (filter == EVFILT_READ ? EVENT_READ_RES : EVENT_WRITE_RES)
| ((flags & EV_EOF) ? EVENT_EOF_RES : 0);
if (ev->flags & EVENT_ACTIVE)
continue;
ev->flags |= EVENT_ACTIVE;
if (ev->flags & EVENT_ONESHOT)
evq_del(ev, 1);
else if (ev->tq && !(ev->flags & EVENT_TIMEOUT_MANUAL))
timeout_reset(ev, timeout);
ev->next_ready = ev_ready;
ev_ready = ev;
}
if (!ev_ready) return 0;
end:
evq->ev_ready = ev_ready;
return 0;
}
static struct event *
win32iocp_process (struct event_queue *evq, struct event *ev_ready, msec_t now)
{
const HANDLE iocph = evq->iocp.h;
OVERLAPPED_ENTRY entries[NENTRY];
ULONG nentries = 0;
for (; ; ) {
struct win32overlapped *ov;
struct event *ev;
BOOL status;
int cancelled = 0;
if (pGetQueuedCompletionStatusEx) {
if (!nentries && !pGetQueuedCompletionStatusEx(iocph,
entries, NENTRY, &nentries, 0L, FALSE))
break;
{
const OVERLAPPED_ENTRY *ove = &entries[--nentries];
const DWORD err = (DWORD) ove->lpOverlapped->Internal;
ov = (struct win32overlapped *) ove->lpOverlapped;
status = !err;
cancelled = (err == STATUS_CANCELLED);
}
} else {
ULONG_PTR key;
DWORD nr;
status = GetQueuedCompletionStatus(iocph, &nr, &key,
(OVERLAPPED **) &ov, 0L);
if (!status) {
const DWORD err = GetLastError();
if (err == WAIT_TIMEOUT)
break;
cancelled = (err == ERROR_OPERATION_ABORTED);
}
}
if (!ov) {
if (pGetQueuedCompletionStatusEx) continue;
break; /* error */
}
ev = ov->ev;
cancelled = ev ? cancelled : 1;
win32iocp_del_overlapped(evq, ov);
if (cancelled)
continue;
if (!status)
ev->flags |= EVENT_EOF_RES;
else if (ov == ev->w.iocp.rov) {
ev->w.iocp.rov = NULL;
ev->flags |= EVENT_READ_RES;
ev->flags &= ~EVENT_RPENDING; /* have to set IOCP read request */
} else {
ev->w.iocp.wov = NULL;
ev->flags |= EVENT_WRITE_RES;
ev->flags &= ~EVENT_WPENDING; /* have to set IOCP write request */
}
if (ev->flags & EVENT_ACTIVE)
continue;
ev->flags |= EVENT_ACTIVE;
if (ev->flags & EVENT_ONESHOT)
evq_del(ev, 1);
else if (ev->tq && !(ev->flags & EVENT_TIMEOUT_MANUAL)) {
if (now == 0L) {
now = evq->now = sys_milliseconds();
}
timeout_reset(ev, now);
}
ev->next_ready = ev_ready;
ev_ready = ev;
}
return ev_ready;
}
/**
* Process a received buffer with commands and data.
*
* @param data
* The buffer to process.
*
* @param len
* The length of the buffer.
*/
void commands_process_packet(unsigned char *data, unsigned int len) {
if (!len) {
return;
}
COMM_PACKET_ID packet_id;
int32_t ind = 0;
uint16_t sample_len;
uint8_t decimation;
bool at_start;
static mc_configuration mcconf, mcconf_old; // Static to save some stack space
app_configuration appconf;
uint16_t flash_res;
uint32_t new_app_offset;
chuck_data chuck_d_tmp;
packet_id = data[0];
data++;
len--;
switch (packet_id) {
case COMM_FW_VERSION:
ind = 0;
send_buffer[ind++] = COMM_FW_VERSION;
send_buffer[ind++] = FW_VERSION_MAJOR;
send_buffer[ind++] = FW_VERSION_MINOR;
commands_send_packet(send_buffer, ind);
break;
case COMM_JUMP_TO_BOOTLOADER:
flash_helper_jump_to_bootloader();
break;
case COMM_ERASE_NEW_APP:
ind = 0;
flash_res = flash_helper_erase_new_app(buffer_get_uint32(data, &ind));
ind = 0;
send_buffer[ind++] = COMM_ERASE_NEW_APP;
send_buffer[ind++] = flash_res == FLASH_COMPLETE ? 1 : 0;
commands_send_packet(send_buffer, ind);
break;
case COMM_WRITE_NEW_APP_DATA:
ind = 0;
new_app_offset = buffer_get_uint32(data, &ind);
flash_res = flash_helper_write_new_app_data(new_app_offset, data + ind, len - ind);
ind = 0;
send_buffer[ind++] = COMM_WRITE_NEW_APP_DATA;
send_buffer[ind++] = flash_res == FLASH_COMPLETE ? 1 : 0;
commands_send_packet(send_buffer, ind);
break;
case COMM_GET_VALUES:
ind = 0;
send_buffer[ind++] = COMM_GET_VALUES;
buffer_append_float16(send_buffer, NTC_TEMP(ADC_IND_TEMP_MOS1), 1e1, &ind);
buffer_append_float16(send_buffer, NTC_TEMP(ADC_IND_TEMP_MOS2), 1e1, &ind);
buffer_append_float16(send_buffer, NTC_TEMP(ADC_IND_TEMP_MOS3), 1e1, &ind);
buffer_append_float16(send_buffer, NTC_TEMP(ADC_IND_TEMP_MOS4), 1e1, &ind);
buffer_append_float16(send_buffer, NTC_TEMP(ADC_IND_TEMP_MOS5), 1e1, &ind);
buffer_append_float16(send_buffer, NTC_TEMP(ADC_IND_TEMP_MOS6), 1e1, &ind);
buffer_append_float16(send_buffer, NTC_TEMP(ADC_IND_TEMP_PCB), 1e1, &ind);
buffer_append_float32(send_buffer, mc_interface_read_reset_avg_motor_current(), 1e2, &ind);
buffer_append_float32(send_buffer, mc_interface_read_reset_avg_input_current(), 1e2, &ind);
buffer_append_float16(send_buffer, mc_interface_get_duty_cycle_now(), 1e3, &ind);
buffer_append_float32(send_buffer, mc_interface_get_rpm(), 1e0, &ind);
buffer_append_float16(send_buffer, GET_INPUT_VOLTAGE(), 1e1, &ind);
buffer_append_float32(send_buffer, mc_interface_get_amp_hours(false), 1e4, &ind);
buffer_append_float32(send_buffer, mc_interface_get_amp_hours_charged(false), 1e4, &ind);
buffer_append_float32(send_buffer, mc_interface_get_watt_hours(false), 1e4, &ind);
buffer_append_float32(send_buffer, mc_interface_get_watt_hours_charged(false), 1e4, &ind);
buffer_append_int32(send_buffer, mc_interface_get_tachometer_value(false), &ind);
buffer_append_int32(send_buffer, mc_interface_get_tachometer_abs_value(false), &ind);
send_buffer[ind++] = mc_interface_get_fault();
// TODO: send FOC values id, iq, abs
commands_send_packet(send_buffer, ind);
break;
case COMM_SET_DUTY:
ind = 0;
mc_interface_set_duty((float)buffer_get_int32(data, &ind) / 100000.0);
timeout_reset();
break;
case COMM_SET_CURRENT:
ind = 0;
mc_interface_set_current((float)buffer_get_int32(data, &ind) / 1000.0);
timeout_reset();
break;
case COMM_SET_CURRENT_BRAKE:
ind = 0;
mc_interface_set_brake_current((float)buffer_get_int32(data, &ind) / 1000.0);
timeout_reset();
break;
case COMM_SET_RPM:
ind = 0;
mc_interface_set_pid_speed((float)buffer_get_int32(data, &ind));
timeout_reset();
break;
case COMM_SET_POS:
ind = 0;
mc_interface_set_pid_pos((float)buffer_get_int32(data, &ind) / 1000000.0);
timeout_reset();
break;
case COMM_SET_DETECT:
mcconf = *mc_interface_get_configuration();
ind = 0;
display_position_mode = data[ind++];
if (mcconf.motor_type == MOTOR_TYPE_BLDC) {
if (display_position_mode == DISP_POS_MODE_NONE) {
mc_interface_release_motor();
} else if (display_position_mode == DISP_POS_MODE_INDUCTANCE) {
mcpwm_set_detect();
}
}
timeout_reset();
break;
case COMM_SET_SERVO_POS:
#if SERVO_OUT_ENABLE
ind = 0;
#if SERVO_OUT_SIMPLE
servo_simple_set_output(buffer_get_float16(data, 1000.0, &ind));
#else
servos[0].pos = (int16_t)(buffer_get_float16(data, 1000.0, &ind) * 255.0);
#endif
#endif
break;
case COMM_SET_MCCONF:
mcconf = *mc_interface_get_configuration();
ind = 0;
mcconf.pwm_mode = data[ind++];
mcconf.comm_mode = data[ind++];
mcconf.motor_type = data[ind++];
mcconf.sensor_mode = data[ind++];
mcconf.l_current_max = buffer_get_float32(data, 1000.0, &ind);
mcconf.l_current_min = buffer_get_float32(data, 1000.0, &ind);
mcconf.l_in_current_max = buffer_get_float32(data, 1000.0, &ind);
mcconf.l_in_current_min = buffer_get_float32(data, 1000.0, &ind);
mcconf.l_abs_current_max = buffer_get_float32(data, 1000.0, &ind);
mcconf.l_min_erpm = buffer_get_float32(data, 1000.0, &ind);
mcconf.l_max_erpm = buffer_get_float32(data, 1000.0, &ind);
mcconf.l_max_erpm_fbrake = buffer_get_float32(data, 1000.0, &ind);
mcconf.l_max_erpm_fbrake_cc = buffer_get_float32(data, 1000.0, &ind);
mcconf.l_min_vin = buffer_get_float32(data, 1000.0, &ind);
mcconf.l_max_vin = buffer_get_float32(data, 1000.0, &ind);
mcconf.l_battery_cut_start = buffer_get_float32(data, 1000.0, &ind);
mcconf.l_battery_cut_end = buffer_get_float32(data, 1000.0, &ind);
mcconf.l_slow_abs_current = data[ind++];
mcconf.l_rpm_lim_neg_torque = data[ind++];
mcconf.l_temp_fet_start = buffer_get_float32(data, 1000.0, &ind);
mcconf.l_temp_fet_end = buffer_get_float32(data, 1000.0, &ind);
mcconf.l_temp_motor_start = buffer_get_float32(data, 1000.0, &ind);
mcconf.l_temp_motor_end = buffer_get_float32(data, 1000.0, &ind);
mcconf.l_min_duty = buffer_get_float32(data, 1000000.0, &ind);
mcconf.l_max_duty = buffer_get_float32(data, 1000000.0, &ind);
mcconf.lo_current_max = mcconf.l_current_max;
mcconf.lo_current_min = mcconf.l_current_min;
mcconf.lo_in_current_max = mcconf.l_in_current_max;
mcconf.lo_in_current_min = mcconf.l_in_current_min;
mcconf.sl_min_erpm = (float)buffer_get_int32(data, &ind) / 1000.0;
mcconf.sl_min_erpm_cycle_int_limit = (float)buffer_get_int32(data, &ind) / 1000.0;
mcconf.sl_max_fullbreak_current_dir_change = (float)buffer_get_int32(data, &ind) / 1000.0;
mcconf.sl_cycle_int_limit = (float)buffer_get_int32(data, &ind) / 1000.0;
mcconf.sl_phase_advance_at_br = (float)buffer_get_int32(data, &ind) / 1000.0;
mcconf.sl_cycle_int_rpm_br = (float)buffer_get_int32(data, &ind) / 1000.0;
mcconf.sl_bemf_coupling_k = (float)buffer_get_int32(data, &ind) / 1000.0;
memcpy(mcconf.hall_table, data + ind, 8);
ind += 8;
mcconf.hall_sl_erpm = (float)buffer_get_int32(data, &ind) / 1000.0;
mcconf.foc_current_kp = buffer_get_float32(data, 1e5, &ind);
mcconf.foc_current_ki = buffer_get_float32(data, 1e5, &ind);
mcconf.foc_f_sw = buffer_get_float32(data, 1e3, &ind);
mcconf.foc_dt_us = buffer_get_float32(data, 1e6, &ind);
mcconf.foc_encoder_inverted = data[ind++];
mcconf.foc_encoder_offset = buffer_get_float32(data, 1e3, &ind);
mcconf.foc_encoder_ratio = buffer_get_float32(data, 1e3, &ind);
mcconf.foc_sensor_mode = data[ind++];
mcconf.foc_pll_kp = buffer_get_float32(data, 1e3, &ind);
mcconf.foc_pll_ki = buffer_get_float32(data, 1e3, &ind);
mcconf.foc_motor_l = buffer_get_float32(data, 1e8, &ind);
mcconf.foc_motor_r = buffer_get_float32(data, 1e5, &ind);
mcconf.foc_motor_flux_linkage = buffer_get_float32(data, 1e5, &ind);
mcconf.foc_observer_gain = buffer_get_float32(data, 1e0, &ind);
mcconf.foc_duty_dowmramp_kp = buffer_get_float32(data, 1e3, &ind);
mcconf.foc_duty_dowmramp_ki = buffer_get_float32(data, 1e3, &ind);
mcconf.foc_openloop_rpm = buffer_get_float32(data, 1e3, &ind);
mcconf.foc_sl_openloop_hyst = buffer_get_float32(data, 1e3, &ind);
mcconf.foc_sl_openloop_time = buffer_get_float32(data, 1e3, &ind);
mcconf.foc_sl_d_current_duty = buffer_get_float32(data, 1e3, &ind);
mcconf.foc_sl_d_current_factor = buffer_get_float32(data, 1e3, &ind);
memcpy(mcconf.foc_hall_table, data + ind, 8);
ind += 8;
mcconf.foc_sl_erpm = (float)buffer_get_int32(data, &ind) / 1000.0;
mcconf.s_pid_kp = (float)buffer_get_int32(data, &ind) / 1000000.0;
mcconf.s_pid_ki = (float)buffer_get_int32(data, &ind) / 1000000.0;
mcconf.s_pid_kd = (float)buffer_get_int32(data, &ind) / 1000000.0;
mcconf.s_pid_min_erpm = (float)buffer_get_int32(data, &ind) / 1000.0;
mcconf.p_pid_kp = (float)buffer_get_int32(data, &ind) / 1000000.0;
mcconf.p_pid_ki = (float)buffer_get_int32(data, &ind) / 1000000.0;
mcconf.p_pid_kd = (float)buffer_get_int32(data, &ind) / 1000000.0;
mcconf.p_pid_ang_div = (float)buffer_get_int32(data, &ind) / 100000.0;
mcconf.cc_startup_boost_duty = (float)buffer_get_int32(data, &ind) / 1000000.0;
mcconf.cc_min_current = (float)buffer_get_int32(data, &ind) / 1000.0;
mcconf.cc_gain = (float)buffer_get_int32(data, &ind) / 1000000.0;
mcconf.cc_ramp_step_max = buffer_get_float32(data, 1e5, &ind);
mcconf.m_fault_stop_time_ms = buffer_get_int32(data, &ind);
mcconf.m_duty_ramp_step = (float)buffer_get_float32(data, 1000000.0, &ind);
mcconf.m_duty_ramp_step_rpm_lim = (float)buffer_get_float32(data, 1000000.0, &ind);
mcconf.m_current_backoff_gain = (float)buffer_get_float32(data, 1000000.0, &ind);
mcconf.m_encoder_counts = buffer_get_uint32(data, &ind);
mcconf.m_sensor_port_mode = data[ind++];
conf_general_store_mc_configuration(&mcconf);
mc_interface_set_configuration(&mcconf);
ind = 0;
send_buffer[ind++] = packet_id;
commands_send_packet(send_buffer, ind);
break;
case COMM_GET_MCCONF:
case COMM_GET_MCCONF_DEFAULT:
if (packet_id == COMM_GET_MCCONF) {
mcconf = *mc_interface_get_configuration();
} else {
conf_general_get_default_mc_configuration(&mcconf);
}
ind = 0;
send_buffer[ind++] = packet_id;
send_buffer[ind++] = mcconf.pwm_mode;
send_buffer[ind++] = mcconf.comm_mode;
send_buffer[ind++] = mcconf.motor_type;
send_buffer[ind++] = mcconf.sensor_mode;
buffer_append_int32(send_buffer, (int32_t)(mcconf.l_current_max * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.l_current_min * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.l_in_current_max * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.l_in_current_min * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.l_abs_current_max * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.l_min_erpm * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.l_max_erpm * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.l_max_erpm_fbrake * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.l_max_erpm_fbrake_cc * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.l_min_vin * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.l_max_vin * 1000.0), &ind);
buffer_append_float32(send_buffer, mcconf.l_battery_cut_start, 1000.0, &ind);
buffer_append_float32(send_buffer, mcconf.l_battery_cut_end, 1000.0, &ind);
send_buffer[ind++] = mcconf.l_slow_abs_current;
send_buffer[ind++] = mcconf.l_rpm_lim_neg_torque;
buffer_append_int32(send_buffer, (int32_t)(mcconf.l_temp_fet_start * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.l_temp_fet_end * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.l_temp_motor_start * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.l_temp_motor_end * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.l_min_duty * 1000000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.l_max_duty * 1000000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.sl_min_erpm * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.sl_min_erpm_cycle_int_limit * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.sl_max_fullbreak_current_dir_change * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.sl_cycle_int_limit * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.sl_phase_advance_at_br * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.sl_cycle_int_rpm_br * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.sl_bemf_coupling_k * 1000.0), &ind);
memcpy(send_buffer + ind, mcconf.hall_table, 8);
ind += 8;
buffer_append_int32(send_buffer, (int32_t)(mcconf.hall_sl_erpm * 1000.0), &ind);
buffer_append_float32(send_buffer, mcconf.foc_current_kp, 1e5, &ind);
buffer_append_float32(send_buffer, mcconf.foc_current_ki, 1e5, &ind);
buffer_append_float32(send_buffer, mcconf.foc_f_sw, 1e3, &ind);
buffer_append_float32(send_buffer, mcconf.foc_dt_us, 1e6, &ind);
send_buffer[ind++] = mcconf.foc_encoder_inverted;
buffer_append_float32(send_buffer, mcconf.foc_encoder_offset, 1e3, &ind);
buffer_append_float32(send_buffer, mcconf.foc_encoder_ratio, 1e3, &ind);
send_buffer[ind++] = mcconf.foc_sensor_mode;
buffer_append_float32(send_buffer, mcconf.foc_pll_kp, 1e3, &ind);
buffer_append_float32(send_buffer, mcconf.foc_pll_ki, 1e3, &ind);
buffer_append_float32(send_buffer, mcconf.foc_motor_l, 1e8, &ind);
buffer_append_float32(send_buffer, mcconf.foc_motor_r, 1e5, &ind);
buffer_append_float32(send_buffer, mcconf.foc_motor_flux_linkage, 1e5, &ind);
buffer_append_float32(send_buffer, mcconf.foc_observer_gain, 1e0, &ind);
buffer_append_float32(send_buffer, mcconf.foc_duty_dowmramp_kp, 1e3, &ind);
buffer_append_float32(send_buffer, mcconf.foc_duty_dowmramp_ki, 1e3, &ind);
buffer_append_float32(send_buffer, mcconf.foc_openloop_rpm, 1e3, &ind);
buffer_append_float32(send_buffer, mcconf.foc_sl_openloop_hyst, 1e3, &ind);
buffer_append_float32(send_buffer, mcconf.foc_sl_openloop_time, 1e3, &ind);
buffer_append_float32(send_buffer, mcconf.foc_sl_d_current_duty, 1e3, &ind);
buffer_append_float32(send_buffer, mcconf.foc_sl_d_current_factor, 1e3, &ind);
memcpy(send_buffer + ind, mcconf.foc_hall_table, 8);
ind += 8;
buffer_append_int32(send_buffer, (int32_t)(mcconf.foc_sl_erpm * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.s_pid_kp * 1000000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.s_pid_ki * 1000000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.s_pid_kd * 1000000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.s_pid_min_erpm * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.p_pid_kp * 1000000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.p_pid_ki * 1000000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.p_pid_kd * 1000000.0), &ind);
buffer_append_float32(send_buffer, mcconf.p_pid_ang_div, 1e5, &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.cc_startup_boost_duty * 1000000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.cc_min_current * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.cc_gain * 1000000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.cc_ramp_step_max * 1000000.0), &ind);
buffer_append_int32(send_buffer, mcconf.m_fault_stop_time_ms, &ind);
buffer_append_float32(send_buffer, mcconf.m_duty_ramp_step, 1000000.0, &ind);
buffer_append_float32(send_buffer, mcconf.m_duty_ramp_step_rpm_lim, 1000000.0, &ind);
buffer_append_float32(send_buffer, mcconf.m_current_backoff_gain, 1000000.0, &ind);
buffer_append_uint32(send_buffer, mcconf.m_encoder_counts, &ind);
send_buffer[ind++] = mcconf.m_sensor_port_mode;
commands_send_packet(send_buffer, ind);
break;
case COMM_SET_APPCONF:
appconf = *app_get_configuration();
ind = 0;
appconf.controller_id = data[ind++];
appconf.timeout_msec = buffer_get_uint32(data, &ind);
appconf.timeout_brake_current = (float)buffer_get_int32(data, &ind) / 1000.0;
appconf.send_can_status = data[ind++];
appconf.send_can_status_rate_hz = buffer_get_uint16(data, &ind);
appconf.app_to_use = data[ind++];
appconf.app_ppm_conf.ctrl_type = data[ind++];
appconf.app_ppm_conf.pid_max_erpm = (float)buffer_get_int32(data, &ind) / 1000.0;
appconf.app_ppm_conf.hyst = (float)buffer_get_int32(data, &ind) / 1000.0;
appconf.app_ppm_conf.pulse_start = (float)buffer_get_int32(data, &ind) / 1000.0;
appconf.app_ppm_conf.pulse_end = (float)buffer_get_int32(data, &ind) / 1000.0;
appconf.app_ppm_conf.median_filter = data[ind++];
appconf.app_ppm_conf.safe_start = data[ind++];
appconf.app_ppm_conf.rpm_lim_start = (float)buffer_get_int32(data, &ind) / 1000.0;
appconf.app_ppm_conf.rpm_lim_end = (float)buffer_get_int32(data, &ind) / 1000.0;
appconf.app_ppm_conf.multi_esc = data[ind++];
appconf.app_ppm_conf.tc = data[ind++];
appconf.app_ppm_conf.tc_max_diff = (float)buffer_get_int32(data, &ind) / 1000.0;
appconf.app_adc_conf.ctrl_type = data[ind++];
appconf.app_adc_conf.hyst = (float)buffer_get_int32(data, &ind) / 1000.0;
appconf.app_adc_conf.voltage_start = (float)buffer_get_int32(data, &ind) / 1000.0;
appconf.app_adc_conf.voltage_end = (float)buffer_get_int32(data, &ind) / 1000.0;
appconf.app_adc_conf.use_filter = data[ind++];
appconf.app_adc_conf.safe_start = data[ind++];
appconf.app_adc_conf.cc_button_inverted = data[ind++];
appconf.app_adc_conf.rev_button_inverted = data[ind++];
appconf.app_adc_conf.voltage_inverted = data[ind++];
appconf.app_adc_conf.rpm_lim_start = (float)buffer_get_int32(data, &ind) / 1000.0;
appconf.app_adc_conf.rpm_lim_end = (float)buffer_get_int32(data, &ind) / 1000.0;
appconf.app_adc_conf.multi_esc = data[ind++];
appconf.app_adc_conf.tc = data[ind++];
appconf.app_adc_conf.tc_max_diff = (float)buffer_get_int32(data, &ind) / 1000.0;
appconf.app_adc_conf.update_rate_hz = buffer_get_uint16(data, &ind);
appconf.app_uart_baudrate = buffer_get_uint32(data, &ind);
appconf.app_chuk_conf.ctrl_type = data[ind++];
appconf.app_chuk_conf.hyst = buffer_get_float32(data, 1000.0, &ind);
appconf.app_chuk_conf.rpm_lim_start = buffer_get_float32(data, 1000.0, &ind);
appconf.app_chuk_conf.rpm_lim_end = buffer_get_float32(data, 1000.0, &ind);
appconf.app_chuk_conf.ramp_time_pos = buffer_get_float32(data, 1000.0, &ind);
appconf.app_chuk_conf.ramp_time_neg = buffer_get_float32(data, 1000.0, &ind);
appconf.app_chuk_conf.stick_erpm_per_s_in_cc = buffer_get_float32(data, 1000.0, &ind);
appconf.app_chuk_conf.multi_esc = data[ind++];
appconf.app_chuk_conf.tc = data[ind++];
appconf.app_chuk_conf.tc_max_diff = buffer_get_float32(data, 1000.0, &ind);
appconf.app_nrf_conf.speed = data[ind++];
appconf.app_nrf_conf.power = data[ind++];
appconf.app_nrf_conf.crc_type = data[ind++];
appconf.app_nrf_conf.retry_delay = data[ind++];
appconf.app_nrf_conf.retries = data[ind++];
appconf.app_nrf_conf.channel = data[ind++];
memcpy(appconf.app_nrf_conf.address, data + ind, 3);
ind += 3;
appconf.app_nrf_conf.send_crc_ack = data[ind++];
conf_general_store_app_configuration(&appconf);
app_set_configuration(&appconf);
timeout_configure(appconf.timeout_msec, appconf.timeout_brake_current);
ind = 0;
send_buffer[ind++] = packet_id;
commands_send_packet(send_buffer, ind);
break;
case COMM_GET_APPCONF:
case COMM_GET_APPCONF_DEFAULT:
if (packet_id == COMM_GET_APPCONF) {
appconf = *app_get_configuration();
} else {
conf_general_get_default_app_configuration(&appconf);
}
ind = 0;
send_buffer[ind++] = packet_id;
send_buffer[ind++] = appconf.controller_id;
buffer_append_uint32(send_buffer, appconf.timeout_msec, &ind);
buffer_append_int32(send_buffer, (int32_t)(appconf.timeout_brake_current * 1000.0), &ind);
send_buffer[ind++] = appconf.send_can_status;
buffer_append_uint16(send_buffer, appconf.send_can_status_rate_hz, &ind);
send_buffer[ind++] = appconf.app_to_use;
send_buffer[ind++] = appconf.app_ppm_conf.ctrl_type;
buffer_append_int32(send_buffer, (int32_t)(appconf.app_ppm_conf.pid_max_erpm * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(appconf.app_ppm_conf.hyst * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(appconf.app_ppm_conf.pulse_start * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(appconf.app_ppm_conf.pulse_end * 1000.0), &ind);
send_buffer[ind++] = appconf.app_ppm_conf.median_filter;
send_buffer[ind++] = appconf.app_ppm_conf.safe_start;
buffer_append_int32(send_buffer, (int32_t)(appconf.app_ppm_conf.rpm_lim_start * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(appconf.app_ppm_conf.rpm_lim_end * 1000.0), &ind);
send_buffer[ind++] = appconf.app_ppm_conf.multi_esc;
send_buffer[ind++] = appconf.app_ppm_conf.tc;
buffer_append_int32(send_buffer, (int32_t)(appconf.app_ppm_conf.tc_max_diff * 1000.0), &ind);
send_buffer[ind++] = appconf.app_adc_conf.ctrl_type;
buffer_append_int32(send_buffer, (int32_t)(appconf.app_adc_conf.hyst * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(appconf.app_adc_conf.voltage_start * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(appconf.app_adc_conf.voltage_end * 1000.0), &ind);
send_buffer[ind++] = appconf.app_adc_conf.use_filter;
send_buffer[ind++] = appconf.app_adc_conf.safe_start;
send_buffer[ind++] = appconf.app_adc_conf.cc_button_inverted;
send_buffer[ind++] = appconf.app_adc_conf.rev_button_inverted;
send_buffer[ind++] = appconf.app_adc_conf.voltage_inverted;
buffer_append_int32(send_buffer, (int32_t)(appconf.app_adc_conf.rpm_lim_start * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(appconf.app_adc_conf.rpm_lim_end * 1000.0), &ind);
send_buffer[ind++] = appconf.app_adc_conf.multi_esc;
send_buffer[ind++] = appconf.app_adc_conf.tc;
buffer_append_int32(send_buffer, (int32_t)(appconf.app_adc_conf.tc_max_diff * 1000.0), &ind);
buffer_append_uint16(send_buffer, appconf.app_adc_conf.update_rate_hz, &ind);
buffer_append_uint32(send_buffer, appconf.app_uart_baudrate, &ind);
send_buffer[ind++] = appconf.app_chuk_conf.ctrl_type;
buffer_append_float32(send_buffer, appconf.app_chuk_conf.hyst, 1000.0, &ind);
buffer_append_float32(send_buffer, appconf.app_chuk_conf.rpm_lim_start, 1000.0, &ind);
buffer_append_float32(send_buffer, appconf.app_chuk_conf.rpm_lim_end, 1000.0, &ind);
buffer_append_float32(send_buffer, appconf.app_chuk_conf.ramp_time_pos, 1000.0, &ind);
buffer_append_float32(send_buffer, appconf.app_chuk_conf.ramp_time_neg, 1000.0, &ind);
buffer_append_float32(send_buffer, appconf.app_chuk_conf.stick_erpm_per_s_in_cc, 1000.0, &ind);
send_buffer[ind++] = appconf.app_chuk_conf.multi_esc;
send_buffer[ind++] = appconf.app_chuk_conf.tc;
buffer_append_int32(send_buffer, (int32_t)(appconf.app_chuk_conf.tc_max_diff * 1000.0), &ind);
send_buffer[ind++] = appconf.app_nrf_conf.speed;
send_buffer[ind++] = appconf.app_nrf_conf.power;
send_buffer[ind++] = appconf.app_nrf_conf.crc_type;
send_buffer[ind++] = appconf.app_nrf_conf.retry_delay;
send_buffer[ind++] = appconf.app_nrf_conf.retries;
send_buffer[ind++] = appconf.app_nrf_conf.channel;
memcpy(send_buffer + ind, appconf.app_nrf_conf.address, 3);
ind += 3;
send_buffer[ind++] = appconf.app_nrf_conf.send_crc_ack;
commands_send_packet(send_buffer, ind);
break;
case COMM_SAMPLE_PRINT:
ind = 0;
at_start = data[ind++];
sample_len = buffer_get_uint16(data, &ind);
decimation = data[ind++];
mc_interface_sample_print_data(at_start, sample_len, decimation);
break;
case COMM_TERMINAL_CMD:
data[len] = '\0';
terminal_process_string((char*)data);
break;
case COMM_DETECT_MOTOR_PARAM:
ind = 0;
detect_current = buffer_get_float32(data, 1e3, &ind);
detect_min_rpm = buffer_get_float32(data, 1e3, &ind);
detect_low_duty = buffer_get_float32(data, 1e3, &ind);
chEvtSignal(detect_tp, (eventmask_t) 1);
break;
case COMM_DETECT_MOTOR_R_L: {
mcconf = *mc_interface_get_configuration();
mcconf_old = mcconf;
mcconf.motor_type = MOTOR_TYPE_FOC;
mc_interface_set_configuration(&mcconf);
float r = 0.0;
float l = 0.0;
bool res = mcpwm_foc_measure_res_ind(&r, &l);
mc_interface_set_configuration(&mcconf_old);
if (!res) {
r = 0.0;
l = 0.0;
}
ind = 0;
send_buffer[ind++] = COMM_DETECT_MOTOR_R_L;
buffer_append_float32(send_buffer, r, 1e6, &ind);
buffer_append_float32(send_buffer, l, 1e3, &ind);
commands_send_packet(send_buffer, ind);
}
break;
case COMM_DETECT_MOTOR_FLUX_LINKAGE: {
ind = 0;
float current = buffer_get_float32(data, 1e3, &ind);
float min_rpm = buffer_get_float32(data, 1e3, &ind);
float duty = buffer_get_float32(data, 1e3, &ind);
float resistance = buffer_get_float32(data, 1e6, &ind);
float linkage;
bool res = conf_general_measure_flux_linkage(current, duty, min_rpm, resistance, &linkage);
if (!res) {
linkage = 0.0;
}
ind = 0;
send_buffer[ind++] = COMM_DETECT_MOTOR_FLUX_LINKAGE;
buffer_append_float32(send_buffer, linkage, 1e7, &ind);
commands_send_packet(send_buffer, ind);
}
break;
case COMM_DETECT_ENCODER: {
if (encoder_is_configured()) {
mcconf = *mc_interface_get_configuration();
mcconf_old = mcconf;
ind = 0;
float current = buffer_get_float32(data, 1e3, &ind);
mcconf.motor_type = MOTOR_TYPE_FOC;
mcconf.foc_f_sw = 10000.0;
mcconf.foc_current_kp = 0.01;
mcconf.foc_current_ki = 10.0;
mc_interface_set_configuration(&mcconf);
float offset = 0.0;
float ratio = 0.0;
bool inverted = false;
mcpwm_foc_encoder_detect(current, false, &offset, &ratio, &inverted);
mc_interface_set_configuration(&mcconf_old);
ind = 0;
send_buffer[ind++] = COMM_DETECT_ENCODER;
buffer_append_float32(send_buffer, offset, 1e6, &ind);
buffer_append_float32(send_buffer, ratio, 1e6, &ind);
send_buffer[ind++] = inverted;
commands_send_packet(send_buffer, ind);
} else {
ind = 0;
send_buffer[ind++] = COMM_DETECT_ENCODER;
buffer_append_float32(send_buffer, 1001.0, 1e6, &ind);
buffer_append_float32(send_buffer, 0.0, 1e6, &ind);
send_buffer[ind++] = false;
commands_send_packet(send_buffer, ind);
}
}
break;
case COMM_DETECT_HALL_FOC: {
mcconf = *mc_interface_get_configuration();
if (mcconf.m_sensor_port_mode == SENSOR_PORT_MODE_HALL) {
mcconf_old = mcconf;
ind = 0;
float current = buffer_get_float32(data, 1e3, &ind);
mcconf.motor_type = MOTOR_TYPE_FOC;
mcconf.foc_f_sw = 10000.0;
mcconf.foc_current_kp = 0.01;
mcconf.foc_current_ki = 10.0;
mc_interface_set_configuration(&mcconf);
uint8_t hall_tab[8];
bool res = mcpwm_foc_hall_detect(current, hall_tab);
mc_interface_set_configuration(&mcconf_old);
ind = 0;
send_buffer[ind++] = COMM_DETECT_HALL_FOC;
memcpy(send_buffer + ind, hall_tab, 8);
ind += 8;
send_buffer[ind++] = res ? 0 : 1;
commands_send_packet(send_buffer, ind);
} else {
ind = 0;
send_buffer[ind++] = COMM_DETECT_HALL_FOC;
memset(send_buffer, 255, 8);
ind += 8;
send_buffer[ind++] = 0;
}
}
break;
case COMM_REBOOT:
// Lock the system and enter an infinite loop. The watchdog will reboot.
__disable_irq();
for(;;){};
break;
case COMM_ALIVE:
timeout_reset();
break;
case COMM_GET_DECODED_PPM:
ind = 0;
send_buffer[ind++] = COMM_GET_DECODED_PPM;
buffer_append_int32(send_buffer, (int32_t)(servodec_get_servo(0) * 1000000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(servodec_get_last_pulse_len(0) * 1000000.0), &ind);
commands_send_packet(send_buffer, ind);
break;
case COMM_GET_DECODED_ADC:
ind = 0;
send_buffer[ind++] = COMM_GET_DECODED_ADC;
buffer_append_int32(send_buffer, (int32_t)(app_adc_get_decoded_level() * 1000000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(app_adc_get_voltage() * 1000000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(app_adc_get_decoded_level2() * 1000000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(app_adc_get_voltage2() * 1000000.0), &ind);
commands_send_packet(send_buffer, ind);
break;
case COMM_GET_DECODED_CHUK:
ind = 0;
send_buffer[ind++] = COMM_GET_DECODED_CHUK;
buffer_append_int32(send_buffer, (int32_t)(app_nunchuk_get_decoded_chuk() * 1000000.0), &ind);
commands_send_packet(send_buffer, ind);
break;
case COMM_FORWARD_CAN:
comm_can_send_buffer(data[0], data + 1, len - 1, false);
break;
case COMM_SET_CHUCK_DATA:
ind = 0;
chuck_d_tmp.js_x = data[ind++];
chuck_d_tmp.js_y = data[ind++];
chuck_d_tmp.bt_c = data[ind++];
chuck_d_tmp.bt_z = data[ind++];
chuck_d_tmp.acc_x = buffer_get_int16(data, &ind);
chuck_d_tmp.acc_y = buffer_get_int16(data, &ind);
chuck_d_tmp.acc_z = buffer_get_int16(data, &ind);
app_nunchuk_update_output(&chuck_d_tmp);
break;
case COMM_CUSTOM_APP_DATA:
if (appdata_func) {
appdata_func(data, len);
}
break;
default:
break;
}
}
EVQ_API int
evq_wait (struct event_queue *evq, msec_t timeout)
{
struct event *ev_ready = NULL;
struct win32thr *wth = &evq->head;
struct win32thr *threads = wth->next;
CRITICAL_SECTION *head_cs = &wth->cs;
HANDLE head_signal = wth->signal;
int n = wth->n;
int sig_ready = 0;
DWORD wait_res;
if (threads && win32thr_poll(evq) && evq_is_empty(evq))
return 0;
if (timeout != 0L) {
timeout = timeout_get(wth->tq, timeout, evq->now);
if (timeout == 0L) {
ev_ready = timeout_process(wth->tq, NULL, evq->now);
goto end;
}
}
if (is_WinNT) {
if (!iocp_is_empty(evq))
ev_ready = win32iocp_process(evq, NULL, 0L);
if (ev_ready) {
evq->ev_ready = ev_ready;
timeout = 0L;
} else {
/* head_signal is resetted by IOCP WSARecv/WSASend */
EnterCriticalSection(head_cs);
if (evq->sig_ready)
timeout = 0L;
LeaveCriticalSection(head_cs);
}
}
sys_vm_leave();
wait_res = MsgWaitForMultipleObjects(n + 1, wth->handles, FALSE, timeout,
(evq->win_msg ? QS_ALLEVENTS : 0));
sys_vm_enter();
evq->now = sys_milliseconds();
ev_ready = evq->ev_ready;
if (wait_res == WAIT_TIMEOUT) {
if (ev_ready) goto end;
if (!wth->tq && !evq->sig_ready)
return EVQ_TIMEOUT;
}
if (wait_res == (DWORD) (WAIT_OBJECT_0 + n + 1)) {
struct event *ev = evq->win_msg;
if (ev && !(ev->flags & EVENT_ACTIVE)) {
ev->flags |= EVENT_ACTIVE;
ev->next_ready = ev_ready;
ev_ready = ev;
}
goto end;
}
if (wait_res == WAIT_FAILED)
return EVQ_FAILED;
timeout = evq->now;
if (!iocp_is_empty(evq))
ev_ready = win32iocp_process(evq, ev_ready, timeout);
wth->idx = wait_res;
if (threads) {
EnterCriticalSection(head_cs);
ResetEvent(head_signal);
threads = evq->wth_ready;
evq->wth_ready = NULL;
sig_ready = evq->sig_ready;
evq->sig_ready = 0;
LeaveCriticalSection(head_cs);
if (wait_res == (DWORD) (WAIT_OBJECT_0 + n))
wth = threads;
else
wth->next_ready = threads;
} else {
wth->next_ready = NULL;
if (evq->sig_ready) {
EnterCriticalSection(head_cs);
ResetEvent(head_signal);
sig_ready = evq->sig_ready;
evq->sig_ready = 0;
LeaveCriticalSection(head_cs);
}
}
if (sig_ready)
ev_ready = signal_process_interrupt(evq, sig_ready, ev_ready, timeout);
for (; wth; wth = wth->next_ready) {
HANDLE *hp; /* event handles */
const int idx = wth->idx;
int i;
wth->state = WTHR_SLEEP;
if (wth->tq) {
if (idx == WAIT_TIMEOUT) {
ev_ready = timeout_process(wth->tq, ev_ready, timeout);
continue;
}
}
hp = &wth->handles[idx];
n = wth->n;
i = idx;
if (i >= n) continue; /* some events deleted? */
/* Traverse array of events */
for (; ; ) {
WSANETWORKEVENTS ne;
struct event *ev = wth->events[i];
const int ev_flags = ev->flags;
unsigned int res = 0;
if (!(ev_flags & EVENT_SOCKET)) {
if (ev_flags & EVENT_PID) {
DWORD status;
GetExitCodeProcess(ev->fd, &status);
res = (status << EVENT_EOF_SHIFT_RES);
} else
ResetEvent(ev->fd); /* all events must be manual-reset */
res |= EVENT_READ_RES;
} else if (!WSAEnumNetworkEvents((int) ev->fd, *hp, &ne)) {
if ((ev_flags & EVENT_READ)
&& (ne.lNetworkEvents & WFD_READ))
res = EVENT_READ_RES;
if ((ev_flags & EVENT_WRITE)
&& (ne.lNetworkEvents & WFD_WRITE))
res |= EVENT_WRITE_RES;
if (ne.lNetworkEvents & FD_CLOSE)
res |= EVENT_EOF_RES;
}
if (res) {
ev->flags |= EVENT_ACTIVE | res;
ev->next_ready = ev_ready;
ev_ready = ev;
if (ev_flags & EVENT_ONESHOT) {
win32thr_del(wth, ev);
--i, --n, --hp;
} else if (ev->tq && !(ev_flags & EVENT_TIMEOUT_MANUAL))
timeout_reset(ev, timeout);
}
/* skip inactive events */
do {
if (++i == n)
goto end_thread;
} while (WaitForSingleObject(*(++hp), 0) != WAIT_OBJECT_0);
}
end_thread:
((void) 0); /* avoid warning */
}
/* always check window messages */
{
struct event *ev = evq->win_msg;
if (ev && GetQueueStatus(QS_ALLEVENTS)) {
ev->next_ready = ev_ready;
ev_ready = ev;
}
}
if (!ev_ready)
return (wait_res == WAIT_TIMEOUT && !sig_ready)
? EVQ_TIMEOUT : 0;
end:
evq->ev_ready = ev_ready;
return 0;
}
