/**
* @brief send a message to the @p fd file descriptor
* @param fd the file descritor to write on
* @param msg the message to send
* @returns 0 on success, -1 on error.
*/
int send_message(int fd, message *msg) {
if(write_wrapper(fd, &(msg->head), sizeof(struct msg_header)))
return -1;
if(write_wrapper(fd, msg->data, msg->head.size))
return -1;
#ifndef NDEBUG
print( DEBUG, "message sent (fd=%d)", fd );
dump_message(msg);
#endif
return 0;
}
/**
* @brief handle a received message for a child
* @param conn the connection that sent @p m
* @param m the received ::message
* @returns 0 on success, -1 on error.
*/
int on_child_message(conn_node *conn, message *m) {
child_node *c;
int write_error, blind_error;
write_error = blind_error = 0;
pthread_mutex_lock(&(conn->children.control.mutex));
for(c=(child_node *) conn->children.list.head;c && c->id != m->head.id; c=(child_node *) c->next);
if(c) {
if(c->handler->input_parser) {
c->handler->input_parser(c, m);
} else if(c->handler->have_stdin) {
write_error = write_wrapper(c->stdin_fd, m->data, m->head.size);
} else {
blind_error = 1;
}
}
pthread_mutex_unlock(&(conn->children.control.mutex));
if(!c) {
print( ERROR, "child #%u not found", m->head.id );
} else if(write_error) {
print( ERROR, "cannot send message to child #%u", m->head.id );
} else if(blind_error) {
print( ERROR, "received message for blind child #%u", m->head.id );
} else {
return 0;
}
return -1;
}
static int
select_write_wrapper(handle_t fd , struct timeval * time, unsigned char * data , int len)
{
fd_set wrfs;
int ret;
FD_ZERO( &wrfs );
FD_SET(fd , &wrfs);
ret = select( fd+1, NULL, &wrfs, NULL, time);
if (ret<0) {
fprintf(stderr,
"vrpn_atmellib::select_write_wrapper::error during waiting for writing permission: %s (%i)\n",
strerror(errno) ,errno);
return ATMELLIB_ERROR_NORESPVAL;
}
else if (ret==0) {
// time expired
fprintf(stderr, "vrpn_atmellib::select_write_wrapper: timed out in wrapper\n" );
return ATMELLIB_ERROR_NORESPVAL;
}
// successful select -> write down command
// write twice to ensure that the atmel receives it
//write_wrapper(fd , (void *) data, len);
return write_wrapper(fd , (void *) data, len);
}
/* ----------------------------------------------------- */
void
oflush(void)
{
if (obufsize) {
#ifdef CONVERT
write_wrapper(1, outbuf, obufsize);
#else
write(1, outbuf, obufsize);
#endif
obufsize = 0;
}
#ifdef DBG_OUTRPT
// if (0)
{
static char xbuf[128];
sprintf(xbuf, ESC_STR "[s" ESC_STR "[H" " [%lu/%lu] " ESC_STR "[u",
szLastOutput, szTotalOutput);
write(1, xbuf, strlen(xbuf));
szLastOutput = 0;
}
#endif // DBG_OUTRPT
fsync(1);
}
void
output(const char *s, int len)
{
#ifdef DBG_OUTRPT
int i = 0;
if (fakeEscape)
for (i = 0; i < obufsize; i++)
outbuf[i] = fakeEscFilter(outbuf[i]);
szTotalOutput += len;
szLastOutput += len;
#endif // DBG_OUTRPT
/* Invalid if len >= OBUFSIZE */
assert(len<OBUFSIZE);
if (obufsize + len > OBUFSIZE) {
#ifdef CONVERT
write_wrapper(1, outbuf, obufsize);
#else
write(1, outbuf, obufsize);
#endif
obufsize = 0;
}
memcpy(outbuf + obufsize, s, len);
obufsize += len;
}
static int write_wrapper_str(struct asfd *asfd, char wcmd, const char *wsrc)
{
static struct iobuf wbuf;
wbuf.cmd=wcmd;
wbuf.buf=(char *)wsrc;
wbuf.len=strlen(wsrc);
return write_wrapper(asfd, &wbuf);
}
int
process_server( int sockfd_new )
{
while(1)
{
fd_set readfd;
FD_ZERO(&readfd);
FD_SET(sockfd_new ,&readfd);
struct timeval tv;
tv.tv_sec = TIME_OUT_DEFAULT;
tv.tv_usec = 0;
if( select(sockfd_new+1, &readfd, NULL, NULL, &tv) < 0 )
{
if( EINTR == errno )
{
continue;
}
SERVER_DEBUG( "select error:%s\n", strerror(errno) );
return -1;
} else if( FD_ISSET(sockfd_new, &readfd) ) {
char buffer[SO_BUF_LEN_MAX];
memset( buffer, 0, sizeof(buffer) );
SERVER_DEBUG( "receive message:\n" );
ssize_t length = 0;
if( ( length = read_wrapper(sockfd_new, buffer, sizeof(buffer)) ) < 0 )
{
SERVER_DEBUG( "read error:%s\n", strerror(errno) );
return -1;
}
SERVER_DEBUG( "read size = %d\n", length );
if( 0 < length )
{
SERVER_DEBUG( "send message:\n" );
const char* message = get_time_string();
if( ( length = write_wrapper( sockfd_new, message, strlen(message) ) ) < 0 )
{
SERVER_DEBUG( "write error:%s\n", strerror(errno) );
return -1;
}
SERVER_DEBUG( "write size = %d\n", length );
} else {
SERVER_DEBUG( "read error, close socket\n" );
return -1;
}
} else {
continue;
}
}
kill( parent, SIGCHLD );
return 0;
}
int write_all(int fd, const void *buf, size_t count) {
ssize_t write_bytes;
while (count > 0) {
write_bytes = write_wrapper(fd, buf, count);
if (write_bytes <= 0) return -1;
buf += write_bytes;
count -= write_bytes;
}
return 0;
}
/* Write to a file descriptor, from a buffer, of a certain size
* param socket_fd: socket file descriptor
* param buffer: Buffer to write
* param length: number of bytes to write, -1 on error
*/
ssize_t socket_write(int socket_fd, char* buffer, size_t length) {
ssize_t nwrite;
nwrite = write_wrapper(socket_fd, buffer, length);
if (nwrite != length) {
fprintf(stderr, "Failed write.\n");
return -1;
}
return nwrite;
}
static int list_manifest(struct asfd *asfd,
const char *fullpath, regex_t *regex,
const char *browsedir, struct conf *conf)
{
int ars=0;
int ret=0;
struct sbuf *sb=NULL;
struct manio *manio=NULL;
char *manifest_dir=NULL;
char *last_bd_match=NULL;
size_t bdlen=0;
if(!(manifest_dir=prepend_s(fullpath,
conf->protocol==PROTO_BURP1?"manifest.gz":"manifest"))
|| !(manio=manio_alloc())
|| manio_init_read(manio, manifest_dir)
|| !(sb=sbuf_alloc(conf)))
{
log_and_send_oom(asfd, __func__);
goto error;
}
manio_set_protocol(manio, conf->protocol);
if(browsedir) bdlen=strlen(browsedir);
while(1)
{
int show=0;
if((ars=manio_sbuf_fill(manio, asfd, sb, NULL, NULL, conf))<0)
goto error;
else if(ars>0)
goto end; // Finished OK.
if(write_status(STATUS_LISTING, sb->path.buf, conf))
goto error;
if(browsedir)
{
int r;
if((r=check_browsedir(browsedir,
&sb->path.buf, bdlen, &last_bd_match))<0)
goto error;
if(!r) continue;
show++;
}
else
{
if(check_regex(regex, sb->path.buf))
show++;
}
if(show)
{
if(write_wrapper(asfd, &sb->attr)
|| write_wrapper(asfd, &sb->path))
goto error;
if(sbuf_is_link(sb)
&& write_wrapper(asfd, &sb->link))
goto error;
}
sbuf_free_content(sb);
}
error:
ret=-1;
end:
sbuf_free(&sb);
free_w(&manifest_dir);
manio_free(&manio);
free_w(&last_bd_match);
return ret;
}
int
process_http_proxy( int sockfd_new )
{
while(1)
{
fd_set readfd;
FD_ZERO(&readfd);
FD_SET(sockfd_new, &readfd);
struct timeval tv;
tv.tv_sec = TIME_OUT_DEFAULT;
tv.tv_usec = 0;
if( select(sockfd_new+1, &readfd, NULL, NULL, &tv) < 0 )
{
if( EINTR == errno )
{
continue;
}
SERVER_DEBUG( "select error:%s\n", strerror(errno) );
return -1;
} else if( FD_ISSET(sockfd_new, &readfd) ) {
char buffer[SO_BUF_LEN_MAX];
SERVER_DEBUG( "receive downstream message:\n" );
ssize_t length = 0;
if( ( length = read_wrapper(sockfd_new, buffer, sizeof(buffer)) ) < 0 )
{
SERVER_DEBUG( "read error:%s\n", strerror(errno) );
return -1;
}
SERVER_DEBUG( "read size = %d \n", length );
if( length > 0 )
{
//to do
buffer[length] = '\0';
//parse http_data
char host_ip[HOST_IP_LEN_MAX] = {0};
if( parse_http_data( buffer, length, host_ip, sizeof(host_ip) ) < 0 )
{
SERVER_DEBUG( "parse_http_data error\n" );
return -1;
} else {
int sockfd = 0;
if( ( sockfd = initial_socket() ) < 0 )
{
SERVER_DEBUG( "initial_socket error\n" );
return -1;
}
struct sockaddr_in client_addr;
memset( &client_addr, 0, sizeof(client_addr) );
client_addr.sin_family = AF_INET;
client_addr.sin_port = htons(HOST_PORT_DEFAULT);
inet_pton( AF_INET, host_ip, &client_addr.sin_addr );
if( connect( sockfd, (struct sockaddr*)&client_addr, sizeof(client_addr) ) < 0 )
{
SERVER_DEBUG( "client connect %s:%d error:%s\n", host_ip, HOST_PORT_DEFAULT, strerror(errno) );
close( sockfd );
return -1;
} else {
SERVER_DEBUG( "connect successfully %s:%d \n", host_ip, HOST_PORT_DEFAULT );
if( setsockopt_nonblock(sockfd) < 0 )
{
SERVER_DEBUG( "setsockopt_nonblock error\n" );
close( sockfd );
return -1;
}
SERVER_DEBUG( "send message to upsteam:\n" );
if( ( length = write_wrapper( sockfd, buffer, length ) ) == -1 )
{
SERVER_DEBUG( "write error:%s\n", strerror(errno) );
close( sockfd );
return -1;
}
SERVER_DEBUG( "write size = %d\n", length );
while(1)
{
FD_ZERO(&readfd);
FD_SET(sockfd_new, &readfd);
FD_SET(sockfd ,&readfd);
tv.tv_sec = TIME_OUT_DEFAULT;
tv.tv_usec = 0;
if( select(sockfd+1, &readfd, NULL, NULL, &tv) < 0 )
{
if( EINTR == errno )
{
continue;
}
SERVER_DEBUG( "select error:%s\n", strerror(errno) );
return -1;
} else if( FD_ISSET(sockfd, &readfd) ) {
SERVER_DEBUG( "receive upstream message:\n" );
if( data_exchange( sockfd_new, sockfd, buffer, sizeof(buffer) ) < 0)
{
SERVER_DEBUG( "data_exchange error\n" );
close( sockfd );
return -1;
}
} else if( FD_ISSET(sockfd_new, &readfd) ) {
SERVER_DEBUG( "receive downstream message:\n" );
if( data_exchange( sockfd, sockfd_new, buffer, sizeof(buffer) ) < 0)
{
SERVER_DEBUG( "data_exchange error\n" );
close( sockfd );
return -1;
}
} else {
SERVER_DEBUG( "receive upstream message timeout\n" );
continue;
} //select timeout
} //while(1)
} //connect
} //parse_http_data
} else { //read_wrapper <= 0
SERVER_DEBUG( "receive downstream message error, client socket closed\n" );
return -1;
}
} else { //select timeout
SERVER_DEBUG( "receive downstream message timeout\n" );
}
}
kill( parent, SIGCHLD );
return 0;
}
int epoll_server( int sockfd )
{
#define EVENTS_MAX 100
int epfd = epoll_create( EVENTS_MAX );
if( epfd < 0 )
{
SERVER_DEBUG( "epoll_create error:%s\n", strerror( errno ) );
return -1;
}
struct epoll_event epollev;
epollev.events = EPOLLIN;
epollev.data.fd = sockfd;
if( epoll_ctl( epfd, EPOLL_CTL_ADD, sockfd, &epollev) < 0 )
{
SERVER_DEBUG( "epoll_ctl error:%s\n", strerror( errno ) );
close( epfd );
return -1;
}
struct epoll_event events[EVENTS_MAX];
int connection = 0;
while( 1 )
{
#define TIME_OUT 5000
int ret = epoll_wait( epfd, events, EVENTS_MAX, TIME_OUT );
SERVER_DEBUG( "epoll_wait ret:%d\n", ret );
if( ret < 0 )
{
SERVER_DEBUG( "epoll_wait error:%s\n", strerror( errno ) );
close( epfd );
//handle other fdes
return -1;
} else if( 0 == ret ) {
SERVER_DEBUG( "TIME_OUT,listenning...\n" );
continue;
} else {
int i = 0;
for( i = 0; i < ret; ++i )
{
if( events[i].data.fd == sockfd )
{
//accept new connection
int sockfd_new = handle_new_connection( sockfd );
if( sockfd_new < 0 )
{
SERVER_DEBUG( "handle_new_connection error\n" );
return -1;
}
//epollev.events = EPOLLIN|EPOLLOUT|EPOLLET;
epollev.events = EPOLLIN|EPOLLET;
epollev.data.fd = sockfd_new;
if( epoll_ctl( epfd, EPOLL_CTL_ADD, sockfd_new, &epollev) < 0 )
{
SERVER_DEBUG( "epoll_ctl error:%s\n", strerror( errno ) );
close( epfd );
return -1;
}
SERVER_DEBUG( "total connections:%d\n", ++connection );
} else {
//read-write
int sockfd = events[i].data.fd;
char buffer[SO_BUF_LEN_MAX];
memset( buffer, 0, sizeof(buffer) );
SERVER_DEBUG( "receive message:\n" );
ssize_t length = 0;
if( ( length = read_wrapper(sockfd, buffer, sizeof(buffer)) ) < 0 )
{
SERVER_DEBUG( "read error:%s\n", strerror(errno) );
return -1;
}
SERVER_DEBUG( "read size = %d\n", length );
if( 0 == length )
{
SERVER_DEBUG( "closing socket\n" );
epollev.data.fd = sockfd;
if( epoll_ctl( epfd, EPOLL_CTL_DEL, sockfd, &epollev) < 0 )
{
SERVER_DEBUG( "epoll_ctl error:%s\n", strerror( errno ) );
close( epfd );
return -1;
}
close( sockfd );
} else {
SERVER_DEBUG( "buffer:%s\n", buffer );
const char* message = get_time_string();
SERVER_DEBUG( "send message:%s\n", message );
if( ( length = write_wrapper( sockfd, message, strlen(message) ) ) < 0 )
{
SERVER_DEBUG( "write error:%s\n", strerror(errno) );
return -1;
}
SERVER_DEBUG( "write size = %d\n", length );
}
}
}
}
}
return 0;
}
/*!
\brief write_data() physically sends the data to the ECU.
\param message is the pointer to an Io_Message structure
*/
G_MODULE_EXPORT gboolean write_data(Io_Message *message)
{
static GMutex *serio_mutex = NULL;
static Serial_Params *serial_params = NULL;
static Firmware_Details *firmware = NULL;
static gfloat *factor = NULL;
OutputData *output = (OutputData *)message->payload;
gint res = 0;
gchar * err_text = NULL;
guint i = 0;
gint j = 0;
gint len = 0;
guint8 * buffer = NULL;
gint burst_len = 0;
gboolean notifies = FALSE;
gint notif_divisor = 32;
WriteMode mode = MTX_CMD_WRITE;
gboolean retval = TRUE;
DBlock *block = NULL;
static GMutex mutex;
static void (*store_new_block)(gpointer) = NULL;
static void (*set_ecu_data)(gpointer,gint *) = NULL;
ENTER();
if (!firmware)
firmware = (Firmware_Details *)DATA_GET(global_data,"firmware");
if (!serial_params)
serial_params = (Serial_Params *)DATA_GET(global_data,"serial_params");
if (!serio_mutex)
serio_mutex = (GMutex *)DATA_GET(global_data,"serio_mutex");
if (!factor)
factor = (gfloat *)DATA_GET(global_data,"sleep_correction");
if (!set_ecu_data)
get_symbol("set_ecu_data",(void **)&set_ecu_data);
if (!store_new_block)
get_symbol("store_new_block",(void **)&store_new_block);
g_return_val_if_fail(firmware,FALSE);
g_return_val_if_fail(serial_params,FALSE);
g_return_val_if_fail(serio_mutex,FALSE);
g_return_val_if_fail(factor,FALSE);
g_return_val_if_fail(set_ecu_data,FALSE);
g_return_val_if_fail(store_new_block,FALSE);
g_mutex_lock(&mutex);
g_mutex_lock(serio_mutex);
if (output)
mode = (WriteMode)(GINT)DATA_GET(output->data,"mode");
if (DATA_GET(global_data,"offline"))
{
switch (mode)
{
case MTX_SIMPLE_WRITE:
set_ecu_data(output->data,NULL);
break;
case MTX_CHUNK_WRITE:
store_new_block(output->data);
break;
case MTX_CMD_WRITE:
break;
}
g_mutex_unlock(serio_mutex);
g_mutex_unlock(&mutex);
EXIT();
return TRUE; /* can't write anything if offline */
}
if (!DATA_GET(global_data,"connected"))
{
g_mutex_unlock(serio_mutex);
g_mutex_unlock(&mutex);
EXIT();
return FALSE; /* can't write anything if disconnected */
}
/* For MS3 1.1 which uses a CRC32 wrapped serial stream, we can't easily
* do the old way as it had a bunch of fugly worarounds for ECU editions
* that couldn't handle bursts and needed to be artificially throttled.
*/
if(DATA_GET(message->data,"burst_write"))
{
buffer = (guint8 *)DATA_GET(message->data,"burst_buffer");
if (!buffer)
{
MTXDBG(CRITICAL|SERIAL_WR,_("MS3 CRC32 burst blob is not stored in the OutputData->data structure, ABORTING\n"));
EXIT();
return FALSE;
}
burst_len = (GINT)DATA_GET(message->data,"burst_len");
QUIET_MTXDBG(SERIAL_WR,_("Writing MS3 burst write %i bytes\n"),burst_len);
res = write_wrapper(serial_params->fd,buffer,burst_len, &len);
/* Send write command */
if (!res)
{
MTXDBG((Dbg_Class)(SERIAL_WR|CRITICAL),_("Error writing MS3 burst block ERROR \"%s\"!!!\n"),err_text);
retval = FALSE;
}
}
else
{
g_return_val_if_fail(message,FALSE);
g_return_val_if_fail(message->sequence,FALSE);
g_return_val_if_fail(message->sequence->len > 0,FALSE);
for (i=0;i<message->sequence->len;i++)
{
block = g_array_index(message->sequence,DBlock *,i);
/* printf("Block pulled\n");*/
if (block->type == ACTION)
{
/* printf("Block type of ACTION!\n");*/
if (block->action == SLEEP)
{
/* printf("Sleeping for %i usec\n", block->arg);*/
MTXDBG(SERIAL_WR,_("Sleeping for %i microseconds \n"),block->arg);
g_usleep((*factor)*block->arg);
}
}
else if (block->type == DATA)
{
/* printf("Block type of DATA!\n");*/
if (block->len > 100)
notifies = TRUE;
/* ICK bad form man, writing one byte at a time, due to ECU's that can't take full wire speed without
dropping chars due to uber tiny buffers */
for (j=0;j<block->len;j++)
{
/*printf("comms.c data[%i] is %i, block len is %i\n",j,block->data[j],block->len);*/
if ((notifies) && ((j % notif_divisor) == 0))
thread_update_widget("info_label",MTX_LABEL,g_strdup_printf(_("<b>Sending %i of %i bytes</b>"),j,block->len));
QUIET_MTXDBG(SERIAL_WR,_("Writing argument %i byte %i of %i, \"%.2X\"\n"),i,j+1,block->len,block->data[j]);
res = write_wrapper(serial_params->fd,&(block->data[j]),1, &len);
/* Send write command */
if (!res)
{
MTXDBG((Dbg_Class)(SERIAL_WR|CRITICAL),_("Error writing block offset %i, value \"%.2X\" ERROR \"%s\"!!!\n"),j,block->data[j],err_text);
retval = FALSE;
}
if (firmware->capabilities & MS2)
g_usleep((*factor)*firmware->interchardelay*1000);
}
}
}
}
if (notifies)
{
thread_update_widget("info_label",MTX_LABEL,g_strdup("<b>Transfer Completed</b>"));
g_timeout_add(2000,(GSourceFunc)reset_infolabel_wrapper,NULL);
}
/* If sucessfull update ecu_data as well, this way, current
* and pending match, in the case of a failed write, the
* update_write_status() function will catch it and rollback as needed
*/
if ((output) && (retval))
{
if (mode == MTX_SIMPLE_WRITE)
set_ecu_data(output->data,NULL);
else if (mode == MTX_CHUNK_WRITE)
store_new_block(output->data);
}
g_mutex_unlock(serio_mutex);
g_mutex_unlock(&mutex);
EXIT();
return retval;
}
