1 files changed, 462 insertions, 25 deletions

diff --git a/wmediumd/wmediumd.c b/wmediumd/wmediumd.c
index 6b3ae5d..0f4f050 100644
--- a/wmediumd/wmediumd.c
+++ b/wmediumd/wmediumd.c
@@ -31,12 +31,15 @@
 #include <getopt.h>
 #include <signal.h>
 #include <math.h>
+#include <sys/syslog.h>
 #include <sys/timerfd.h>
 #include <errno.h>
 #include <limits.h>
+#include <time.h>
 #include <unistd.h>
 #include <stdarg.h>
 #include <endian.h>
+#include <sys/msg.h>
 #include <usfstl/loop.h>
 #include <usfstl/sched.h>
 #include <usfstl/schedctrl.h>
@@ -47,6 +50,8 @@
 #include "ieee80211.h"
 #include "config.h"
 #include "api.h"
+#include "pmsr.h"
+#include "grpc.h"
 
 USFSTL_SCHEDULER(scheduler);
 
@@ -58,11 +63,59 @@ enum {
 	HWSIM_NUM_VQS,
 };
 
+static char *stpcpy_safe(char *dst, const char *src) {
+	if (dst == NULL) {
+		return NULL;
+	}
+
+	if (src == NULL) {
+		*dst = '\0';
+		return dst;
+	}
+
+	return stpcpy(dst, src);
+}
+
+static int strlen_safe(const char *src) {
+	return src == NULL ? 0 : strlen(src);
+}
+
 static inline int div_round(int a, int b)
 {
 	return (a + b - 1) / b;
 }
 
+static inline u64 sec_to_ns(time_t sec)
+{
+	return sec * 1000 * 1000 * 1000;
+}
+
+static inline u64 ns_to_us(u64 ns)
+{
+	return ns / 1000L;
+}
+
+static inline u64 ts_to_ns(struct timespec ts)
+{
+	return sec_to_ns(ts.tv_sec) + ts.tv_nsec;
+}
+
+static inline double distance_to_rtt(double distance)
+{
+	const long light_speed = 299792458L;
+	return distance / light_speed;
+}
+
+static inline double sec_to_ps(double sec)
+{
+	return sec * 1000 * 1000 * 1000;
+}
+
+static inline double meter_to_mm(double meter)
+{
+	return meter * 1000;
+}
+
 static inline int pkt_duration(int len, int rate)
 {
 	/* preamble + signal + t_sym * n_sym, rate in 100 kbps */
@@ -142,7 +195,6 @@ static inline bool frame_is_probe_req(struct frame *frame)
 		(FTYPE_MGMT | STYPE_PROBE_REQ);
 }
 
-
 static inline bool frame_has_zero_rates(const struct frame *frame)
 {
 	for (int i = 0; i < frame->tx_rates_count; i++) {
@@ -784,7 +836,7 @@ static void wmediumd_deliver_frame(struct usfstl_job *job)
 			if (memcmp(src, station->addr, ETH_ALEN) == 0)
 				continue;
 
-			if (is_multicast_ether_addr(dest)) {
+			if (is_multicast_ether_addr(dest) && station->client != NULL) {
 				int snr, rate_idx, signal;
 				double error_prob;
 
@@ -884,6 +936,232 @@ int nl_err_cb(struct sockaddr_nl *nla, struct nlmsgerr *nlerr, void *arg)
 	return NL_SKIP;
 }
 
+static int send_pmsr_result_ftm(struct nl_msg *msg,
+				struct pmsr_request_ftm *req,
+				struct wmediumd *ctx,
+				struct station *sender,
+				struct station *receiver)
+{
+	struct nlattr *ftm;
+	int err;
+
+	ftm = nla_nest_start(msg, NL80211_PMSR_TYPE_FTM);
+	if (!ftm)
+		return -ENOMEM;
+
+	if (!receiver) {
+		err = nla_put_u32(msg, NL80211_PMSR_FTM_RESP_ATTR_FAIL_REASON,
+				  NL80211_PMSR_FTM_FAILURE_NO_RESPONSE);
+		goto out;
+	}
+
+	double distance =
+		sqrt(pow(sender->x - receiver->x, 2) + pow(sender->y - receiver->y, 2));
+	double distance_in_mm = meter_to_mm(distance);
+	double rtt_in_ps = sec_to_ps(distance_to_rtt(distance));
+	if (distance_in_mm > UINT64_MAX || rtt_in_ps > UINT64_MAX) {
+		w_logf(ctx, LOG_WARNING,
+		       "%s: Devices are too far away", __func__);
+		return nla_put_u32(msg, NL80211_PMSR_FTM_RESP_ATTR_FAIL_REASON,
+				   NL80211_PMSR_FTM_FAILURE_NO_RESPONSE);
+	}
+
+	int rssi = receiver->tx_power -
+		   ctx->calc_path_loss(NULL, sender, receiver);
+
+	if (nla_put_u32(msg, NL80211_PMSR_FTM_RESP_ATTR_NUM_FTMR_ATTEMPTS,
+			req->ftmr_retries) ||
+		nla_put_u32(msg, NL80211_PMSR_FTM_RESP_ATTR_NUM_FTMR_SUCCESSES,
+			req->ftmr_retries) ||
+		nla_put_u8(msg, NL80211_PMSR_FTM_RESP_ATTR_NUM_BURSTS_EXP,
+			req->num_bursts_exp) ||
+		nla_put_u8(msg, NL80211_PMSR_FTM_RESP_ATTR_BURST_DURATION,
+			 req->burst_duration) ||
+		nla_put_u8(msg, NL80211_PMSR_FTM_RESP_ATTR_FTMS_PER_BURST,
+			 req->ftms_per_burst) ||
+		nla_put_s32(msg, NL80211_PMSR_FTM_RESP_ATTR_RSSI_AVG, rssi) ||
+		nla_put_s32(msg, NL80211_PMSR_FTM_RESP_ATTR_RSSI_SPREAD, 0) ||
+		nla_put_u64(msg, NL80211_PMSR_FTM_RESP_ATTR_RTT_AVG,
+			  (uint64_t)rtt_in_ps) ||
+		nla_put_u64(msg, NL80211_PMSR_FTM_RESP_ATTR_RTT_VARIANCE, 0) ||
+		nla_put_u64(msg, NL80211_PMSR_FTM_RESP_ATTR_RTT_SPREAD, 0) ||
+		nla_put_u64(msg, NL80211_PMSR_FTM_RESP_ATTR_DIST_AVG,
+			 (uint64_t)distance_in_mm) ||
+		nla_put_u64(msg, NL80211_PMSR_FTM_RESP_ATTR_DIST_VARIANCE, 0) ||
+		nla_put_u64(msg, NL80211_PMSR_FTM_RESP_ATTR_DIST_SPREAD, 0)) {
+		w_logf(ctx, LOG_ERR, "%s: Failed to fill a payload\n", __func__);
+		return -ENOMEM;
+	}
+
+	if (req->request_lci && receiver->lci) {
+		nla_put_string(msg, NL80211_PMSR_FTM_RESP_ATTR_LCI,
+			       receiver->lci);
+	}
+
+	if (req->request_civicloc && receiver->civicloc) {
+		nla_put_string(msg, NL80211_PMSR_FTM_RESP_ATTR_CIVICLOC,
+			       receiver->civicloc);
+	}
+
+out:
+	if (ftm)
+		nla_nest_end(msg, ftm);
+
+	return 0;
+}
+
+static int send_pmsr_result_peer(struct nl_msg *msg,
+				 struct pmsr_request_peer *req,
+				 struct wmediumd *ctx,
+				 struct station *sender)
+{
+	struct nlattr *peer, *resp, *data;
+	struct station *receiver;
+	int status;
+	struct timespec ts;
+	u64 host_time_ns;
+	u64 ap_tsf_us;
+	int err;
+
+	peer = nla_nest_start(msg, 1);
+	if (!peer)
+		return -ENOMEM;
+
+	receiver = get_station_by_addr(ctx, req->addr);
+	if (receiver)
+		status = NL80211_PMSR_STATUS_SUCCESS;
+	else {
+		w_logf(ctx, LOG_WARNING, "%s: unknown pmsr target " MAC_FMT "\n",
+		       __func__, MAC_ARGS(req->addr));
+		status = NL80211_PMSR_STATUS_FAILURE;
+	}
+
+	if (clock_gettime(CLOCK_BOOTTIME, &ts)) {
+		w_logf(ctx, LOG_ERR, "%s: clock_gettime() failed\n", __func__);
+		return -EINVAL;
+	}
+
+	host_time_ns = ts_to_ns(ts);
+	ap_tsf_us = ns_to_us(ts_to_ns(ts));
+
+	err = nla_put(msg, NL80211_PMSR_PEER_ATTR_ADDR, ETH_ALEN, req->addr);
+	if (err)
+		return err;
+
+	resp = nla_nest_start(msg, NL80211_PMSR_PEER_ATTR_RESP);
+	if (!resp)
+		return -ENOMEM;
+
+	if (nla_put_u32(msg, NL80211_PMSR_RESP_ATTR_STATUS, status) ||
+		nla_put_u64(msg, NL80211_PMSR_RESP_ATTR_HOST_TIME, host_time_ns) ||
+		nla_put_u64(msg, NL80211_PMSR_RESP_ATTR_AP_TSF, ap_tsf_us) ||
+		nla_put_flag(msg, NL80211_PMSR_RESP_ATTR_FINAL)) {
+		w_logf(ctx, LOG_ERR, "%s: Failed to fill a payload\n", __func__);
+		return -ENOMEM;
+	}
+
+	data = nla_nest_start(msg, NL80211_PMSR_RESP_ATTR_DATA);
+	if (!data)
+		return -ENOMEM;
+
+	err = send_pmsr_result_ftm(msg, &req->ftm, ctx, sender, receiver);
+
+	nla_nest_end(msg, data);
+	nla_nest_end(msg, resp);
+	nla_nest_end(msg, peer);
+
+	return err;
+}
+
+static void send_pmsr_result(struct pmsr_request* request, struct wmediumd *ctx,
+			     struct station *sender, struct client *client)
+{
+	struct nl_msg *msg;
+	struct nlattr *pmsr, *peers;
+	struct pmsr_request_peer *peer;
+	int cnt;
+	int err;
+
+	msg = nlmsg_alloc();
+	if (!msg) {
+		w_logf(ctx, LOG_ERR, "%s: nlmsg_alloc failed\n", __func__);
+		return;
+	}
+
+	if (!genlmsg_put(msg, NL_AUTO_PID, NL_AUTO_SEQ, ctx->family_id,
+			 0, NLM_F_REQUEST, HWSIM_CMD_REPORT_PMSR, VERSION_NR)) {
+		w_logf(ctx, LOG_ERR, "%s: genlmsg_put failed\n", __func__);
+		goto out;
+	}
+
+	err = nla_put(msg, HWSIM_ATTR_ADDR_TRANSMITTER,
+		      ETH_ALEN, sender->hwaddr);
+
+	pmsr = nla_nest_start(msg, HWSIM_ATTR_PMSR_RESULT);
+	if (!pmsr)
+		goto out;
+
+	peers = nla_nest_start(msg, NL80211_PMSR_ATTR_PEERS);
+	if (!peers)
+		goto out;
+
+	list_for_each_entry(peer, &request->peers, list) {
+		err = send_pmsr_result_peer(msg, peer, ctx, sender);
+		if (err) {
+			w_logf(ctx, LOG_ERR,
+			       "%s: Failed to send pmsr result from " MAC_FMT \
+			       " to " MAC_FMT ". Stopping\n", __func__,
+			       MAC_ARGS(sender->addr),
+			       MAC_ARGS(peer->addr));
+			break;
+		}
+	}
+
+	nla_nest_end(msg, peers);
+	nla_nest_end(msg, pmsr);
+
+	wmediumd_send_to_client(ctx, client, msg);
+
+out:
+	nlmsg_free(msg);
+}
+
+static void process_start_pmsr(struct nlattr *attrs[], struct wmediumd *ctx,
+			       struct client *client)
+{
+	u8 *hwaddr;
+	struct station *sender;
+
+	struct pmsr_request request;
+	struct pmsr_request_peer *peer, *tmp;
+	int err;
+
+	if (!attrs[HWSIM_ATTR_ADDR_TRANSMITTER]) {
+		w_logf(ctx, LOG_ERR, "%s: Missing sender information\n",
+		       __func__);
+	}
+
+	hwaddr = (u8 *)nla_data(attrs[HWSIM_ATTR_ADDR_TRANSMITTER]);
+	sender = get_station_by_addr(ctx, hwaddr);
+	if (!sender) {
+		w_logf(ctx, LOG_ERR, "%s: Unknown sender " MAC_FMT "\n",
+		       __func__, MAC_ARGS(hwaddr));
+		return;
+	}
+
+	err = parse_pmsr_request(attrs[HWSIM_ATTR_PMSR_REQUEST], &request);
+	if (err)
+		goto out;
+
+	send_pmsr_result(&request, ctx, sender, client);
+
+out:
+	list_for_each_entry_safe(peer, tmp, &request.peers, list) {
+		list_del(&peer->list);
+		free(peer);
+	}
+}
+
 /*
  * Handle events from the kernel.  Process CMD_FRAME events and queue them
  * for later delivery with the scheduler.
@@ -902,7 +1180,7 @@ static void _process_messages(struct nl_msg *msg,
 	struct frame *frame;
 	struct ieee80211_hdr *hdr;
 	u8 *src, *hwaddr, *addr;
-	void *new;
+	void *new_addrs;
 	unsigned int i;
 
 	genlmsg_parse(nlh, 0, attrs, HWSIM_ATTR_MAX, NULL);
@@ -976,13 +1254,16 @@ static void _process_messages(struct nl_msg *msg,
 		if (!sender)
 			break;
 		for (i = 0; i < sender->n_addrs; i++) {
-			if (memcmp(sender->addrs[i].addr, addr, ETH_ALEN) == 0)
+			if (memcmp(sender->addrs[i].addr, addr, ETH_ALEN) == 0) {
+				sender->addrs[i].count += 1;
 				return;
+			}
 		}
-		new = realloc(sender->addrs, ETH_ALEN * (sender->n_addrs + 1));
-		if (!new)
+		new_addrs = realloc(sender->addrs, sizeof(struct addr) * (sender->n_addrs + 1));
+		if (!new_addrs)
 			break;
-		sender->addrs = new;
+		sender->addrs = new_addrs;
+		sender->addrs[sender->n_addrs].count = 1;
 		memcpy(sender->addrs[sender->n_addrs].addr, addr, ETH_ALEN);
 		sender->n_addrs += 1;
 		break;
@@ -998,13 +1279,23 @@ static void _process_messages(struct nl_msg *msg,
 		for (i = 0; i < sender->n_addrs; i++) {
 			if (memcmp(sender->addrs[i].addr, addr, ETH_ALEN))
 				continue;
-			sender->n_addrs -= 1;
-			memmove(sender->addrs[i].addr,
-				sender->addrs[sender->n_addrs].addr,
-				ETH_ALEN);
+			sender->addrs[i].count -= 1;
+			if (sender->addrs[i].count <= 0) {
+				sender->n_addrs -= 1;
+				memmove(&sender->addrs[i],
+					&sender->addrs[sender->n_addrs],
+					sizeof(struct addr));
+			}
 			break;
 		}
 		break;
+	case HWSIM_CMD_START_PMSR:
+		process_start_pmsr(attrs, ctx, client);
+		break;
+
+	case HWSIM_CMD_ABORT_PMSR:
+		// Do nothing. Too late to abort any PMSR.
+		break;
 	}
 }
 
@@ -1109,18 +1400,33 @@ static int process_reload_current_config_message(struct wmediumd *ctx) {
 static int process_get_stations_message(struct wmediumd *ctx, ssize_t *response_len, unsigned char **response_data) {
 	struct station *station;
 	int station_count = 0;
+	int extra_data_len = 0;
 
+	// *reponse_data contains struct wmediumd_station_infos
+	// and then lci and civiclocs for each station follows afterwards.
 	list_for_each_entry(station, &ctx->stations, list) {
 		if (station->client != NULL) {
 			++station_count;
+			extra_data_len += strlen_safe(station->lci) + 1;
+			extra_data_len += strlen_safe(station->civicloc) + 1;
 		}
 	}
 
-	*response_len = sizeof(uint32_t) + sizeof(struct wmediumd_station_info) * station_count;
-	struct wmediumd_station_infos *station_infos = malloc(*response_len);
+	int station_len = sizeof(uint32_t) + sizeof(struct wmediumd_station_info) * station_count;
+	*response_len = station_len + extra_data_len;
+	*response_data = malloc(*response_len);
 
+	if (*response_data == NULL) {
+		w_logf(ctx, LOG_ERR, "%s: failed allocate response data\n", __func__);
+		return -1;
+	}
+
+	struct wmediumd_station_infos *station_infos = (struct wmediumd_station_infos *)*response_data;
 	station_infos->count = station_count;
 	int station_index = 0;
+	// Pointer to the memory after structs wmediumd_station_infos
+	// to write lci and civicloc for each station.
+	char *extra_data_cursor = (char *)&(station_infos->stations[station_count]);
 
 	list_for_each_entry(station, &ctx->stations, list) {
 		if (station->client != NULL) {
@@ -1130,18 +1436,71 @@ static int process_get_stations_message(struct wmediumd *ctx, ssize_t *response_
 
 			station_info->x = station->x;
 			station_info->y = station->y;
-
+			station_info->lci_offset = extra_data_cursor - (char *)station_info;
+			extra_data_cursor = stpcpy_safe(extra_data_cursor, station->lci) + 1;
+			station_info->civicloc_offset = extra_data_cursor - (char *)station_info;
+			extra_data_cursor = stpcpy_safe(extra_data_cursor, station->civicloc) + 1;
 			station_info->tx_power = station->tx_power;
-
 			station_index++;
 		}
 	}
 
-	*response_data = (unsigned char *)station_infos;
+	return 0;
+}
+
+static int process_set_position_message(struct wmediumd *ctx, struct wmediumd_set_position *set_position) {
+	struct station *node = get_station_by_addr(ctx, set_position->mac);
+
+	if (node == NULL) {
+		return -1;
+	}
+
+	node->x = set_position->x;
+	node->y = set_position->y;
+
+	calc_path_loss(ctx);
 
 	return 0;
 }
 
+static int process_set_lci_message(struct wmediumd *ctx, struct wmediumd_set_lci *set_lci, size_t data_len) {
+	struct station *node = get_station_by_addr(ctx, set_lci->mac);
+
+	if (node == NULL) {
+		return -1;
+	}
+	int expected_len = data_len - offsetof(struct wmediumd_set_lci, lci) - 1;
+	if (set_lci->lci[expected_len] != '\0') {
+		return -1;
+	}
+
+	if (node->lci) {
+		free(node->lci);
+	}
+	node->lci = strdup(set_lci->lci);
+
+	return node->lci != NULL;
+}
+
+static int process_set_civicloc_message(struct wmediumd *ctx, struct wmediumd_set_civicloc *set_civicloc, size_t data_len) {
+	struct station *node = get_station_by_addr(ctx, set_civicloc->mac);
+
+	if (node == NULL) {
+		return -1;
+	}
+	int expected_len = data_len - offsetof(struct wmediumd_set_civicloc, civicloc) - 1;
+	if (set_civicloc->civicloc[expected_len] != '\0') {
+		return -1;
+	}
+
+	if (node->civicloc) {
+		free(node->civicloc);
+	}
+	node->civicloc = strdup(set_civicloc->civicloc);
+
+	return node->civicloc != NULL;
+}
+
 static const struct usfstl_vhost_user_ops wmediumd_vu_ops = {
 	.connected = wmediumd_vu_connected,
 	.handle = wmediumd_vu_handle,
@@ -1161,6 +1520,43 @@ static void close_pcapng(struct wmediumd *ctx) {
 
 static void init_pcapng(struct wmediumd *ctx, const char *filename);
 
+static void wmediumd_grpc_service_handler(struct usfstl_loop_entry *entry) {
+	struct wmediumd *ctx = entry->data;
+
+	// Receive request type from WmediumdService
+	uint64_t request_type;
+	read(entry->fd, &request_type, sizeof(uint64_t));
+
+	struct wmediumd_grpc_message request_body;
+	uint64_t response_type;
+
+	// Receive request body from WmediumdService and do the task.
+	// TODO(273384914): Support more request types.
+	switch (request_type) {
+	case REQUEST_SET_POSITION:
+		if (msgrcv(ctx->msq_id, &request_body, sizeof(struct wmediumd_set_position), GRPC_REQUEST, 0) != sizeof(struct wmediumd_set_position)) {
+			w_logf(ctx, LOG_ERR, "%s: failed to get set_position request body\n", __func__);
+		}
+
+		if (process_set_position_message(ctx, (struct wmediumd_set_position *)(request_body.data)) < 0) {
+			w_logf(ctx, LOG_ERR, "%s: failed to execute set_position\n", __func__);
+			response_type = RESPONSE_INVALID;
+		}
+		response_type = RESPONSE_ACK;
+		break;
+	default:
+		w_logf(ctx, LOG_ERR, "%s: unknown request type\n", __func__);
+		response_type = RESPONSE_INVALID;
+		break;
+	}
+
+	// TODO(273384914): Send response with response_type
+	return;
+}
+
+// TODO(273384914): Deprecate messages used in wmediumd_control after
+// implementing in wmediumd_grpc_service_handler to be used in the command
+// 'cvd env'.
 static void wmediumd_api_handler(struct usfstl_loop_entry *entry)
 {
 	struct client *client = container_of(entry, struct client, loop);
@@ -1175,20 +1571,31 @@ static void wmediumd_api_handler(struct usfstl_loop_entry *entry)
 	ssize_t len;
 
 	len = read(entry->fd, &hdr, sizeof(hdr));
-	if (len != sizeof(hdr))
+	if (len != sizeof(hdr)) {
+		if (len > 0) {
+			// Skipping log if the fd is closed.
+			w_logf(ctx, LOG_ERR, "%s: failed to read header\n", __func__);
+		}
 		goto disconnect;
+	}
 
 	/* safety valve */
-	if (hdr.data_len > 1024 * 1024)
+	if (hdr.data_len > 1024 * 1024) {
+		w_logf(ctx, LOG_ERR, "%s: too large data\n", __func__);
 		goto disconnect;
+	}
 
 	data = malloc(hdr.data_len);
-	if (!data)
+	if (!data) {
+		w_logf(ctx, LOG_ERR, "%s: failed to malloc\n", __func__);
 		goto disconnect;
+	}
 
 	len = read(entry->fd, data, hdr.data_len);
-	if (len != hdr.data_len)
+	if (len != hdr.data_len) {
+		w_logf(ctx, LOG_ERR, "%s: failed to read data\n", __func__);
 		goto disconnect;
+	}
 
 	switch (hdr.type) {
 	case WMEDIUMD_MSG_REGISTER:
@@ -1243,18 +1650,18 @@ static void wmediumd_api_handler(struct usfstl_loop_entry *entry)
 	case WMEDIUMD_MSG_SET_SNR:
 		if (process_set_snr_message(ctx, (struct wmediumd_set_snr *)data) < 0) {
 			response = WMEDIUMD_MSG_INVALID;
-                }
+		}
 		break;
 	case WMEDIUMD_MSG_RELOAD_CONFIG:
 		if (process_reload_config_message(ctx,
 				(struct wmediumd_reload_config *)data) < 0) {
 			response = WMEDIUMD_MSG_INVALID;
-                }
+		}
 		break;
 	case WMEDIUMD_MSG_RELOAD_CURRENT_CONFIG:
 		if (process_reload_current_config_message(ctx) < 0) {
 			response = WMEDIUMD_MSG_INVALID;
-                }
+		}
 		break;
 	case WMEDIUMD_MSG_START_PCAP:
 		init_pcapng(ctx, ((struct wmediumd_start_pcap *)data)->pcap_path);
@@ -1262,6 +1669,25 @@ static void wmediumd_api_handler(struct usfstl_loop_entry *entry)
 	case WMEDIUMD_MSG_STOP_PCAP:
 		close_pcapng(ctx);
 		break;
+	case WMEDIUMD_MSG_SET_POSITION:
+		if (process_set_position_message(ctx, (struct wmediumd_set_position *)data) < 0) {
+			response = WMEDIUMD_MSG_INVALID;
+		}
+		break;
+	case WMEDIUMD_MSG_SET_LCI:
+		if (process_set_lci_message(ctx,
+				(struct wmediumd_set_lci *)data,
+				hdr.data_len) < 0) {
+			response = WMEDIUMD_MSG_INVALID;
+		}
+		break;
+	case WMEDIUMD_MSG_SET_CIVICLOC:
+		if (process_set_civicloc_message(ctx,
+				(struct wmediumd_set_civicloc *)data,
+				hdr.data_len) < 0) {
+			response = WMEDIUMD_MSG_INVALID;
+		}
+		break;
 	case WMEDIUMD_MSG_ACK:
 		assert(client->wait_for_ack == true);
 		assert(hdr.data_len == 0);
@@ -1269,6 +1695,7 @@ static void wmediumd_api_handler(struct usfstl_loop_entry *entry)
 		/* don't send a response to a response, of course */
 		return;
 	default:
+		w_logf(ctx, LOG_ERR, "%s: unknown message\n", __func__);
 		response = WMEDIUMD_MSG_INVALID;
 		break;
 	}
@@ -1277,8 +1704,10 @@ static void wmediumd_api_handler(struct usfstl_loop_entry *entry)
 	hdr.type = response;
 	hdr.data_len = response_len;
 	len = write(entry->fd, &hdr, sizeof(hdr));
-	if (len != sizeof(hdr))
+	if (len != sizeof(hdr)) {
+		w_logf(ctx, LOG_ERR, "%s: failed to write response header\n", __func__);
 		goto disconnect;
+	}
 
 	if (response_data != NULL) {
 		if (response_len != 0) {
@@ -1286,6 +1715,7 @@ static void wmediumd_api_handler(struct usfstl_loop_entry *entry)
 
 			if (len != response_len) {
 				free(response_data);
+				w_logf(ctx, LOG_ERR, "%s: failed to write response data\n", __func__);
 				goto disconnect;
 			}
 		}
@@ -1480,7 +1910,7 @@ static void init_pcapng(struct wmediumd *ctx, const char *filename)
 #define VIRTIO_F_VERSION_1 32
 #endif
 
-int main(int argc, char *argv[])
+int wmediumd_main(int argc, char *argv[], int event_fd, int msq_id)
 {
 	int opt;
 	struct wmediumd ctx = {};
@@ -1631,6 +2061,13 @@ int main(int argc, char *argv[])
 		usfstl_sched_wallclock_init(&scheduler, 1000);
 	}
 
+	// Control event_fd to communicate WmediumdService.
+	ctx.grpc_loop.handler = wmediumd_grpc_service_handler;
+	ctx.grpc_loop.data = &ctx;
+	ctx.grpc_loop.fd = event_fd;
+	usfstl_loop_register(&ctx.grpc_loop);
+	ctx.msq_id = msq_id;
+
 	while (1) {
 		if (time_socket) {
 			usfstl_sched_next(&scheduler);