wpad: use malloc instead of lwp_wkspace alloc, check for OOM error

gc/wiiuse/wiiuse.h

@@ -625,6 +625,7 @@ typedef struct wiimote_t {
 		WCONST ubyte exp_timeout;			/**< timeout for expansion handshake		*/
 	#elif defined(GEKKO)
 		WCONST lwp_queue cmdq;
+		WCONST u8 *queue_buffer;
 		WCONST struct bd_addr bdaddr;		/**< bt address								*/
 		WCONST char bdaddr_str[18];			/**< readable bt address					*/
 		WCONST struct bte_pcb *sock;	/**< output socket							*/

wiiuse/io_wii.c

@@ -13,10 +13,8 @@
 #include "io.h"
 
 #define MAX_COMMANDS					0x100
-#define MAX_WIIMOTES					6
 
 static vu32* const _ipcReg = (u32*)0xCD000000;
-static u8 *__queue_buffer[MAX_WIIMOTES] = { 0, 0, 0, 0, 0 };
 
 extern void parse_event(struct wiimote_t *wm);
 extern void idle_cycle(struct wiimote_t* wm);
@@ -265,17 +263,16 @@ void wiiuse_sensorbar_enable(int enable)
 	__wiiuse_sensorbar_enable(enable);
 }
 
-void wiiuse_init_cmd_queue(struct wiimote_t *wm)
+int wiiuse_init_cmd_queue(struct wiimote_t *wm)
 {
 	u32 size;
 
-	if (!__queue_buffer[wm->unid]) {
-		size = (MAX_COMMANDS*sizeof(struct cmd_blk_t));
-		__queue_buffer[wm->unid] = __lwp_wkspace_allocate(size);
-		if(!__queue_buffer[wm->unid]) return;
-	}
+	size = (MAX_COMMANDS*sizeof(struct cmd_blk_t));
+	wm->queue_buffer = malloc(size);
+	if(!wm->queue_buffer) return ERR_MEM;
 
-	__lwp_queue_initialize(&wm->cmdq,__queue_buffer[wm->unid],MAX_COMMANDS,sizeof(struct cmd_blk_t));
+	__lwp_queue_initialize(&wm->cmdq,wm->queue_buffer,MAX_COMMANDS,sizeof(struct cmd_blk_t));
+	return ERR_OK;
 }
 
 int wiiuse_io_write(struct wiimote_t *wm,ubyte *buf,int len)

wiiuse/wiiuse.c

@@ -54,6 +54,32 @@ static __inline__ void __wiiuse_push_command(struct wiimote_t *wm,struct cmd_blk
 	_CPU_ISR_Restore(level);
 }
 
+void wiiuse_cleanup(struct wiimote_t **wm, int wiimotes)
+{
+	int i = 0;
+
+	if (!wm)
+		return;
+
+	for (; i < wiimotes; ++i) {
+		if (wm[i]) {
+			if (wm[i]->queue_buffer) {
+				free(wm[i]->queue_buffer);
+				wm[i]->queue_buffer = NULL;
+			}
+
+			if (wm[i]->sock) {
+				bte_free(wm[i]->sock);
+				wm[i]->sock = NULL;
+			}
+
+			free(wm[i]);
+		}
+	}
+
+	free(wm);
+}
+
 #ifndef GEKKO
 struct wiimote_t** wiiuse_init(int wiimotes) {
 #else
@@ -66,14 +92,14 @@ struct wiimote_t** wiiuse_init(int wiimotes, wii_event_cb event_cb) {
 		return NULL;
 
 	if (!__wm) {
-		__wm = __lwp_wkspace_allocate(sizeof(struct wiimote_t*) * wiimotes);
+		__wm = malloc(sizeof(struct wiimote_t*) * wiimotes);
 		if(!__wm) return NULL;
 		memset(__wm, 0, sizeof(struct wiimote_t*) * wiimotes);
 	}
 
 	for (i = 0; i < wiimotes; ++i) {
 		if(!__wm[i])
-			__wm[i] = __lwp_wkspace_allocate(sizeof(struct wiimote_t));
+			__wm[i] = malloc(sizeof(struct wiimote_t));
 
 		memset(__wm[i], 0, sizeof(struct wiimote_t));
 		__wm[i]->unid = i;
@@ -88,7 +114,10 @@ struct wiimote_t** wiiuse_init(int wiimotes, wii_event_cb event_cb) {
 			__wm[i]->sock = NULL;
 			__wm[i]->bdaddr = *BD_ADDR_ANY;
 			__wm[i]->event_cb = event_cb;
-			wiiuse_init_cmd_queue(__wm[i]);
+			if (wiiuse_init_cmd_queue(__wm[i])) {
+				WIIUSE_ERROR("Could not allocate command queue");
+				return NULL;
+			}
 		#elif defined(unix)
 			__wm[i]->bdaddr = *BDADDR_ANY;
 			__wm[i]->out_sock = -1;

wiiuse/wiiuse_internal.h

@@ -267,7 +267,7 @@ struct op_t
 } __attribute__((packed));
 
 /* not part of the api */
-void wiiuse_init_cmd_queue(struct wiimote_t *wm);
+int wiiuse_init_cmd_queue(struct wiimote_t *wm);
 void wiiuse_send_next_command(struct wiimote_t *wm);
 int wiiuse_set_report_type(struct wiimote_t* wm,cmd_blk_cb cb);
 int wiiuse_sendcmd(struct wiimote_t *wm,ubyte report_type,ubyte *msg,int len,cmd_blk_cb cb);

wiiuse/wpad.c

@@ -1774,6 +1774,7 @@ s32 WPAD_Shutdown(void)
 	__wpads_used = 0;
 
 	__wiiuse_sensorbar_enable(0);
+	wiiuse_cleanup(__wpads, WPAD_MAX_DEVICES);
 	_CPU_ISR_Restore(level);
 
 	BTE_Shutdown();