int tims(ClientSession_t *session)
{
/* returns values:
* 0 to quit
* -1 on failure
* 1 on success */
char *arg;
size_t scriptlen = 0;
int ret;
char *script = NULL, *scriptname = NULL;
sort_result_t *sort_result = NULL;
clientbase_t *ci = session->ci;
TRACE(TRACE_DEBUG,"[%p] [%d][%s]", session, session->command_type, commands[session->command_type]);
switch (session->command_type) {
case TIMS_LOUT:
ci_write(ci, "OK \"Bye.\"\r\n");
session->state = QUIT;
return 1;
case TIMS_STLS:
ci_write(ci, "NO\r\n");
return 1;
case TIMS_CAPA:
send_greeting(session);
return 1;
case TIMS_AUTH:
/* We currently only support plain authentication,
* which means that the command we accept will look
* like this: Authenticate "PLAIN" "base64-password"
* */
session->args = g_list_first(session->args);
if (! (session->args && session->args->data))
return 1;
arg = (char *)session->args->data;
if (strcasecmp(arg, "PLAIN") == 0) {
int i = 0;
u64_t useridnr;
if (! g_list_next(session->args))
return tims_error(session, "NO \"Missing argument.\"\r\n");
session->args = g_list_next(session->args);
arg = (char *)session->args->data;
char **tmp64 = NULL;
tmp64 = base64_decodev(arg);
if (tmp64 == NULL)
return tims_error(session, "NO \"SASL decode error.\"\r\n");
for (i = 0; tmp64[i] != NULL; i++)
;
if (i < 3)
tims_error(session, "NO \"Too few encoded SASL arguments.\"\r\n");
/* The protocol specifies that the base64 encoding
* be made up of three parts: proxy, username, password
* Between them are NULLs, which are conveniently encoded
* by the base64 process... */
if (auth_validate(ci, tmp64[1], tmp64[2], &useridnr) == 1) {
ci_authlog_init(ci, THIS_MODULE, tmp64[1], AUTHLOG_ACT);
ci_write(ci, "OK\r\n");
session->state = AUTH;
session->useridnr = useridnr;
session->username = g_strdup(tmp64[1]);
session->password = g_strdup(tmp64[2]);
client_session_set_timeout(session, server_conf->timeout);
} else {
ci_authlog_init(ci, THIS_MODULE, tmp64[1], AUTHLOG_ERR);
g_strfreev(tmp64);
return tims_error(session, "NO \"Username or password incorrect.\"\r\n");
}
g_strfreev(tmp64);
} else
return tims_error(session, "NO \"Authentication scheme not supported.\"\r\n");
return 1;
case TIMS_PUTS:
if (session->state != AUTH) {
ci_write(ci, "NO \"Please authenticate first.\"\r\n");
break;
}
session->args = g_list_first(session->args);
if (! (session->args && session->args->data))
return 1;
scriptname = (char *)session->args->data;
session->args = g_list_next(session->args);
assert(session->args);
script = (char *)session->args->data;
scriptlen = strlen(script);
TRACE(TRACE_INFO, "Client sending script of length [%ld]", scriptlen);
if (scriptlen >= UINT_MAX)
return tims_error(session, "NO \"Invalid script length.\"\r\n");
if (dm_sievescript_quota_check(session->useridnr, scriptlen))
return tims_error(session, "NO \"Script exceeds available space.\"\r\n");
/* Store the script temporarily,
* validate it, then rename it. */
if (dm_sievescript_add(session->useridnr, "@!temp-script!@", script)) {
dm_sievescript_delete(session->useridnr, "@!temp-script!@");
return tims_error(session, "NO \"Error inserting script.\"\r\n");
}
sort_result = sort_validate(session->useridnr, "@!temp-script!@");
if (sort_result == NULL) {
dm_sievescript_delete(session->useridnr, "@!temp-script!@");
return tims_error(session, "NO \"Error inserting script.\"\r\n");
} else if (sort_get_error(sort_result) > 0) {
dm_sievescript_delete(session->useridnr, "@!temp-script!@");
return tims_error(session, "NO \"Script error: %s.\"\r\n", sort_get_errormsg(sort_result));
}
/* According to the draft RFC, a script with the same
* name as an existing script should [atomically] replace it. */
if (dm_sievescript_rename(session->useridnr, "@!temp-script!@", scriptname))
return tims_error(session, "NO \"Error inserting script.\"\r\n");
ci_write(ci, "OK \"Script successfully received.\"\r\n");
break;
case TIMS_SETS:
if (session->state != AUTH) {
ci_write(ci, "NO \"Please authenticate first.\"\r\n");
break;
}
scriptname = NULL;
session->args = g_list_first(session->args);
if ((session->args && session->args->data))
scriptname = (char *)session->args->data;
if (strlen(scriptname)) {
if (! dm_sievescript_activate(session->useridnr, scriptname))
ci_write(ci, "NO \"Error activating script.\"\r\n");
else
ci_write(ci, "OK \"Script activated.\"\r\n");
} else {
ret = dm_sievescript_get(session->useridnr, &scriptname);
if (scriptname == NULL) {
ci_write(ci, "OK \"No scripts are active at this time.\"\r\n");
} else {
if (! dm_sievescript_deactivate(session->useridnr, scriptname))
ci_write(ci, "NO \"Error deactivating script.\"\r\n");
else
ci_write(ci, "OK \"All scripts deactivated.\"\r\n");
g_free(scriptname);
}
}
return 1;
case TIMS_GETS:
if (session->state != AUTH) {
ci_write(ci, "NO \"Please authenticate first.\"\r\n");
break;
}
session->args = g_list_first(session->args);
if (! (session->args && session->args->data))
return 1;
scriptname = (char *)session->args->data;
if (! strlen(scriptname))
return tims_error(session, "NO \"Script name required.\"\r\n");
ret = dm_sievescript_getbyname(session->useridnr, scriptname, &script);
if (script == NULL) {
return tims_error(session, "NO \"Script not found.\"\r\n");
} else if (ret < 0) {
g_free(script);
return tims_error(session, "NO \"Internal error.\"\r\n");
} else {
ci_write(ci, "{%u+}\r\n", (unsigned int)strlen(script));
ci_write(ci, "%s\r\n", script);
ci_write(ci, "OK\r\n");
g_free(script);
}
return 1;
case TIMS_DELS:
if (session->state != AUTH) {
ci_write(ci, "NO \"Please authenticate first.\"\r\n");
break;
}
session->args = g_list_first(session->args);
if (! (session->args && session->args->data))
return 1;
scriptname = (char *)session->args->data;
if (! strlen(scriptname))
return tims_error(session, "NO \"Script name required.\"\r\n");
if (! dm_sievescript_delete(session->useridnr, scriptname))
return tims_error(session, "NO \"Error deleting script.\"\r\n");
else
ci_write(ci, "OK \"Script deleted.\"\r\n", scriptname);
return 1;
case TIMS_SPAC:
if (session->state != AUTH) {
ci_write(ci, "NO \"Please authenticate first.\"\r\n");
break;
}
// Command is in format: HAVESPACE "scriptname" 12345
// TODO: this is a fake
if (dm_sievescript_quota_check(session->useridnr, 12345) == DM_SUCCESS)
ci_write(ci, "OK (QUOTA)\r\n");
else
ci_write(ci, "NO (QUOTA) \"Quota exceeded\"\r\n");
return 1;
case TIMS_LIST:
if (session->state != AUTH) {
ci_write(ci, "NO \"Please authenticate first.\"\r\n");
break;
}
GList *scriptlist = NULL;
if (dm_sievescript_list (session->useridnr, &scriptlist) < 0) {
ci_write(ci, "NO \"Internal error.\"\r\n");
} else {
if (g_list_length(scriptlist) == 0) {
/* The command hasn't failed, but there aren't any scripts */
ci_write(ci, "OK \"No scripts found.\"\r\n");
} else {
scriptlist = g_list_first(scriptlist);
while (scriptlist) {
sievescript_info_t *info = (sievescript_info_t *) scriptlist->data;
ci_write(ci, "\"%s\"%s\r\n", info->name, (info-> active == 1 ? " ACTIVE" : ""));
if (! g_list_next(scriptlist)) break;
scriptlist = g_list_next(scriptlist);
}
ci_write(ci, "OK\r\n");
}
g_list_destroy(scriptlist);
}
return 1;
default:
return tims_error(session, "NO \"What are you trying to say here?\"\r\n");
}
if (sort_result)
sort_free_result(sort_result);
return 1;
}
int tims_tokenizer(ClientSession_t *session, char *buffer)
{
int command_type = 0;
char *command, *value = NULL;
if (! session->command_type) {
command = buffer;
strip_crlf(command);
g_strstrip(command);
if (! strlen(command)) return FALSE; /* ignore empty commands */
value = strstr(command, " "); /* look for the separator */
if (value != NULL) {
*value++ = '\0'; /* set a \0 on the command end and skip space */
if (strlen(value) == 0)
value = NULL; /* no value specified */
}
for (command_type = TIMS_LOUT; command_type < TIMS_END; command_type++)
if (strcasecmp(command, commands[command_type]) == 0)
break;
/* commands that are allowed to have no arguments */
if ((value == NULL) && !(command_type <= TIMS_LIST) && (command_type < TIMS_END))
return tims_error(session, "NO \"This command requires an argument.\"\r\n");
TRACE (TRACE_DEBUG, "command [%s] value [%s]\n", command, value);
session->command_type = command_type;
}
if (session->ci->rbuff_size) {
size_t l = strlen(buffer);
size_t n = min(session->ci->rbuff_size, l);
g_string_append_len(session->rbuff, buffer, n);
session->ci->rbuff_size -= n;
if (! session->ci->rbuff_size) {
session->args = g_list_append(session->args, g_strdup(session->rbuff->str));
g_string_printf(session->rbuff,"%s","");
session->parser_state = TRUE;
}
TRACE(TRACE_DEBUG, "state [%d], size [%ld]", session->parser_state, session->ci->rbuff_size);
return session->parser_state;
}
if (value) {
char *s = value;
int i = 0;
char p = 0, c = 0;
gboolean inquote = FALSE;
GString *t = g_string_new("");
while ((c = s[i++])) {
if (inquote) {
if (c == '"' && p != '\\') {
session->args = g_list_append(session->args, t->str);
g_string_free(t, FALSE);
t = g_string_new("");
inquote = FALSE;
} else {
g_string_append_c(t,c);
}
p = c;
continue;
}
if (c == '"' && p != '\\') {
inquote = TRUE;
p = c;
continue;
}
if (c == ' ' && p != '\\') {
p = c;
continue;
}
if (c == '{') { // a literal...
session->ci->rbuff_size = strtoull(s+i, NULL, 10);
return FALSE;
}
g_string_append_c(t,c);
p = c;
}
if (t->len) {
session->args = g_list_append(session->args, t->str);
g_string_free(t,FALSE);
}
}
session->parser_state = TRUE;
session->args = g_list_first(session->args);
while (session->args) {
TRACE(TRACE_DEBUG,"arg: [%s]", (char *)session->args->data);
if (! g_list_next(session->args))
break;
session->args = g_list_next(session->args);
}
//session->args = g_list_append(session->args,g_strdup(value));
TRACE(TRACE_DEBUG, "[%p] cmd [%d], state [%d] [%s]", session, session->command_type, session->parser_state, buffer);
return session->parser_state;
}
/* rtnet_sendmsg - Called by the TiMS driver to send a message over RTnet */
int rtnet_sendmsg(rtdm_user_info_t *user_info, const struct msghdr *msg)
{
int i;
int ret;
tims_msg_head *p_head = msg->msg_iov[0].iov_base;
uint32_t msglen = p_head->msglen;
tims_msg_head head_tmp;
struct iovec iov[msg->msg_iovlen + 1];
unsigned int remain;
struct msghdr rtnet_msg;
struct sockaddr_in dest_addr;
for (i = 0; i < rtnet.mbxRouteNum; i++) {
if (rtnet.mbxRoute[i].mbx == p_head->dest) {
dest_addr.sin_family = AF_INET;
dest_addr.sin_port = htons(TIMS_MSG_ROUTER_PORT);
dest_addr.sin_addr.s_addr = rtnet.mbxRoute[i].ip;
rtnet_msg.msg_name = &dest_addr;
rtnet_msg.msg_namelen = sizeof(dest_addr);
rtnet_msg.msg_control = NULL;
rtnet_msg.msg_controllen = 0;
rtnet_msg.msg_flags = 0;
/* Check whether the length of the message exceeds the size of an
* UDP packet. */
if (msglen > TIMS_RTNET_MAX_MSG_SIZE) {
/* We have to split the message up. */
memcpy(&head_tmp, p_head, TIMS_HEADLEN);
memcpy(&iov[1], msg->msg_iov, sizeof(struct iovec) *
msg->msg_iovlen);
/* The head of the first message part is not modified aside
* from one bit in the flags field. */
p_head->flags |= TIMS_RTNET_SPLIT_START;
rtnet_msg.msg_iov = &iov[1];
rtnet_msg.msg_iovlen = msg->msg_iovlen;
/* The iovlens have to be cut so that they reflect exactly a
* length of a UDP packet minus the IP and UDP header len. */
remain = rtnet_limit_msg(&rtnet_msg, TIMS_RTNET_MAX_MSG_SIZE);
/* Send the initial part of the message. */
ret = rtdm_sendmsg(rtnet.fd, &rtnet_msg, 0);
if (ret < TIMS_RTNET_MAX_MSG_SIZE) {
tims_error("[RTnet]: %x -> %x: Can't forward first part "
"of splitted message, type %i, msglen %i, "
"msg part len %i. rt_socket_sendto(), code = "
"%d\n", p_head->src, p_head->dest,
p_head->type, p_head->msglen,
TIMS_RTNET_MAX_MSG_SIZE, ret);
return ret;
}
tims_dbginfo("%x -> %x, send first part of splitted message "
"(%d bytes in whole) over RTnet\n", p_head->src,
p_head->dest, p_head->msglen);
/* Prepare head_tmp which is prepended to every consecutive
* part of the splitted message. */
head_tmp.flags |= TIMS_RTNET_SPLIT;
head_tmp.msglen = TIMS_RTNET_MAX_MSG_SIZE;
head_tmp.seq_nr = 1;
rtnet_msg.msg_iov = iov;
/* This is important as we prepend an additional iovec at the
* front of the array. */
rtnet_msg.msg_iovlen++;
msglen -= TIMS_RTNET_MAX_MSG_SIZE;
do {
iov[0].iov_base = &head_tmp;
iov[0].iov_len = TIMS_HEADLEN;
if (msglen <= TIMS_RTNET_MAX_MSG_SIZE - TIMS_HEADLEN) {
/* Final part of the splitted message. */
head_tmp.msglen= msglen + TIMS_HEADLEN;
msglen = 0;
head_tmp.flags |= TIMS_RTNET_SPLIT_STOP;
} else
msglen = msglen - TIMS_RTNET_MAX_MSG_SIZE +
TIMS_HEADLEN;
/* Again, limit the message part to be sent. But before,
* fix the iov_len belonging to the last buffer sent. */
iov[rtnet_msg.msg_iovlen - 1].iov_len =
msg->msg_iov[rtnet_msg.msg_iovlen - 2].iov_len -
remain;
remain = rtnet_limit_msg(&rtnet_msg, head_tmp.msglen);
/* Send a consecutive part of the message. */
ret = rtdm_sendmsg(rtnet.fd, &rtnet_msg, 0);
if (ret < head_tmp.msglen) {
tims_error("[RTnet]: %x -> %x: Can't forward part %i "
"of splitted message, type %i, msglen %i, msg "
"part len %i. rt_socket_sendto(), code = %d\n",
head_tmp.src, head_tmp.dest, head_tmp.seq_nr,
head_tmp.type, p_head->msglen, head_tmp.msglen,
ret);
return ret;
}
tims_dbginfo("%x -> %x, send consecutive part of "
"splitted message (%d bytes) "
"over RTnet\n", p_head->src, p_head->dest,
head_tmp.msglen);
head_tmp.seq_nr++;
} while (msglen);
/* Adjust return value to the length of the whole message. */
ret = p_head->msglen;
} else {
/* The message fits into one RTnet UDP packet. */
rtnet_msg.msg_iov = msg->msg_iov;
rtnet_msg.msg_iovlen = msg->msg_iovlen;
ret = rtdm_sendmsg(rtnet.fd, &rtnet_msg, 0);
if (ret < p_head->msglen) {
tims_error("[RTnet]: %x -> %x: Can't forward message, "
"type %i, msglen %i. "
"rt_socket_sendto(), code = %d\n",
p_head->src, p_head->dest, p_head->type,
p_head->msglen, ret);
return ret;
}
}
tims_dbginfo("%x -> %x, send global message (%d bytes) over RTnet"
"\n", p_head->src, p_head->dest, p_head->msglen);
return ret;
}
}
return -ENODEV; // destination address not found in table
}
static int rtnet_recv_message(tims_mbx* p_recv_mbx, tims_msg_head *head)
{
tims_mbx_slot* recv_slot = NULL;
#if 0
tims_msg_head* head_part = NULL;
#endif
/* We need an additional iovec for heads of consecutive packets of
splitted messages, therefore + 1. */
struct iovec iov[get_max_pages(head->msglen > TIMS_RTNET_MAX_MSG_SIZE ?
TIMS_RTNET_MAX_MSG_SIZE + 1: head->msglen + 1)];
tims_msg_head consec_head;
struct msghdr recv_msg;
unsigned long p_recv_map = 0;
// void* p_recv = NULL;
unsigned long recv_page = 0;
unsigned long recv_bytes = head->msglen;
unsigned long recv_bytes_usr;
int ret;
int free_in_page = 0;
int akt_copy_size = 0;
/* Check if the packet is a consecutive part of a splitted message. */
if (head->flags & TIMS_RTNET_SPLIT) {
/* A splitted msg continues. We have to put it into an already
* existing write slot. */
recv_slot = tims_get_cont_write_slot(p_recv_mbx, head->src);
if (!recv_slot) {
tims_error("[RTnet]: Received a consecutive part of a splitted "
"msg without having its beginning! %08x -> %08x \n",
head->src, p_recv_mbx->address);
rtdm_recv(rtnet.fd, head, 0, 0); // clean up
return -EBADSLT;
}
/* Check the sequence number. */
if (head->seq_nr != ++recv_slot->seq_nr) {
tims_error("[RTnet]: Received a consecutive part of a splitted "
"msg with wrong sequence number %d, should be %d "
"(%08x -> %08x). Removing incomplete message.\n",
head->seq_nr, recv_slot->seq_nr, head->src,
p_recv_mbx->address);
ret = -ENOMSG;
goto recvmsg_error_clean;
}
} else {
/* A new message. We have to ensure that there is no stale incomplete
* message in the write list of this mbx from that src address. */
recv_slot = tims_get_cont_write_slot(p_recv_mbx, head->src);
if (recv_slot) {
tims_warn("[RTnet]: Received a new msg from %08x to %08x while "
"waiting for the end of a splitted msg; now removing "
"it.\n", head->src, p_recv_mbx->address);
/* Remove write slot with incomplete message. */
tims_put_write_slot_error(p_recv_mbx, recv_slot);
}
// get local mailbox slot
recv_slot = tims_get_write_slot(p_recv_mbx, head->priority);
if (!recv_slot) {
tims_error("[RTnet]: No free write slot in mailbox %08x \n",
p_recv_mbx->address);
rtdm_recv(rtnet.fd, head, 0, 0); // clean up
return -ENOSPC;
}
/* If a splitted msg starts adjust recv_bytes. If not bail out if
* the msglen is greater than TIMS_RTNET_MAX_MSG_SIZE. */
if (head->flags & TIMS_RTNET_SPLIT_START) {
recv_bytes = TIMS_RTNET_MAX_MSG_SIZE;
recv_slot->seq_nr = 0;
} else if (unlikely(recv_bytes > TIMS_RTNET_MAX_MSG_SIZE)) {
tims_error("[RTnet]: Msg for mbx %08x has more than %d bytes, "
"too large for RTnet!\n", p_recv_mbx->address,
TIMS_RTNET_MAX_MSG_SIZE);
ret = -EFBIG;
goto recvmsg_error_clean;
}
}
//
// create msg vector
//
memset(&recv_msg, 0, sizeof(struct msghdr));
if (test_bit(TIMS_MBX_BIT_USRSPCBUFFER, &p_recv_mbx->flags)) {
// receive mailbox is in userpace
if (head->flags & TIMS_RTNET_SPLIT) {
iov[0].iov_base = &consec_head;
iov[0].iov_len = TIMS_HEADLEN;
p_recv_map = recv_slot->next_part_map;
recv_page = recv_slot->next_map_idx;
recv_bytes_usr = recv_bytes - TIMS_HEADLEN;
recv_msg.msg_iovlen = 1;
} else {
p_recv_map = recv_slot->p_head_map;
// p_recv = recv_slot->p_head;
recv_page = recv_slot->map_idx;
recv_bytes_usr = recv_bytes;
recv_msg.msg_iovlen = 0;
recv_slot->next_part_map = p_recv_map;
}
recv_msg.msg_iov = iov;
while (recv_bytes_usr) {
free_in_page = (p_recv_map & PAGE_MASK) + PAGE_SIZE - p_recv_map;
akt_copy_size = free_in_page > recv_bytes_usr ?
recv_bytes_usr : free_in_page;
iov[recv_msg.msg_iovlen].iov_base = (void *)p_recv_map;
iov[recv_msg.msg_iovlen].iov_len = akt_copy_size;
recv_msg.msg_iovlen++;
free_in_page -= akt_copy_size;
recv_bytes_usr -= akt_copy_size;
recv_slot->next_part_map += akt_copy_size;
if (!free_in_page)
{
recv_page++;
if (recv_slot->p_mbx->p_mapInfo[recv_page].mapped)
{
p_recv_map = recv_slot->p_mbx->p_mapInfo[recv_page].virtual;
}
else
{
ret = -ENOMEM;
goto recvmsg_error_clean;
}
recv_slot->next_part_map = p_recv_map;
}
// userpace task receives data from userspace mailbox
static unsigned long tims_copy_userslot_user(rtdm_user_info_t *user_info,
tims_mbx_slot *slot,
const struct msghdr *msg)
{
unsigned long akt_copy_size = 0;
unsigned long copy_bytes = 0;
unsigned long bytes_copied = 0;
unsigned long bytes_in_page = 0;
int i;
unsigned long ret;
unsigned long p_src_map = slot->p_head_map;
void *p_src = slot->p_head;
void *p_dest = NULL;
unsigned long src_page = slot->map_idx;
tims_msg_head *p_head = (tims_msg_head *)slot->p_head_map;
unsigned long databytes = p_head->msglen - TIMS_HEADLEN;
for (i=0; i<2; i++)
{
if (!i)
copy_bytes = TIMS_HEADLEN;
else
copy_bytes = databytes;
p_dest = msg->msg_iov[i].iov_base;
// check destination pointer
ret = rtdm_rw_user_ok(user_info, p_dest, copy_bytes);
if (!ret)
{
tims_error("ERROR: userspace destination 0x%p (%lu bytes) NOT OK \n",
p_dest, copy_bytes);
return ret;
}
// copy data
while (copy_bytes)
{
bytes_in_page = get_remain_bytes_in_page(p_src_map);
akt_copy_size = min_t(unsigned long, bytes_in_page, copy_bytes);
ret = rtdm_copy_to_user(user_info, p_dest, (void *)p_src_map,
akt_copy_size);
if (ret)
{
if (ret < 0)
{
tims_error("ERROR while copy userbuffer -> user, "
"code = %lu \n", ret);
}
else
{
tims_error("ERROR while copy userbuffer -> user, "
"only %lu/%lu bytes were copied \n",
akt_copy_size - ret, akt_copy_size);
}
return ret;
}
bytes_in_page -= akt_copy_size;
copy_bytes -= akt_copy_size;
p_src_map += akt_copy_size;
p_src += akt_copy_size;
p_dest += akt_copy_size;
bytes_copied += akt_copy_size;
if (!bytes_in_page)
{
src_page++;
if (slot->p_mbx->p_mapInfo[src_page].mapped)
{
p_src_map = slot->p_mbx->p_mapInfo[src_page].virtual;
}
else
{
return -EFAULT;
}
}
static void sync_task_func(void *arg)
{
int ret;
rtdm_lockctx_t lock_ctx;
nanosecs_abs_t timestamp;
nanosecs_abs_t timestamp_master;
rtser_event_t ser_rx_event;
can_frame_t can_frame = {
.can_id = clock_sync_can_id,
.can_dlc = sizeof(timestamp),
};
struct iovec iov = {
.iov_base = &can_frame,
.iov_len = sizeof(can_frame_t),
};
struct msghdr msg = {
.msg_name = NULL,
.msg_namelen = 0,
.msg_iov = &iov,
.msg_iovlen = 1,
.msg_control = NULL,
.msg_controllen = 0,
};
if (clock_sync_mode == SYNC_CAN_SLAVE) {
msg.msg_control = ×tamp;
msg.msg_controllen = sizeof(timestamp);
}
while (1) {
switch (clock_sync_mode) {
case SYNC_SER_MASTER:
timestamp = cpu_to_be64(rtdm_clock_read());
ret = sync_dev_ctx->ops->write_rt(sync_dev_ctx, NULL,
×tamp, sizeof(timestamp));
if (ret != sizeof(timestamp)) {
tims_error("[CLOCK SYNC]: can't write serial time stamp, "
"code = %d\n", ret);
goto exit_task;
}
rtdm_task_wait_period();
break;
case SYNC_SER_SLAVE:
ret = sync_dev_ctx->ops->ioctl_rt(sync_dev_ctx, NULL,
RTSER_RTIOC_WAIT_EVENT, &ser_rx_event);
if (ret < 0) {
tims_error("[CLOCK SYNC]: can't read serial time stamp, "
"code = %d\n", ret);
goto exit_task;
}
ret = sync_dev_ctx->ops->read_rt(sync_dev_ctx, NULL,
×tamp_master, sizeof(timestamp_master));
if (ret != sizeof(timestamp_master)) {
tims_error("[CLOCK SYNC]: can't read serial time stamp, "
"code = %d\n", ret);
goto exit_task;
}
timestamp_master = be64_to_cpu(timestamp_master);
rtdm_lock_get_irqsave(&sync_lock, lock_ctx);
clock_offset =
timestamp_master - ser_rx_event.rxpend_timestamp;
rtdm_lock_put_irqrestore(&sync_lock, lock_ctx);
break;
case SYNC_CAN_MASTER:
// workaround for kernel working on user data
iov.iov_len = sizeof(can_frame_t);
iov.iov_base = &can_frame;
// workaround end
*(nanosecs_abs_t *)can_frame.data =
cpu_to_be64(rtdm_clock_read());
ret = sync_dev_ctx->ops->sendmsg_rt(sync_dev_ctx, NULL,
&msg, 0);
if (ret < 0) {
tims_error("[CLOCK SYNC]: can't send CAN time stamp, "
"code = %d\n", ret);
goto exit_task;
}
rtdm_task_wait_period();
break;
case SYNC_CAN_SLAVE:
// workaround for kernel working on user data
iov.iov_len = sizeof(can_frame_t);
iov.iov_base = &can_frame;
// workaround end
ret = sync_dev_ctx->ops->recvmsg_rt(sync_dev_ctx, NULL,
&msg, 0);
if (ret < 0) {
tims_error("[CLOCK SYNC]: can't receive CAN time stamp, "
"code = %d\n", ret);
return;
}
timestamp_master =
be64_to_cpu(*(nanosecs_abs_t *)can_frame.data);
rtdm_lock_get_irqsave(&sync_lock, lock_ctx);
clock_offset = timestamp_master - timestamp;
rtdm_lock_put_irqrestore(&sync_lock, lock_ctx);
break;
}
}
exit_task:
rtdm_context_unlock(sync_dev_ctx);
}
static __initdata char *mode_str[] = {
"Local Clock", "RTnet", "CAN Master", "CAN Slave",
"Serial Master", "Serial Slave"
};
static __initdata struct rtser_config sync_serial_config = {
.config_mask = RTSER_SET_BAUD | RTSER_SET_FIFO_DEPTH |
RTSER_SET_TIMESTAMP_HISTORY | RTSER_SET_EVENT_MASK,
.baud_rate = 115200,
.fifo_depth = RTSER_FIFO_DEPTH_8,
.timestamp_history = RTSER_RX_TIMESTAMP_HISTORY,
.event_mask = RTSER_EVENT_RXPEND,
};
int __init tims_clock_init(void)
{
struct can_filter filter;
int nr_filters = 1;
struct ifreq can_ifr;
struct sockaddr_can can_addr;
int ret;
if (clock_sync_mode < SYNC_NONE || clock_sync_mode > SYNC_SER_SLAVE) {
tims_error("invalid clock_sync_mode %d", clock_sync_mode);
return -EINVAL;
}
printk("TIMS: clock sync mode is %s\n", mode_str[clock_sync_mode]);
printk("TIMS: clock sync dev is %s\n", clock_sync_dev);
rtdm_lock_init(&sync_lock);
switch(clock_sync_mode) {
case SYNC_NONE:
return 0;
case SYNC_RTNET:
sync_dev_fd = rt_dev_open(clock_sync_dev, O_RDONLY);
if (sync_dev_fd < 0)
goto sync_dev_error;
set_bit(TIMS_INIT_BIT_SYNC_DEV, &init_flags);
break;
case SYNC_CAN_MASTER:
case SYNC_CAN_SLAVE:
sync_dev_fd = rt_dev_socket(PF_CAN, SOCK_RAW, 0);
if (sync_dev_fd < 0) {
tims_error("[CLOCK SYNC]: error opening CAN socket: %d\n",
sync_dev_fd);
return sync_dev_fd;
}
set_bit(TIMS_INIT_BIT_SYNC_DEV, &init_flags);
strcpy(can_ifr.ifr_name, clock_sync_dev);
ret = rt_dev_ioctl(sync_dev_fd, SIOCGIFINDEX, &can_ifr);
if (ret) {
tims_info("[CLOCK SYNC]: error resolving CAN interface: %d\n",
ret);
return ret;
}
if (clock_sync_mode == SYNC_CAN_MASTER)
nr_filters = 0;
else {
filter.can_id = clock_sync_can_id;
filter.can_mask = 0xFFFFFFFF;
}
ret = rt_dev_setsockopt(sync_dev_fd, SOL_CAN_RAW, CAN_RAW_FILTER,
&filter, nr_filters*sizeof(can_filter_t));
if (ret < 0)
goto config_error;
/* Bind socket to default CAN ID */
can_addr.can_family = AF_CAN;
can_addr.can_ifindex = can_ifr.ifr_ifindex;
ret = rt_dev_bind(sync_dev_fd, (struct sockaddr *)&can_addr,
sizeof(can_addr));
if (ret < 0)
goto config_error;
/* Enable timestamps for incoming packets */
ret = rt_dev_ioctl(sync_dev_fd, RTCAN_RTIOC_TAKE_TIMESTAMP,
RTCAN_TAKE_TIMESTAMPS);
if (ret < 0)
goto config_error;
/* Calculate transmission delay */
ret = rt_dev_ioctl(sync_dev_fd, SIOCGCANBAUDRATE, &can_ifr);
if (ret < 0)
goto config_error;
/* (47+64 bit) * 1.000.000.000 (ns/sec) / baudrate (bit/s) */
sync_delay = 1000 * (111000000 / can_ifr.ifr_ifru.ifru_ivalue);
break;
case SYNC_SER_MASTER:
case SYNC_SER_SLAVE:
sync_dev_fd = rt_dev_open(clock_sync_dev, O_RDWR);
if (sync_dev_fd < 0)
goto sync_dev_error;
set_bit(TIMS_INIT_BIT_SYNC_DEV, &init_flags);
ret = rt_dev_ioctl(sync_dev_fd, RTSER_RTIOC_SET_CONFIG,
&sync_serial_config);
if (ret < 0)
goto config_error;
/* (80 bit) * 1.000.000.000 (ns/sec) / baudrate (bit/s) */
sync_delay = 1000 * (80000000 / sync_serial_config.baud_rate);
break;
}
sync_dev_ctx = rtdm_context_get(sync_dev_fd);
if (clock_sync_mode != SYNC_RTNET) {
ret = rtdm_task_init(&sync_task, "TIMSClockSync", sync_task_func,
NULL, CLOCK_SYNC_PRIORITY, CLOCK_SYNC_PERIOD);
if (ret < 0)
return ret;
set_bit(TIMS_INIT_BIT_SYNC_TASK, &init_flags);
}
return 0;
sync_dev_error:
tims_error("[CLOCK SYNC]: cannot open %s\n", clock_sync_dev);
return sync_dev_fd;
config_error:
tims_info("[CLOCK SYNC]: error configuring sync device: %d\n", ret);
return ret;
}
void tims_clock_cleanup(void)
{
if (test_and_clear_bit(TIMS_INIT_BIT_SYNC_DEV, &init_flags))
rt_dev_close(sync_dev_fd);
if (test_and_clear_bit(TIMS_INIT_BIT_SYNC_TASK, &init_flags))
rtdm_task_join_nrt(&sync_task, 100);
}
