mirror of https://github.com/Desuuuu/klipper.git
command: Refactor message block generation
Separate out the buffer management, message encoding, and message framing code. Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
This commit is contained in:
parent
88f4c38dca
commit
1c3cbe9456
154
src/command.c
154
src/command.c
|
@ -104,9 +104,85 @@ error:
|
||||||
shutdown("Command parser error");
|
shutdown("Command parser error");
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// Encode a message
|
||||||
|
static uint8_t
|
||||||
|
command_encodef(char *buf, uint8_t buf_len
|
||||||
|
, const struct command_encoder *ce, va_list args)
|
||||||
|
{
|
||||||
|
if (buf_len <= MESSAGE_MIN)
|
||||||
|
// Ack/Nak message
|
||||||
|
return buf_len;
|
||||||
|
char *p = &buf[MESSAGE_HEADER_SIZE];
|
||||||
|
char *maxend = &p[buf_len - MESSAGE_MIN];
|
||||||
|
uint8_t num_params = READP(ce->num_params);
|
||||||
|
const uint8_t *param_types = READP(ce->param_types);
|
||||||
|
*p++ = READP(ce->msg_id);
|
||||||
|
while (num_params--) {
|
||||||
|
if (p > maxend)
|
||||||
|
goto error;
|
||||||
|
uint8_t t = READP(*param_types);
|
||||||
|
param_types++;
|
||||||
|
uint32_t v;
|
||||||
|
switch (t) {
|
||||||
|
case PT_uint32:
|
||||||
|
case PT_int32:
|
||||||
|
case PT_uint16:
|
||||||
|
case PT_int16:
|
||||||
|
case PT_byte:
|
||||||
|
if (sizeof(v) > sizeof(int) && t >= PT_uint16)
|
||||||
|
if (t == PT_int16)
|
||||||
|
v = (int32_t)va_arg(args, int);
|
||||||
|
else
|
||||||
|
v = va_arg(args, unsigned int);
|
||||||
|
else
|
||||||
|
v = va_arg(args, uint32_t);
|
||||||
|
p = encode_int(p, v);
|
||||||
|
break;
|
||||||
|
case PT_string: {
|
||||||
|
char *s = va_arg(args, char*), *lenp = p++;
|
||||||
|
while (*s && p<maxend)
|
||||||
|
*p++ = *s++;
|
||||||
|
*lenp = p-lenp-1;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
case PT_progmem_buffer:
|
||||||
|
case PT_buffer: {
|
||||||
|
v = va_arg(args, int);
|
||||||
|
if (v > maxend-p)
|
||||||
|
v = maxend-p;
|
||||||
|
*p++ = v;
|
||||||
|
char *s = va_arg(args, char*);
|
||||||
|
if (t == PT_progmem_buffer)
|
||||||
|
memcpy_P(p, s, v);
|
||||||
|
else
|
||||||
|
memcpy(p, s, v);
|
||||||
|
p += v;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
default:
|
||||||
|
goto error;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return p - buf + MESSAGE_TRAILER_SIZE;
|
||||||
|
error:
|
||||||
|
shutdown("Message encode error");
|
||||||
|
}
|
||||||
|
|
||||||
|
// Add header and trailer bytes to a message block
|
||||||
|
static void
|
||||||
|
command_add_frame(char *buf, uint8_t msglen)
|
||||||
|
{
|
||||||
|
buf[MESSAGE_POS_LEN] = msglen;
|
||||||
|
buf[MESSAGE_POS_SEQ] = next_sequence;
|
||||||
|
uint16_t crc = crc16_ccitt(buf, msglen - MESSAGE_TRAILER_SIZE);
|
||||||
|
buf[msglen - MESSAGE_TRAILER_CRC + 0] = crc >> 8;
|
||||||
|
buf[msglen - MESSAGE_TRAILER_CRC + 1] = crc;
|
||||||
|
buf[msglen - MESSAGE_TRAILER_SYNC] = MESSAGE_SYNC;
|
||||||
|
}
|
||||||
|
|
||||||
static uint8_t in_sendf;
|
static uint8_t in_sendf;
|
||||||
|
|
||||||
// Encode a message and transmit it
|
// Encode and transmit a "response" message
|
||||||
void
|
void
|
||||||
_sendf(const struct command_encoder *ce, ...)
|
_sendf(const struct command_encoder *ce, ...)
|
||||||
{
|
{
|
||||||
|
@ -116,81 +192,19 @@ _sendf(const struct command_encoder *ce, ...)
|
||||||
return;
|
return;
|
||||||
writeb(&in_sendf, 1);
|
writeb(&in_sendf, 1);
|
||||||
|
|
||||||
uint8_t max_size = READP(ce->max_size);
|
uint8_t buf_len = READP(ce->max_size) + MESSAGE_MIN;
|
||||||
char *buf = console_get_output(max_size + MESSAGE_MIN);
|
char *buf = console_get_output(buf_len);
|
||||||
if (!buf)
|
if (!buf)
|
||||||
goto done;
|
goto done;
|
||||||
char *p = &buf[MESSAGE_HEADER_SIZE];
|
va_list args;
|
||||||
if (max_size) {
|
va_start(args, ce);
|
||||||
char *maxend = &p[max_size];
|
uint8_t msglen = command_encodef(buf, buf_len, ce, args);
|
||||||
va_list args;
|
va_end(args);
|
||||||
va_start(args, ce);
|
command_add_frame(buf, msglen);
|
||||||
uint8_t num_params = READP(ce->num_params);
|
|
||||||
const uint8_t *param_types = READP(ce->param_types);
|
|
||||||
*p++ = READP(ce->msg_id);
|
|
||||||
while (num_params--) {
|
|
||||||
if (p > maxend)
|
|
||||||
goto error;
|
|
||||||
uint8_t t = READP(*param_types);
|
|
||||||
param_types++;
|
|
||||||
uint32_t v;
|
|
||||||
switch (t) {
|
|
||||||
case PT_uint32:
|
|
||||||
case PT_int32:
|
|
||||||
case PT_uint16:
|
|
||||||
case PT_int16:
|
|
||||||
case PT_byte:
|
|
||||||
if (sizeof(v) > sizeof(int) && t >= PT_uint16)
|
|
||||||
if (t == PT_int16)
|
|
||||||
v = (int32_t)va_arg(args, int);
|
|
||||||
else
|
|
||||||
v = va_arg(args, unsigned int);
|
|
||||||
else
|
|
||||||
v = va_arg(args, uint32_t);
|
|
||||||
p = encode_int(p, v);
|
|
||||||
break;
|
|
||||||
case PT_string: {
|
|
||||||
char *s = va_arg(args, char*), *lenp = p++;
|
|
||||||
while (*s && p<maxend)
|
|
||||||
*p++ = *s++;
|
|
||||||
*lenp = p-lenp-1;
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
case PT_progmem_buffer:
|
|
||||||
case PT_buffer: {
|
|
||||||
v = va_arg(args, int);
|
|
||||||
if (v > maxend-p)
|
|
||||||
v = maxend-p;
|
|
||||||
*p++ = v;
|
|
||||||
char *s = va_arg(args, char*);
|
|
||||||
if (t == PT_progmem_buffer)
|
|
||||||
memcpy_P(p, s, v);
|
|
||||||
else
|
|
||||||
memcpy(p, s, v);
|
|
||||||
p += v;
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
default:
|
|
||||||
goto error;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
va_end(args);
|
|
||||||
}
|
|
||||||
|
|
||||||
// Send message to serial port
|
|
||||||
uint8_t msglen = p+MESSAGE_TRAILER_SIZE - buf;
|
|
||||||
buf[MESSAGE_POS_LEN] = msglen;
|
|
||||||
buf[MESSAGE_POS_SEQ] = next_sequence;
|
|
||||||
uint16_t crc = crc16_ccitt(buf, p-buf);
|
|
||||||
*p++ = crc>>8;
|
|
||||||
*p++ = crc;
|
|
||||||
*p++ = MESSAGE_SYNC;
|
|
||||||
console_push_output(msglen);
|
console_push_output(msglen);
|
||||||
done:
|
done:
|
||||||
writeb(&in_sendf, 0);
|
writeb(&in_sendf, 0);
|
||||||
return;
|
return;
|
||||||
error:
|
|
||||||
shutdown("Message encode error");
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void
|
||||||
|
|
Loading…
Reference in New Issue