finalized microphone implementation

Author: itsmattkc

lib/CMakeLists.txt

@@ -32,6 +32,7 @@ if (VANILLA_BUILD_TESTS)
         target_include_directories(${TEST_NAME} PRIVATE ${CMAKE_CURRENT_SOURCE_DIR})
     endfunction()
 
+    add_test(audioheader "test/audioheader.c")
     add_test(bittest "test/bittest.c")
     add_test(reversebittest "test/reversebit.c")
     add_test(reversebitstresstest "test/reversebitstresstest.c")

lib/gamepad/audio.c

@@ -8,123 +8,122 @@
 
 #include <pthread.h>
 #include <stdint.h>
+#include <stdlib.h>
 #include <string.h>
 #include <sys/time.h>
+#include <unistd.h>
 
 #include "gamepad.h"
 #include "vanilla.h"
 #include "util.h"
 
-#pragma pack(push, 1)
-typedef struct {
-    unsigned format : 3;
-    unsigned mono : 1;
-    unsigned vibrate : 1;
-    unsigned type : 1;
-    unsigned seq_id : 10;
-    unsigned payload_size : 16;
-    unsigned timestamp : 32;
-    unsigned char payload[2048];
-} AudioPacket;
-const static unsigned int TYPE_AUDIO = 0;
-const static unsigned int TYPE_VIDEO = 1;
-#pragma pack(pop)
-
-typedef struct {
-    uint32_t timestamp;
-    uint32_t unknown_freq_0[2];
-    uint32_t unknown_freq_1[2];
-    uint32_t video_format;
-} AudioPacketVideoFormat;
-
-static unsigned char queued_audio[2048];
+static unsigned char queued_audio[8192];
 static size_t queued_audio_start = 0;
 static size_t queued_audio_end = 0;
 static pthread_mutex_t queued_audio_mutex;
+static pthread_cond_t queued_audio_cond;
 
 int send_audio_packet(const void *data, size_t len)
 {
     pthread_mutex_lock(&queued_audio_mutex);
 
-    for (size_t i = 0; i < len; ) {
+	for (size_t i = 0; i < len; ) {
         size_t phys = queued_audio_end % sizeof(queued_audio);
         size_t max_write = MIN(sizeof(queued_audio) - phys, len - i);
         memcpy(queued_audio + phys, ((const unsigned char *) data) + i, max_write);
+
         i += max_write;
         queued_audio_end += max_write;
+
+		// Skip start ahead if necessary
+		if (queued_audio_end > queued_audio_start + sizeof(queued_audio)) {
+			queued_audio_start = queued_audio_end - sizeof(queued_audio);
+		}
     }
 
+	pthread_cond_broadcast(&queued_audio_cond);
+
     pthread_mutex_unlock(&queued_audio_mutex);
 
     return VANILLA_SUCCESS;
 }
 
-static void handle_queued_audio(gamepad_context_t *ctx, int seq_id)
+static void *handle_queued_audio(void *data)
 {
-    AudioPacket ap;
+    // Mic only ever sends 512 bytes at a time
+	const size_t MIC_PAYLOAD_SIZE = 512;
 
-    // Initialize payload size to zero
-    const size_t MIC_PAYLOAD_SIZE = 512;
-    ap.payload_size = 0;
+	gamepad_context_t *ctx = (gamepad_context_t *) data;
 
-    // Lock mutex
     pthread_mutex_lock(&queued_audio_mutex);
 
-    // If available, copy over to our packet
-    if (queued_audio_end >= (queued_audio_start + MIC_PAYLOAD_SIZE)) {
-        for (size_t i = 0; i < MIC_PAYLOAD_SIZE; ) {
-            size_t phys = queued_audio_start % sizeof(queued_audio);
-            size_t max_write = MIN(sizeof(queued_audio) - phys, MIC_PAYLOAD_SIZE - i);
-            memcpy(ap.payload + i, queued_audio + queued_audio_start, max_write);
-            i += max_write;
-            queued_audio_start += max_write;
-        }
-        ap.payload_size = MIC_PAYLOAD_SIZE;
-    }
+	while (!is_interrupted()) {
+		while (!is_interrupted() && queued_audio_end < (queued_audio_start + MIC_PAYLOAD_SIZE)) {
+			// Wait for more data
+			pthread_cond_wait(&queued_audio_cond, &queued_audio_mutex);
+		}
+
+		if (is_interrupted()) {
+			break;
+		}
+
+		AudioPacket ap;
+
+		// Copy data into our packet
+		for (size_t i = 0; i < MIC_PAYLOAD_SIZE; ) {
+			size_t phys = queued_audio_start % sizeof(queued_audio);
+			size_t max_write = MIN(sizeof(queued_audio) - phys, MIC_PAYLOAD_SIZE - i);
+			memcpy(ap.payload + i, queued_audio + phys, max_write);
+
+			i += max_write;
+			queued_audio_start += max_write;
+		}
+
+		// Don't need access to the buffer while we set up the packet
+		pthread_mutex_unlock(&queued_audio_mutex);
+
+		// Set up remaining default parameters
+		ap.format = 6;
+		ap.mono = 1;
+		ap.vibrate = 0;
+		ap.type = TYPE_AUDIO; // Audio data
+		ap.timestamp = 0; // Gamepad actually sends no timestamp
+		ap.payload_size = MIC_PAYLOAD_SIZE;
+
+		static unsigned int seq_id = 0;
+		ap.seq_id = seq_id++;
+
+		// Reverse bits on params
+		ap.format = reverse_bits(ap.format, 3);
+		ap.seq_id = reverse_bits(ap.seq_id, 10);
+		ap.payload_size = reverse_bits(ap.payload_size, 16);//ntohs(ap.payload_size);
+		// ap.timestamp = reverse_bits(ap.timestamp, 32); // Not necessary because timestamp is 0
+
+		// Further reverse bits
+		unsigned char *bytes = (unsigned char *) &ap;
+		const size_t header_sz = sizeof(AudioPacket) - sizeof(ap.payload);
+		for (int i = 0; i < 4; i++) { // 4 instead of 8 because ap.timestamp == 0
+			bytes[i] = (unsigned char) reverse_bits(bytes[i], 8);
+		}
+
+		// Console expects 512 bytes every 16 ms so make sure we achieve that interval
+		static struct timeval last;
+		struct timeval now;
+		gettimeofday(&now, 0);
+		long diff = (now.tv_sec - last.tv_sec) * 1000000 + (now.tv_usec - last.tv_usec);
+		static const long target_delta = 16000;
+		if (diff < target_delta) {
+			usleep(target_delta - diff);
+		}
+		gettimeofday(&last, 0);
+
+		// Send packet to console
+		send_to_console(ctx->socket_aud, &ap, header_sz + MIC_PAYLOAD_SIZE, PORT_AUD);
+
+    	pthread_mutex_lock(&queued_audio_mutex);
+	}
 
-    // Unlock
     pthread_mutex_unlock(&queued_audio_mutex);
-
-    // If we didn't get audio, return here
-    if (!ap.payload_size) {
-        return;
-    }
-
-    // struct timeval tv;
-    // gettimeofday(&tv, 0);
-    // if ((tv.tv_sec % 2) == 1) {
-        // for (int i = 0; i < ap.payload_size; i++) {
-        //     ap.payload[i] = (rand() % 0xFF);
-        // }
-        // vanilla_log("NOISE!");
-    // } else {
-    //     memset(ap.payload, 0x00, ap.payload_size);
-    //     vanilla_log("...quiet...");
-    // }
-
-    // Set up remaining default parameters
-    ap.format = 6;
-    ap.mono = 1;
-    ap.vibrate = 0;
-    ap.type = TYPE_AUDIO; // Audio data
-    ap.seq_id = seq_id + 1;
-    ap.timestamp = 0;
-
-    // Reverse bits on params
-    ap.format = reverse_bits(ap.format, 3);
-    ap.seq_id = reverse_bits(ap.seq_id, 10);
-    ap.payload_size = reverse_bits(ap.payload_size, 16);//ntohs(ap.payload_size);
-    // ap.timestamp = reverse_bits(ap.timestamp, 32); // Not necessary because timestamp is 0
-
-    // Further reverse bits
-    unsigned char *bytes = (unsigned char *) &ap;
-    const size_t header_sz = sizeof(AudioPacket) - sizeof(ap.payload);
-    for (int i = 0; i < header_sz; i++) {
-        bytes[i] = (unsigned char) reverse_bits(bytes[i], 8);
-    }
-
-    // Send packet to console
-    send_to_console(ctx->socket_aud, &ap, header_sz + ap.payload_size, PORT_AUD);
 }
 
 void handle_audio_packet(gamepad_context_t *ctx, unsigned char *data, size_t len)
@@ -162,7 +161,7 @@ void handle_audio_packet(gamepad_context_t *ctx, unsigned char *data, size_t len
     uint8_t vibrate_val = ap->vibrate;
     push_event(ctx->event_loop, VANILLA_EVENT_VIBRATE, &vibrate_val, sizeof(vibrate_val));
 
-    handle_queued_audio(ctx, ap->seq_id);
+    handle_queued_audio(ctx);
 }
 
 void *listen_audio(void *x)
@@ -175,6 +174,14 @@ void *listen_audio(void *x)
     queued_audio_end = 0;
 
     pthread_mutex_init(&queued_audio_mutex, 0);
+	pthread_cond_init(&queued_audio_cond, 0);
+
+	pthread_t mic_thread;
+	int mic_thread_created = 1;
+	if (pthread_create(&mic_thread, 0, handle_queued_audio, info) != 0) {
+		vanilla_log("Failed to create mic thread");
+		mic_thread_created = 0;
+	}
 
     do {
         size = recv(info->socket_aud, data, sizeof(data), 0);
@@ -183,7 +190,13 @@ void *listen_audio(void *x)
         }
     } while (!is_interrupted());
 
-    pthread_mutex_destroy(&queued_audio_mutex);
+	if (mic_thread_created) {
+		pthread_cond_broadcast(&queued_audio_cond);
+		pthread_join(mic_thread, 0);
+	}
+
+    pthread_cond_destroy(&queued_audio_cond);
+	pthread_mutex_destroy(&queued_audio_mutex);
 
     pthread_exit(NULL);
 

lib/gamepad/audio.h

@@ -2,8 +2,31 @@
 #define GAMEPAD_AUDIO_H
 
 #include <stddef.h>
+#include <stdint.h>
+
+#pragma pack(push, 1)
+typedef struct {
+    unsigned format : 3;
+    unsigned mono : 1;
+    unsigned vibrate : 1;
+    unsigned type : 1;
+    unsigned seq_id : 10;
+    unsigned payload_size : 16;
+    unsigned timestamp : 32;
+    unsigned char payload[2048];
+} AudioPacket;
+const static unsigned int TYPE_AUDIO = 0;
+const static unsigned int TYPE_VIDEO = 1;
+#pragma pack(pop)
+
+typedef struct {
+    uint32_t timestamp;
+    uint32_t unknown_freq_0[2];
+    uint32_t unknown_freq_1[2];
+    uint32_t video_format;
+} AudioPacketVideoFormat;
 
 void *listen_audio(void *x);
 int send_audio_packet(const void *data, size_t len);
 
-#endif // GAMEPAD_AUDIO_H
+#endif // GAMEPAD_AUDIO_H

lib/gamepad/gamepad.c

@@ -105,7 +105,10 @@ void send_to_console(int fd, const void *data, size_t data_size, uint16_t port)
 
     ssize_t sent = sendto(fd, data, data_size, 0, (const struct sockaddr *) &addr, addr_size);
     if (sent == -1) {
-        vanilla_log("Failed to send to Wii U socket: fd: %d, port: %d, errno: %i", fd, console_port, skterr());
+		int err = skterr();
+		if (err != 111) { // 111 is connection refused, occurs if we lose connection, but we'll already know that for other reasons so we don't need to spam the console with this error
+			vanilla_log("Failed to send to Wii U socket: fd: %d, port: %d, errno: %i", fd, console_port, skterr());
+		}
     }
 }
 

lib/test/audioheader.c

@@ -0,0 +1,49 @@
+#include <stdint.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include "gamepad/audio.h"
+#include "util.h"
+
+int main(int argc, const char **argv)
+{
+	if (argc != 2) {
+		printf("Usage: %s <hex-string>\n", argv[0]);
+		return 1;
+	}
+
+	const char *s = argv[1];
+	uint8_t bytes[8];
+	if (strlen(s) != sizeof(bytes) * 2) {
+		printf("ERROR: Invalid hex data\n");
+		return 1;
+	}
+
+	char tmp_hex[3];
+	tmp_hex[2] = 0;
+	for (size_t i = 0; i < sizeof(bytes); i++) {
+		memcpy(tmp_hex, s + i * 2, 2);
+		bytes[i] = strtol(tmp_hex, 0, 16);
+	}
+
+	for (int i = 0; i < sizeof(bytes); i++) {
+        bytes[i] = (unsigned char) reverse_bits(bytes[i], 8);
+    }
+
+    AudioPacket *ap = (AudioPacket *) bytes;
+
+    ap->format = reverse_bits(ap->format, 3);
+    ap->seq_id = reverse_bits(ap->seq_id, 10);
+    ap->payload_size = reverse_bits(ap->payload_size, 16);//ntohs(ap->payload_size);
+    ap->timestamp = reverse_bits(ap->timestamp, 32);
+
+	printf("Format: %u\n", ap->format);
+	printf("Mono: %u\n", ap->mono);
+	printf("Vibrate: %u\n", ap->vibrate);
+	printf("Type: %u\n", ap->type);
+	printf("Seq ID: %u\n", ap->seq_id);
+	printf("Payload Size: %u\n", ap->payload_size);
+	printf("Timestamp: %u\n", ap->timestamp);
+
+	return 0;
+}

pipe/linux/wpa.c

@@ -876,7 +876,7 @@ void *sync_with_console_internal(void *data)
                         nlprint("CRED RECV: %.*s", buf_len, buf);
                     }
 
-                    if (!memcmp("<3>WPS-CRED-RECEIVED", buf, 20)) {
+                    if (strstr(buf, "WPS-SUCCESS")) {
                         nlprint("RECEIVED AUTHENTICATION FROM CONSOLE");
                         cred_received = 1;
                         break;