Nyaaori/rockbox
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity
rockbox/utils/hwstub/lib/hwstub_net.cpp
Amaury Pouly 3d8a08ca25 hwstub: rewrite and expand library 
Rewrite the hwstub library in C++, with a clean and modular design.
The library was designed from the ground up to be aware of multithreading
issues and to handle memory allocation nicely with shared pointers.

Compared to the original library, it brings the following major features:
- support for JZ boot devices, it is very easy to add support for others
- support for network transparent operations (through sockets): both tcp
  and unix domains are support

Change-Id: I75899cb9c7aa938c17ede2bb3f468e7a55d625b4
2016-04-08 19:37:30 +02:00

1351 lines
41 KiB
C++
Raw Blame History

/***************************************************************************
* __________ __ ___.
* Open \______ \ ____ ____ | | _\_ |__ _______ ___
* Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ /
* Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < <
* Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \
* \/ \/ \/ \/ \/
* $Id$
*
* Copyright (C) 2016 by Amaury Pouly
*
* This program is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* as published by the Free Software Foundation; either version 2
* of the License, or (at your option) any later version.
*
* This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
* KIND, either express or implied.
*
****************************************************************************/
#include "hwstub_net.hpp"
#include <sys/socket.h>
#include <sys/un.h>
#include <unistd.h>
#include <cstddef>
#include <arpa/inet.h>
#include <sys/types.h>
#include <netdb.h>
namespace hwstub {
namespace net {
/**
* Context
*/
context::context()
:m_state(state::HELLO), m_error(error::SUCCESS)
{
}
context::~context()
{
}
std::shared_ptr<context> context::create_socket(int socket_fd)
{
// NOTE: can't use make_shared() because of the protected ctor */
return std::shared_ptr<socket_context>(new socket_context(socket_fd));
}
std::string context::default_unix_path()
{
return "hwstub";
}
std::string context::default_tcp_domain()
{
return "localhost";
}
std::string context::default_tcp_port()
{
return "6666";
}
namespace
{
/* len is the total length, including a 0 character if any */
int create_unix_low(bool abstract, const char *path, size_t len, bool conn,
std::string *error)
{
struct sockaddr_un address;
if(len > sizeof(address.sun_path))
{
if(error)
*error = "unix path is too long";
return -1;
}
int socket_fd = socket(PF_UNIX, SOCK_STREAM, 0);
if(socket_fd < 0)
{
if(error)
*error = "socket() failed";
return -1;
}
memset(&address, 0, sizeof(struct sockaddr_un));
address.sun_family = AF_UNIX;
/* NOTE memcpy, we don't want to add a extra 0 at the end */
memcpy(address.sun_path, path, len);
/* for abstract name, replace first character by 0 */
if(abstract)
address.sun_path[0] = 0;
/* NOTE sun_path is the last field of the structure */
size_t sz = offsetof(struct sockaddr_un, sun_path) + len;
/* NOTE don't give sizeof(address) because for abstract names it would contain
* extra garbage */
if(conn)
{
if(connect(socket_fd, (struct sockaddr *)&address, sz) != 0)
{
close(socket_fd);
if(error)
*error = "connect() failed";
return -1;
}
else
return socket_fd;
}
else
{
if(bind(socket_fd, (struct sockaddr *)&address, sz) != 0)
{
close(socket_fd);
if(error)
*error = "bind() failed";
return -1;
}
else
return socket_fd;
}
}
int create_tcp_low(const std::string& domain, const std::string& _port, bool server, std::string *error)
{
std::string port = _port.size() != 0 ? _port : context::default_tcp_port();
int socket_fd = -1;
struct addrinfo hints;
memset(&hints, 0, sizeof(struct addrinfo));
hints.ai_family = AF_UNSPEC; /* allow IPv4 or IPv6 */
hints.ai_socktype = SOCK_STREAM;
hints.ai_flags = 0;
hints.ai_protocol = 0; /* any protocol */
struct addrinfo *result;
int err = getaddrinfo(domain.c_str(), port.c_str(), &hints, &result);
if(err != 0)
{
if(error)
*error = std::string("getaddrinfo failed: ") + gai_strerror(err);
return -1;
}
/* getaddrinfo() returns a list of address structures.
* Try each address until we successfully connect(2).
* If socket(2) (or connect(2)/bind(2)) fails, we (close the socket
* and) try the next address. */
for(struct addrinfo *rp = result; rp != nullptr; rp = rp->ai_next)
{
socket_fd = socket(rp->ai_family, rp->ai_socktype, rp->ai_protocol);
if(socket_fd == -1)
continue;
int err = 0;
if(server)
err = bind(socket_fd, rp->ai_addr, rp->ai_addrlen);
else
err = connect(socket_fd, rp->ai_addr, rp->ai_addrlen);
if(err < 0)
{
close(socket_fd);
socket_fd = -1;
}
else
break; /* success */
}
/* no address was tried */
if(socket_fd < 0 && error)
*error = "getaddrinfo() returned no usable result (socket()/connect()/bind() failed)";
return socket_fd;
}
}
std::shared_ptr<context> context::create_tcp(const std::string& domain,
const std::string& port, std::string *error)
{
int fd = create_tcp_low(domain, port, false, error);
if(fd >= 0)
return context::create_socket(fd);
else
return std::shared_ptr<context>();
}
std::shared_ptr<context> context::create_unix(const std::string& path, std::string *error)
{
int fd = create_unix_low(false, path.c_str(), path.size() + 1, true, error);
if(fd >= 0)
return context::create_socket(fd);
else
return std::shared_ptr<context>();
}
std::shared_ptr<context> context::create_unix_abstract(const std::string& path, std::string *error)
{
std::string fake_path = "#" + path; /* the # will be overriden by 0 */
int fd = create_unix_low(true, fake_path.c_str(), fake_path.size(), true, error);
if(fd >= 0)
return context::create_socket(fd);
else
return std::shared_ptr<context>();
}
uint32_t context::from_ctx_dev(ctx_dev_t dev)
{
return (uint32_t)(uintptr_t)dev; /* NOTE safe because it was originally a 32-bit int */
}
hwstub::context::ctx_dev_t context::to_ctx_dev(uint32_t dev)
{
return (ctx_dev_t)(uintptr_t)dev; /* NOTE assume that sizeof(void *)>=sizeof(uint32_t) */
}
error context::fetch_device_list(std::vector<ctx_dev_t>& list, void*& ptr)
{
(void) ptr;
delayed_init();
if(m_state == state::DEAD)
return m_error;
uint32_t args[HWSTUB_NET_ARGS] = {0};
uint8_t *data = nullptr;
size_t data_sz = 0;
debug() << "[net::ctx] --> GET_DEV_LIST\n";
error err = send_cmd(HWSERVER_GET_DEV_LIST, args, nullptr, 0, &data, &data_sz);
debug() << "[net::ctx] <-- GET_DEV_LIST ";
if(err != error::SUCCESS)
{
debug() << "failed: " << error_string(err) << "\n";
return err;
}
/* sanity check on size */
if(data_sz % 4)
{
debug() << "failed: invalid list size\n";
delete[] data;
return error::PROTOCOL_ERROR;
}
debug() << "\n";
list.clear();
/* each entry is a 32-bit ID in network order size */
uint32_t *data_list = (uint32_t *)data;
for(size_t i = 0; i < data_sz / 4; i++)
list.push_back(to_ctx_dev(from_net_order(data_list[i])));
delete[] data;
return error::SUCCESS;
}
void context::destroy_device_list(void *ptr)
{
(void)ptr;
}
error context::create_device(ctx_dev_t dev, std::shared_ptr<hwstub::device>& hwdev)
{
// NOTE: can't use make_shared() because of the protected ctor */
hwdev.reset(new device(shared_from_this(), from_ctx_dev(dev)));
return error::SUCCESS;
}
bool context::match_device(ctx_dev_t dev, std::shared_ptr<hwstub::device> hwdev)
{
device *udev = dynamic_cast<device*>(hwdev.get());
return udev != nullptr && udev->device_id() == from_ctx_dev(dev);
}
uint32_t context::to_net_order(uint32_t u)
{
return htonl(u);
}
uint32_t context::from_net_order(uint32_t u)
{
return ntohl(u);
}
error context::send_cmd(uint32_t cmd, uint32_t args[HWSTUB_NET_ARGS], uint8_t *send_data,
size_t send_size, uint8_t **recv_data, size_t *recv_size)
{
/* make sure with have the lock, this function might be called concurrently
* by the different threads */
std::unique_lock<std::recursive_mutex> lock(m_mutex);
if(m_state == state::DEAD)
return m_error;
/* do a delayed init, unless with are doing a HELLO */
if(m_state == state::HELLO && cmd != HWSERVER_HELLO)
delayed_init();
/* build header */
struct hwstub_net_hdr_t hdr;
hdr.magic = to_net_order(HWSERVER_MAGIC);
hdr.cmd = to_net_order(cmd);
for(size_t i = 0; i < HWSTUB_NET_ARGS; i++)
hdr.args[i] = to_net_order(args[i]);
hdr.length = to_net_order((uint32_t)send_size);
/* send header */
size_t sz = sizeof(hdr);
error err = send((void *)&hdr, sz);
if(err != error::SUCCESS)
{
m_state = state::DEAD;
m_error = err;
return err;
}
if(sz != sizeof(hdr))
{
m_state = state::DEAD;
m_error = error::PROTOCOL_ERROR;
}
/* send data */
if(send_size > 0)
{
sz = send_size;
err = send((void *)send_data, sz);
if(err != error::SUCCESS)
{
m_state = state::DEAD;
m_error = err;
return err;
}
if(sz != send_size)
{
m_state = state::DEAD;
m_error = error::PROTOCOL_ERROR;
}
}
/* receive header */
sz = sizeof(hdr);
err = recv((void *)&hdr, sz);
if(err != error::SUCCESS)
{
m_state = state::DEAD;
m_error = err;
return err;
}
if(sz != sizeof(hdr))
{
m_state = state::DEAD;
m_error = error::PROTOCOL_ERROR;
return m_error;
}
/* correct byte order */
hdr.magic = from_net_order(hdr.magic);
hdr.cmd = from_net_order(hdr.cmd);
hdr.length = from_net_order(hdr.length);
/* copy arguments */
for(size_t i = 0; i < HWSTUB_NET_ARGS; i++)
args[i] = from_net_order(hdr.args[i]);
/* check header */
if(hdr.magic != HWSERVER_MAGIC)
{
m_state = state::DEAD;
m_error = error::PROTOCOL_ERROR;
return m_error;
}
/* check NACK */
if(hdr.cmd == HWSERVER_NACK(cmd))
{
/* translate error */
switch(args[0])
{
case HWERR_FAIL: err = error::ERROR; break;
case HWERR_INVALID_ID: err = error::ERROR; break; /* should not happen */
case HWERR_DISCONNECTED: err = error::DISCONNECTED; break;
}
return err;
}
/* check not ACK */
if(hdr.cmd != HWSERVER_ACK(cmd))
{
m_state = state::DEAD;
m_error = error::PROTOCOL_ERROR;
return m_error;
}
/* receive additional data */
uint8_t *data = nullptr;
if(hdr.length > 0)
{
data = new uint8_t[hdr.length];
sz = hdr.length;
err = recv((void *)data, sz);
if(err != error::SUCCESS)
{
m_state = state::DEAD;
m_error = err;
return err;
}
if(sz != hdr.length)
{
m_state = state::DEAD;
m_error = error::PROTOCOL_ERROR;
return m_error;
}
}
/* copy data if user want it */
if(recv_data)
{
if(*recv_data == nullptr)
{
*recv_data = data;
*recv_size = hdr.length;
}
else if(*recv_size < hdr.length)
{
delete[] data;
return error::OVERFLW;
}
else
{
*recv_size = hdr.length;
memcpy(*recv_data, data, *recv_size);
delete[] data;
}
}
/* throw it away otherwise */
else
{
delete[] data;
}
return error::SUCCESS;
}
void context::delayed_init()
{
/* only do HELLO if we haven't do it yet */
if(m_state != state::HELLO)
return;
debug() << "[net::ctx] --> HELLO " << HWSTUB_VERSION_MAJOR << "."
<< HWSTUB_VERSION_MINOR << "\n";
/* send HELLO with our version and see what the server is up to */
uint32_t args[HWSTUB_NET_ARGS] = {0};
args[0] = HWSTUB_VERSION_MAJOR << 8 | HWSTUB_VERSION_MINOR;
error err = send_cmd(HWSERVER_HELLO, args, nullptr, 0, nullptr, nullptr);
if(err != error::SUCCESS)
{
debug() << "[net::ctx] <-- HELLO failed: " << error_string(err) << "\n";
m_state = state::DEAD;
m_error = err;
return;
}
/* check the server is running the same version */
debug() << "[net::ctx] <-- HELLO " << ((args[0] & 0xff00) >> 8) << "." << (args[0] & 0xff) << "";
if(args[0] != (HWSTUB_VERSION_MAJOR << 8 | HWSTUB_VERSION_MINOR))
{
debug() << " (mismatch)\n";
m_state = state::DEAD;
m_error = error::SERVER_MISMATCH;
}
debug() << " (good)\n";
/* good, we can now send commands */
m_state = state::IDLE;
}
void context::stop_context()
{
/* make sure with have the lock, this function might be call asynchronously */
std::unique_lock<std::recursive_mutex> lock(m_mutex);
/* if dead, don't do anything */
if(m_state == state::DEAD)
return;
/* only send BYE if we are initialized */
if(m_state == state::IDLE)
{
debug() << "[net::ctx] --> BYE\n";
/* send BYE */
uint32_t args[HWSTUB_NET_ARGS] = {0};
error err = send_cmd(HWSERVER_BYE, args, nullptr, 0, nullptr, nullptr);
if(err != error::SUCCESS)
{
debug() << "[net::ctx] <-- BYE failed: " << error_string(err) << "\n";
m_state = state::DEAD;
m_error = err;
return;
}
debug() << "[net::ctx] <-- BYE\n";
}
/* now we are dead */
m_state = state::DEAD;
m_error = error::SERVER_DISCONNECTED;
}
/**
* Socket context
*/
socket_context::socket_context(int socket_fd)
:m_socketfd(socket_fd)
{
set_timeout(std::chrono::milliseconds(1000));
}
socket_context::~socket_context()
{
stop_context();
close(m_socketfd);
}
void socket_context::set_timeout(std::chrono::milliseconds ms)
{
struct timeval tv;
tv.tv_usec = 1000 * (ms.count() % 1000);
tv.tv_sec = ms.count() / 1000;
/* set timeout for the client operations */
setsockopt(m_socketfd, SOL_SOCKET, SO_RCVTIMEO, (char *)&tv, sizeof(tv));
setsockopt(m_socketfd, SOL_SOCKET, SO_SNDTIMEO, (char *)&tv, sizeof(tv));
}
error socket_context::send(void *buffer, size_t& sz)
{
debug() << "[net::ctx::sock] send(" << sz << "): ";
int ret = ::send(m_socketfd, buffer, sz, MSG_NOSIGNAL);
if(ret >= 0)
{
debug() << "good(" << ret << ")\n";
sz = (size_t)ret;
return error::SUCCESS;
}
/* convert some errors */
debug() << "fail(" << errno << "," << strerror(errno) << ")\n";
switch(errno)
{
#if EAGAIN != EWOULDBLOCK
case EAGAIN:
#endif
case EWOULDBLOCK: return error::TIMEOUT;
case ECONNRESET: case EPIPE: return error::SERVER_DISCONNECTED;
default: return error::NET_ERROR;
}
}
error socket_context::recv(void *buffer, size_t& sz)
{
debug() << "[net::ctx::sock] recv(" << sz << "): ";
int ret = ::recv(m_socketfd, buffer, sz, MSG_WAITALL);
if(ret > 0)
{
debug() << "good(" << ret << ")\n";
sz = (size_t)ret;
return error::SUCCESS;
}
if(ret == 0)
{
debug() << "disconnected\n";
return error::SERVER_DISCONNECTED;
}
debug() << "fail(" << errno << "," << strerror(errno) << ")\n";
switch(errno)
{
#if EAGAIN != EWOULDBLOCK
case EAGAIN:
#endif
case EWOULDBLOCK: return error::TIMEOUT;
default: return error::NET_ERROR;
}
}
/**
* Device
*/
device::device(std::shared_ptr<hwstub::context> ctx, uint32_t devid)
:hwstub::device(ctx), m_device_id(devid)
{
}
device::~device()
{
}
uint32_t device::device_id()
{
return m_device_id;
}
error device::open_dev(std::shared_ptr<hwstub::handle>& handle)
{
std::shared_ptr<hwstub::context> hctx = get_context();
if(!hctx)
return error::NO_CONTEXT;
context *ctx = dynamic_cast<context*>(hctx.get());
ctx->debug() << "[net::dev] --> DEV_OPEN(" << m_device_id << ")\n";
/* ask the server to open the device, note that the device ID may not exists
* anymore */
uint32_t args[HWSTUB_NET_ARGS] = {0};
args[0] = m_device_id;
error err = ctx->send_cmd(HWSERVER_DEV_OPEN, args, nullptr, 0, nullptr, nullptr);
if(err != error::SUCCESS)
{
ctx->debug() << "[net::ctx::dev] <-- DEV_OPEN failed: " << error_string(err) << "\n";
return err;
}
ctx->debug() << "[net::ctx::dev] <-- DEV_OPEN: handle = " << args[0] << "\n";
// NOTE: can't use make_shared() because of the protected ctor */
handle.reset(new hwstub::net::handle(shared_from_this(), args[0]));
return error::SUCCESS;
}
bool device::has_multiple_open() const
{
return false;
}
/**
* Handle
*/
handle::handle(std::shared_ptr<hwstub::device> dev, uint32_t hid)
:hwstub::handle(dev), m_handle_id(hid)
{
}
handle::~handle()
{
/* try to close the handle, if context is still accessible */
std::shared_ptr<hwstub::context> hctx = get_device()->get_context();
if(hctx)
{
context *ctx = dynamic_cast<context*>(hctx.get());
ctx->debug() << "[net::handle] --> DEV_CLOSE(" << m_handle_id << ")\n";
uint32_t args[HWSTUB_NET_ARGS] = {0};
args[0] = m_handle_id;
error err = ctx->send_cmd(HWSERVER_DEV_CLOSE, args, nullptr, 0, nullptr, nullptr);
if(err != error::SUCCESS)
ctx->debug() << "[net::handle] <-- DEV_CLOSE failed: " << error_string(err) << "\n";
else
ctx->debug() << "[net::handle] <-- DEV_CLOSE\n";
}
}
error handle::read_dev(uint32_t addr, void *buf, size_t& sz, bool atomic)
{
std::shared_ptr<hwstub::context> hctx = get_device()->get_context();
if(!hctx)
return error::NO_CONTEXT;
context *ctx = dynamic_cast<context*>(hctx.get());
ctx->debug() << "[net::handle] --> READ(" << m_handle_id << ",0x" << std::hex
<< addr << "," << sz << "," << atomic << ")\n";
uint32_t args[HWSTUB_NET_ARGS] = {0};
args[0] = m_handle_id;
args[1] = addr;
args[2] = sz;
args[3] = atomic ? HWSERVER_RW_ATOMIC : 0;
error err = ctx->send_cmd(HWSERVER_READ, args, nullptr, 0, (uint8_t **)&buf, &sz);
if(err != error::SUCCESS)
{
ctx->debug() << "[net::handle] <-- READ failed: " << error_string(err) << "\n";
return err;
}
ctx->debug() << "[net::handle] <-- READ\n";
return error::SUCCESS;
}
error handle::write_dev(uint32_t addr, const void *buf, size_t& sz, bool atomic)
{
std::shared_ptr<hwstub::context> hctx = get_device()->get_context();
if(!hctx)
return error::NO_CONTEXT;
context *ctx = dynamic_cast<context*>(hctx.get());
ctx->debug() << "[net::handle] --> WRITE(" << m_handle_id << ",0x" << std::hex
<< addr << "," << sz << "," << atomic << ")\n";
uint32_t args[HWSTUB_NET_ARGS] = {0};
args[0] = m_handle_id;
args[1] = addr;
args[2] = atomic ? HWSERVER_RW_ATOMIC : 0;
error err = ctx->send_cmd(HWSERVER_WRITE, args, (uint8_t *)buf, sz, nullptr, nullptr);
if(err != error::SUCCESS)
{
ctx->debug() << "[net::handle] <-- WRITE failed: " << error_string(err) << "\n";
return err;
}
ctx->debug() << "[net::handle] <-- WRITE\n";
return error::SUCCESS;
}
error handle::get_dev_desc(uint16_t desc, void *buf, size_t& buf_sz)
{
std::shared_ptr<hwstub::context> hctx = get_device()->get_context();
if(!hctx)
return error::NO_CONTEXT;
context *ctx = dynamic_cast<context*>(hctx.get());
ctx->debug() << "[net::handle] --> GET_DESC(" << m_handle_id << ",0x" << std::hex
<< desc << "," << buf_sz << ")\n";
uint32_t args[HWSTUB_NET_ARGS] = {0};
args[0] = m_handle_id;
args[1] = desc;
args[2] = buf_sz;
error err = ctx->send_cmd(HWSERVER_GET_DESC, args, nullptr, 0, (uint8_t **)&buf, &buf_sz);
if(err != error::SUCCESS)
{
ctx->debug() << "[net::handle] <-- GET_DESC failed: " << error_string(err) << "\n";
return err;
}
ctx->debug() << "[net::handle] <-- GET_DESC\n";
return error::SUCCESS;
}
error handle::get_dev_log(void *buf, size_t& buf_sz)
{
std::shared_ptr<hwstub::context> hctx = get_device()->get_context();
if(!hctx)
return error::NO_CONTEXT;
context *ctx = dynamic_cast<context*>(hctx.get());
ctx->debug() << "[net::handle] --> GET_LOG(" << buf_sz << ")\n";
uint32_t args[HWSTUB_NET_ARGS] = {0};
args[0] = m_handle_id;
args[1] = buf_sz;
error err = ctx->send_cmd(HWSERVER_GET_LOG, args, nullptr, 0, (uint8_t **)&buf, &buf_sz);
if(err != error::SUCCESS)
{
ctx->debug() << "[net::handle] <-- GET_LOG failed: " << error_string(err) << "\n";
return err;
}
ctx->debug() << "[net::handle] <-- GET_LOG\n";
return error::SUCCESS;
}
error handle::exec_dev(uint32_t addr, uint16_t flags)
{
(void) addr;
(void) flags;
return error::DUMMY;
}
error handle::status() const
{
return hwstub::handle::status();
}
size_t handle::get_buffer_size()
{
return 2048;
}
/**
* Server
*/
server::server(std::shared_ptr<hwstub::context> ctx)
:m_context(ctx)
{
clear_debug();
}
server::~server()
{
}
void server::stop_server()
{
std::unique_lock<std::recursive_mutex> lock(m_mutex);
/* ask all client threads to stop */
for(auto& cl : m_client)
cl.exit = true;
/* wait for each thread to stop */
for(auto& cl : m_client)
cl.future.wait();
}
std::shared_ptr<server> server::create_unix(std::shared_ptr<hwstub::context> ctx,
const std::string& path, std::string *error)
{
int fd = create_unix_low(false, path.c_str(), path.size() + 1, false, error);
if(fd >= 0)
return socket_server::create_socket(ctx, fd);
else
return std::shared_ptr<server>();
}
std::shared_ptr<server> server::create_unix_abstract(std::shared_ptr<hwstub::context> ctx,
const std::string& path, std::string *error)
{
std::string fake_path = "#" + path; /* the # will be overriden by 0 */
int fd = create_unix_low(true, fake_path.c_str(), fake_path.size(), false, error);
if(fd >= 0)
return socket_server::create_socket(ctx, fd);
else
return std::shared_ptr<server>();
}
std::shared_ptr<server> server::create_socket(std::shared_ptr<hwstub::context> ctx,
int socket_fd)
{
return socket_server::create(ctx, socket_fd);
}
std::shared_ptr<server> server::create_tcp(std::shared_ptr<hwstub::context> ctx,
const std::string& domain, const std::string& port, std::string *error)
{
int fd = create_tcp_low(domain, port, true, error);
if(fd >= 0)
return socket_server::create_socket(ctx, fd);
else
return std::shared_ptr<server>();
}
void server::set_debug(std::ostream& os)
{
m_debug = &os;
}
std::ostream& server::debug()
{
return *m_debug;
}
server::client_state::client_state(srv_client_t cl, std::future<void>&& f)
:client(cl), future(std::move(f)), exit(false), next_dev_id(42),
next_handle_id(19)
{
}
void server::client_thread2(server *s, client_state *cs)
{
s->client_thread(cs);
}
uint32_t server::to_net_order(uint32_t u)
{
return htonl(u);
}
uint32_t server::from_net_order(uint32_t u)
{
return ntohl(u);
}
void server::client_thread(client_state *state)
{
debug() << "[net::srv::client] start: " << state->client << "\n";
while(!state->exit)
{
/* wait for some header */
struct hwstub_net_hdr_t hdr;
size_t sz = sizeof(hdr);
error err;
/* wait for some command, or exit flag */
do
err = recv(state->client, (void *)&hdr, sz);
while(err == error::TIMEOUT && !state->exit);
if(state->exit || err != error::SUCCESS || sz != sizeof(hdr))
break;
/* convert to host order */
hdr.magic = from_net_order(hdr.magic);
hdr.cmd = from_net_order(hdr.cmd);
hdr.length = from_net_order(hdr.length);
/* copy arguments */
for(size_t i = 0; i < HWSTUB_NET_ARGS; i++)
hdr.args[i] = from_net_order(hdr.args[i]);
/* check header */
if(hdr.magic != HWSERVER_MAGIC)
break;
/* receive data
* FIXME check length here */
uint8_t *data = nullptr;
if(hdr.length > 0)
{
data = new uint8_t[hdr.length];
sz = hdr.length;
/* wait for some command, or exit flag */
do
err = recv(state->client, (void *)data, sz);
while(err == error::TIMEOUT && !state->exit);
if(state->exit || err != error::SUCCESS || sz != hdr.length)
{
delete[] data;
break;
}
}
/* hande command */
uint8_t *send_data = nullptr;
size_t send_size = 0;
err = handle_cmd(state, hdr.cmd, hdr.args, data, hdr.length,
send_data, send_size);
/* free data */
delete[] data;
/* construct header */
if(err != error::SUCCESS)
{
hdr.magic = to_net_order(HWSERVER_MAGIC);
hdr.cmd = to_net_order(HWSERVER_NACK(hdr.cmd));
hdr.length = to_net_order(0);
hdr.args[0] = to_net_order(HWERR_FAIL);
for(size_t i = 1; i < HWSTUB_NET_ARGS; i++)
hdr.args[i] = to_net_order(0);
send_size = 0;
}
else
{
hdr.magic = to_net_order(HWSERVER_MAGIC);
hdr.cmd = to_net_order(HWSERVER_ACK(hdr.cmd));
hdr.length = to_net_order(send_size);
for(size_t i = 0; i < HWSTUB_NET_ARGS; i++)
hdr.args[i] = to_net_order(hdr.args[i]);
}
/* send header */
sz = sizeof(hdr);
do
err = send(state->client, (void *)&hdr, sz);
while(err == error::TIMEOUT && !state->exit);
if(state->exit || err != error::SUCCESS || sz != sizeof(hdr))
{
delete[] send_data;
break;
}
/* send data if there is some */
if(send_size > 0)
{
sz = send_size;
do
err = send(state->client, (void *)send_data, sz);
while(err == error::TIMEOUT && !state->exit);
delete[] send_data;
if(state->exit || err != error::SUCCESS || sz != send_size)
break;
}
}
debug() << "[net::srv::client] stop: " << state->client << "\n";
/* clean client state to avoiding keeping references to objets */
state->dev_map.clear();
state->handle_map.clear();
/* kill client */
terminate_client(state->client);
}
void server::client_arrived(srv_client_t client)
{
std::unique_lock<std::recursive_mutex> lock(m_mutex);
debug() << "[net::srv] client arrived: " << client << "\n";
/* the naive way would be to use a std::thread but this class is annoying
* because it is impossible to check if a thread has exited, except by calling
* join() which is blocking. Fortunately, std::packaged_task and std::future
* provide a way around this */
std::packaged_task<void(server*, client_state*)> task(&server::client_thread2);
m_client.emplace_back(client, task.get_future());
std::thread(std::move(task), this, &m_client.back()).detach();
}
void server::client_left(srv_client_t client)
{
std::unique_lock<std::recursive_mutex> lock(m_mutex);
debug() << "[net::srv] client left: " << client << "\n";
/* find thread and set its exit flag, also cleanup threads that finished */
for(auto it = m_client.begin(); it != m_client.end();)
{
/* check if thread has finished */
if(it->future.wait_for(std::chrono::milliseconds(0)) == std::future_status::ready)
{
it = m_client.erase(it);
continue;
}
/* set exit flag if this our thread */
if(it->client == client)
it->exit = true;
++it;
}
}
error server::handle_cmd(client_state *state, uint32_t cmd, uint32_t args[HWSTUB_NET_ARGS],
uint8_t *recv_data, size_t recv_size, uint8_t*& send_data, size_t& send_size)
{
send_data = nullptr;
send_size = 0;
/* NOTE: commands are serialized by the client thread, this function is thus
* thread safe WITH RESPECT TO CLIENT DATA. If you need to use global data here,
* protect it by a mutex or make sure it is safe (hwstub context is thread-safe) */
/* HELLO */
if(cmd == HWSERVER_HELLO)
{
debug() << "[net::srv::cmd] --> HELLO " << ((args[0] & 0xff00) >> 8)
<< "." << (args[0] & 0xff);
if(args[0] != (HWSTUB_VERSION_MAJOR << 8 | HWSTUB_VERSION_MINOR))
{
debug() << " (mismatch)\n";
return error::ERROR;
}
debug() << " (good)\n";
debug() << "[net::srv::cmd] <-- HELLO " << HWSTUB_VERSION_MAJOR << "."
<< HWSTUB_VERSION_MINOR << "\n";
/* send HELLO with our version */
args[0] = HWSTUB_VERSION_MAJOR << 8 | HWSTUB_VERSION_MINOR;
return error::SUCCESS;
}
/* BYE */
else if(cmd == HWSERVER_BYE)
{
debug() << "[net::srv::cmd] --> BYE\n";
/* ask client thread to exit after this */
state->exit = true;
debug() << "[net::srv::cmd] <-- BYE\n";
return error::SUCCESS;
}
/* GET_DEV_LIST */
else if(cmd == HWSERVER_GET_DEV_LIST)
{
debug() << "[net::srv::cmd] --> GET_DEV_LIST\n";
/* fetch list again */
std::vector<std::shared_ptr<hwstub::device>> list;
error err = m_context->get_device_list(list);
if(err != error::SUCCESS)
{
debug() << "[net::srv::cmd] cannot fetch list: " << hwstub::error_string(err) << "\n";
debug() << "[net::srv::cmd] <-- GET_DEV_LIST (error)\n";
return err;
}
/* update list: drop device that left */
std::vector<uint32_t> to_drop;
for(auto it : state->dev_map)
{
bool still_there = false;
/* this has quadratic complexity, optimize this if needed */
for(auto dev : list)
if(it.second == dev)
still_there = true;
if(!still_there)
to_drop.push_back(it.first);
}
for(auto id : to_drop)
state->dev_map.erase(state->dev_map.find(id));
/* add new devices */
std::vector<std::shared_ptr<hwstub::device>> to_add;
for(auto dev : list)
{
bool already_there = false;
for(auto it : state->dev_map)
if(it.second == dev)
already_there = true;
if(!already_there)
to_add.push_back(dev);
}
for(auto dev : to_add)
state->dev_map[state->next_dev_id++] = dev;
/* create response list */
send_size = sizeof(uint32_t) * state->dev_map.size();
send_data = new uint8_t[send_size];
uint32_t *p = (uint32_t *)send_data;
for(auto it : state->dev_map)
*p++ = to_net_order(it.first);
debug() << "[net::srv::cmd] <-- GET_DEV_LIST\n";
return error::SUCCESS;
}
/* DEV_OPEN */
else if(cmd == HWSERVER_DEV_OPEN)
{
uint32_t devid = args[0];
debug() << "[net::srv::cmd] --> DEV_OPEN(" << devid << ")\n";
/* check ID is valid */
auto it = state->dev_map.find(devid);
if(it == state->dev_map.end())
{
debug() << "[net::srv::cmd] unknwon device ID\n";
debug() << "[net::srv::cmd] <-- DEV_OPEN (error)\n";
return error::ERROR;
}
/* good, now try to get a handle */
std::shared_ptr<hwstub::handle> handle;
error err = it->second->open(handle);
if(err != error::SUCCESS)
{
debug() << "[net::srv::cmd] cannot open device: " << hwstub::error_string(err) << "\n";
return err;
}
/* record ID and return it */
args[0] = state->next_handle_id;
state->handle_map[state->next_handle_id++] = handle;
return error::SUCCESS;
}
/* DEV_CLOSE */
else if(cmd == HWSERVER_DEV_CLOSE)
{
uint32_t hid = args[0];
debug() << "[net::srv::cmd] --> DEV_CLOSE(" << hid << ")\n";
/* check ID is valid */
auto it = state->handle_map.find(hid);
if(it == state->handle_map.end())
{
debug() << "[net::srv::cmd] unknwon handle ID\n";
debug() << "[net::srv::cmd] <-- DEV_CLOSE (error)\n";
return error::ERROR;
}
/* release ID and handle */
state->handle_map.erase(it);
debug() << "[net::srv::cmd] <-- DEV_CLOSE\n";
return error::SUCCESS;
}
/* HWSERVER_GET_DESC */
else if(cmd == HWSERVER_GET_DESC)
{
uint32_t hid = args[0];
uint32_t did = args[1];
uint32_t len = args[2];
debug() << "[net::srv::cmd] --> GET_DESC(" << hid << ",0x" << std::hex << did
<< "," << len << ")\n";
/* check ID is valid */
auto it = state->handle_map.find(hid);
if(it == state->handle_map.end())
{
debug() << "[net::srv::cmd] unknown handle ID\n";
debug() << "[net::srv::cmd] <-- GET_DESC (error)\n";
return error::ERROR;
}
/* query desc */
send_size = len;
send_data = new uint8_t[send_size];
error err = it->second->get_desc(did, send_data, send_size);
if(err != error::SUCCESS)
{
delete[] send_data;
debug() << "[net::srv::cmd] cannot get descriptor: " << error_string(err) << "\n";
debug() << "[net::srv::cmd] <-- GET_DESC (error)\n";
return err;
}
debug() << "[net::srv::cmd] <-- GET_DESC\n";
return error::SUCCESS;
}
/* HWSERVER_GET_LOG */
else if(cmd == HWSERVER_GET_LOG)
{
uint32_t hid = args[0];
uint32_t len = args[1];
debug() << "[net::srv::cmd] --> GET_LOG(" << hid << "," << len << ")\n";
/* check ID is valid */
auto it = state->handle_map.find(hid);
if(it == state->handle_map.end())
{
debug() << "[net::srv::cmd] unknown handle ID\n";
debug() << "[net::srv::cmd] <-- GET_DESC (error)\n";
return error::ERROR;
}
/* query log */
send_size = len;
send_data = new uint8_t[send_size];
error err = it->second->get_log(send_data, send_size);
if(err != error::SUCCESS)
{
delete[] send_data;
debug() << "[net::srv::cmd] cannot get log: " << error_string(err) << "\n";
debug() << "[net::srv::cmd] <-- GET_LOG (error)\n";
return err;
}
if(send_size == 0)
delete[] send_data;
debug() << "[net::srv::cmd] <-- GET_LOG\n";
return error::SUCCESS;
}
/* HWSERVER_READ */
else if(cmd == HWSERVER_READ)
{
uint32_t hid = args[0];
uint32_t addr = args[1];
uint32_t len = args[2];
uint32_t flags = args[3];
debug() << "[net::srv::cmd] --> READ(" << hid << ",0x" << std::hex << addr << ","
<< len << ",0x" << std::hex << flags << ")\n";
/* check ID is valid */
auto it = state->handle_map.find(hid);
if(it == state->handle_map.end())
{
debug() << "[net::srv::cmd] unknown handle ID\n";
debug() << "[net::srv::cmd] <-- READ (error)\n";
return error::ERROR;
}
/* read */
send_size = len;
send_data = new uint8_t[send_size];
error err = it->second->read(addr, send_data, send_size, !!(flags & HWSERVER_RW_ATOMIC));
if(err != error::SUCCESS)
{
delete[] send_data;
debug() << "[net::srv::cmd] cannot read: " << error_string(err) << "\n";
debug() << "[net::srv::cmd] <-- READ (error)\n";
return err;
}
debug() << "[net::srv::cmd] <-- READ\n";
return error::SUCCESS;
}
/* HWSERVER_WRITE */
else if(cmd == HWSERVER_WRITE)
{
uint32_t hid = args[0];
uint32_t addr = args[1];
uint32_t flags = args[2];
debug() << "[net::srv::cmd] --> WRITE(" << hid << ",0x" << std::hex << addr << ","
<< recv_size << ",0x" << std::hex << flags << ")\n";
/* check ID is valid */
auto it = state->handle_map.find(hid);
if(it == state->handle_map.end())
{
debug() << "[net::srv::cmd] unknown handle ID\n";
debug() << "[net::srv::cmd] <-- WRITE (error)\n";
return error::ERROR;
}
/* write */
error err = it->second->write(addr, recv_data, recv_size, !!(flags & HWSERVER_RW_ATOMIC));
if(err != error::SUCCESS)
{
delete[] send_data;
debug() << "[net::srv::cmd] cannot write: " << error_string(err) << "\n";
debug() << "[net::srv::cmd] <-- WRITE (error)\n";
return err;
}
debug() << "[net::srv::cmd] <-- WRITE\n";
return error::SUCCESS;
}
else
{
debug() << "[net::srv::cmd] <-> unknown cmd (0x" << std::hex << cmd << ")\n";
return error::ERROR;
}
}
/*
* Socket server
*/
socket_server::socket_server(std::shared_ptr<hwstub::context> contex, int socket_fd)
:server(contex), m_socketfd(socket_fd)
{
m_discovery_exit = false;
set_timeout(std::chrono::milliseconds(1000));
m_discovery_thread = std::thread(&socket_server::discovery_thread1, this);
}
socket_server::~socket_server()
{
/* first stop discovery thread to make sure no more clients are created */
m_discovery_exit = true;
m_discovery_thread.join();
close(m_socketfd);
/* ask server to do a clean stop */
stop_server();
}
std::shared_ptr<server> socket_server::create(std::shared_ptr<hwstub::context> ctx,
int socket_fd)
{
// NOTE: can't use make_shared() because of the protected ctor */
return std::shared_ptr<socket_server>(new socket_server(ctx, socket_fd));
}
void socket_server::set_timeout(std::chrono::milliseconds ms)
{
m_timeout.tv_usec = 1000 * (ms.count() % 1000);
m_timeout.tv_sec = ms.count() / 1000;
}
int socket_server::from_srv_client(srv_client_t cli)
{
return (int)(intptr_t)cli;
}
socket_server::srv_client_t socket_server::to_srv_client(int fd)
{
return (srv_client_t)(intptr_t)fd;
}
void socket_server::discovery_thread1(socket_server *s)
{
s->discovery_thread();
}
void socket_server::terminate_client(srv_client_t client)
{
debug() << "[net::srv::sock] terminate client: " << client << "\n";
/* simply close connection */
close(from_srv_client(client));
}
error socket_server::send(srv_client_t client, void *buffer, size_t& sz)
{
debug() << "[net::ctx::sock] send(" << client << ", " << sz << "): ";
int ret = ::send(from_srv_client(client), buffer, sz, MSG_NOSIGNAL);
if(ret >= 0)
{
debug() << "good(" << ret << ")\n";
sz = (size_t)ret;
return error::SUCCESS;
}
/* convert some errors */
debug() << "fail(" << errno << "," << strerror(errno) << ")\n";
switch(errno)
{
#if EAGAIN != EWOULDBLOCK
case EAGAIN:
#endif
case EWOULDBLOCK: return error::TIMEOUT;
case ECONNRESET: case EPIPE: return error::SERVER_DISCONNECTED;
default: return error::NET_ERROR;
}
}
error socket_server::recv(srv_client_t client, void *buffer, size_t& sz)
{
debug() << "[net::ctx::sock] recv(" << client << ", " << sz << "): ";
int ret = ::recv(from_srv_client(client), buffer, sz, MSG_WAITALL);
if(ret > 0)
{
debug() << "good(" << ret << ")\n";
sz = (size_t)ret;
return error::SUCCESS;
}
if(ret == 0)
{
debug() << "disconnected\n";
return error::SERVER_DISCONNECTED;
}
debug() << "fail(" << errno << "," << strerror(errno) << ")\n";
switch(errno)
{
#if EAGAIN != EWOULDBLOCK
case EAGAIN:
#endif
case EWOULDBLOCK: return error::TIMEOUT;
default: return error::NET_ERROR;
}
}
void socket_server::discovery_thread()
{
debug() << "[net::srv:sock::discovery] start\n";
/* begin listening to incoming connections */
if(listen(m_socketfd, LISTEN_QUEUE_SIZE) < 0)
{
debug() << "[net::srv::sock::discovery] listen() failed: " << errno << "\n";
return;
}
/* handle connections */
while(!m_discovery_exit)
{
/* since accept() is blocking, use select to ensure a timeout */
struct timeval tmo = m_timeout; /* NOTE select() can overwrite timeout */
fd_set set;
FD_ZERO(&set);
FD_SET(m_socketfd, &set);
/* wait for some activity */
int ret = select(m_socketfd + 1, &set, nullptr, nullptr, &tmo);
if(ret < 0 || !FD_ISSET(m_socketfd, &set))
continue;
int clifd = accept(m_socketfd, nullptr, nullptr);
if(clifd >= 0)
{
debug() << "[net::srv::sock::discovery] new client\n";
/* set timeout for the client operations */
setsockopt(clifd, SOL_SOCKET, SO_RCVTIMEO, (char *)&m_timeout, sizeof(m_timeout));
setsockopt(clifd, SOL_SOCKET, SO_SNDTIMEO, (char *)&m_timeout, sizeof(m_timeout));
client_arrived(to_srv_client(clifd));
}
}
debug() << "[net::srv:sock::discovery] stop\n";
}
} // namespace uri
} // namespace net
Reference in a new issue View git blame Copy permalink