static void parse_modrm(uint8_t **code, int *r, struct modrm_rm_t *rm)
{
uint8_t modrm = parse_byte(code);
*r = GET_MODRM_R(modrm);
int mod = GET_MODRM_MOD(modrm);
if (mod == 3)
{
rm->flags = MODRM_PURE_REGISTER;
rm->base = GET_MODRM_RM(modrm);
rm->index = -1;
return;
}
rm->flags = 0;
int sib_bytes = 0;
int modrm_rm = GET_MODRM_RM(modrm);
if (modrm_rm == 4)
{
/* ModR/M with SIB byte */
sib_bytes = 1;
int sib = parse_byte(code);
rm->scale = GET_SIB_SCALE(sib);
if ((rm->index = GET_SIB_INDEX(sib)) == 4)
rm->index = -1;
if ((rm->base = GET_SIB_BASE(sib)) == 5 && mod == 0)
{
rm->base = -1;
mod = 2; /* For use later to correctly extract disp32 */
}
}
else
{
/* ModR/M without SIB byte */
rm->index = -1;
rm->scale = 0;
if (mod == 0 && modrm_rm == 5) /* disp32 */
{
rm->base = -1;
rm->disp = (int32_t)parse_dword(code);
return;
}
rm->base = modrm_rm;
}
/* Displacement */
if (mod == 1) /* disp8 */
rm->disp = (int8_t)parse_byte(code);
else if (mod == 2) /* disp32 */
rm->disp = (int32_t)parse_dword(code);
else /* no disp */
rm->disp = 0;
}
inline void to32i(srcitr begin, srcitr end, dstitr target)
{
uint16_t state = initial_state;
for(srcitr i = begin; i != end; i++) {
int32_t x = parse_byte((unsigned char)*i, state);
if(x >= 0) {
*target = x;
++target;
}
}
int32_t x = parse_byte(-1, state);
if(x >= 0) {
*target = x;
++target;
}
}
uint8_t
TrexRpcCommand::parse_port(const Json::Value ¶ms, Json::Value &result) {
uint8_t port_id = parse_byte(params, "port_id", result);
validate_port_id(port_id, result);
return (port_id);
}
static u08 cmd_input(const u08 *cmd, u08 len)
{
switch(cmd[1]) {
case 'g': // get next event or no event
input_get_next_event();
return CMD_NO_REPLY;
case 'w': // wait for next event or timeout (n!=0)
if(len==4) {
u08 timeout_100ms;
if(!parse_byte(cmd+2,&timeout_100ms)) {
return CMD_NO_BYTE;
}
input_wait_for_next_event(timeout_100ms);
return CMD_NO_REPLY;
} else {
return CMD_SYNTAX_ERR;
}
case 'c':
input_clear_queue();
break;
default:
return CMD_UNKNOWN_ERR;
}
return CMD_OK;
}
// distance returned in reading_cm, signal_ok is set to true if sensor reports a strong signal
bool AP_RangeFinder_BLPing::get_reading(uint16_t &reading_cm)
{
if (uart == nullptr) {
return false;
}
float sum_cm = 0;
uint16_t count = 0;
// read any available lines from the lidar
int16_t nbytes = uart->available();
while (nbytes-- > 0) {
const int16_t b = uart->read();
if (b < 0) {
break;
}
if (parse_byte(b)) {
count++;
sum_cm += distance_cm;
}
}
if (count > 0) {
// return average distance of readings
reading_cm = sum_cm / count;
// request another distance
send_message(BLDPIN_MSGID_DISTANCE_SIMPLE, nullptr, 0);
return true;
}
// no readings so return false
return false;
}
static uint32_t __forceinline parse_moffset(uint8_t **code, int imm_bytes)
{
if (imm_bytes == 1)
return parse_byte(code);
else if (imm_bytes == 2)
return parse_word(code);
else
return parse_dword(code);
}
static int32_t __forceinline parse_rel(uint8_t **code, int rel_bytes)
{
if (rel_bytes == 1)
return (int8_t)parse_byte(code);
else if (rel_bytes == 2)
return (int16_t)parse_word(code);
else
return (int32_t)parse_dword(code);
}
static u08 cmd_goto(const u08 *cmd, u08 len)
{
if(len > 5) {
return CMD_SYNTAX_ERR;
}
u08 x,y;
if(!parse_byte(cmd+1,&x)) {
return CMD_NO_BYTE;
}
if(!parse_byte(cmd+3,&y)) {
return CMD_NO_BYTE;
}
console_t *c = console_get_current();
console_goto(c, x, y);
return CMD_OK;
}
static u08 parse_vals(const u08 *arg, u08 num)
{
for(u08 i=0; i<num; i++) {
if(!parse_byte(arg,&vals[i])) {
return CMD_NO_BYTE;
}
arg+=2;
}
return CMD_OK;
}
static u08 cmd_picture(const u08 *cmd, u08 len)
{
switch(cmd[1]) {
case 'l': // load a picture
if(len>6) {
u08 x,y;
if(!parse_byte(cmd+2,&x)) {
return CMD_NO_BYTE;
}
if(!parse_byte(cmd+4,&y)) {
return CMD_NO_BYTE;
}
return picture_load((const char *)(cmd+6),x,y);
}
else {
return CMD_SYNTAX_ERR;
}
default:
return CMD_UNKNOWN_ERR;
}
}
static int parse_params(char *s, uint8_t *params)
{
int len = 0;
while (*s) {
if (parse_byte(s, params))
return -1;
s += 2;
params++;
len++;
}
return len;
}
static u08 parse_hex_byte(u08 def)
{
if(in_size < 2)
return def;
u08 val;
if(parse_byte(in,&val)) {
in_size -= 2;
in += 2;
return val;
} else {
return def;
}
}
static u08 cmd_flags(const u08 *cmd, u08 len)
{
console_t *c = console_get_current();
if(len>3) {
return CMD_SYNTAX_ERR;
}
// read flags value
if(len == 1) {
cmd_reply('s', c->flags);
return CMD_NO_REPLY;
}
else {
// try to parse value
u08 flags = c->flags;
if(len == 2) {
switch(cmd[1]) {
case 'x':
flags = (flags & ~FLAGS_FONT_2Y) | FLAGS_FONT_2X;
break;
case 'y':
flags = (flags & ~FLAGS_FONT_2X) | FLAGS_FONT_2Y;
break;
case 'b':
flags |= FLAGS_FONT_2XY;
break;
case 'n':
flags &= ~FLAGS_FONT_2XY;
break;
case 'A':
flags = (flags & ~FLAGS_FONT_CHARSET_MASK) | 0 << (FLAGS_FONT_CHARSET_SHIFT);
break;
case 'B':
flags = (flags & ~FLAGS_FONT_CHARSET_MASK) | 1 << (FLAGS_FONT_CHARSET_SHIFT);
break;
}
} else {
if(!parse_byte(cmd+1,&flags)) {
return CMD_NO_BYTE;
}
}
c->flags = flags;
}
return CMD_OK;
}
static u08 cmd_color(const u08 *cmd, u08 len)
{
if(len>3) {
return CMD_SYNTAX_ERR;
}
console_t *c = console_get_current();
// read color value
if(len == 1) {
cmd_reply('c', c->color);
return CMD_NO_REPLY;
}
else {
u08 color;
if(len == 2) {
/* set foreground color */
if(!parse_nybble(cmd[1], &color)) {
return CMD_NO_NYBBLE;
}
c->color = (color << 4) | (c->color & 0xf);
} else {
/* set both colors */
if(cmd[1] != '-') {
if(!parse_byte(cmd+1, &color)) {
return CMD_NO_BYTE;
}
c->color = color;
} else {
/* set background color */
if(!parse_nybble(cmd[2], &color)) {
return CMD_NO_NYBBLE;
}
c->color = (c->color & 0xf0) | color;
}
}
}
return CMD_OK;
}
bool ScanDataBuilder::Parse(char*& buffer, size_t& size) {
char* prev_buf = NULL;
bool success;
while (method_index <= 25) {
// std::cout << "method index: " << method_index <<std::endl;
prev_buf = buffer;
switch (method_index) {
case -1:
success = parse_byte(buffer, size, lms100_cola::STX_BYTE);
break;
case 0:
success = parse_command_type(buffer, size, data);
break;
case 1:
success = parse_command(buffer, size, data);
break;
case 2:
success = parse_version_number(buffer, size, data);
break;
case 3:
success = parse_device_number(buffer, size, data);
break;
case 4:
success = parse_serial_number(buffer, size, data);
break;
case 5:
success = parse_device_status(buffer, size, data);
break;
case 6:
success = parse_telegram_counter(buffer, size, data);
break;
case 7:
success = parse_scan_counter(buffer, size, data);
break;
case 8:
success = parse_time_since_startup(buffer, size, data);
break;
case 9:
success = parse_time_of_transmission(buffer, size, data);
break;
case 10:
success = parse_status_of_digital_inputs(buffer, size, data);
break;
case 11:
success = parse_status_of_digital_outputs(buffer, size, data);
break;
case 12:
success = parse_reserved_byte(buffer, size, data);
break;
case 13:
success = parse_scan_frequency(buffer, size, data);
break;
case 14:
success = parse_measurement_frequency(buffer, size, data);
break;
case 15:
success = parse_encoders(buffer, size, data);
break;
case 16:
success = parse_amount_of_16_bit_channels(buffer, size, data);
break;
case 17:
success = parse_content(buffer, size, data);
break;
case 18:
success = parse_scale_factor(buffer, size, data);
break;
case 19:
success = parse_scale_factor_offset(buffer, size, data);
break;
case 20:
success = parse_start_angle(buffer, size, data);
break;
case 21:
success = parse_steps(buffer, size, data);
break;
case 22:
success = parse_amount_of_data(buffer, size, data);
break;
case 23:
success = parse_data(buffer, size, data);
break;
case 24:
success = parse_irrelevant_data(buffer, size);
break;
case 25:
success = parse_byte(buffer, size, lms100_cola::ETX_BYTE);
break;
default:
return false;
}
// method didn't have enough data to parse
if (!success) {
return false;
}
method_index += 1;
}
return true;
}
