Fixes to dtplug protocol

diff --git a/lib/protocols/dtplug/slave.c b/lib/protocols/dtplug/slave.c
index 48ea3c6..1006d6f 100644 (file)
--- a/lib/protocols/dtplug/slave.c
+++ b/lib/protocols/dtplug/slave.c
@@ -26,7 +26,6 @@
 #include "lib/stdio.h"
 #include "lib/string.h"
 #include "drivers/serial.h"
-#include "extdrv/status_led.h"
 #include "lib/time.h"
 
 #include "lib/protocols/dtplug/defs.h"
@@ -122,7 +121,6 @@ static void dtplug_protocol_decode(struct dtplug_protocol_handle* handle, uint8_
        static uint8_t full_size = 0;
        static struct header* info = NULL;
 
-       status_led(red_on);
        /* Do not start reception before receiving the packet start character */
        if ((rx_ptr == 0) && (c != FIRST_PACKET_CHAR)) {
                return;
@@ -179,7 +177,6 @@ static void dtplug_protocol_decode(struct dtplug_protocol_handle* handle, uint8_
                } else {
                        handle->rx_packet = handle->packets;
                }
-               status_led(green_on);
                /* And get ready to receive the next packet */
                goto next_packet;
        }
@@ -244,6 +241,7 @@ void dtplug_protocol_send_reply(struct dtplug_protocol_handle* handle,
                /* Store the new error if any */
                if (error != NO_ERROR) {
                        dtplug_protocol_add_error_to_list(handle, &(question->info), error);
+                       error = NO_ERROR;
                }
                /* The master wants to get all errors, give them even if there is an error in the sequence number */
                if (question->info.type == PKT_TYPE_GET_ERRORS) {
@@ -274,7 +272,9 @@ void dtplug_protocol_send_reply(struct dtplug_protocol_handle* handle,
                        tx_info->seq_num |= PACKET_IS_ERROR;
                        tx_info->err.error_code = error;
                        if (error == GOT_MANY_ERRORS) {
-                               tx_info->err.info = handle->num_errors_stored;
+                               tx_info->err.info = (handle->num_errors_stored / 2);
+                       } else {
+                               tx_info->err.info = question->info.seq_num;
                        }
                } else {
                        /* Append possible data */
@@ -283,7 +283,10 @@ void dtplug_protocol_send_reply(struct dtplug_protocol_handle* handle,
                                if (size && (size <= 2)) {
                                        tx_info->seq_num |= QUICK_DATA_PACKET;
                                        tx_info->quick_data[0] = data_src[0];
-                                       tx_info->quick_data[1] = data_src[1];
+                                       if (size == 2) {
+                                               tx_info->seq_num |= QUICK_DATA_PACKET_TWO_BYTES;
+                                               tx_info->quick_data[1] = data_src[1];
+                                       }
                                } else {
                                        /* Copy data, compute checksum (also OK for a data_send_size of 0) */
                                        sum = 0;
@@ -386,7 +389,6 @@ static int dtplug_protocol_common_handles(struct dtplug_protocol_handle* handle,
                case PKT_TYPE_PING:
                        question->info.seq_num |= PACKET_NEEDS_REPLY; /* Make sure the reply will be sent */
                        dtplug_protocol_send_reply(handle, question, NO_ERROR, 0, NULL); /* A ping needs no aditional data */
-                       dtplug_protocol_release_old_packet(handle);
                        break;
                case PKT_TYPE_RESET:
                        /* Software reset of the board. No way out. */
@@ -416,7 +418,6 @@ static int dtplug_protocol_common_handles(struct dtplug_protocol_handle* handle,
                                        dtplug_protocol_send_reply(handle, question, NO_ERROR, 6, time_buff);
                                }
                        }
-                       dtplug_protocol_release_old_packet(handle);
                        break;
                case PKT_TYPE_SET_USER_INFO:
                        {
@@ -438,7 +439,7 @@ static int dtplug_protocol_common_handles(struct dtplug_protocol_handle* handle,
                                memcpy(&(tmp_data[offset]), &(question->data[2]), size);
                                /* Update the user flash information pages */
                                if (dtplug_protocol_user_flash_update(handle, question, tmp_data, sizeof(struct user_info)) != 0) {
-                                       /* Reply got sent, if return value is not 0 */
+                                       /* Reply got sent if return value is not 0 */
                                        break;
                                }
                        }
@@ -449,25 +450,20 @@ static int dtplug_protocol_common_handles(struct dtplug_protocol_handle* handle,
                        question->info.seq_num |= PACKET_NEEDS_REPLY; /* Make sure the reply will be sent */
                        tmp_val_swap = byte_swap_32(handle->packet_count);
                        dtplug_protocol_send_reply(handle, question, NO_ERROR, 4, (uint8_t*)(&tmp_val_swap));
-                       dtplug_protocol_release_old_packet(handle);
                        break;
                case PKT_TYPE_GET_ERRORS:
                        question->info.seq_num |= PACKET_NEEDS_REPLY; /* Make sure the reply will be sent */
                        dtplug_protocol_send_reply(handle, question, NO_ERROR, 0, NULL); /* Error handling code will take care of filling the message */
-                       dtplug_protocol_release_old_packet(handle);
                        break;
                case PKT_TYPE_GET_NUM_ERRORS:
                        question->info.seq_num |= PACKET_NEEDS_REPLY; /* Make sure the reply will be sent */
                        tmp_val_swap = byte_swap_32(handle->errors_count);
                        dtplug_protocol_send_reply(handle, question, NO_ERROR, 4, (uint8_t*)(&tmp_val_swap));
-                       dtplug_protocol_release_old_packet(handle);
                        break;
                default:
                        /* We do not handle this type, it must be a board specific one */
                        return 0;
        }
-       /* Packet handled */
-       status_led(green_off);
        return 1;
 }
 
@@ -482,6 +478,8 @@ struct packet* dtplug_protocol_get_next_packet_ok(struct dtplug_protocol_handle*
                struct packet* pkt_tmp = (struct packet*)handle->packet_ok;
                if (dtplug_protocol_common_handles(handle, pkt_tmp) == 0) {
                        return pkt_tmp;
+               } else {
+                       dtplug_protocol_release_old_packet(handle);
                }
        }
        return NULL;