bool checkreturn pb_skip_varint(pb_istream_t *stream) { uint8_t byte; do { if (!pb_read(stream, &byte, 1)) return false; } while (byte & 0x80); return true; }
static bool read_string(pb_istream_t *stream, const pb_field_t *field, void **arg) { size_t len = stream->bytes_left; char *str = malloc(len + 1); if (!pb_read(stream, str, len)) return false; str[len] = '\0'; printf("Server replied %s\n", str); free(str); return true; }
static bool read_string(pb_istream_t *stream, const pb_field_t *field, void **arg) { uint8_t buf[16] = {0}; size_t len = stream->bytes_left; if (len > sizeof(buf) - 1 || !pb_read(stream, buf, len)) return false; TEST(strcmp((char*)buf, *arg) == 0); return true; }
void pb_read_str(struct pbuf* pbuf, char* str, int maxlen, int* pos) { uint32_t strlen; /* Read the string length */ pb_read_arrl(pbuf, &strlen, sizeof(strlen), pos); assert(strlen < maxlen - 1); /* Read the string and padding */ pb_read(pbuf, str, strlen, pos); str[strlen] = '\0'; pb_alignl(pos); }
static char *read_string(pb_file *pbf, int size) { char *page; if((page = (char *) malloc(size + 1))) { pb_read(pbf, page, size); page[size] = '\0'; } else { fprintf(stderr, "Malloc of page buffer failed.\n"); fprintf(stderr, "Error: %s\n", strerror(errno)); exit(1); } return page; }
static char *new_filename(pb_file *pbf, ub4 len) { char *filename; if(!(filename = (char *) malloc(len + 1))) { fprintf(stderr, "Malloc failed of filename\n"); fprintf(stderr, "Error: %s\n", strerror(errno)); } pb_read(pbf, filename, len); filename[len] = '\0'; #ifdef DEBUG printf("filename is %s\n", filename); #endif return filename; }
void task_main(void *pvParameters) { while(pb_read(pb2) == 0) { // wait for button pressed. } vTaskDelay(1000); for (int speed = 1000; speed < 10000; speed = speed + 50) { motor_go_forward(); motor_speed_set(speed, motor_ch_all); motor_start(motor_ch_all); timer_delay_mil(1000); motor_stop(motor_ch_all); timer_delay_mil(500); motor_go_backward(); motor_speed_set(speed, motor_ch_all); motor_start(motor_ch_all); timer_delay_mil(1000); motor_stop(motor_ch_all); timer_delay_mil(500); motor_turn_right(); motor_speed_set(speed, motor_ch_all); motor_start(motor_ch_all); timer_delay_mil(1000); motor_stop(motor_ch_all); timer_delay_mil(500); motor_turn_left(); motor_speed_set(speed, motor_ch_all); motor_start(motor_ch_all); timer_delay_mil(1000); motor_stop(motor_ch_all); timer_delay_mil(500); } // pidMotorMoveFor1Cell(85); while(1) { } }
bool checkreturn pb_decode_varint(pb_istream_t *stream, uint64_t *dest) { uint8_t byte; int bitpos = 0; *dest = 0; while (bitpos < 64 && pb_read(stream, &byte, 1)) { *dest |= (uint64_t)(byte & 0x7F) << bitpos; bitpos += 7; if (!(byte & 0x80)) return true; } PB_RETURN_ERROR(stream, "varint overflow"); }
static int cont_grep(regex_t *exp, regex_t *nl_exp, int fd, pb_file *pbf, int options) { int retflag = TRUE; int i; int j; ub4 csize; ub4 usize; ub2 fnlen; ub2 eflen; ub2 flags; ub2 method; ub1 file_header[30]; char *filename; char *str_stream; regmatch_t *match_array; regmatch_t *nl_offsets; if(pb_read(pbf, (file_header + 4), 26) != 26) { perror("read"); retflag = FALSE; } else { decd_siz(&csize, &usize, &fnlen, &eflen, &flags, &method, file_header); filename = new_filename(pbf, fnlen); lseek(fd, eflen, SEEK_CUR); if(filename[fnlen - 1] != '/') { str_stream = (method == 8 || (flags & 0x0008)) ? (char *) inflate_string(pbf, &csize, &usize) : read_string(pbf, csize); if(flags & 0x008) check_crc(pbf, str_stream, usize); mk_ascii(str_stream, usize); match_array = fnd_match(exp, str_stream, &i); if((options & JG_PRINT_LINE_NUMBER) && i) nl_offsets = fnd_match(nl_exp, str_stream, &j); prnt_mtchs(exp, filename, str_stream, match_array, nl_offsets, i, j, options); if(match_array) free(match_array); free(str_stream); } free(filename); retflag = TRUE; } return retflag; }
static Bytef *hrd_inflate_str(pb_file *pbf, ub4 *csize, ub4 *usize) { Bytef *out_buff; Bytef *tmp; Bytef in_buff[RDSZ]; unsigned int rdamt; int i; int zret; i = 1; out_buff = NULL; zret = Z_OK; while(zret != Z_STREAM_END && (rdamt = pb_read(pbf, in_buff, RDSZ))) { zs.avail_in = rdamt; zs.avail_out = 0; zs.next_in = in_buff; do { if((tmp = (Bytef *) realloc(out_buff, (RDSZ * i) + 1))) { out_buff = tmp; zs.next_out = &(out_buff[(RDSZ * (i - 1)) - zs.avail_out]); zs.avail_out += RDSZ; i++; } else { fprintf(stderr, "Realloc of out_buff failed.\n"); fprintf(stderr, "Error: %s\n", strerror(errno)); exit(1); } } while((zret = inflate(&zs, 0)) == Z_OK); report_str_error(zret); } pb_push(pbf, zs.next_in, zs.avail_in); out_buff[(RDSZ * (i - 1)) - zs.avail_out] = '\0'; *usize = zs.total_out; *csize = zs.total_in; inflateReset(&zs); return out_buff; }
bool checkreturn pb_decode_varint(pb_istream_t *stream, uint64_t *dest) { uint8_t byte; uint8_t bitpos = 0; uint64_t result = 0; do { if (bitpos >= 64) PB_RETURN_ERROR(stream, "varint overflow"); if (!pb_read(stream, &byte, 1)) return false; result |= (uint64_t)(byte & 0x7F) << bitpos; bitpos = (uint8_t)(bitpos + 7); } while (byte & 0x80); *dest = result; return true; }
static void check_crc(pb_file *pbf, const char *stream, ub4 usize) { ub4 crc; ub4 lcrc; ub1 scratch[16]; crc = crc32(crc, NULL, 0); crc = crc32(crc, (const unsigned char *)stream, usize); if(pb_read(pbf, scratch, 16) != 16) { perror("read"); exit(1); } if(UNPACK_UB4(scratch, 0) != 0x08074b50) { fprintf(stderr, "Error! Missing data descriptor!\n"); exit(1); } lcrc = UNPACK_UB4(scratch, 4); if(crc != lcrc){ fprintf(stderr, "Error! CRCs do not match! Got %x, expected %x\n", crc, lcrc); exit(1); } }
static Bytef *ez_inflate_str(pb_file *pbf, ub4 csize, ub4 usize) { Bytef *out_buff; Bytef *in_buff; unsigned int rdamt; if((zs.next_in = in_buff = (Bytef *) malloc(csize))) { if((zs.next_out = out_buff = (Bytef *) malloc(usize + 1))) { if((rdamt = pb_read(pbf, zs.next_in, csize)) == csize) { zs.avail_in = csize; zs.avail_out = usize; report_str_error(inflate(&zs, 0)); free(in_buff); inflateReset(&zs); out_buff[usize] = '\0'; } else { fprintf(stderr, "Read failed on input file.\n"); fprintf(stderr, "Tried to read %u but read %u instead.\n", csize, rdamt); free(in_buff); free(out_buff); exit(1); } } else { fprintf(stderr, "Malloc of out_buff failed.\n"); fprintf(stderr, "Error: %s\n", strerror(errno)); free(in_buff); exit(1); } } else { fprintf(stderr, "Malloc of in_buff failed.\n"); fprintf(stderr, "Error: %s\n", strerror(errno)); exit(1); } return out_buff; }
int read_data() { unsigned char buffer[BUFFER_SIZE]; oem4_header_t header; int result, read_len, rd_cnt; unsigned int crc; pushback_reader_t efd; oem4_bestpos_t bestpos; oem4_bestvel_t bestvel; gps_pos_novatel_t gps_pos; gps_vel_novatel_t gps_vel; struct timespec tloc; static int is_init = 0; //in case of read-errors from ser. device this function will be called several times in case of error from main. // if(is_init) { erret(pb_reset(&efd)); } else { erret(pb_init(fd, BUFFER_SIZE*2, &efd)); is_init = 1; } memset(buffer, 0, sizeof(buffer)); memset(&gps_pos, 0, sizeof(gps_pos)); memset(&gps_vel, 0, sizeof(gps_vel)); rd_cnt = 0; result = 0; while(1) { // read sync erret(pb_read(&efd, buffer, (unsigned int *)&read_len, OEM4_SYNC_SIZE)); if(read_len<=0) fprintf(stderr,"read timeout\r\n"); // read enough (3) sync bytes? if (read_len != OEM4_SYNC_SIZE) { // no, pusback what we had read so far and try again... erret(pb_unread(&efd, buffer, read_len)); continue; } // increment counter of read bytes rd_cnt += read_len; // sync found? if (buffer[0] != OEM4_BIN_SYNC_HEADER[0] || buffer[1] != OEM4_BIN_SYNC_HEADER[1] || buffer[2] != OEM4_BIN_SYNC_HEADER[2]) { // search for sync message or part of sync message result = oem4_search_sync(buffer, rd_cnt); // unread everything from the beginning of the sync message // if no (partial) sync message is found, nothing is unread (rd_cnt - result) == 0 erret(pb_unread(&efd, &buffer[result], rd_cnt - result)); // everything is pushed back, therefore nothing is read rd_cnt = 0; continue; } // read head length erret(pb_read(&efd, &buffer[rd_cnt], (unsigned int *)&read_len, 1)); // nothing was read? if (result == 0) { // search for sync message or part of sync message result = oem4_search_sync(buffer, rd_cnt); // push back and try again erret(pb_unread(&efd, &buffer[result], rd_cnt - result)); rd_cnt = 0; continue; } rd_cnt += read_len; // assign header length header.length = buffer[3]; // header too long for the buffer? if(header.length + OEM4_SYNC_SIZE >= BUFFER_SIZE) { // this is a serious error, search for sync result = oem4_search_sync(buffer, rd_cnt); // unread everything from the beginning of the sync message erret(pb_unread(&efd, &buffer[result], rd_cnt - result)); rd_cnt = 0; continue; } // read header, keep in mind that four bytes of the header are allready read erret(pb_read(&efd, &buffer[rd_cnt], (unsigned int *)&read_len, header.length - 4)); // read less than expected? if (read_len != header.length - 4) { // search for sync message or part of sync message result = oem4_search_sync(buffer, rd_cnt + read_len); // push back what we had and try again erret(pb_unread(&efd, &buffer[result], rd_cnt + read_len - result)); rd_cnt = 0; continue; } rd_cnt += read_len; oem4_fill_header(&header, buffer); // data too long for the buffer? if ((header.message_length + header.length + OEM4_SYNC_SIZE + OEM4_CRC_SIZE) >= BUFFER_SIZE) { // this is a serious error, search for sync result = oem4_search_sync(buffer, rd_cnt); // unread everything from the beginning of the sync message erret(pb_unread(&efd, &buffer[result], rd_cnt - result)); rd_cnt = 0; continue; } // read data and CRC erret(pb_read(&efd, &buffer[rd_cnt], (unsigned int *)&read_len, header.message_length + OEM4_CRC_SIZE)); // read less than expected? if (read_len != header.message_length + OEM4_CRC_SIZE) { // this is a serious error, search for sync result = oem4_search_sync(buffer, rd_cnt + read_len); // push back what we had and try again erret(pb_unread(&efd, &buffer[result], rd_cnt + read_len - result)); rd_cnt = 0; continue; } rd_cnt += read_len; crc32_block(buffer, rd_cnt, &crc); // CRC ok? (crc == 0) if (crc) { // this is a serious error, search for sync result = oem4_search_sync(buffer, rd_cnt); // unread everything from the beginning of the sync message erret(pb_unread(&efd, &buffer[result], rd_cnt - result)); rd_cnt = 0; continue; } // decode message switch(header.message_id) { case OEM4_BESTPOS: if (!oem4_get_best_pos(&bestpos, &buffer[header.length], header.message_length)) { #if AR_VERBOSE==1 oem4_dump_best_pos(&bestpos); #endif memcpy(&bestpos.gps_header, &header, sizeof(header)); create_gps_position(&bestpos, &gps_pos); SHM_WriteSlot(config.ishm_pos_out, &gps_pos, sizeof(gps_pos)); clock_gettime(CLOCK_REALTIME, &tloc); if(abs(tloc.tv_sec - gps_pos.gps_time.tv_sec) > 60*60*4 && gps_pos.solution_type>0) clock_settime(CLOCK_REALTIME, &gps_pos.gps_time); } break; case OEM4_BESTVEL: if (!oem4_get_best_vel(&bestvel, &buffer[header.length], header.message_length)) { memcpy(&bestvel.gps_header, &header, sizeof(header)); create_gps_velocity(&bestvel, &gps_vel); SHM_WriteSlot(config.ishm_vel_out, &gps_vel, sizeof(gps_vel)); } break; } rd_cnt = 0; } return 0; }
int proc_err(pb_t *ppb, int pc, paramv_t *pv) { register func_t *f; register int i; register ctx_t *ctx; #ifdef xCTR2 if (tTf(6, 8)) lprintf("proc_err: new = %d\n", Ctx.ctx_new); #endif pb_prime(ppb, PB_ERR); /* ** Scan back on the list of context dependencies. ** If we come to someone who can process this message, ** we go ahead and do it. We also take this ** opportunity to unwind the context list & call the ** cleanup functions. */ for (ctx = &Ctx; ctx != NULL; ctx = ctx->ctx_link) { setprocname(ctx->ctx_name); f = ctx->ctx_fn; #ifdef xCTR2 if (tTf(6, 9)) lprintf("proc_err: unwinding %s: errfn=%x, ppb=%x, link=%x, resp=%d, fn=%x\n", getprocname(), ctx->ctx_errfn, ctx->ctx_ppb, ctx->ctx_link, ctx->ctx_resp, f); #endif /* Do the actual error processing. */ ppb->pb_proc = ctx->ctx_resp; if (ctx->ctx_errfn != NULL) i = (*ctx->ctx_errfn)(pc, pv); else i = -1; #ifdef xCTR2 if (tTf(6, 11)) lprintf("proc_err: errcode %d\n", i); #endif if (i == 0) break; else if (i > 0) { /* turn into nonfatal error */ ppb->pb_stat |= PB_INFO; ppb->pb_proc = PB_FRONT; } else { /* call the cleanup function */ if (f != NULL && f->fn_active > 0) { (*f->fn_cleanup)(1); } } /* arrange to leave if parent not in this process */ if (ppb->pb_proc != Cm.cm_myproc) { send_off(ppb, pc, pv); pb_flush(ppb); /* throw away dead contexts and exit */ break; } } if (ctx == NULL) { syserr("proc_err: no parent"); } #ifdef xCTR3 MonPpb = getmonppb(); if (tTf(6, 12)) { lprintf("proc_err: cleanup: ctx=%x, ->_link=%x, MonPpb = ", ctx, ctx->ctx_link); pb_dump(MonPpb, TRUE); } #endif /* pop contexts down to ctx and exit */ ctx = ctx->ctx_link; while (Ctx.ctx_link != ctx) { if (Ctx.ctx_link == NULL) syserr("proc_err: underflow"); Ctx.ctx_new = TRUE; resetp(); } /* ** Flush input pipe. ** THIS CODE IS ONLY NEEDED TO MAKE READMON WORK, AND ** SHOULD BE REMOVED WHEN READMON GOES AWAY!! */ if (ctx == NULL) { Cm.cm_input = Cm.cm_rinput; MonPpb = getmonppb(); while (!BITISSET(PB_EOF, MonPpb->pb_stat)) { pb_read(MonPpb); } MonPpb->pb_st = PB_UNKNOWN; } longjmp(Ctx.ctx_jbuf, 1); }
int inflate_file(pb_file *pbf, int out_fd, struct zipentry *ze){ Bytef in_buff[RDSZ]; Bytef out_buff[RDSZ]; unsigned int rdamt; int rtval; ub4 crc = 0; zs.avail_in = 0; crc = crc32(crc, NULL, 0); /* initialize crc */ /* loop until we've consumed all the compressed data */ for(;;){ if(zs.avail_in == 0){ if((rdamt = pb_read(pbf, in_buff, RDSZ)) == 0) break; else if((int)rdamt < 0){ perror("read"); exit(1); } #ifdef DEBUG printf("%d bytes read\n", rdamt); #endif zs.next_in = in_buff; zs.avail_in = rdamt; } zs.next_out = out_buff; zs.avail_out = RDSZ; if((rtval = inflate(&zs, 0)) != Z_OK){ if(rtval == Z_STREAM_END){ #ifdef DEBUG printf("end of stream\n"); #endif if(zs.avail_out != RDSZ){ crc = crc32(crc, out_buff, (RDSZ - zs.avail_out)); if(out_fd >= 0) if(write(out_fd, out_buff, (RDSZ - zs.avail_out)) != (int)(RDSZ - zs.avail_out)){ perror("write"); exit(1); } } break; } else { fprintf(stderr, "Error inflating file! (%d)\n", rtval); exit(1); } } else { if(zs.avail_out != RDSZ){ crc = crc32(crc, out_buff, (RDSZ - zs.avail_out)); if(out_fd >= 0) if(write(out_fd, out_buff, (RDSZ - zs.avail_out)) != (int)(RDSZ - zs.avail_out)){ perror("write"); exit(1); } zs.next_out = out_buff; zs.avail_out = RDSZ; } } } #ifdef DEBUG printf("done inflating\n"); #endif #ifdef DEBUG printf("%d bytes left over\n", zs.avail_in); #endif #ifdef DEBUG printf("CRC is %x\n", crc); #endif ze->crc = crc; pb_push(pbf, zs.next_in, zs.avail_in); ze->usize = zs.total_out; inflateReset(&zs); return 0; }