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++; } } }