rosetta/toolchain/mxdbg/client/context.c

#include "context.h"

#include "packet.h"

#include <arpa/inet.h>
#include <magenta/debug/packet.h>
#include <netinet/in.h>
#include <signal.h>
#include <stdio.h>
#include <stdlib.h>
#include <sys/socket.h>
#include <unistd.h>

void debug_context_init(struct debug_context *ctx)
{
	memset(ctx, 0x0, sizeof *ctx);
	ctx->ctx_sock = -1;
	pthread_mutex_init(&ctx->ctx_packet_lock, NULL);
	pthread_cond_init(&ctx->ctx_packet_cond, NULL);
}

void debug_context_cleanup(struct debug_context *ctx)
{
	ctx->ctx_quit = true;

	if (ctx->ctx_sock != -1) {
		close(ctx->ctx_sock);
	}

	pthread_cond_broadcast(&ctx->ctx_packet_cond);
	pthread_kill(ctx->ctx_packet_thread, SIGINT);
	pthread_kill(ctx->ctx_event_thread, SIGINT);

	printf("joining threads...\n");
	pthread_join(ctx->ctx_packet_thread, NULL);
	pthread_join(ctx->ctx_event_thread, NULL);
	printf("threads joined\n");
}

static void *packet_thread(void *arg)
{
	struct debug_context *ctx = arg;
	while (!ctx->ctx_quit) {
		struct mxdbg_packet hdr;
		int err = 0;
		size_t r;

		err = debug_context_recv(ctx, &hdr, sizeof hdr, &r);
		if (err != 0) {
			printf("packet recv failed\n");
			continue;
		}

		if (r != sizeof hdr) {
			printf("packet size is wrong\n");
			continue;
		}

		if (!mxdbg_packet_validate(&hdr)) {
			printf("packet is malformed\n");
			continue;
		}

		struct packet *packet = packet_create(hdr.pkt_length);
		if (!packet) {
			printf("packet alloc failed\n");
			continue;
		}

		memcpy(&packet->p_packet, &hdr, sizeof hdr);
		err = debug_context_recv_packet_data(
			ctx,
			&packet->p_packet,
			&r);
		if (err != 0) {
			printf("packet data read failed\n");
			free(packet);
			continue;
		}

		pthread_mutex_lock(&ctx->ctx_packet_lock);
		fx_queue_push_back(&ctx->ctx_packets, &packet->p_entry);
		pthread_cond_broadcast(&ctx->ctx_packet_cond);
		pthread_mutex_unlock(&ctx->ctx_packet_lock);
	}

	return NULL;
}

static void *event_thread(void *arg)
{
	struct debug_context *ctx = arg;
	while (!ctx->ctx_quit) {
		struct packet *packet
			= debug_context_await_packet(ctx, MXDBG_PACKET_EVENT);

		ctx->ctx_on_event(ctx, packet);
		packet_destroy(packet);
	}

	return NULL;
}

void debug_context_start_threads(struct debug_context *ctx)
{
	pthread_create(&ctx->ctx_packet_thread, NULL, packet_thread, ctx);
	pthread_create(&ctx->ctx_event_thread, NULL, event_thread, ctx);
}

int debug_context_connect_net(
	struct debug_context *ctx,
	const char *address,
	int port)
{
	ctx->ctx_sock = socket(AF_INET, SOCK_STREAM, 0);
	if (ctx->ctx_sock < 0) {
		return -1;
	}

	struct sockaddr_in dest = {0};
	dest.sin_family = AF_INET;
	dest.sin_port = htons(port);
	inet_pton(AF_INET, address, &dest.sin_addr);

	return connect(ctx->ctx_sock, (struct sockaddr *)&dest, sizeof dest);
}

int debug_context_recv(
	struct debug_context *ctx,
	void *p,
	size_t len,
	size_t *nr_read)
{
	size_t total_read = 0;
	unsigned char *buf = p;

	while (total_read < len) {
		long r = read(ctx->ctx_sock, p + total_read, len - total_read);
		if (r < 0) {
			return -1;
		}

		total_read += r;
	}

	*nr_read = total_read;
	return 0;
}

int debug_context_send_packet(
	struct debug_context *ctx,
	const struct mxdbg_packet *packet)
{
	pthread_mutex_lock(&ctx->ctx_sock_lock);
	int err = 0;
	size_t len = packet->pkt_length;
	size_t total_written = 0;
	const unsigned char *buf = (const unsigned char *)packet;

	while (total_written < len) {
		long w
			= write(ctx->ctx_sock,
		                buf + total_written,
		                len - total_written);
		if (w < 0) {
			err = -1;
			break;
		}

		total_written += w;
	}

	pthread_mutex_unlock(&ctx->ctx_sock_lock);
	return err;
}

int debug_context_recv_packet_data(
	struct debug_context *ctx,
	struct mxdbg_packet *packet,
	size_t *nr_read)
{
	if (packet->pkt_magic != MXDBG_PACKET_MAGIC) {
		return -1;
	}

	if (!packet->pkt_length) {
		*nr_read = 0;
		return 0;
	}

	void *dest = packet + 1;
	return debug_context_recv(
		ctx,
		dest,
		packet->pkt_length - sizeof *packet,
		nr_read);
}

static struct packet *dequeue_packet(
	struct debug_context *ctx,
	enum mxdbg_packet_type packet_type)
{
	fx_queue_entry *cur = fx_queue_first(&ctx->ctx_packets);
	while (cur) {
		struct packet *packet = fx_unbox(struct packet, cur, p_entry);

		if (packet->p_packet.pkt_type == packet_type) {
			fx_queue_delete(&ctx->ctx_packets, cur);
			return packet;
		}

		cur = fx_queue_next(cur);
	}

	return NULL;
}

struct packet *debug_context_await_packet(
	struct debug_context *ctx,
	enum mxdbg_packet_type packet_type)
{
	pthread_mutex_lock(&ctx->ctx_packet_lock);
	struct packet *result = NULL;

	while (1) {
		result = dequeue_packet(ctx, packet_type);
		if (result) {
			break;
		}

		pthread_cond_wait(&ctx->ctx_packet_cond, &ctx->ctx_packet_lock);
	}

	pthread_mutex_unlock(&ctx->ctx_packet_lock);
	return result;
}