void
usbip_run (const USB_DEVICE_DESCRIPTOR *dev_dsc)                                /* simple TCP server */
{
  struct sockaddr_in serv, cli;
  int listenfd, sockfd, nb;
#ifdef LINUX
  unsigned int clilen;
#else
  int clilen;
#endif
  unsigned char attached;



#ifndef LINUX
  WSAStartup (wVersionRequested, &wsaData);
  if (wsaData.wVersion != wVersionRequested)
    {
      fprintf (stderr, "\n Wrong version\n");
      exit (-1);
    }

#endif

  if ((listenfd = socket (PF_INET, SOCK_STREAM, 0)) < 0)
    {
      printf ("socket error : %s \n", strerror (errno));
      exit (1);
    };

  int reuse = 1;
  if (setsockopt(listenfd, SOL_SOCKET, SO_REUSEADDR, (const char*)&reuse, sizeof(reuse)) < 0)
      perror("setsockopt(SO_REUSEADDR) failed");

  memset (&serv, 0, sizeof (serv));
  serv.sin_family = AF_INET;
  serv.sin_addr.s_addr = htonl (INADDR_ANY);
  serv.sin_port = htons (TCP_SERV_PORT);

  if (bind (listenfd, (sockaddr *) & serv, sizeof (serv)) < 0)
    {
      printf ("bind error : %s \n", strerror (errno));
      exit (1);
    };

  if (listen (listenfd, SOMAXCONN) < 0)
    {
      printf ("listen error : %s \n", strerror (errno));
      exit (1);
    };

  for (;;)
    {

      clilen = sizeof (cli);
      if (
          (sockfd =
           accept (listenfd, (sockaddr *) & cli,  & clilen)) < 0)
        {
          printf ("accept error : %s \n", strerror (errno));
          exit (1);
        };
        printf("Connection address:%s\n",inet_ntoa(cli.sin_addr));
        attached=0;
  
        while(1)
        {
          if(! attached)
          {
             OP_REQ_DEVLIST req;
             if ((nb = recv (sockfd, (char *)&req, sizeof(OP_REQ_DEVLIST), 0)) != sizeof(OP_REQ_DEVLIST))
             {
               //printf ("receive error : %s \n", strerror (errno));
               break;
             };
#ifdef _DEBUG
             print_recv((char *)&req, sizeof(OP_REQ_DEVLIST),"OP_REQ_DEVLIST");
#endif
             req.command=ntohs(req.command);
             printf("Header Packet\n");  
             printf("command: 0x%02X\n",req.command);
             if(req.command == 0x8005)
             {
               OP_REP_DEVLIST list;
               printf("list of devices\n");

               handle_device_list(dev_dsc,&list);

               if (send (sockfd, (char *)&list.header, sizeof(OP_REP_DEVLIST_HEADER), 0) != sizeof(OP_REP_DEVLIST_HEADER))
               {
                   printf ("send error : %s \n", strerror (errno));
                   break;
               };
               if (send (sockfd, (char *)&list.device, sizeof(OP_REP_DEVLIST_DEVICE), 0) != sizeof(OP_REP_DEVLIST_DEVICE))
               {
                   printf ("send error : %s \n", strerror (errno));
                   break;
               };
               if (send (sockfd, (char *)list.interfaces, sizeof(OP_REP_DEVLIST_INTERFACE)*list.device.bNumInterfaces, 0) != sizeof(OP_REP_DEVLIST_INTERFACE)*list.device.bNumInterfaces)
               {
                   printf ("send error : %s \n", strerror (errno));
                   break;
               };
               free(list.interfaces);
             }
             else if(req.command == 0x8003) 
             {
               char busid[32];
               OP_REP_IMPORT rep;
               printf("attach device\n");
               if ((nb = recv (sockfd, busid, 32, 0)) != 32)
               {
                 printf ("receive error : %s \n", strerror (errno));
                 break;
               };
#ifdef _DEBUG
             print_recv(busid, 32,"Busid");
#endif
               handle_attach(dev_dsc,&rep);
               if (send (sockfd, (char *)&rep, sizeof(OP_REP_IMPORT), 0) != sizeof(OP_REP_IMPORT))
               {
                   printf ("send error : %s \n", strerror (errno));
                   break;
               };
               attached = 1;
             }
          }
          else
          {
             printf("------------------------------------------------\n"); 
             printf("handles requests\n");
             USBIP_CMD_SUBMIT cmd;
             USBIP_RET_SUBMIT usb_req;
             if ((nb = recv (sockfd, (char *)&cmd, sizeof(USBIP_CMD_SUBMIT), 0)) != sizeof(USBIP_CMD_SUBMIT))
             {
               printf ("receive error : %s \n", strerror (errno));
               break;
             };
#ifdef _DEBUG
             print_recv((char *)&cmd, sizeof(USBIP_CMD_SUBMIT),"USBIP_CMD_SUBMIT");
#endif
             unpack((int *)&cmd,sizeof(USBIP_CMD_SUBMIT));               
             printf("usbip cmd %u\n",cmd.command);
             printf("usbip seqnum %u\n",cmd.seqnum);
             printf("usbip devid %u\n",cmd.devid);
             printf("usbip direction %u\n",cmd.direction);
             printf("usbip ep %u\n",cmd.ep);
             printf("usbip flags %u\n",cmd.transfer_flags);
             printf("usbip number of packets %u\n",cmd.number_of_packets);
             printf("usbip interval %u\n",cmd.interval);
#ifdef LINUX
             printf("usbip setup %llu\n",cmd.setup);
#else
             printf("usbip setup %I64u\n",cmd.setup);
#endif
             printf("usbip buffer lenght  %u\n",cmd.transfer_buffer_length);
             usb_req.command=0;
             usb_req.seqnum=cmd.seqnum;
             usb_req.devid=cmd.devid;
             usb_req.direction=cmd.direction;
             usb_req.ep=cmd.ep;
             usb_req.status=0;
             usb_req.actual_length=0;
             usb_req.start_frame=0;
             usb_req.number_of_packets=0;
             usb_req.error_count=0;
             usb_req.setup=cmd.setup;
             
             if(cmd.command == 1)
               handle_usb_request(sockfd, &usb_req, cmd.transfer_buffer_length);
             

             if(cmd.command == 2) //unlink urb
             {
                printf("####################### Unlink URB %u  (not working!!!)\n",cmd.transfer_flags);
             //FIXME
               /*              
                USBIP_RET_UNLINK ret;  
                printf("####################### Unlink URB %u\n",cmd.transfer_flags);
                ret.command=htonl(0x04);
                ret.devid=htonl(cmd.devid);
                ret.direction=htonl(cmd.direction);
                ret.ep=htonl(cmd.ep);
                ret.seqnum=htonl(cmd.seqnum);
                ret.status=htonl(1);
 
                if (send (sockfd, (char *)&ret, sizeof(USBIP_RET_UNLINK), 0) != sizeof(USBIP_RET_UNLINK))
                {
                  printf ("send error : %s \n", strerror (errno));
                  exit(-1);
                };
               */ 
             }

             if(cmd.command > 2)
             {
                printf("Unknown USBIP cmd!\n");  
                close (sockfd);
#ifndef LINUX
                WSACleanup ();
#endif
                return;  
             };
 
          } 
       }
       close (sockfd);
    };
#ifndef LINUX
  WSACleanup ();
#endif
};
static int handle_device_list(const u8_t *desc, int connfd)
{
	struct op_common header = {
		.version = htons(USBIP_VERSION),
		.code = htons(OP_REP_DEVLIST),
		.status = 0,
	};

	LOG_DBG("desc %p", desc);

	if (send(connfd, &header, sizeof(header), 0) != sizeof(header)) {
		LOG_ERR("send() header failed: %s", strerror(errno));
		return errno;
	}

	/* Send number of devices */
	u32_t ndev = htonl(1);

	if (send(connfd, &ndev, sizeof(ndev), 0) != sizeof(ndev)) {
		LOG_ERR("send() ndev failed: %s", strerror(errno));
		return errno;
	}

	send_device(desc, connfd);

	send_interfaces(desc, connfd);

	return 0;
}

static void handle_usbip_submit(int connfd, struct usbip_header *hdr)
{
	struct usbip_submit *req = &hdr->u.submit;
	int read;

	LOG_DBG("");

	read = recv(connfd, req, sizeof(*req), 0);
	if (read != sizeof(*req)) {
		LOG_ERR("recv() failed: %s", strerror(errno));
		return;
	}

	usbip_header_dump((void *)hdr);

	if (ntohl(hdr->common.ep) == 0) {
		handle_usb_control(hdr);
	} else {
		handle_usb_data(hdr);
	}
}

static void handle_usbip_unlink(int connfd, struct usbip_header *hdr)
{
	struct usbip_unlink *req = &hdr->u.unlink;
	u64_t setup_padding;
	int read;

	LOG_DBG("");

	read = recv(connfd, req, sizeof(hdr->u), 0);
	if (read != sizeof(hdr->u)) {
		LOG_ERR("recv() failed: %s", strerror(errno));
		return;
	}

	/* Read also padding */
	read = recv(connfd, &setup_padding, sizeof(setup_padding), 0);
	if (read != sizeof(setup_padding)) {
		LOG_ERR("recv() failed: %s", strerror(errno));
		return;
	}

	usbip_header_dump((void *)hdr);

	/* TODO: unlink */
}

static int handle_import(const u8_t *desc, int connfd)
{
	struct op_common header = {
		.version = htons(USBIP_VERSION),
		.code = htons(OP_REP_IMPORT),
		.status = 0,
	};
	char busid[32];

	LOG_DBG("attach device");

	if (recv(connfd, busid, 32, 0) != sizeof(busid)) {
		LOG_ERR("recv() failed: %s", strerror(errno));
		return errno;
	}

	if (send(connfd, &header, sizeof(header), 0) != sizeof(header)) {
		LOG_ERR("send() header failed: %s", strerror(errno));
		return errno;
	}

	send_device(desc, connfd);

	return 0;
}

extern struct usb_desc_header __usb_descriptor_start[];

void usbip_start(void)
{
	struct sockaddr_in srv;
	unsigned char attached;
	int listenfd, connfd;
	const u8_t *desc;
	int reuse = 1;

	LOG_DBG("Starting");

	/*
	 * Do not use usb_get_device_descriptor();
	 * to prevent double string fixing
	 */
	desc = (const u8_t *)__usb_descriptor_start;
	if (!desc) {
		LOG_ERR("Descriptors are not set");
		posix_exit(EXIT_FAILURE);
	}

	listenfd = socket(PF_INET, SOCK_STREAM | SOCK_NONBLOCK, 0);
	if (listenfd < 0) {
		LOG_ERR("socket() failed: %s", strerror(errno));
		posix_exit(EXIT_FAILURE);
	}

	if (setsockopt(listenfd, SOL_SOCKET, SO_REUSEADDR,
		       (const char *)&reuse, sizeof(reuse)) < 0) {
		LOG_WRN("setsockopt() failed: %s", strerror(errno));
	}

	memset(&srv, 0, sizeof(srv));
	srv.sin_family = AF_INET;
	srv.sin_addr.s_addr = htonl(INADDR_ANY);
	srv.sin_port = htons(USBIP_PORT);

	if (bind(listenfd, (struct sockaddr *)&srv, sizeof(srv)) < 0) {
		LOG_ERR("bind() failed: %s", strerror(errno));
		posix_exit(EXIT_FAILURE);
	}

	if (listen(listenfd, SOMAXCONN) < 0) {
		LOG_ERR("listen() failed: %s", strerror(errno));
		posix_exit(EXIT_FAILURE);
	}

	while (true) {
		struct sockaddr_in client_addr;
		socklen_t client_addr_len = sizeof(client_addr);

		connfd = accept4(listenfd, (struct sockaddr *)&client_addr,
				 &client_addr_len, SOCK_NONBLOCK);
		if (connfd < 0) {
			if (errno == EAGAIN || errno == EWOULDBLOCK) {
				/* Non-blocking accept */
				k_sleep(100);

				continue;
			}

			LOG_ERR("accept() failed: %s", strerror(errno));
			posix_exit(EXIT_FAILURE);
		}

		connfd_global = connfd;

		LOG_DBG("Connection: %s", inet_ntoa(client_addr.sin_addr));

		/* Set attached 0 */
		attached = 0U;

		while (true) {
			struct usbip_header cmd;
			struct usbip_header_common *hdr = &cmd.common;
			int read;

			if (!attached) {
				struct op_common req;

				read = recv(connfd, &req, sizeof(req), 0);
				if (read < 0) {
					if (errno == EAGAIN ||
					    errno == EWOULDBLOCK) {
						/* Non-blocking accept */
						k_sleep(100);

						continue;
					}
				}

				if (read != sizeof(req)) {
					LOG_WRN("wrong length, %d", read);

					/* Closing connection */
					break;
				}

				LOG_HEXDUMP_DBG((u8_t *)&req, sizeof(req),
						"Got request");

				LOG_DBG("Code: 0x%x", ntohs(req.code));

				switch (ntohs(req.code)) {
				case OP_REQ_DEVLIST:
					handle_device_list(desc, connfd);
					break;
				case OP_REQ_IMPORT:
					if (!handle_import(desc, connfd)) {
						attached = 1U;
					}
					break;
				default:
					LOG_ERR("Unhandled code: 0x%x",
						ntohs(req.code));
					break;
				}

				continue;
			}

			/* Handle attached case */

			read = recv(connfd, hdr, sizeof(*hdr), 0);
			if (read < 0) {
				if (errno == EAGAIN || errno == EWOULDBLOCK) {
					/* Non-blocking accept */
					k_sleep(100);

					continue;
				}
			}

			LOG_HEXDUMP_DBG((u8_t *)hdr, read, "Got cmd");

			if (read != sizeof(*hdr)) {
				LOG_ERR("recv wrong length: %d", read);

				/* Closing connection */
				break;
			}

			devid_global = ntohl(hdr->devid);
			seqnum_global = ntohl(hdr->seqnum);

			switch (ntohl(hdr->command)) {
			case USBIP_CMD_SUBMIT:
				handle_usbip_submit(connfd, &cmd);
				break;
			case USBIP_CMD_UNLINK:
				handle_usbip_unlink(connfd, &cmd);
				break;
			default:
				LOG_ERR("Unknown command: 0x%x",
					ntohl(hdr->command));
				close(connfd);
				return;
			}
		}

		LOG_DBG("Closing connection");
		close(connfd);
	}
}

int usbip_recv(u8_t *buf, size_t len)
{
	return recv(connfd_global, buf, len, 0);
}

int usbip_send(u8_t ep, const u8_t *data, size_t len)
{
	return send(connfd_global, data, len, 0);
}

int usbip_send_common(u8_t ep, u32_t data_len)
{
	struct usbip_submit_rsp rsp;

	rsp.common.command = htonl(USBIP_RET_SUBMIT);
	rsp.common.seqnum = htonl(seqnum_global);
	rsp.common.devid = htonl(0);
	rsp.common.direction = htonl(0); /* TODO get from ep */
	rsp.common.ep = htonl(ep);

	rsp.status = htonl(0);
	rsp.actual_length = htonl(data_len);
	rsp.start_frame = htonl(0);
	rsp.number_of_packets = htonl(0);
	rsp.error_count = htonl(0);

	rsp.setup = htonl(0);

	return usbip_send(ep, (u8_t *)&rsp, sizeof(rsp));
}