static int
handle_reply (rr_dev dev, const char *reply, size_t nbytes, size_t term_bytes)
{
if (dev->opt_log_cb)
dev->opt_log_cb (dev, RR_LOG_RECV, reply, nbytes + term_bytes, dev->opt_log_cl);
switch(dev->proto) {
case RR_PROTO_FIVED:
return fived_handle_reply (dev, reply, nbytes);
case RR_PROTO_TONOKIP:
return tonokip_handle_reply (dev, reply, nbytes);
case RR_PROTO_SIMPLE:
if (!strncasecmp ("ok", reply, 2) && dev->reply_cb) {
rr_dev_handle_ok (dev);
dev->reply_cb (dev, RR_OK, 0.0, NULL, dev->reply_cl);
} else if (dev->error_cb)
dev->error_cb (dev, RR_E_UNKNOWN_REPLY, reply, nbytes, dev->error_cl);
return 0;
default:
return RR_E_UNSUPPORTED_PROTO;
}
return 0;
}
int tonokip_handle_reply(rr_dev device, const char *reply, size_t nbytes) {
if(!strncmp("ok", reply, 2)) {
if(device->onreply) {
device->onreply(device, device->onreply_data, RR_OK, 0);
}
} else if(!strncmp("Resend:", reply, 7)) {
/* Line number begins 7 bytes in */
return resend(device, atoll(reply + 7), reply, nbytes);
} else if(!strncmp("T:", reply, 2)) {
if(device->onreply) {
char *point;
device->onreply(device, device->onreply_data, RR_BED_TEMP,
strtof(reply+2, &point));
if(!strncmp("B:", point+1, 2)) {
device->onreply(device, device->onreply_data, RR_BED_TEMP,
strtof(point+3, NULL));
}
}
} else {
if(device->onerr) {
device->onerr(device, device->onerr_data, RR_E_UNKNOWN_REPLY, reply, nbytes);
}
return RR_E_UNKNOWN_REPLY;
}
return 0;
}
int handle_reply(rr_dev device, const char *reply, size_t nbytes) {
if(device->onrecv) {
device->onrecv(device, device->onrecv_data, reply, nbytes);
}
switch(device->proto) {
case RR_PROTO_FIVED:
return fived_handle_reply(device, reply, nbytes);
case RR_PROTO_TONOKIP:
return tonokip_handle_reply(device, reply, nbytes);
case RR_PROTO_SIMPLE:
if(!strncmp("ok", reply, 2) && device->onreply) {
device->onreply(device, device->onreply_data, RR_OK, 0);
} else if(device->onerr) {
device->onerr(device, device->onerr_data, RR_E_UNKNOWN_REPLY, reply, nbytes);
}
return 0;
default:
return RR_E_UNSUPPORTED_PROTO;
}
return 0;
}
void
rr_dev_set_paused (rr_dev dev, int priority, int paused)
{
dev->paused[priority] = paused;
/* re-start client's writing */
if (!paused) {
dev->more_cb (dev, dev->more_cl);
dev->wait_wr_cb (dev, 1, dev->wait_wr_cl);
}
}
/* What an horrible method - the protocol is so bad flow control
wise we can jam up, and need to be coaxed back into life */
void
rr_dev_kick (rr_dev dev)
{
dev->send_next = 1;
dev->init_send_count = dev->dev_cmdqueue_size - 1;
dev->wait_wr_cb (dev, 1, dev->wait_wr_cl);
}
/* Helper for emitting and returning errors */
rr_error
rr_dev_emit_error (rr_dev dev, rr_error err, const char *block, int nbytes)
{
if (dev->error_cb)
dev->error_cb (dev, err, block, nbytes, dev->error_cl);
return err;
}
int
rr_dev_resend (rr_dev dev, unsigned long lineno, const char *reply, size_t nbytes)
{
int resent = 0;
blocknode *node;
/* sent cache slot 0 is most recent */
while (1) {
if (!(node = rr_dev_pop_from_queue (dev, RR_PRIO_SENTCACHE)))
break;
rr_dev_log (dev, RR_DEBUG_HIGH, "pop sent node line %d '%s' (%d bytes)\n",
(int)node->line, node->block, node->blocksize);
if (node->line >= lineno) {
rr_dev_prepend_to_queue (dev, RR_PRIO_RESEND, node);
resent++;
} else { /* put it back and look elsewhere */
rr_dev_prepend_to_queue (dev, RR_PRIO_SENTCACHE, node);
break;
}
}
if (resent == 0) {
/* Perhaps line is in the resend queue, and we got an:
* rs: 3
* rs: 6
* type sequence so try peel forward the resend queue.
*/
while (1) {
if (!(node = rr_dev_pop_from_queue (dev, RR_PRIO_RESEND)))
break;
rr_dev_log (dev, RR_DEBUG_HIGH,
"pop resend node line %d '%s' (%d bytes)\n",
(int)node->line, node->block, node->blocksize);
if (node->line < lineno) {
rr_dev_prepend_to_queue (dev, RR_PRIO_SENTCACHE, node);
resent++;
} else { /* put it back and give up */
rr_dev_prepend_to_queue (dev, RR_PRIO_RESEND, node);
break;
}
}
if (resent == 0) {
rr_dev_log (dev, RR_DEBUG_ALWAYS,
"re-send request for unknown (too old) line %ld from cache size %d\n",
lineno, dev->sendsize[RR_PRIO_SENTCACHE]);
rr_dev_emit_error (dev, RR_E_UNCACHED_RESEND, reply, nbytes);
}
}
dev->send_next = 1;
dev->wait_wr_cb (dev, 1, dev->wait_wr_cl);
return 0;
}
void
rr_dev_handle_ok (rr_dev dev)
{
int buffered = rr_dev_buffered_lines (dev);
/* Send as many commands as we get ok's */
if (dev->init_send_count > 0)
dev->init_send_count--;
dev->send_next = 1;
if (buffered < dev->dev_cmdqueue_size) {
rr_dev_log (dev, RR_DEBUG_MEDIUM,
"request more %d < %d\n", buffered, dev->dev_cmdqueue_size);
dev->more_cb (dev, dev->more_cl);
}
dev->wait_wr_cb (dev, 1, dev->wait_wr_cl);
if (dev->debug_output > RR_DEBUG_ALWAYS)
{ /* Check the sendsize accounts add up */
int i = 0;
for (i = 0; i < RR_PRIO_COUNT; ++i) {
blocknode *p;
int count = 0;
for (p = dev->sendhead[i]; p; p = p->next) {
if (!p->next && dev->sendtail[i] != p)
rr_dev_log (dev, RR_DEBUG_MEDIUM,
"Error: queue (%d) broken tail pointer %p vs %p\n",
i, p, dev->sendtail[i]);
count++;
}
if (count != dev->sendsize[i])
rr_dev_log (dev, RR_DEBUG_MEDIUM,
"Error: queue (%d) size mismatch: %d vs %d\n",
i, count, dev->sendsize[i]);
}
}
}
/* print to the log */
void
rr_dev_log (rr_dev dev, int debug_level, const char *format, ...)
{
int len;
va_list args;
char buffer[4096];
if (debug_level > dev->debug_output)
return;
va_start (args, format);
len = vsnprintf (buffer, 4095, format, args);
access (buffer, 0);
if (dev->opt_log_cb)
dev->opt_log_cb (dev, RR_LOG_MSG, buffer, len, dev->opt_log_cl);
va_end (args);
}
void rr_enqueue_internal(rr_dev device, rr_prio priority, void *cbdata, const char *block, size_t nbytes, long long line) {
blocknode *node = malloc(sizeof(blocknode));
node->next = NULL;
node->cbdata = cbdata;
node->block = malloc(nbytes);
memcpy(node->block, block, nbytes);
node->blocksize = nbytes;
node->line = line;
if(!device->sendhead[priority]) {
device->sendhead[priority] = node;
device->sendtail[priority] = node;
device->want_writable(device, device->ww_data, 1);
} else {
device->sendtail[priority]->next = node;
device->sendtail[priority] = node;
}
}
int
rr_dev_enqueue_internal (rr_dev dev, rr_prio priority,
const char *block, size_t nbytes,
long long line)
{
char *comment;
/*
* comments are normal in gcode, preceeded by a ';'
* but we don't want to send those either
*/
if ((comment = strchr (block, ';')))
nbytes = comment - block;
/*
* Elide both newlines, and whitespace that gets in the way
* of checksums that are valid.
*/
while (nbytes > 0 && isspace (block[nbytes - 1]))
nbytes--;
if (nbytes == 0)
return 0;
blocknode *node = malloc(sizeof(blocknode));
node->next = NULL;
node->block = malloc (nbytes + 1);
node->blocksize = nbytes;
node->line = line;
memcpy(node->block, block, nbytes);
node->block[nbytes] = '\0';
rr_dev_append_to_queue (dev, priority, node);
dev->wait_wr_cb (dev, 1, dev->wait_wr_cl);
return 0;
}
int rr_handle_writable(rr_dev device) {
ssize_t result;
if(device->sendbuf_fill == 0) {
/* Last block is gone; prepare to send a new block */
int prio;
blocknode *node = NULL;
for(prio = RR_PRIO_COUNT - 1; prio >= 0; --prio) {
node = device->sendhead[prio];
if(node) {
/* We have a block to send! Get it ready. */
device->bytes_sent = 0;
result = fmtblock(device, node);
if(result < 0) {
/* FIXME: This will confuse code expecting errno to be set */
return result;
}
device->sendbuf_fill = result;
device->sending_prio = prio;
break;
}
}
if(!node) {
/* No data to write */
device->want_writable(device, device->ww_data, 0);
return 0;
}
}
/* Perform write */
do {
result = write(device->fd, device->sendbuf + device->bytes_sent, device->sendbuf_fill - device->bytes_sent);
} while(result < 0 && errno == EINTR);
if(result < 0) {
return result;
}
device->bytes_sent += result;
if(device->bytes_sent == device->sendbuf_fill) {
/* We've sent the complete block. */
blocknode *node = device->sendhead[device->sending_prio];
if(device->onsend) {
device->onsend(device, device->onsend_data, node->cbdata, device->sendbuf, device->sendbuf_fill);
}
device->sendhead[device->sending_prio] = node->next;
node->line = device->lineno;
++(device->lineno);
/* Update sent cache */
if(device->sentcache[device->sentcachesize - 1]) {
blocknode_free(device->sentcache[device->sentcachesize - 1]);
}
ssize_t i;
for(i = device->sentcachesize - 1; i > 0 ; --i) {
device->sentcache[i] = device->sentcache[i-1];
}
device->sentcache[0] = node;
/* Indicate that we're ready for the next. */
device->sendbuf_fill = 0;
}
return result;
}
int fived_handle_reply(rr_dev device, const char *reply, size_t nbytes) {
if(!strncasecmp("ok", reply, 2)) {
if(device->onreply) {
device->onreply(device, device->onreply_data, RR_OK, 0);
/* Parse values */
char *i;
for(i = (char*)reply; i < reply + nbytes; ++i) {
switch(toupper(*i)) {
case 'T':
device->onreply(device, device->onreply_data, RR_NOZZLE_TEMP,
strtof(i+2, &i));
break;
case 'B':
device->onreply(device, device->onreply_data, RR_BED_TEMP,
strtof(i+2, &i));
break;
case 'C':
break;
case 'X':
device->onreply(device, device->onreply_data, RR_X_POS,
strtof(i+2, &i));
break;
case 'Y':
device->onreply(device, device->onreply_data, RR_Y_POS,
strtof(i+2, &i));
break;
case 'Z':
device->onreply(device, device->onreply_data, RR_Z_POS,
strtof(i+2, &i));
break;
case 'E':
device->onreply(device, device->onreply_data, RR_E_POS,
strtof(i+2, &i));
break;
default:
if(device->onerr) {
device->onerr(device, device->onerr_data, RR_E_UNKNOWN_REPLY, reply, nbytes);
}
break;
}
}
}
} else if(!strncasecmp("rs", reply, 2) || !strncasecmp("resend", reply, 6)) {
/* check where the line number starts */
size_t n_start = strcspn(reply, "123456789");
if(n_start) {
long long lineno = strtoll(reply + n_start, NULL, 10);
/* check if lineno is in the range of sent lines*/
if(lineno < device->lineno && strncmp("-", reply + n_start - 1, 1)) {
return resend(device, lineno, reply, nbytes);
} else {
if(device->onerr) {
device->onerr(device, device->onerr_data, RR_E_UNSENT_RESEND, reply, nbytes);
}
return RR_E_UNSENT_RESEND;
}
} else {
if(device->onerr) {
device->onerr(device, device->onerr_data, RR_E_MALFORMED_RESEND_REQUEST, reply, nbytes);
}
return RR_E_MALFORMED_RESEND_REQUEST;
}
} else if(!strncmp("!!", reply, 2)) {
if(device->onerr) {
device->onerr(device, device->onerr_data, RR_E_HARDWARE_FAULT, reply, nbytes);
}
return RR_E_HARDWARE_FAULT;
} else if (!strncasecmp ("start", reply, 5)) {
/*
* This is non-intuitive. If we reset the controller, when we next send
* a command sequence, on the first command we will get a 'start',
* meaning we should reset the line number. Problem is we then send
* the rest of the command sequence and get another 'start' in mid
* flow for some controllers, which gets us out of sync. Ergo we need
* to reset the line number with a command each time we hit one of
* these.
*/
rr_reset_lineno (device);
} else {
if(device->onerr) {
device->onerr(device, device->onerr_data, RR_E_UNKNOWN_REPLY, reply, nbytes);
}
return RR_E_UNKNOWN_REPLY;
}
return 0;
}
static void
float_reply (rr_dev dev, char **i, rr_reply type)
{
if (dev->reply_cb)
dev->reply_cb (dev, type, strtof (*i + 2, i), NULL, dev->reply_cl);
}
int
rr_dev_handle_writable (rr_dev dev)
{
ssize_t result;
if (dev->sendbuf_fill == 0) {
if (dev->init_send_count <= 0 && !dev->send_next) {
rr_dev_log (dev, RR_DEBUG_MEDIUM,
"writeable - init count is %d, no send_next queue %d resend %d\n",
dev->init_send_count, dev->dev_cmdqueue_size,
dev->sendsize[RR_PRIO_RESEND]);
/* wait until there is space in the device buffer and/or an ok */
dev->wait_wr_cb (dev, 0, dev->wait_wr_cl);
return 0;
}
/* Last block is gone; prepare to send a new block */
int prio;
blocknode *node = NULL;
for (prio = RR_PRIO_COUNT - 1; prio >= 0; --prio) {
if (dev->paused[prio])
continue;
node = dev->sendhead[prio];
if (node) {
/* We have a block to send! Get it ready. */
dev->bytes_sent = 0;
result = fmtblock (dev, node);
if (result < 0) {
/* FIXME: This will confuse code expecting errno to be set */
return result;
}
if (result == 0)
rr_dev_log (dev, RR_DEBUG_ALWAYS,
"unusual error - nothing in block to write\n");
dev->send_next = 0;
dev->sendbuf_fill = result;
dev->sending_prio = prio;
break;
}
}
if (!node) {
/* No data to write */
dev->wait_wr_cb (dev, 0, dev->wait_wr_cl);
return 0;
}
}
/* Perform write */
do {
result = write (dev->fd, dev->sendbuf + dev->bytes_sent, dev->sendbuf_fill - dev->bytes_sent);
} while (result < 0 && errno == EINTR);
if (result < 0)
return result;
if (dev->opt_log_cb)
dev->opt_log_cb (dev, RR_LOG_SEND, dev->sendbuf + dev->bytes_sent,
dev->sendbuf_fill - dev->bytes_sent,
dev->opt_log_cl);
dev->bytes_sent += result;
if (dev->bytes_sent == dev->sendbuf_fill) {
/* We've sent the complete block. */
blocknode *node = rr_dev_pop_from_queue (dev, dev->sending_prio);
if (node->line < 0) {
node->line = dev->lineno;
++(dev->lineno);
}
/* Update sent cache */
assert (node->block != NULL);
rr_dev_prepend_to_queue (dev, RR_PRIO_SENTCACHE, node);
if (dev->sendsize[RR_PRIO_SENTCACHE] > (dev->sentcachesize * 3 / 2))
shrink_sentcache (dev);
/* Indicate that we're ready for the next. */
dev->sendbuf_fill = 0;
}
return result;
}
