diff mbox series

[v2,4/6] net: add fastboot TCP6 support

Message ID 20230510165959.2228978-4-dimorinny@google.com
State Superseded
Delegated to: Ramon Fried
Headers show
Series [v2,1/6] net: split IP_TCP header into separate IP/IP6 and TCP headers | expand

Commit Message

Dmitrii Merkurev May 10, 2023, 4:59 p.m. UTC
fastboot tcp command remains the same, but started
listening IP6 in case it's enabled.

Signed-off-by: Dmitrii Merkurev <dimorinny@google.com>
Cc: Ying-Chun Liu (PaulLiu) <paul.liu@linaro.org>
Cc: Simon Glass <sjg@chromium.org>
Сс: Joe Hershberger <joe.hershberger@ni.com>
Сс: Ramon Fried <rfried.dev@gmail.com>
---
 include/net/fastboot_tcp.h |  2 +-
 net/fastboot_tcp.c         | 72 ++++++++++++++++++++++++++++++++------
 2 files changed, 62 insertions(+), 12 deletions(-)

Comments

Paul Liu July 3, 2023, 3:48 p.m. UTC | #1
Reviewed-by: Ying-Chun Liu (PaulLiu) <paul.liu@linaro.org>


On Thu, 11 May 2023 at 01:00, Dmitrii Merkurev <dimorinny@google.com> wrote:

> fastboot tcp command remains the same, but started
> listening IP6 in case it's enabled.
>
> Signed-off-by: Dmitrii Merkurev <dimorinny@google.com>
> Cc: Ying-Chun Liu (PaulLiu) <paul.liu@linaro.org>
> Cc: Simon Glass <sjg@chromium.org>
> Сс: Joe Hershberger <joe.hershberger@ni.com>
> Сс: Ramon Fried <rfried.dev@gmail.com>
> ---
>  include/net/fastboot_tcp.h |  2 +-
>  net/fastboot_tcp.c         | 72 ++++++++++++++++++++++++++++++++------
>  2 files changed, 62 insertions(+), 12 deletions(-)
>
> diff --git a/include/net/fastboot_tcp.h b/include/net/fastboot_tcp.h
> index 6cf29d52e9..98986fa10a 100644
> --- a/include/net/fastboot_tcp.h
> +++ b/include/net/fastboot_tcp.h
> @@ -7,7 +7,7 @@
>  #define __NET_FASTBOOT_TCP_H__
>
>  /**
> - * Wait for incoming tcp fastboot comands.
> + * Wait for incoming TCP fastboot comands.
>   */
>  void fastboot_tcp_start_server(void);
>
> diff --git a/net/fastboot_tcp.c b/net/fastboot_tcp.c
> index 2eb52ea256..d93b52ede5 100644
> --- a/net/fastboot_tcp.c
> +++ b/net/fastboot_tcp.c
> @@ -6,8 +6,10 @@
>  #include <common.h>
>  #include <fastboot.h>
>  #include <net.h>
> +#include <net6.h>
>  #include <net/fastboot_tcp.h>
>  #include <net/tcp.h>
> +#include <net/tcp6.h>
>
>  static char command[FASTBOOT_COMMAND_LEN] = {0};
>  static char response[FASTBOOT_RESPONSE_LEN] = {0};
> @@ -20,18 +22,30 @@ static u16 curr_dport;
>  static u32 curr_tcp_seq_num;
>  static u32 curr_tcp_ack_num;
>  static unsigned int curr_request_len;
> +static bool is_ipv6;
> +static size_t ip_header_size;
>  static enum fastboot_tcp_state {
>         FASTBOOT_CLOSED,
>         FASTBOOT_CONNECTED,
>         FASTBOOT_DISCONNECTING
>  } state = FASTBOOT_CLOSED;
>
> +static int command_handled_id;
> +static bool command_handled_success;
> +
>  static void fastboot_tcp_answer(u8 action, unsigned int len)
>  {
>         const u32 response_seq_num = curr_tcp_ack_num;
>         const u32 response_ack_num = curr_tcp_seq_num +
>                   (curr_request_len > 0 ? curr_request_len : 1);
>
> +#if defined(CONFIG_IPV6)
> +       if (is_ipv6) {
> +               net_send_tcp_packet6(len, htons(curr_sport),
> htons(curr_dport),
> +                                    action, response_seq_num,
> response_ack_num);
> +               return;
> +       }
> +#endif
>         net_send_tcp_packet(len, htons(curr_sport), htons(curr_dport),
>                             action, response_seq_num, response_ack_num);
>  }
> @@ -47,7 +61,7 @@ static void fastboot_tcp_send_packet(u8 action, const
> uchar *data, unsigned int
>         uchar *pkt = net_get_async_tx_pkt_buf();
>
>         memset(pkt, '\0', PKTSIZE);
> -       pkt += net_eth_hdr_size() + IP_TCP_HDR_SIZE + TCP_TSOPT_SIZE + 2;
> +       pkt += net_eth_hdr_size() + ip_header_size + TCP_HDR_SIZE +
> TCP_TSOPT_SIZE + 2;
>         memcpy(pkt, data, len);
>         fastboot_tcp_answer(action, len);
>         memset(pkt, '\0', PKTSIZE);
> @@ -59,7 +73,7 @@ static void fastboot_tcp_send_message(const char
> *message, unsigned int len)
>         uchar *pkt = net_get_async_tx_pkt_buf();
>
>         memset(pkt, '\0', PKTSIZE);
> -       pkt += net_eth_hdr_size() + IP_TCP_HDR_SIZE + TCP_TSOPT_SIZE + 2;
> +       pkt += net_eth_hdr_size() + ip_header_size + TCP_HDR_SIZE +
> TCP_TSOPT_SIZE + 2;
>         // Put first 8 bytes as a big endian message length
>         memcpy(pkt, &len_be, 8);
>         pkt += 8;
> @@ -68,10 +82,9 @@ static void fastboot_tcp_send_message(const char
> *message, unsigned int len)
>         memset(pkt, '\0', PKTSIZE);
>  }
>
> -static void fastboot_tcp_handler_ipv4(uchar *pkt, u16 dport,
> -                                     struct in_addr sip, u16 sport,
> -                                     u32 tcp_seq_num, u32 tcp_ack_num,
> -                                     u8 action, unsigned int len)
> +static void fastboot_tcp_handler(uchar *pkt, u16 dport, u16 sport,
> +                                u32 tcp_seq_num, u32 tcp_ack_num,
> +                                u8 action, unsigned int len)
>  {
>         int fastboot_command_id;
>         u64 command_size;
> @@ -88,7 +101,6 @@ static void fastboot_tcp_handler_ipv4(uchar *pkt, u16
> dport,
>         case FASTBOOT_CLOSED:
>                 if (tcp_push) {
>                         if (len != handshake_length ||
> -                           strlen(pkt) != handshake_length ||
>                             memcmp(pkt, handshake, handshake_length) != 0)
> {
>                                 fastboot_tcp_reset();
>                                 break;
> @@ -111,18 +123,25 @@ static void fastboot_tcp_handler_ipv4(uchar *pkt,
> u16 dport,
>                         pkt += 8;
>
>                         // Only single packet messages are supported ATM
> -                       if (strlen(pkt) != command_size) {
> +                       if (len != command_size) {
>                                 fastboot_tcp_reset();
>                                 break;
>                         }
>                         strlcpy(command, pkt, len + 1);
>                         fastboot_command_id =
> fastboot_handle_command(command, response);
>                         fastboot_tcp_send_message(response,
> strlen(response));
> -                       fastboot_handle_boot(fastboot_command_id,
> -                                            strncmp("OKAY", response, 4)
> == 0);
> +
> +                       command_handled_id = fastboot_command_id;
> +                       command_handled_success = strncmp("OKAY",
> response, 4) == 0;
>                 }
>                 break;
>         case FASTBOOT_DISCONNECTING:
> +               if (command_handled_success) {
> +                       fastboot_handle_boot(command_handled_id,
> command_handled_success);
> +                       command_handled_id = 0;
> +                       command_handled_success = false;
> +               }
> +
>                 if (tcp_push)
>                         state = FASTBOOT_CLOSED;
>                 break;
> @@ -137,10 +156,41 @@ static void fastboot_tcp_handler_ipv4(uchar *pkt,
> u16 dport,
>         curr_request_len = 0;
>  }
>
> +static void fastboot_tcp_handler_ipv4(uchar *pkt, u16 dport,
> +                                     struct in_addr sip, u16 sport,
> +                                     u32 tcp_seq_num, u32 tcp_ack_num,
> +                                     u8 action, unsigned int len)
> +{
> +       is_ipv6 = false;
> +       ip_header_size = IP_HDR_SIZE;
> +       fastboot_tcp_handler(pkt, dport, sport,
> +                            tcp_seq_num, tcp_ack_num,
> +                            action, len);
> +}
> +
> +#if defined(CONFIG_IPV6)
> +static void fastboot_tcp_handler_ipv6(uchar *pkt, u16 dport,
> +                                     struct in6_addr sip, u16 sport,
> +                                     u32 tcp_seq_num, u32 tcp_ack_num,
> +                                     u8 action, unsigned int len)
> +{
> +       is_ipv6 = true;
> +       ip_header_size = IP6_HDR_SIZE;
> +       fastboot_tcp_handler(pkt, dport, sport,
> +                            tcp_seq_num, tcp_ack_num,
> +                            action, len);
> +}
> +#endif
> +
>  void fastboot_tcp_start_server(void)
>  {
>         printf("Using %s device\n", eth_get_name());
> -       printf("Listening for fastboot command on tcp %pI4\n", &net_ip);
>
> +       printf("Listening for fastboot command on tcp %pI4\n", &net_ip);
>         tcp_set_tcp_handler(fastboot_tcp_handler_ipv4);
> +
> +#if defined(CONFIG_IPV6)
> +       printf("Listening for fastboot command on %pI6\n", &net_ip6);
> +       net_set_tcp_handler6(fastboot_tcp_handler_ipv6);
> +#endif
>  }
> --
> 2.40.1.606.ga4b1b128d6-goog
>
>
Simon Glass July 23, 2023, 3:48 a.m. UTC | #2
Hi Dmitrii,

On Wed, 10 May 2023 at 11:00, Dmitrii Merkurev <dimorinny@google.com> wrote:
>
> fastboot tcp command remains the same, but started
> listening IP6 in case it's enabled.
>
> Signed-off-by: Dmitrii Merkurev <dimorinny@google.com>
> Cc: Ying-Chun Liu (PaulLiu) <paul.liu@linaro.org>
> Cc: Simon Glass <sjg@chromium.org>
> Сс: Joe Hershberger <joe.hershberger@ni.com>
> Сс: Ramon Fried <rfried.dev@gmail.com>
> ---
>  include/net/fastboot_tcp.h |  2 +-
>  net/fastboot_tcp.c         | 72 ++++++++++++++++++++++++++++++++------
>  2 files changed, 62 insertions(+), 12 deletions(-)
>
> diff --git a/include/net/fastboot_tcp.h b/include/net/fastboot_tcp.h
> index 6cf29d52e9..98986fa10a 100644
> --- a/include/net/fastboot_tcp.h
> +++ b/include/net/fastboot_tcp.h
> @@ -7,7 +7,7 @@
>  #define __NET_FASTBOOT_TCP_H__
>
>  /**
> - * Wait for incoming tcp fastboot comands.
> + * Wait for incoming TCP fastboot comands.
>   */
>  void fastboot_tcp_start_server(void);
>
> diff --git a/net/fastboot_tcp.c b/net/fastboot_tcp.c
> index 2eb52ea256..d93b52ede5 100644
> --- a/net/fastboot_tcp.c
> +++ b/net/fastboot_tcp.c
> @@ -6,8 +6,10 @@
>  #include <common.h>
>  #include <fastboot.h>
>  #include <net.h>
> +#include <net6.h>
>  #include <net/fastboot_tcp.h>
>  #include <net/tcp.h>
> +#include <net/tcp6.h>
>
>  static char command[FASTBOOT_COMMAND_LEN] = {0};
>  static char response[FASTBOOT_RESPONSE_LEN] = {0};
> @@ -20,18 +22,30 @@ static u16 curr_dport;
>  static u32 curr_tcp_seq_num;
>  static u32 curr_tcp_ack_num;
>  static unsigned int curr_request_len;
> +static bool is_ipv6;
> +static size_t ip_header_size;
>  static enum fastboot_tcp_state {
>         FASTBOOT_CLOSED,
>         FASTBOOT_CONNECTED,
>         FASTBOOT_DISCONNECTING
>  } state = FASTBOOT_CLOSED;
>
> +static int command_handled_id;
> +static bool command_handled_success;
> +
>  static void fastboot_tcp_answer(u8 action, unsigned int len)
>  {
>         const u32 response_seq_num = curr_tcp_ack_num;
>         const u32 response_ack_num = curr_tcp_seq_num +
>                   (curr_request_len > 0 ? curr_request_len : 1);
>
> +#if defined(CONFIG_IPV6)

Can you use if() here?

> +       if (is_ipv6) {
> +               net_send_tcp_packet6(len, htons(curr_sport), htons(curr_dport),
> +                                    action, response_seq_num, response_ack_num);
> +               return;
> +       }
> +#endif
>         net_send_tcp_packet(len, htons(curr_sport), htons(curr_dport),
>                             action, response_seq_num, response_ack_num);
>  }
> @@ -47,7 +61,7 @@ static void fastboot_tcp_send_packet(u8 action, const uchar *data, unsigned int
>         uchar *pkt = net_get_async_tx_pkt_buf();
>
>         memset(pkt, '\0', PKTSIZE);
> -       pkt += net_eth_hdr_size() + IP_TCP_HDR_SIZE + TCP_TSOPT_SIZE + 2;
> +       pkt += net_eth_hdr_size() + ip_header_size + TCP_HDR_SIZE + TCP_TSOPT_SIZE + 2;
>         memcpy(pkt, data, len);
>         fastboot_tcp_answer(action, len);
>         memset(pkt, '\0', PKTSIZE);
> @@ -59,7 +73,7 @@ static void fastboot_tcp_send_message(const char *message, unsigned int len)
>         uchar *pkt = net_get_async_tx_pkt_buf();
>
>         memset(pkt, '\0', PKTSIZE);
> -       pkt += net_eth_hdr_size() + IP_TCP_HDR_SIZE + TCP_TSOPT_SIZE + 2;
> +       pkt += net_eth_hdr_size() + ip_header_size + TCP_HDR_SIZE + TCP_TSOPT_SIZE + 2;
>         // Put first 8 bytes as a big endian message length
>         memcpy(pkt, &len_be, 8);
>         pkt += 8;
> @@ -68,10 +82,9 @@ static void fastboot_tcp_send_message(const char *message, unsigned int len)
>         memset(pkt, '\0', PKTSIZE);
>  }
>

[..]
 +
>  void fastboot_tcp_start_server(void)
>  {
>         printf("Using %s device\n", eth_get_name());
> -       printf("Listening for fastboot command on tcp %pI4\n", &net_ip);
>
> +       printf("Listening for fastboot command on tcp %pI4\n", &net_ip);
>         tcp_set_tcp_handler(fastboot_tcp_handler_ipv4);
> +
> +#if defined(CONFIG_IPV6)
> +       printf("Listening for fastboot command on %pI6\n", &net_ip6);
> +       net_set_tcp_handler6(fastboot_tcp_handler_ipv6);
> +#endif

and here. This needs some thought as we don't want to add #if to C
code where possible.

Regards,
Simon
Dmitrii Merkurev July 25, 2023, 10:06 p.m. UTC | #3
>
> Can you use if() here?


Yes, fixed

On Sun, Jul 23, 2023 at 4:48 AM Simon Glass <sjg@chromium.org> wrote:

> Hi Dmitrii,
>
> On Wed, 10 May 2023 at 11:00, Dmitrii Merkurev <dimorinny@google.com>
> wrote:
> >
> > fastboot tcp command remains the same, but started
> > listening IP6 in case it's enabled.
> >
> > Signed-off-by: Dmitrii Merkurev <dimorinny@google.com>
> > Cc: Ying-Chun Liu (PaulLiu) <paul.liu@linaro.org>
> > Cc: Simon Glass <sjg@chromium.org>
> > Сс: Joe Hershberger <joe.hershberger@ni.com>
> > Сс: Ramon Fried <rfried.dev@gmail.com>
> > ---
> >  include/net/fastboot_tcp.h |  2 +-
> >  net/fastboot_tcp.c         | 72 ++++++++++++++++++++++++++++++++------
> >  2 files changed, 62 insertions(+), 12 deletions(-)
> >
> > diff --git a/include/net/fastboot_tcp.h b/include/net/fastboot_tcp.h
> > index 6cf29d52e9..98986fa10a 100644
> > --- a/include/net/fastboot_tcp.h
> > +++ b/include/net/fastboot_tcp.h
> > @@ -7,7 +7,7 @@
> >  #define __NET_FASTBOOT_TCP_H__
> >
> >  /**
> > - * Wait for incoming tcp fastboot comands.
> > + * Wait for incoming TCP fastboot comands.
> >   */
> >  void fastboot_tcp_start_server(void);
> >
> > diff --git a/net/fastboot_tcp.c b/net/fastboot_tcp.c
> > index 2eb52ea256..d93b52ede5 100644
> > --- a/net/fastboot_tcp.c
> > +++ b/net/fastboot_tcp.c
> > @@ -6,8 +6,10 @@
> >  #include <common.h>
> >  #include <fastboot.h>
> >  #include <net.h>
> > +#include <net6.h>
> >  #include <net/fastboot_tcp.h>
> >  #include <net/tcp.h>
> > +#include <net/tcp6.h>
> >
> >  static char command[FASTBOOT_COMMAND_LEN] = {0};
> >  static char response[FASTBOOT_RESPONSE_LEN] = {0};
> > @@ -20,18 +22,30 @@ static u16 curr_dport;
> >  static u32 curr_tcp_seq_num;
> >  static u32 curr_tcp_ack_num;
> >  static unsigned int curr_request_len;
> > +static bool is_ipv6;
> > +static size_t ip_header_size;
> >  static enum fastboot_tcp_state {
> >         FASTBOOT_CLOSED,
> >         FASTBOOT_CONNECTED,
> >         FASTBOOT_DISCONNECTING
> >  } state = FASTBOOT_CLOSED;
> >
> > +static int command_handled_id;
> > +static bool command_handled_success;
> > +
> >  static void fastboot_tcp_answer(u8 action, unsigned int len)
> >  {
> >         const u32 response_seq_num = curr_tcp_ack_num;
> >         const u32 response_ack_num = curr_tcp_seq_num +
> >                   (curr_request_len > 0 ? curr_request_len : 1);
> >
> > +#if defined(CONFIG_IPV6)
>
> Can you use if() here?
>
> > +       if (is_ipv6) {
> > +               net_send_tcp_packet6(len, htons(curr_sport),
> htons(curr_dport),
> > +                                    action, response_seq_num,
> response_ack_num);
> > +               return;
> > +       }
> > +#endif
> >         net_send_tcp_packet(len, htons(curr_sport), htons(curr_dport),
> >                             action, response_seq_num, response_ack_num);
> >  }
> > @@ -47,7 +61,7 @@ static void fastboot_tcp_send_packet(u8 action, const
> uchar *data, unsigned int
> >         uchar *pkt = net_get_async_tx_pkt_buf();
> >
> >         memset(pkt, '\0', PKTSIZE);
> > -       pkt += net_eth_hdr_size() + IP_TCP_HDR_SIZE + TCP_TSOPT_SIZE + 2;
> > +       pkt += net_eth_hdr_size() + ip_header_size + TCP_HDR_SIZE +
> TCP_TSOPT_SIZE + 2;
> >         memcpy(pkt, data, len);
> >         fastboot_tcp_answer(action, len);
> >         memset(pkt, '\0', PKTSIZE);
> > @@ -59,7 +73,7 @@ static void fastboot_tcp_send_message(const char
> *message, unsigned int len)
> >         uchar *pkt = net_get_async_tx_pkt_buf();
> >
> >         memset(pkt, '\0', PKTSIZE);
> > -       pkt += net_eth_hdr_size() + IP_TCP_HDR_SIZE + TCP_TSOPT_SIZE + 2;
> > +       pkt += net_eth_hdr_size() + ip_header_size + TCP_HDR_SIZE +
> TCP_TSOPT_SIZE + 2;
> >         // Put first 8 bytes as a big endian message length
> >         memcpy(pkt, &len_be, 8);
> >         pkt += 8;
> > @@ -68,10 +82,9 @@ static void fastboot_tcp_send_message(const char
> *message, unsigned int len)
> >         memset(pkt, '\0', PKTSIZE);
> >  }
> >
>
> [..]
>  +
> >  void fastboot_tcp_start_server(void)
> >  {
> >         printf("Using %s device\n", eth_get_name());
> > -       printf("Listening for fastboot command on tcp %pI4\n", &net_ip);
> >
> > +       printf("Listening for fastboot command on tcp %pI4\n", &net_ip);
> >         tcp_set_tcp_handler(fastboot_tcp_handler_ipv4);
> > +
> > +#if defined(CONFIG_IPV6)
> > +       printf("Listening for fastboot command on %pI6\n", &net_ip6);
> > +       net_set_tcp_handler6(fastboot_tcp_handler_ipv6);
> > +#endif
>
> and here. This needs some thought as we don't want to add #if to C
> code where possible.
>
> Regards,
> Simon
>
diff mbox series

Patch

diff --git a/include/net/fastboot_tcp.h b/include/net/fastboot_tcp.h
index 6cf29d52e9..98986fa10a 100644
--- a/include/net/fastboot_tcp.h
+++ b/include/net/fastboot_tcp.h
@@ -7,7 +7,7 @@ 
 #define __NET_FASTBOOT_TCP_H__
 
 /**
- * Wait for incoming tcp fastboot comands.
+ * Wait for incoming TCP fastboot comands.
  */
 void fastboot_tcp_start_server(void);
 
diff --git a/net/fastboot_tcp.c b/net/fastboot_tcp.c
index 2eb52ea256..d93b52ede5 100644
--- a/net/fastboot_tcp.c
+++ b/net/fastboot_tcp.c
@@ -6,8 +6,10 @@ 
 #include <common.h>
 #include <fastboot.h>
 #include <net.h>
+#include <net6.h>
 #include <net/fastboot_tcp.h>
 #include <net/tcp.h>
+#include <net/tcp6.h>
 
 static char command[FASTBOOT_COMMAND_LEN] = {0};
 static char response[FASTBOOT_RESPONSE_LEN] = {0};
@@ -20,18 +22,30 @@  static u16 curr_dport;
 static u32 curr_tcp_seq_num;
 static u32 curr_tcp_ack_num;
 static unsigned int curr_request_len;
+static bool is_ipv6;
+static size_t ip_header_size;
 static enum fastboot_tcp_state {
 	FASTBOOT_CLOSED,
 	FASTBOOT_CONNECTED,
 	FASTBOOT_DISCONNECTING
 } state = FASTBOOT_CLOSED;
 
+static int command_handled_id;
+static bool command_handled_success;
+
 static void fastboot_tcp_answer(u8 action, unsigned int len)
 {
 	const u32 response_seq_num = curr_tcp_ack_num;
 	const u32 response_ack_num = curr_tcp_seq_num +
 		  (curr_request_len > 0 ? curr_request_len : 1);
 
+#if defined(CONFIG_IPV6)
+	if (is_ipv6) {
+		net_send_tcp_packet6(len, htons(curr_sport), htons(curr_dport),
+				     action, response_seq_num, response_ack_num);
+		return;
+	}
+#endif
 	net_send_tcp_packet(len, htons(curr_sport), htons(curr_dport),
 			    action, response_seq_num, response_ack_num);
 }
@@ -47,7 +61,7 @@  static void fastboot_tcp_send_packet(u8 action, const uchar *data, unsigned int
 	uchar *pkt = net_get_async_tx_pkt_buf();
 
 	memset(pkt, '\0', PKTSIZE);
-	pkt += net_eth_hdr_size() + IP_TCP_HDR_SIZE + TCP_TSOPT_SIZE + 2;
+	pkt += net_eth_hdr_size() + ip_header_size + TCP_HDR_SIZE + TCP_TSOPT_SIZE + 2;
 	memcpy(pkt, data, len);
 	fastboot_tcp_answer(action, len);
 	memset(pkt, '\0', PKTSIZE);
@@ -59,7 +73,7 @@  static void fastboot_tcp_send_message(const char *message, unsigned int len)
 	uchar *pkt = net_get_async_tx_pkt_buf();
 
 	memset(pkt, '\0', PKTSIZE);
-	pkt += net_eth_hdr_size() + IP_TCP_HDR_SIZE + TCP_TSOPT_SIZE + 2;
+	pkt += net_eth_hdr_size() + ip_header_size + TCP_HDR_SIZE + TCP_TSOPT_SIZE + 2;
 	// Put first 8 bytes as a big endian message length
 	memcpy(pkt, &len_be, 8);
 	pkt += 8;
@@ -68,10 +82,9 @@  static void fastboot_tcp_send_message(const char *message, unsigned int len)
 	memset(pkt, '\0', PKTSIZE);
 }
 
-static void fastboot_tcp_handler_ipv4(uchar *pkt, u16 dport,
-				      struct in_addr sip, u16 sport,
-				      u32 tcp_seq_num, u32 tcp_ack_num,
-				      u8 action, unsigned int len)
+static void fastboot_tcp_handler(uchar *pkt, u16 dport, u16 sport,
+				 u32 tcp_seq_num, u32 tcp_ack_num,
+				 u8 action, unsigned int len)
 {
 	int fastboot_command_id;
 	u64 command_size;
@@ -88,7 +101,6 @@  static void fastboot_tcp_handler_ipv4(uchar *pkt, u16 dport,
 	case FASTBOOT_CLOSED:
 		if (tcp_push) {
 			if (len != handshake_length ||
-			    strlen(pkt) != handshake_length ||
 			    memcmp(pkt, handshake, handshake_length) != 0) {
 				fastboot_tcp_reset();
 				break;
@@ -111,18 +123,25 @@  static void fastboot_tcp_handler_ipv4(uchar *pkt, u16 dport,
 			pkt += 8;
 
 			// Only single packet messages are supported ATM
-			if (strlen(pkt) != command_size) {
+			if (len != command_size) {
 				fastboot_tcp_reset();
 				break;
 			}
 			strlcpy(command, pkt, len + 1);
 			fastboot_command_id = fastboot_handle_command(command, response);
 			fastboot_tcp_send_message(response, strlen(response));
-			fastboot_handle_boot(fastboot_command_id,
-					     strncmp("OKAY", response, 4) == 0);
+
+			command_handled_id = fastboot_command_id;
+			command_handled_success = strncmp("OKAY", response, 4) == 0;
 		}
 		break;
 	case FASTBOOT_DISCONNECTING:
+		if (command_handled_success) {
+			fastboot_handle_boot(command_handled_id, command_handled_success);
+			command_handled_id = 0;
+			command_handled_success = false;
+		}
+
 		if (tcp_push)
 			state = FASTBOOT_CLOSED;
 		break;
@@ -137,10 +156,41 @@  static void fastboot_tcp_handler_ipv4(uchar *pkt, u16 dport,
 	curr_request_len = 0;
 }
 
+static void fastboot_tcp_handler_ipv4(uchar *pkt, u16 dport,
+				      struct in_addr sip, u16 sport,
+				      u32 tcp_seq_num, u32 tcp_ack_num,
+				      u8 action, unsigned int len)
+{
+	is_ipv6 = false;
+	ip_header_size = IP_HDR_SIZE;
+	fastboot_tcp_handler(pkt, dport, sport,
+			     tcp_seq_num, tcp_ack_num,
+			     action, len);
+}
+
+#if defined(CONFIG_IPV6)
+static void fastboot_tcp_handler_ipv6(uchar *pkt, u16 dport,
+				      struct in6_addr sip, u16 sport,
+				      u32 tcp_seq_num, u32 tcp_ack_num,
+				      u8 action, unsigned int len)
+{
+	is_ipv6 = true;
+	ip_header_size = IP6_HDR_SIZE;
+	fastboot_tcp_handler(pkt, dport, sport,
+			     tcp_seq_num, tcp_ack_num,
+			     action, len);
+}
+#endif
+
 void fastboot_tcp_start_server(void)
 {
 	printf("Using %s device\n", eth_get_name());
-	printf("Listening for fastboot command on tcp %pI4\n", &net_ip);
 
+	printf("Listening for fastboot command on tcp %pI4\n", &net_ip);
 	tcp_set_tcp_handler(fastboot_tcp_handler_ipv4);
+
+#if defined(CONFIG_IPV6)
+	printf("Listening for fastboot command on %pI6\n", &net_ip6);
+	net_set_tcp_handler6(fastboot_tcp_handler_ipv6);
+#endif
 }