static int write_data_port(ushort_t port, void * src, uint_t length, struct vm_device * dev) {
struct serial_state * state = (struct serial_state *)dev->private_data;
char * val = (char *)src;
PrintDebug("Write to Data Port 0x%x (val=%x)\n", port, *(char*)src);
if (length != 1) {
PrintDebug("Invalid length(%d) in write to 0x%x\n", length, port);
return -1;
}
switch (port) {
case COM1_DATA_PORT:
queue_data(&(state->com1.tx_buffer), *val);
break;
case COM2_DATA_PORT:
queue_data(&(state->com2.tx_buffer), *val);
break;
case COM3_DATA_PORT:
queue_data(&(state->com3.tx_buffer), *val);
break;
case COM4_DATA_PORT:
queue_data(&(state->com4.tx_buffer), *val);
break;
default:
return -1;
}
return length;
}
/*
* Decrypt in EAX mode
*/
void EAX_Decryption::write(const byte input[], size_t length)
{
while(length)
{
const size_t copied = std::min<size_t>(length, queue.size() - queue_end);
queue.copy(queue_end, input, copied);
input += copied;
length -= copied;
queue_end += copied;
while((queue_end - queue_start) > TAG_SIZE)
{
size_t removed = (queue_end - queue_start) - TAG_SIZE;
do_write(&queue[queue_start], removed);
queue_start += removed;
}
if(queue_start + TAG_SIZE == queue_end &&
queue_start >= queue.size() / 2)
{
SecureVector<byte> queue_data(TAG_SIZE);
queue_data.copy(&queue[queue_start], TAG_SIZE);
queue.copy(&queue_data[0], TAG_SIZE);
queue_start = 0;
queue_end = TAG_SIZE;
}
}
}
int MySQL_Data_Stream::write_to_net() {
int bytes_io=0;
int s = queue_data(queueOUT);
if (s==0) return 0;
VALGRIND_DISABLE_ERROR_REPORTING;
//bytes_io = ( encrypted ? SSL_write (ssl, queue_r_ptr(queueOUT), s) : send(fd, queue_r_ptr(queueOUT), s, 0) );
// splitting the ternary operation in IF condition for better readability
if (encrypted) {
bytes_io = SSL_write (ssl, queue_r_ptr(queueOUT), s);
} else {
//bytes_io = send(fd, queue_r_ptr(queueOUT), s, 0);
// fix on bug #183
bytes_io = send(fd, queue_r_ptr(queueOUT), s, MSG_NOSIGNAL);
}
VALGRIND_ENABLE_ERROR_REPORTING;
if (bytes_io < 0) {
if (encrypted==false) {
if (mypolls->fds[poll_fds_idx].revents & POLLOUT) { // in write_to_net_poll() we has remove this safety
// so we enforce it here
shut_soft();
}
} else {
int ssl_ret=SSL_get_error(ssl, bytes_io);
if (ssl_ret!=SSL_ERROR_WANT_READ && ssl_ret!=SSL_ERROR_WANT_WRITE) shut_soft();
}
} else {
queue_r(queueOUT, bytes_io);
if (mypolls) mypolls->last_sent[poll_fds_idx]=sess->thread->curtime;
bytes_info.bytes_sent+=bytes_io;
if (mybe) {
// __sync_fetch_and_add(&myds->mybe->mshge->server_bytes.bytes_sent,r);
}
}
return bytes_io;
}
void *cache_malloc(cache_head_t *cache)
{
cache_node_t *a_node;
queue_t *queue;
if (!queue_empty(&cache->free_que))
{
queue = queue_next(&cache->free_que);
queue_remove(queue);
a_node = queue_data(queue, cache_node_t, next);
queue_insert_head(&cache->used_que, queue);
cache->nr_used++;
cache->nr_free--;
/*set the memory to zero*/
memset(a_node->data, 0, cache->size);
return a_node->data;
}
else
{
size_t len = cache->size + sizeof(cache_node_t);
a_node = Malloc(len);
queue_insert_head(&cache->used_que, &a_node->next);
a_node->data = (void *)(a_node+1);
/*set the memory to zero*/
memset(a_node->data, 0, cache->size);
cache->nr_used++;
return a_node->data;
}
}
void notify_ops(CHANNEL * chan, const char *fmt, ...)
{
LIST *list;
CHANUSER *chanUser;
char buf[256];
int len;
va_list ap;
va_start(ap, fmt);
vsnprintf(buf + 4, sizeof(buf) - 4, fmt, ap);
va_end(ap);
len = strlen(buf + 4);
set_len(buf, len);
set_tag(buf, MSG_SERVER_NOSUCH);
for (list = chan->users; list; list = list->next)
{
chanUser = list->data;
ASSERT(chanUser->magic == MAGIC_CHANUSER);
if(ISUSER(chanUser->user->con) && ((chanUser->flags & ON_CHANNEL_OPERATOR) || chanUser->user->level > LEVEL_USER))
{
queue_data(chanUser->user->con, buf, 4 + len);
}
}
}
bool MySQL_Data_Stream::available_data_out() {
int buflen=queue_data(queueOUT);
if (buflen || PSarrayOUT->len) {
return true;
}
return false;
}
void eConsoleAppContainer::write( const char *data, int len )
{
char *tmp = new char[len];
memcpy(tmp, data, len);
outbuf.push(queue_data(tmp,len));
if (out)
out->start();
}
int _rfs_link(struct rfs_instance *instance, const char *path, const char *target)
{
if (instance->sendrecv.socket == -1)
{
return -ECONNABORTED;
}
uint32_t path_len = strlen(path) + 1;
unsigned target_len = strlen(target) + 1;
unsigned overall_size = sizeof(path_len) + path_len + target_len;
struct rfs_command cmd = { cmd_link, overall_size };
char *buffer = malloc(cmd.data_len);
pack(target, target_len,
pack(path, path_len,
pack_32(&path_len, buffer
)));
send_token_t token = { 0 };
if (do_send(&instance->sendrecv,
queue_data(buffer, overall_size,
queue_cmd(&cmd, &token))) < 0)
{
free(buffer);
return -ECONNABORTED;
}
free(buffer);
struct rfs_answer ans = { 0 };
if (rfs_receive_answer(&instance->sendrecv, &ans) == -1)
{
return -ECONNABORTED;
}
if (ans.command != cmd_link
|| ans.data_len != 0)
{
return cleanup_badmsg(instance, &ans);
}
if (ans.ret == 0)
{
delete_from_cache(&instance->attr_cache, path);
}
return ans.ret == -1 ? -ans.ret_errno : ans.ret;
}
static uvc_ctx *channel_queue_get(queue_t *q){
queue_t *node = NULL;
node = queue_last(q);
queue_remove(node);
/*is selected*/
if (node->ext == NULL){
return queue_data(node, uvc_ctx, i_node);
}
else{
return (uvc_ctx *)node->ext;
}
}
void MySQL_Data_Stream::check_data_flow() {
if ( (PSarrayIN->len || queue_data(queueIN) ) && ( PSarrayOUT->len || queue_data(queueOUT) ) ){
// there is data at both sides of the data stream: this is considered a fatal error
proxy_error("Session=%p, DataStream=%p -- Data at both ends of a MySQL data stream: IN <%d bytes %d packets> , OUT <%d bytes %d packets>\n", sess, this, PSarrayIN->len , queue_data(queueIN) , PSarrayOUT->len , queue_data(queueOUT));
shut_soft();
}
//if ((myds_type==MYDS_BACKEND) && (myconn->myconn.net.fd==0) && (revents & POLLOUT)) {
if ((myds_type==MYDS_BACKEND) && myconn && (myconn->fd==0) && (revents & POLLOUT)) {
int rc;
int error;
socklen_t len = sizeof(error);
rc=getsockopt(fd, SOL_SOCKET, SO_ERROR, &error, &len);
assert(rc==0);
if (error==0) {
//myconn->myconn.net.fd=fd; // connect succeeded
myconn->fd=fd; // connect succeeded
} else {
errno=error;
perror("check_data_flow");
shut_soft();
}
}
}
int MySQL_Data_Stream::write_to_net_poll() {
int rc=0;
if (active==FALSE) return rc;
proxy_debug(PROXY_DEBUG_NET,1,"Session=%p, DataStream=%p --\n", sess, this);
if (queue_data(queueOUT)) {
if ((sess->admin==false)) {
//if (poll_fds_idx>-1 && (mypolls->fds[poll_fds_idx].revents & POLLOUT)) {
if (poll_fds_idx>-1) { // NOTE: attempt to force writes
if (net_failure==false)
rc=write_to_net();
}
} else {
rc=write_to_net();
}
}
if (fd>0 && sess->admin==false) set_pollout();
return rc;
}
int parse_data(uint8_t * buffer, size_t size)
{
static struct params prm;
static int prm_valid = 0;
if (!size) return 1;
if (buffer[0] == DSRA_SIG)
{
if ((prm_valid = (read_header(buffer,size,&prm) == 0)))
start_audio(&prm);
return 1;
}
if (buffer[0] == DSRA_SIG_DATA)
{
if (!audio_started)
return 1;
return queue_data(buffer,size);
}
return 1;
}
void uvc_schedule(){
uvc_ctx *ctx;
queue_t *node;
uvc_thread_env *env;
env = uvc_get_env();
for (;;){
if (!queue_empty(&env->ready_queue) ){
node = queue_last(&env->ready_queue);
queue_remove( node);
ctx = queue_data(node, uvc_ctx, task_node);
}
else{
//只有当没有ready任务时才运行uvloop
if (env->uv_task){
ctx = env->uv_task;
}
else{
printf("no task need run ,exit\n");
exit(0);
}
}
if (ctx != NULL){
env->runing_task = ctx;
ctx->status = UVC_STATUS_RUNING;
//printf("task[%s] runing\n",ctx->name);
uvc_resume(ctx);
//printf("task[%s] stoping\n",ctx->name);
}
if (ctx->status == UVC_STATUS_DIE){
coro_stack_free(&ctx->stack);
free(ctx);
}else{
ctx->status = UVC_STATUS_PENDING;
}
}
}
int httproute_call(HttpSession *session)
{
queue_t *route_queue=&session->server->routes;
char *path=session->request.path;
queue_t *q=NULL;
HttpRoute *route;
//printf("httproute_call in !\n");
if(queue_empty(route_queue))return 404;
for(q = queue_prev(route_queue);
q != queue_sentinel(route_queue);
q = queue_last(q) ) {
route = queue_data(q, HttpRoute, node);
if(strcmp(path,route->path)==0 )
{
TraceInfo("route callback found!\n");
return route->func(session,route->arg);
}
}
TraceInfo("route callback not found!\n");
return 404;
}
int lo_server_dispatch_data(lo_server s, void *data, size_t size)
{
int result = 0;
char *path = data;
ssize_t len = lo_validate_string(data, size);
if (len < 0) {
lo_throw(s, -len, "Invalid message path", NULL);
return len;
}
if (!strcmp(data, "#bundle")) {
char *pos;
int remain;
uint32_t elem_len;
lo_timetag ts, now;
ssize_t bundle_result = lo_validate_bundle(data, size);
if (bundle_result < 0) {
lo_throw(s, -bundle_result, "Invalid bundle", NULL);
return bundle_result;
}
pos = (char *)data + len;
remain = size - len;
lo_timetag_now(&now);
ts.sec = lo_otoh32(*((uint32_t *)pos));
pos += 4;
ts.frac = lo_otoh32(*((uint32_t *)pos));
pos += 4;
remain -= 8;
while (remain >= 4) {
lo_message msg;
elem_len = lo_otoh32(*((uint32_t *)pos));
pos += 4;
remain -= 4;
msg = lo_message_deserialise(pos, elem_len, &result);
if (!msg) {
lo_throw(s, result, "Invalid bundle element received", path);
return -result;
}
// set timetag from bundle
msg->ts = ts;
// test for immediate dispatch
if ((ts.sec == LO_TT_IMMEDIATE.sec
&& ts.frac == LO_TT_IMMEDIATE.frac) ||
lo_timetag_diff(ts, now) <= 0.0) {
dispatch_method(s, pos, msg);
lo_message_free(msg);
} else {
queue_data(s, ts, pos, msg);
}
pos += elem_len;
remain -= elem_len;
}
} else {
lo_message msg = lo_message_deserialise(data, size, &result);
if (NULL == msg) {
lo_throw(s, result, "Invalid message received", path);
return -result;
}
dispatch_method(s, data, msg);
lo_message_free(msg);
}
return size;
}
int lo_server_recv(lo_server s)
{
void *data;
size_t size;
char *path;
char *types;
double sched_time = lo_server_next_event_delay(s);
#ifdef WIN32
fd_set ps;
struct timeval stimeout;
int res;
#else
struct pollfd ps;
#endif
again:
if (sched_time > 0.01) {
if (sched_time > 10.0) {
sched_time = 10.0;
}
#ifdef WIN32
if(!initWSock()) return 0;
ps.fd_count = 1;
ps.fd_array[0] = s->socket;
stimeout.tv_sec = sched_time;
stimeout.tv_usec = (sched_time-stimeout.tv_sec)*1.e6;
res = select(1,&ps,NULL,NULL,&stimeout);
if(res == SOCKET_ERROR) {
return 0;
}
if(!res) {
sched_time = lo_server_next_event_delay(s);
if (sched_time > 0.01) {
goto again;
}
return dispatch_queued(s);
}
#else
ps.fd = s->socket;
ps.events = POLLIN | POLLPRI | POLLERR | POLLHUP;
ps.revents = 0;
poll(&ps, 1, (int)(sched_time * 1000.0));
if (ps.revents == POLLERR || ps.revents == POLLHUP) {
return 0;
}
if (!ps.revents) {
sched_time = lo_server_next_event_delay(s);
if (sched_time > 0.01) {
goto again;
}
return dispatch_queued(s);
}
#endif
} else {
return dispatch_queued(s);
}
if (s->protocol == LO_TCP) {
data = lo_server_recv_raw_stream(s, &size);
} else {
data = lo_server_recv_raw(s, &size);
}
if (!data) {
return 0;
}
path = data;
types = data + lo_strsize(path);
if (!strcmp(path, "#bundle")) {
char *pos = types;
uint32_t len;
lo_timetag ts, now;
lo_timetag_now(&now);
ts.sec = lo_otoh32(*((uint32_t *)pos));
pos += 4;
ts.frac = lo_otoh32(*((uint32_t *)pos));
pos += 4;
while (pos - (char *)data < size) {
len = lo_otoh32(*((uint32_t *)pos));
pos += 4;
/* test for immedaite dispatch */
if ((ts.sec == 0 && ts.frac == 1) ||
lo_timetag_diff(ts, now) <= 0.0) {
types = pos + lo_strsize(pos);
dispatch_method(s, pos, types + 1, types + lo_strsize(types));
} else {
queue_data(s, ts, pos, len);
}
pos += len;
}
free(data);
return size;
} else if (*types != ',') {
lo_throw(s, LO_ENOTYPE, "Missing typetag", path);
return -1;
}
dispatch_method(s, path, types+1, data);
free(data);
return size;
}
int _rfs_open(struct rfs_instance *instance, const char *path, int flags, uint64_t *desc)
{
if (instance->sendrecv.socket == -1)
{
return -ECONNABORTED;
}
unsigned path_len = strlen(path) + 1;
uint16_t fi_flags = rfs_file_flags(flags);
unsigned overall_size = sizeof(fi_flags) + path_len;
struct rfs_command cmd = { cmd_open, overall_size };
char *buffer = malloc(cmd.data_len);
pack(path, path_len,
pack_16(&fi_flags, buffer
));
send_token_t token = { 0 };
if (do_send(&instance->sendrecv,
queue_data(buffer, overall_size,
queue_cmd(&cmd, &token))) < 0)
{
free(buffer);
return -ECONNABORTED;
}
free(buffer);
struct rfs_answer ans = { 0 };
if (rfs_receive_answer(&instance->sendrecv, &ans) == -1)
{
return -ECONNABORTED;
}
if (ans.command != cmd_open)
{
return cleanup_badmsg(instance, &ans);
}
if (ans.ret == -1)
{
if (ans.ret_errno == -ENOENT)
{
delete_from_cache(&instance->attr_cache, path);
}
return -ans.ret_errno;
}
uint32_t stat_failed = 0;
uint32_t user_len = 0;
uint32_t group_len = 0;
#define ans_buffer_size sizeof(*desc) \
+ sizeof(stat_failed) + STAT_BLOCK_SIZE + sizeof(user_len) + sizeof(group_len) \
+ (MAX_SUPPORTED_NAME_LEN + 1) + (MAX_SUPPORTED_NAME_LEN + 1)
char ans_buffer[ans_buffer_size] = { 0 };
if (ans.data_len > sizeof(ans_buffer))
{
return cleanup_badmsg(instance, &ans);
}
#undef ans_buffer_size
if (rfs_receive_data(&instance->sendrecv, ans_buffer, ans.data_len) == -1)
{
return -ECONNABORTED;
}
struct stat stbuf = { 0 };
const char *user =
unpack_32(&group_len,
unpack_32(&user_len,
unpack_stat(&stbuf,
unpack_32(&stat_failed,
unpack_64(desc, ans_buffer
)))));
const char *group = user + user_len;
DEBUG("handle: %llu\n", (long long unsigned)(*desc));
stbuf.st_uid = resolve_username(instance, user);
stbuf.st_gid = resolve_groupname(instance, group, user);
if (ans.ret_errno == 0)
{
if (stat_failed == 0)
{
cache_file(&instance->attr_cache, path, &stbuf);
}
resume_add_file_to_open_list(&instance->resume.open_files, path, flags, *desc);
}
else
{
delete_from_cache(&instance->attr_cache, path);
}
return ans.ret == -1 ? -ans.ret_errno : ans.ret;
}
int _rfs_create(struct rfs_instance *instance, const char *path, mode_t mode, int flags, uint64_t *desc)
{
if (instance->sendrecv.socket == -1)
{
return -ECONNABORTED;
}
unsigned path_len = strlen(path) + 1;
uint32_t fmode = mode;
uint16_t fi_flags = rfs_file_flags(flags);
if ((fmode & 0777) == 0)
{
fmode |= 0600;
}
unsigned overall_size = sizeof(fmode) + sizeof(fi_flags) + path_len;
struct rfs_command cmd = { cmd_create, overall_size };
char *buffer = malloc(cmd.data_len);
pack(path, path_len,
pack_16(&fi_flags,
pack_32(&fmode, buffer
)));
send_token_t token = { 0 };
if (do_send(&instance->sendrecv,
queue_data(buffer, overall_size,
queue_cmd(&cmd, &token))) < 0)
{
free(buffer);
return -ECONNABORTED;
}
free(buffer);
struct rfs_answer ans = { 0 };
if (rfs_receive_answer(&instance->sendrecv, &ans) == -1)
{
return -ECONNABORTED;
}
if (ans.command != cmd_create)
{
return cleanup_badmsg(instance, &ans);
}
if (ans.ret == 0)
{
uint64_t handle = (uint64_t)(-1);
if (ans.data_len != sizeof(handle))
{
return cleanup_badmsg(instance, &ans);
}
if (rfs_receive_data(&instance->sendrecv, &handle, ans.data_len) == -1)
{
return -ECONNABORTED;
}
*desc = ntohll(handle);
/* add file to open list, but without O_CREAT flag to just reopen file
* on connection resume */
resume_add_file_to_open_list(&instance->resume.open_files, path, (flags & ~O_CREAT), *desc);
}
return (ans.ret == 0 ? 0 : -ans.ret_errno);
}
int _rfs_lock(struct rfs_instance *instance, const char *path, uint64_t desc, int lock_cmd, struct flock *fl)
{
if (instance->sendrecv.socket == -1)
{
return -ECONNABORTED;
}
if (fl->l_type == F_UNLCK
&& resume_is_file_in_locked_list(instance->resume.locked_files, path) == 0)
{
return 0;
}
uint16_t flags = 0;
switch (lock_cmd)
{
case F_GETLK:
flags = RFS_GETLK;
break;
case F_SETLK:
flags = RFS_SETLK;
break;
case F_SETLKW:
flags = RFS_SETLKW;
break;
default:
return -EINVAL;
}
uint16_t type = 0;
switch (fl->l_type)
{
case F_UNLCK:
type = RFS_UNLCK;
break;
case F_RDLCK:
type = RFS_RDLCK;
break;
case F_WRLCK:
type = RFS_WRLCK;
break;
default:
return -EINVAL;
}
uint16_t whence = fl->l_whence;
uint64_t start = fl->l_start;
uint64_t len = fl->l_len;
uint64_t fd = desc;
#define overall_size sizeof(fd) + sizeof(flags) + sizeof(type) + sizeof(whence) + sizeof(start) + sizeof(len)
char buffer[overall_size] = { 0 };
pack_64(&len,
pack_64(&start,
pack_16(&whence,
pack_16(&type,
pack_16(&flags,
pack_64(&fd, buffer
))))));
struct rfs_command cmd = { cmd_lock, overall_size };
send_token_t token = { 0 };
if (do_send(&instance->sendrecv,
queue_data(buffer, overall_size,
queue_cmd(&cmd, &token))) < 0)
{
return -ECONNABORTED;
}
#undef overall_size
struct rfs_answer ans = { 0 };
if (rfs_receive_answer(&instance->sendrecv, &ans) == -1)
{
return -ECONNABORTED;
}
if (ans.command != cmd_lock
|| ans.data_len != 0)
{
return cleanup_badmsg(instance, &ans);
}
if (ans.ret == 0
&& (lock_cmd == F_SETLK || lock_cmd == F_SETLKW))
{
if (fl->l_type == F_UNLCK)
{
resume_remove_file_from_locked_list(&instance->resume.locked_files, path);
}
else
{
resume_add_file_to_locked_list(&instance->resume.locked_files, path, fl->l_type, is_file_fully_locked(fl));
}
}
return ans.ret != 0 ? -ans.ret_errno : 0;
}
int MySQL_Data_Stream::buffer2array() {
int ret=0;
bool fast_mode=sess->session_fast_forward;
if (queue_data(queueIN)==0) return ret;
if ((queueIN.pkt.size==0) && queue_data(queueIN)<sizeof(mysql_hdr)) {
queue_zero(queueIN);
}
if (fast_mode) {
queueIN.pkt.size=queue_data(queueIN);
ret=queueIN.pkt.size;
queueIN.pkt.ptr=l_alloc(queueIN.pkt.size);
memcpy(queueIN.pkt.ptr, queue_r_ptr(queueIN) , queueIN.pkt.size);
queue_r(queueIN, queueIN.pkt.size);
PSarrayIN->add(queueIN.pkt.ptr,queueIN.pkt.size);
queueIN.pkt.size=0;
return ret;
}
/**/
if (myconn->get_status_compression()==true) {
if ((queueIN.pkt.size==0) && queue_data(queueIN)>=7) {
proxy_debug(PROXY_DEBUG_PKT_ARRAY, 5, "Reading the header of a new compressed packet\n");
memcpy(&queueIN.hdr,queue_r_ptr(queueIN), sizeof(mysql_hdr));
queue_r(queueIN,sizeof(mysql_hdr));
queueIN.pkt.size=queueIN.hdr.pkt_length+sizeof(mysql_hdr)+3;
queueIN.pkt.ptr=l_alloc(queueIN.pkt.size);
memcpy(queueIN.pkt.ptr, &queueIN.hdr, sizeof(mysql_hdr)); // immediately copy the header into the packet
memcpy((unsigned char *)queueIN.pkt.ptr+sizeof(mysql_hdr), queue_r_ptr(queueIN), 3); // copy 3 bytes, the length of the uncompressed payload
queue_r(queueIN,3);
queueIN.partial=7;
mysql_hdr *_hdr;
_hdr=(mysql_hdr *)queueIN.pkt.ptr;
myconn->compression_pkt_id=_hdr->pkt_id;
ret+=7;
}
} else {
/**/
if ((queueIN.pkt.size==0) && queue_data(queueIN)>=sizeof(mysql_hdr)) {
proxy_debug(PROXY_DEBUG_PKT_ARRAY, 5, "Reading the header of a new packet\n");
memcpy(&queueIN.hdr,queue_r_ptr(queueIN),sizeof(mysql_hdr));
pkt_sid=queueIN.hdr.pkt_id;
queue_r(queueIN,sizeof(mysql_hdr));
queueIN.pkt.size=queueIN.hdr.pkt_length+sizeof(mysql_hdr);
queueIN.pkt.ptr=l_alloc(queueIN.pkt.size);
memcpy(queueIN.pkt.ptr, &queueIN.hdr, sizeof(mysql_hdr)); // immediately copy the header into the packet
queueIN.partial=sizeof(mysql_hdr);
ret+=sizeof(mysql_hdr);
// if (myconn->get_status_compression()==true) {
// mysql_hdr *_hdr;
// _hdr=(mysql_hdr *)queueIN.pkt.ptr;
// myconn->compression_pkt_id=_hdr->pkt_id;
// }
}
}
if ((queueIN.pkt.size>0) && queue_data(queueIN)) {
int b= ( queue_data(queueIN) > (queueIN.pkt.size - queueIN.partial) ? (queueIN.pkt.size - queueIN.partial) : queue_data(queueIN) );
proxy_debug(PROXY_DEBUG_PKT_ARRAY, 5, "Copied %d bytes into packet\n", b);
memcpy((unsigned char *)queueIN.pkt.ptr + queueIN.partial, queue_r_ptr(queueIN),b);
queue_r(queueIN,b);
queueIN.partial+=b;
ret+=b;
}
if ((queueIN.pkt.size>0) && (queueIN.pkt.size==queueIN.partial) ) {
if (myconn->get_status_compression()==true) {
Bytef *dest;
uLongf destLen;
proxy_debug(PROXY_DEBUG_PKT_ARRAY, 5, "Copied the whole compressed packet\n");
unsigned int progress=0;
unsigned int datalength;
unsigned int payload_length=0;
unsigned char *u;
u=(unsigned char *)queueIN.pkt.ptr;
payload_length=*(u+6);
payload_length=payload_length*256+*(u+5);
payload_length=payload_length*256+*(u+4);
unsigned char *_ptr=(unsigned char *)queueIN.pkt.ptr+7;
if (payload_length) {
// the payload is compressed
destLen=payload_length;
dest=(Bytef *)l_alloc(destLen);
int rc=uncompress(dest, &destLen, _ptr, queueIN.pkt.size-7);
assert(rc==Z_OK);
datalength=payload_length;
// change _ptr to the new buffer
_ptr=dest;
} else {
// the payload is not compressed
datalength=queueIN.pkt.size-7;
}
while (progress<datalength) {
if (CompPktIN.partial==0) {
mysql_hdr _a;
assert(datalength >= progress + sizeof(mysql_hdr)); // FIXME: this is a too optimistic assumption
memcpy(&_a,_ptr+progress,sizeof(mysql_hdr));
CompPktIN.pkt.size=_a.pkt_length+sizeof(mysql_hdr);
CompPktIN.pkt.ptr=(unsigned char *)l_alloc(CompPktIN.pkt.size);
if ((datalength-progress) >= CompPktIN.pkt.size) {
// we can copy the whole packet
memcpy(CompPktIN.pkt.ptr, _ptr+progress, CompPktIN.pkt.size);
CompPktIN.partial=0; // stays 0
progress+=CompPktIN.pkt.size;
PSarrayIN->add(CompPktIN.pkt.ptr, CompPktIN.pkt.size);
CompPktIN.pkt.ptr=NULL; // sanity
} else {
// not enough data for the whole packet
memcpy(CompPktIN.pkt.ptr, _ptr+progress, (datalength-progress));
CompPktIN.partial+=(datalength-progress);
progress=datalength; // we reached the end
}
} else {
if ((datalength-progress) >= (CompPktIN.pkt.size-CompPktIN.partial)) {
// we can copy till the end of the packet
memcpy((char *)CompPktIN.pkt.ptr + CompPktIN.partial , _ptr+progress, CompPktIN.pkt.size - CompPktIN.partial);
CompPktIN.partial=0;
progress+= CompPktIN.pkt.size - CompPktIN.partial;
PSarrayIN->add(CompPktIN.pkt.ptr, CompPktIN.pkt.size);
CompPktIN.pkt.ptr=NULL; // sanity
} else {
// not enough data for the whole packet
memcpy((char *)CompPktIN.pkt.ptr + CompPktIN.partial , _ptr+progress , (datalength-progress));
CompPktIN.partial+=(datalength-progress);
progress=datalength; // we reached the end
}
}
// mysql_hdr _a;
// memcpy(&_a,_ptr+progress,sizeof(mysql_hdr));
// unsigned int size=_a.pkt_length+sizeof(mysql_hdr);
// unsigned char *ptrP=(unsigned char *)l_alloc(size);
// memcpy(ptrP,_ptr+progress,size);
// progress+=size;
// PSarrayIN->add(ptrP,size);
}
if (payload_length) {
l_free(destLen,dest);
}
l_free(queueIN.pkt.size,queueIN.pkt.ptr);
pkts_recv++;
queueIN.pkt.size=0;
queueIN.pkt.ptr=NULL;
} else {
PSarrayIN->add(queueIN.pkt.ptr,queueIN.pkt.size);
pkts_recv++;
queueIN.pkt.size=0;
queueIN.pkt.ptr=NULL;
}
}
return ret;
}
void vmisc_tasks()
{
uint8_t time_count,time_200ms;
portTickType ticks_now,ticks_old = 0;
uint16_t time_elapsed = 0;
modem_config();
ublox_init();
// debug_config(); //configure debug serial port
/*
* data from external sensors initialized, version M21
* can disable it by commenting here and changing dataFromInternalSensors from 0 to 1
* check externalPackets.h for more information
*/
if (!(dataFromInternalSensors))
/* initialize external sensors data UART3, debug port and this can't be enabled together */
externalDataPortInit();
RPM_config();
ADC_config();
ADC_SoftwareStartConv(ADC1);
ADC_3_config();
ADC_SoftwareStartConv(ADC3);
PWM_out_config();
for(;;)
{
if(WDT_status == 1)
{
IWDG_ReloadCounter();
WDT_status = 0; //clear it so that it can be set again if all is OK
}
if(xbee_tx_count>0) //enable TX interrupts if transmit data is present
USART_ITConfig(USART1, USART_IT_TXE, ENABLE);
if(gps_tx.count >0) //enable TX interrupts if transmit data is present
USART_ITConfig(USART2, USART_IT_TXE, ENABLE);
if(debug_tx_count >0) //enable TX interrupts if transmit data is present
USART_ITConfig(USART3, USART_IT_TXE, ENABLE);
parse_gps_data();
if(xbee_rx_count)
parse_modem_data();
if(xbee_fresh_data) //if received fresh data
{
calculate_xbee(); //call appropriate function based on received data
xbee_fresh_data = 0;
}
if(log_init_flag)
{
if(write_buff_to_card == 1) //if one buffer is full
{
res = f_open(&fil, file_name, FA_OPEN_ALWAYS | FA_WRITE |FA_READ);
f_lseek(&fil, cursor_pos);
if(log_buff_status == 1)
{
f_write(&fil, SD_buffer2.BUFFER, SD_buffer2.write_count, &bw);
cursor_pos += SD_buffer2.write_count;
}
else if(log_buff_status == 2)
{
f_write(&fil, SD_buffer1.BUFFER, SD_buffer1.write_count, &bw);
cursor_pos += SD_buffer1.write_count;
}
write_buff_to_card = 0;
f_close(&fil);
GPIOF->ODR ^= GPIO_Pin_12;
}
}
ticks_now = xTaskGetTickCount()*2;
if((ticks_now - ticks_old) >=100) //do the following every 100ms
{
time_elapsed = ticks_now - ticks_old;
rpm = pulse_cnt*2*60/MOTOR_POLES*(1000/time_elapsed);
pulse_cnt = 0;
ticks_old = ticks_now;
air_speed_indicator();
send_status();
if(link_count >= 5 ) //no data from more than 500ms
{
modem_link_active = 0; //indicate absence of GIB
link_count = 0;
}
else
link_count++;
if(time_200ms == 2) //do the following every 200ms
{
send_gps_data();
time_200ms = 0;
}
else
time_200ms++;
if(time_count == 5) //do the following every 500ms
{
queue_data(); //append debug data for transmission
update_battery();
update_RPM();
send_press_temp();
send_defections();
send_power_propulsion();
time_count = 0;
}
else
time_count++;
}
}
}
