diff --git a/Makefile.inc b/Makefile.inc
index 89234a6..7a558d3 100644
--- a/Makefile.inc
+++ b/Makefile.inc
@@ -1 +1 @@
-OBJS=main.o usb.o usbpad.o mappings.o gcn64_protocol.o n64.o gamecube.o usart1.o bootloader.o
+OBJS=main.o usb.o usbpad.o mappings.o gcn64_protocol.o n64.o gamecube.o usart1.o bootloader.o eeprom.o config.o hiddata.o
diff --git a/config.c b/config.c
new file mode 100644
index 0000000..8010d55
--- /dev/null
+++ b/config.c
@@ -0,0 +1,58 @@
+#include <string.h>
+#include "eeprom.h"
+
+#include "requests.h"
+
+struct eeprom_data_struct g_eeprom_data;
+
+/* Called by the eeprom driver if the content
+ * was invalid and it needs to write defaults
+ * values.  */
+void eeprom_app_write_defaults(void)
+{
+	const char default_serial[SERIAL_NUM_LEN] = { '0','0','0','0','0','1' };
+
+	memcpy(g_eeprom_data.cfg.serial, default_serial, SERIAL_NUM_LEN);
+	g_eeprom_data.cfg.mode = CFG_MODE_STANDARD;
+}
+
+static void config_set_serial(char serial[SERIAL_NUM_LEN])
+{
+	memcpy(g_eeprom_data.cfg.serial, serial, SERIAL_NUM_LEN);
+	eeprom_commit();
+}
+
+unsigned char config_getParam(unsigned char param, unsigned char *value, unsigned char max_len)
+{
+	switch (param)
+	{
+		case CFG_PARAM_MODE:
+			*value = g_eeprom_data.cfg.mode;
+			return 1;
+		case CFG_PARAM_SERIAL:
+			memcpy(value, g_eeprom_data.cfg.serial, SERIAL_NUM_LEN);
+			return SERIAL_NUM_LEN;
+	}
+
+	return 0;
+}
+
+unsigned char config_setParam(unsigned char param, const unsigned char *value)
+{
+	if (!value)
+		return 0;
+
+	switch (param)
+	{
+		case CFG_PARAM_MODE:
+			g_eeprom_data.cfg.mode = value[0];
+			return 1;
+		case CFG_PARAM_SERIAL:
+			config_set_serial((char*)value);
+			return 1;
+	}
+
+	return 0;
+}
+
+
diff --git a/config.h b/config.h
new file mode 100644
index 0000000..8ac7efa
--- /dev/null
+++ b/config.h
@@ -0,0 +1,16 @@
+#ifndef _config_h__
+#define _config_h__
+
+#define SERIAL_NUM_LEN	6
+struct eeprom_cfg {
+	uint8_t serial[SERIAL_NUM_LEN];
+	uint8_t mode;
+};
+
+void eeprom_app_write_defaults(void);
+void eeprom_app_ready(void);
+
+unsigned char config_setParam(unsigned char param, const unsigned char *value);
+unsigned char config_getParam(unsigned char param, unsigned char *value, unsigned char max_len);
+
+#endif
diff --git a/eeprom.c b/eeprom.c
new file mode 100644
index 0000000..3e3a2d9
--- /dev/null
+++ b/eeprom.c
@@ -0,0 +1,72 @@
+/* wusbmote: Wiimote accessory to USB Adapter
+ * Copyright (C) 2012-2014 Raphaël Assénat
+ *
+ * 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 program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ *
+ * The author may be contacted at raph@raphnet.net
+ */
+
+#include <avr/eeprom.h>
+#include <util/crc16.h>
+#include <stdint.h>
+#include <string.h>
+#include "eeprom.h"
+
+static uint16_t calc_geeprom_data_crc(void)
+{
+	uint16_t crc = 0x0000;
+	int i;
+
+	/* Update the CRC */
+	for (i=0; i<EEPROM_USED_SIZE_NOCRC; i++) {
+		crc = _crc_xmodem_update(crc, ((uint8_t*)&g_eeprom_data)[i]);
+	}
+
+	return crc;
+}
+
+void eeprom_commit(void)
+{
+	g_eeprom_data.crc16 = calc_geeprom_data_crc();
+
+	/* Sync eeprom content */
+	eeprom_update_block(&g_eeprom_data, EEPROM_BASE_PTR, EEPROM_USED_SIZE);
+}
+
+static char isCrcValid()
+{
+	return g_eeprom_data.crc16 == calc_geeprom_data_crc();
+}
+
+// return 1 if eeprom was blank
+void eeprom_init(void)
+{
+	eeprom_read_block(&g_eeprom_data, EEPROM_BASE_PTR, EEPROM_USED_SIZE);
+
+	/* Detect new or corrupted content. Program default values if required. */
+	if ((g_eeprom_data.magic != EEPROM_MAGIC) || !isCrcValid())
+	{
+		memset(&g_eeprom_data, 0, EEPROM_USED_SIZE);
+		g_eeprom_data.magic = EEPROM_MAGIC;
+
+		// Call application code to set application defaults
+		eeprom_app_write_defaults();
+
+		// Write the now valid content to the EEPROM at once.
+		eeprom_commit();
+	}
+
+	eeprom_app_ready();
+}
diff --git a/eeprom.h b/eeprom.h
new file mode 100644
index 0000000..0c45fcb
--- /dev/null
+++ b/eeprom.h
@@ -0,0 +1,40 @@
+#ifndef _eeprom_h__
+#define _eeprom_h__
+
+#include <stdint.h>
+
+#define EEPROM_MAGIC	0xfeed
+#define EEPROM_BASE_PTR	((void*)0x0000)
+#define EEPROM_USED_SIZE	(sizeof(struct eeprom_data_struct))
+#define EEPROM_USED_SIZE_NOCRC	(EEPROM_USED_SIZE-2)
+
+#include "config.h" // config.h to struct eeprom_cfg
+
+struct eeprom_data_struct {
+	uint16_t magic;
+	struct eeprom_cfg cfg;
+	uint16_t crc16;
+};
+
+extern struct eeprom_data_struct g_eeprom_data;
+
+/* Application specific function to implement. When
+ * called, write application defaults to g_eeprom_data.
+ *
+ * Only called when eeprom is new or corrupted. */
+extern void eeprom_app_write_defaults(void);
+
+/* Application specific function to implement. Called
+ * after the eeprom content has been loaded or initialized.
+ * A good place to copy values elsewhere or otherwise
+ * act on configuration content. */
+extern void eeprom_app_ready(void);
+
+/* Load, Validate and init eeprom if needed. */
+void eeprom_init(void);
+
+/* Commit changes made to g_eeprom_data */
+void eeprom_commit(void);
+
+#endif // _eeprom_h__
+
diff --git a/hiddata.c b/hiddata.c
new file mode 100644
index 0000000..891c872
--- /dev/null
+++ b/hiddata.c
@@ -0,0 +1,126 @@
+#include <stdio.h>
+#include <string.h>
+#include "requests.h"
+#include "config.h"
+#include "hiddata.h"
+#include "bootloader.h"
+#include "gcn64_protocol.h"
+
+#define CMDBUF_SIZE	64
+
+#define STATE_IDLE			0
+#define STATE_NEW_COMMAND	1	// New command in buffer
+#define STATE_COMMAND_DONE	2	// Result in buffer
+
+//#define DEBUG
+
+extern char g_polling_suspended;
+
+static volatile uint8_t state = STATE_IDLE;
+static unsigned char cmdbuf[CMDBUF_SIZE];
+static volatile unsigned char cmdbuf_len = 0;
+
+/*** Get/Set report called from interrupt context! */
+uint16_t hiddata_get_report(struct usb_request *rq, const uint8_t **dat)
+{
+//	printf("Get data\n");
+	if (state == STATE_COMMAND_DONE) {
+		*dat = cmdbuf;
+		state = STATE_IDLE;
+#ifdef DEBUG
+		printf("hiddata idle, sent %d bytes\r\n", cmdbuf_len);
+#endif
+		return cmdbuf_len;
+	}
+	return 0;
+}
+
+/*** Get/Set report called from interrupt context! */
+uint8_t hiddata_set_report(const struct usb_request *rq, const uint8_t *dat, uint16_t len)
+{
+	int i;
+
+#ifdef DEBUG
+	printf("Set data %d\n", len);
+	for (i=0; i<len; i++) {
+		printf("0x%02x ", dat[i]);
+	}
+	printf("\r\n");
+#endif
+
+	state = STATE_NEW_COMMAND;
+	memcpy(cmdbuf, dat, len);
+	cmdbuf_len = len;
+
+	return 0;
+}
+
+static void hiddata_processCommandBuffer(void)
+{
+	int i;
+	int bits;
+
+	if (cmdbuf_len < 1) {
+		state = STATE_IDLE;
+		return;
+	}
+
+//	printf("Process cmd 0x%02x\r\n", cmdbuf[0]);
+	switch(cmdbuf[0])
+	{
+		case RQ_GCN64_JUMP_TO_BOOTLOADER:
+			enterBootLoader();
+			break;
+		case RQ_GCN64_RAW_SI_COMMAND:
+			// TODO : Range checking
+			// cmd : RQ, LEN, data[]
+			bits = gcn64_transaction(cmdbuf+2, cmdbuf[1]);
+			cmdbuf_len = bits / 8; // The above return a number of bits
+			gcn64_protocol_getBytes(0, cmdbuf_len, cmdbuf + 2);
+			cmdbuf_len += 2; // Answer: RQ, LEN, data[]
+			break;
+		case RQ_GCN64_GET_CONFIG_PARAM:
+			// Cmd : RQ, PARAM
+			// Answer: RQ, PARAM, data[]
+			cmdbuf_len = config_getParam(cmdbuf[1], cmdbuf + 2, CMDBUF_SIZE-2);
+			cmdbuf_len += 2; // Datalen + RQ + PARAM
+			break;
+		case RQ_GCN64_SET_CONFIG_PARAM:
+			// Cmd: RQ, PARAM, data[]
+			config_setParam(cmdbuf[1], cmdbuf+2);
+			// Answer: RQ, PARAM
+			cmdbuf_len = 2;
+			break;
+		case RQ_GCN64_SUSPEND_POLLING:
+			g_polling_suspended = 1;
+			break;
+	}
+
+#ifdef DEBUG
+	printf("Pending data %d\n", cmdbuf_len);
+	for (i=0; i<cmdbuf_len; i++) {
+		printf("0x%02x ", cmdbuf[i]);
+	}
+	printf("\r\n");
+#endif
+
+	state = STATE_COMMAND_DONE;
+}
+
+void hiddata_doTask(void)
+{
+	switch (state)
+	{
+		default:
+			state = STATE_IDLE;
+		case STATE_IDLE:
+			break;
+
+		case STATE_NEW_COMMAND:
+			hiddata_processCommandBuffer();
+			break;
+
+		case STATE_COMMAND_DONE:
+			break;
+	}
+}
diff --git a/hiddata.h b/hiddata.h
new file mode 100644
index 0000000..3160032
--- /dev/null
+++ b/hiddata.h
@@ -0,0 +1,12 @@
+#ifndef _hiddata_h__
+#define _hiddata_h__
+
+#include <stdint.h>
+#include "usb.h"
+
+uint16_t hiddata_get_report(struct usb_request *rq, const uint8_t **dat);
+uint8_t hiddata_set_report(const struct usb_request *rq, const uint8_t *dat, uint16_t len);
+
+void hiddata_doTask(void);
+
+#endif
diff --git a/main.c b/main.c
index 978fd48..ba56171 100644
--- a/main.c
+++ b/main.c
@@ -15,6 +15,8 @@
 #include "n64.h"
 #include "gamecube.h"
 #include "usbpad.h"
+#include "eeprom.h"
+#include "hiddata.h"
 
 uint16_t hid_get_report_main(struct usb_request *rq, const uint8_t **dat);
 uint8_t hid_set_report_main(const struct usb_request *rq, const uint8_t *dat, uint16_t len);
@@ -153,8 +155,8 @@ static struct usb_parameters usb_params = {
 		[1] = {
 			.reportdesc = dataHidReport,
 			.reportdesc_len = sizeof(dataHidReport),
-			.getReport = hid_get_report_data,
-			.setReport = hid_set_report_data,
+			.getReport = hiddata_get_report,
+			.setReport = hiddata_set_report,
 		},
 	},
 };
@@ -444,26 +446,7 @@ uint8_t hid_set_report_main(const struct usb_request *rq, const uint8_t *data, u
 	return 0;
 }
 
-uint16_t hid_get_report_data(struct usb_request *rq, const uint8_t **dat)
-{
-	printf("Get data\n");
-	return 0;
-}
 
-uint8_t hid_set_report_data(const struct usb_request *rq, const uint8_t *dat, uint16_t len)
-{
-	int i;
-	printf("Set data %d\n", len);
-
-	for (i=0; i<len; i++) {
-		printf("0x%02x ", dat[i]);
-	}
-	printf("\n");
-
-	enterBootLoader();
-
-	return 0;
-}
 
 #define NUM_PAD_TYPES	2
 
@@ -490,6 +473,12 @@ Gamepad *detectPad(void)
 	return NULL;
 }
 
+void eeprom_app_ready(void)
+{
+	// TODO : Set serial number from configured value
+}
+
+char g_polling_suspended = 0;
 
 int main(void)
 {
@@ -498,6 +487,7 @@ int main(void)
 
 	hwinit();
 	usart1_init();
+	eeprom_init();
 
 	/* Init the buffer with idle data */
 	usbpad_buildReport(NULL, gamepad_report0);
@@ -516,6 +506,12 @@ int main(void)
 
 		usb_doTasks();
 
+		// Todo : The _delay_ms used to poll the controller at
+		// a fixed interval is slowing down the frequency of
+		// hiddata_doTask calls. Rework this code to use a timer
+		// for polling the controller...
+		hiddata_doTask();
+
 		_delay_ms(5);
 		decideVibration();
 
@@ -526,7 +522,7 @@ int main(void)
 			last_v = gamepad_vibrate;
 		}
 
-		if (pad) {
+		if (pad && !g_polling_suspended) {
 			pad->update();
 
 			if (pad->changed()) {
diff --git a/requests.h b/requests.h
new file mode 100644
index 0000000..beed507
--- /dev/null
+++ b/requests.h
@@ -0,0 +1,18 @@
+#ifndef _gcn64_requests_h__
+#define _gcn64_requests_h__
+
+/* Commands */
+#define RQ_GCN64_SET_CONFIG_PARAM		0x01
+#define RQ_GCN64_GET_CONFIG_PARAM		0x02
+#define RQ_GCN64_SUSPEND_POLLING		0x03
+#define RQ_GCN64_RAW_SI_COMMAND			0x80
+#define RQ_GCN64_JUMP_TO_BOOTLOADER		0xFF
+
+/* Configuration parameters and constants */
+#define CFG_PARAM_MODE			0x00
+#define CFG_MODE_STANDARD   	0x00
+
+#define CFG_PARAM_SERIAL		0x01
+
+
+#endif