From: Uwe Hermann <redacted>
Date: Thu, 10 Mar 2016 22:10:06 +0000 (+0100)
Subject: hantek_6022be.c: Adapt to fx2lafw coding style, minor simplifications.
X-Git-Tag: sigrok-firmware-fx2lafw-0.1.4~41
X-Git-Url: https://sigrok.org/gitaction?a=commitdiff_plain;h=cc789c14085a371976fa7ef1bc281eb7bac34086;p=sigrok-firmware-fx2lafw.git

hantek_6022be.c: Adapt to fx2lafw coding style, minor simplifications.
---

diff --git a/hantek_6022be.c b/hantek_6022be.c
index cc33ea22..07ecc0e1 100644
--- a/hantek_6022be.c
+++ b/hantek_6022be.c
@@ -25,135 +25,145 @@
 #include <delay.h>
 #include <setupdat.h>
 
-// change to support as many interfaces as you need
-BYTE altiface = 0; // alt interface
+/* Change to support as many interfaces as you need. */
+BYTE altiface = 0;
 
 volatile WORD ledcounter = 0;
 
-volatile __bit dosud=FALSE;
-volatile __bit dosuspend=FALSE;
+volatile __bit dosud = FALSE;
+volatile __bit dosuspend = FALSE;
+
+extern __code BYTE highspd_dscr;
+extern __code BYTE fullspd_dscr;
 
 extern void main_init();
 
-void main() {
-
- SETCPUFREQ(CLK_12M); // save energy
-
- main_init();
-
- // set up interrupts.
- USE_USB_INTS();
- 
- ENABLE_SUDAV();
- ENABLE_USBRESET();
- ENABLE_HISPEED(); 
- ENABLE_SUSPEND();
- ENABLE_RESUME();
-
- EA=1;
-
- // init timer2
-    RCAP2L = -500 & 0xff;
-    RCAP2H = (-500 >> 8) & 0xff;
-    T2CON = 0;
-    ET2 = 1;
-    TR2 = 1;
-
-// iic files (c2 load) don't need to renumerate/delay
-// trm 3.6
-#ifndef NORENUM
- RENUMERATE();
-#else
- USBCS &= ~bmDISCON;
-#endif
- 
-    PORTCCFG = 0;
-    PORTACFG = 0;
-    OEC = 0xff;
-    OEA = 0x80;
-
- while(TRUE) {
-
-     if (dosud) {
-       dosud=FALSE;
-       handle_setupdata();
-     }
-
-     if (dosuspend) {
-        dosuspend=FALSE;
-        do {
-           WAKEUPCS |= bmWU|bmWU2; // make sure ext wakeups are cleared
-           SUSPEND=1;
-           PCON |= 1;
-           __asm
-           nop
-           nop
-           nop
-           nop
-           nop
-           nop
-           nop
-           __endasm;
-        } while ( !remote_wakeup_allowed && REMOTE_WAKEUP()); 
-
-        // resume
-        // trm 6.4
-        if ( REMOTE_WAKEUP() ) {
-            delay(5);
-            USBCS |= bmSIGRESUME;
-            delay(15);
-            USBCS &= ~bmSIGRESUME;
-        }
-     }
- }
+void main(void)
+{
+	/* Save energy. */
+	SETCPUFREQ(CLK_12M);
+
+	main_init();
+
+	/* Set up interrupts. */
+	USE_USB_INTS();
+
+	ENABLE_SUDAV();
+	ENABLE_USBRESET();
+	ENABLE_HISPEED(); 
+	ENABLE_SUSPEND();
+	ENABLE_RESUME();
+
+	/* Global (8051) interrupt enable. */
+	EA = 1;
+
+	/* Init timer2. */
+	RCAP2L = -500 & 0xff;
+	RCAP2H = (-500 >> 8) & 0xff;
+	T2CON = 0;
+	ET2 = 1;
+	TR2 = 1;
+
+	RENUMERATE();
+
+	PORTCCFG = 0;
+	PORTACFG = 0;
+	OEC = 0xff;
+	OEA = 0x80;
+
+	while (TRUE) {
+		if (dosud) {
+			dosud = FALSE;
+			handle_setupdata();
+		}
+
+		if (dosuspend) {
+			dosuspend = FALSE;
+			do {
+				/* Make sure ext wakeups are cleared. */
+				WAKEUPCS |= bmWU|bmWU2;
+				SUSPEND = 1;
+				PCON |= 1;
+				__asm
+				nop
+				nop
+				nop
+				nop
+				nop
+				nop
+				nop
+				__endasm;
+			} while (!remote_wakeup_allowed && REMOTE_WAKEUP());
+
+			/* Resume (TRM 6.4). */
+			if (REMOTE_WAKEUP()) {
+				delay(5);
+				USBCS |= bmSIGRESUME;
+				delay(15);
+				USBCS &= ~bmSIGRESUME;
+			}
+		}
+	}
 }
 
-void resume_isr() __interrupt RESUME_ISR {
- CLEAR_RESUME();
+void resume_isr(void) __interrupt RESUME_ISR
+{
+	CLEAR_RESUME();
 }
-  
-void sudav_isr() __interrupt SUDAV_ISR {
- dosud=TRUE;
- CLEAR_SUDAV();
+
+void sudav_isr(void) __interrupt SUDAV_ISR
+{
+	dosud = TRUE;
+	CLEAR_SUDAV();
 }
-void usbreset_isr() __interrupt USBRESET_ISR {
- handle_hispeed(FALSE);
- CLEAR_USBRESET();
+
+void usbreset_isr(void) __interrupt USBRESET_ISR
+{
+	handle_hispeed(FALSE);
+	CLEAR_USBRESET();
 }
-void hispeed_isr() __interrupt HISPEED_ISR {
- handle_hispeed(TRUE);
- CLEAR_HISPEED();
+
+void hispeed_isr(void) __interrupt HISPEED_ISR
+{
+	handle_hispeed(TRUE);
+	CLEAR_HISPEED();
 }
 
-void suspend_isr() __interrupt SUSPEND_ISR {
- dosuspend=TRUE;
- CLEAR_SUSPEND();
+void suspend_isr(void) __interrupt SUSPEND_ISR
+{
+	dosuspend = TRUE;
+	CLEAR_SUSPEND();
 }
 
-void timer2_isr() __interrupt TF2_ISR {
-  PA7 = !PA7;
-  if (ledcounter) {
-    if (--ledcounter == 0) {
-      // clear LED
-      PC0 = 1;
-      PC1 = 1;
-    }
-  }
-  TF2 = 0;
+void timer2_isr(void) __interrupt TF2_ISR
+{
+	PA7 = !PA7;
+	if (ledcounter) {
+		if (--ledcounter == 0) {
+			/* Clear LED. */
+			PC0 = 1;
+			PC1 = 1;
+		}
+	}
+	TF2 = 0;
 }
 
-/* This sets three bits for each channel, one channel at a time.
+/*
+ * This sets three bits for each channel, one channel at a time.
  * For channel 0 we want to set bits 5, 6 & 7
  * For channel 1 we want to set bits 2, 3 & 4
  *
- * We convert the input values that are strange due to original firmware code into the value of the three bits as follows:
+ * We convert the input values that are strange due to original
+ * firmware code into the value of the three bits as follows:
+ *
  * val -> bits
  * 1  -> 010b
  * 2  -> 001b
  * 5  -> 000b
  * 10 -> 011b
  *
- * The third bit is always zero since there are only four outputs connected in the serial selector chip.
+ * The third bit is always zero since there are only four outputs connected
+ * in the serial selector chip.
  *
  * The multiplication of the converted value by 0x24 sets the relevant bits in
  * both channels and then we mask it out to only affect the channel currently
@@ -161,286 +171,302 @@ void timer2_isr() __interrupt TF2_ISR {
  */
 BOOL set_voltage(BYTE channel, BYTE val)
 {
-    BYTE bits, mask;
-    switch (val) {
-    case 1:
-	bits = 0x24 * 2;
-	break;
-    case 2:
-	bits = 0x24 * 1;
-	break;
-    case 5:
-	bits = 0x24 * 0;
-	break;
-    case 10:
-	bits = 0x24 * 3;
-	break;
-    default:
-	return FALSE;
-    }
+	BYTE bits, mask;
+
+	switch (val) {
+	case 1:
+		bits = 0x24 * 2;
+		break;
+	case 2:
+		bits = 0x24 * 1;
+		break;
+	case 5:
+		bits = 0x24 * 0;
+		break;
+	case 10:
+		bits = 0x24 * 3;
+		break;
+	default:
+		return FALSE;
+	}
+
+	mask = (channel) ? 0xe0 : 0x1c;
+	IOC = (IOC & ~mask) | (bits & mask);
 
-    mask = channel ? 0xe0 : 0x1c;
-    IOC = (IOC & ~mask) | (bits & mask);
-    return TRUE;
+	return TRUE;
 }
 
 BOOL set_numchannels(BYTE numchannels)
 {
-    if (numchannels == 1 || numchannels == 2) {
-	BYTE fifocfg = 7 + numchannels;
-	EP2FIFOCFG = fifocfg;
-	EP6FIFOCFG = fifocfg;
-	return TRUE;
-    }
-    return FALSE;
+	if (numchannels == 1 || numchannels == 2) {
+		BYTE fifocfg = 7 + numchannels;
+		EP2FIFOCFG = fifocfg;
+		EP6FIFOCFG = fifocfg;
+		return TRUE;
+	}
+
+	return FALSE;
 }
 
-void clear_fifo()
+void clear_fifo(void)
 {
-    GPIFABORT = 0xff;
-    SYNCDELAY3;
-    FIFORESET = 0x80;
-    SYNCDELAY3;
-    FIFORESET = 0x82;
-    SYNCDELAY3;
-    FIFORESET = 0x86;
-    SYNCDELAY3;
-    FIFORESET = 0;
+	GPIFABORT = 0xff;
+	SYNCDELAY3;
+	FIFORESET = 0x80;
+	SYNCDELAY3;
+	FIFORESET = 0x82;
+	SYNCDELAY3;
+	FIFORESET = 0x86;
+	SYNCDELAY3;
+	FIFORESET = 0;
 }
 
-void stop_sampling()
+void stop_sampling(void)
 {
-    GPIFABORT = 0xff;
-    SYNCDELAY3;
-    if (altiface == 0) {
-	INPKTEND = 6;
-    } else {
-	INPKTEND = 2;
-    }
+	GPIFABORT = 0xff;
+	SYNCDELAY3;
+	INPKTEND = (altiface == 0) ? 6 : 2;
 }
 
-void start_sampling()
+void start_sampling(void)
 {
-    int i;
-    clear_fifo();
-
-    for (i = 0; i < 1000; i++);
-    while (!(GPIFTRIG & 0x80)) {
-	;
-    }
-    SYNCDELAY3;
-    GPIFTCB1 = 0x28;
-    SYNCDELAY3;
-    GPIFTCB0 = 0;
-    if (altiface == 0)
-	GPIFTRIG = 6;
-    else
-	GPIFTRIG = 4;
-
-    // set green led
-    // don't clear led
-    ledcounter = 0;
-    PC0 = 1;
-    PC1 = 0;
+	int i;
+
+	clear_fifo();
+
+	for (i = 0; i < 1000; i++);
+
+	while (!(GPIFTRIG & 0x80))
+		;
+
+	SYNCDELAY3;
+	GPIFTCB1 = 0x28;
+	SYNCDELAY3;
+	GPIFTCB0 = 0;
+	GPIFTRIG = (altiface == 0) ? 6 : 4;
+
+	/* Set green LED, don't clear LED. */
+	ledcounter = 0;
+	PC0 = 1;
+	PC1 = 0;
 }
 
-extern __code BYTE highspd_dscr;
-extern __code BYTE fullspd_dscr;
 void select_interface(BYTE alt)
 {
-    const BYTE *pPacketSize = (USBCS & bmHSM ? &highspd_dscr : &fullspd_dscr)
-	+ (9 + 16*alt + 9 + 4);
-    altiface = alt;
-    if (alt == 0) {
-	// bulk on port 6
-	EP2CFG = 0x00;
-	EP6CFG = 0xe0;
-	EP6GPIFFLGSEL = 1;
-
-	EP6AUTOINLENL = pPacketSize[0];
-	EP6AUTOINLENH = pPacketSize[1];
-    } else {
-	// iso on port 2
-	EP2CFG = 0xd8;
-	EP6CFG = 0x00;
-	EP2GPIFFLGSEL = 1;
-
-	EP2AUTOINLENL = pPacketSize[0];
-	EP2AUTOINLENH = pPacketSize[1] & 0x7;
-	EP2ISOINPKTS = (pPacketSize[1] >> 3) + 1;
-    }
+	const BYTE *pPacketSize = \
+		(USBCS & bmHSM ? &highspd_dscr : &fullspd_dscr)
+		+ (9 + (16 * alt) + 9 + 4);
+
+	altiface = alt;
+
+	if (alt == 0) {
+		/* Bulk on EP6. */
+		EP2CFG = 0x00;
+		EP6CFG = 0xe0;
+		EP6GPIFFLGSEL = 1;
+		EP6AUTOINLENL = pPacketSize[0];
+		EP6AUTOINLENH = pPacketSize[1];
+	} else {
+		/* Iso on EP2. */
+		EP2CFG = 0xd8;
+		EP6CFG = 0x00;
+		EP2GPIFFLGSEL = 1;
+		EP2AUTOINLENL = pPacketSize[0];
+		EP2AUTOINLENH = pPacketSize[1] & 0x7;
+		EP2ISOINPKTS = (pPacketSize[1] >> 3) + 1;
+	}
 }
 
 const struct samplerate_info {
-    BYTE rate;
-    BYTE wait0;
-    BYTE wait1;
-    BYTE opc0;
-    BYTE opc1;
-    BYTE out0;
-    BYTE ifcfg;
+	BYTE rate;
+	BYTE wait0;
+	BYTE wait1;
+	BYTE opc0;
+	BYTE opc1;
+	BYTE out0;
+	BYTE ifcfg;
 } samplerates[] = {
-    { 48,0x80,   0, 3, 0, 0x00, 0xea },
-    { 30,0x80,   0, 3, 0, 0x00, 0xaa },
-    { 24,   1,   0, 2, 1, 0x40, 0xca },
-    { 16,   1,   1, 2, 0, 0x40, 0xca },
-    { 12,   2,   1, 2, 0, 0x40, 0xca },
-    {  8,   3,   2, 2, 0, 0x40, 0xca },
-    {  4,   6,   5, 2, 0, 0x40, 0xca },
-    {  2,  12,  11, 2, 0, 0x40, 0xca },
-    {  1,  24,  23, 2, 0, 0x40, 0xca },
-    { 50,  48,  47, 2, 0, 0x40, 0xca },
-    { 20, 120, 119, 2, 0, 0x40, 0xca },
-    { 10, 240, 239, 2, 0, 0x40, 0xca }
+	{ 48,0x80,   0, 3, 0, 0x00, 0xea },
+	{ 30,0x80,   0, 3, 0, 0x00, 0xaa },
+	{ 24,   1,   0, 2, 1, 0x40, 0xca },
+	{ 16,   1,   1, 2, 0, 0x40, 0xca },
+	{ 12,   2,   1, 2, 0, 0x40, 0xca },
+	{  8,   3,   2, 2, 0, 0x40, 0xca },
+	{  4,   6,   5, 2, 0, 0x40, 0xca },
+	{  2,  12,  11, 2, 0, 0x40, 0xca },
+	{  1,  24,  23, 2, 0, 0x40, 0xca },
+	{ 50,  48,  47, 2, 0, 0x40, 0xca },
+	{ 20, 120, 119, 2, 0, 0x40, 0xca },
+	{ 10, 240, 239, 2, 0, 0x40, 0xca },
 };
 
 BOOL set_samplerate(BYTE rate)
 {
-    BYTE i = 0;
-    while (samplerates[i].rate != rate) {
-	i++;
-	if (i == sizeof(samplerates)/sizeof(samplerates[0]))
-	    return FALSE;
-    }
-
-    IFCONFIG = samplerates[i].ifcfg;
-
-    AUTOPTRSETUP = 7;
-    AUTOPTRH2 = 0xE4;
-    AUTOPTRL2 = 0x00;
-
-    /* The program for low-speed, e.g. 1 MHz, is
-     * wait 24, CTL2=0, FIFO
-     * wait 23, CTL2=1
-     * jump 0, CTL2=1
-     *
-     * The program for 24 MHz is
-     * wait 1, CTL2=0, FIFO
-     * jump 0, CTL2=1
-     *
-     * The program for 30/48 MHz is:
-     * jump 0, CTL2=Z, FIFO, LOOP
-     */
-
-    EXTAUTODAT2 = samplerates[i].wait0;
-    EXTAUTODAT2 = samplerates[i].wait1;
-    EXTAUTODAT2 = 1;
-    EXTAUTODAT2 = 0;
-    EXTAUTODAT2 = 0;
-    EXTAUTODAT2 = 0;
-    EXTAUTODAT2 = 0;
-    EXTAUTODAT2 = 0;
-
-    EXTAUTODAT2 = samplerates[i].opc0;
-    EXTAUTODAT2 = samplerates[i].opc1;
-    EXTAUTODAT2 = 1;
-    EXTAUTODAT2 = 0;
-    EXTAUTODAT2 = 0;
-    EXTAUTODAT2 = 0;
-    EXTAUTODAT2 = 0;
-    EXTAUTODAT2 = 0;
-
-    EXTAUTODAT2 = samplerates[i].out0;
-    EXTAUTODAT2 = 0x44;
-    EXTAUTODAT2 = 0x44;
-    EXTAUTODAT2 = 0x00;
-    EXTAUTODAT2 = 0x00;
-    EXTAUTODAT2 = 0x00;
-    EXTAUTODAT2 = 0x00;
-    EXTAUTODAT2 = 0x00;
-
-    EXTAUTODAT2 = 0;
-    EXTAUTODAT2 = 0;
-    EXTAUTODAT2 = 0;
-    EXTAUTODAT2 = 0;
-    EXTAUTODAT2 = 0;
-    EXTAUTODAT2 = 0;
-    EXTAUTODAT2 = 0;
-    EXTAUTODAT2 = 0;
-
-    for (i = 0; i < 96; i++)
+	BYTE i = 0;
+
+	while (samplerates[i].rate != rate) {
+		i++;
+		if (i == sizeof(samplerates) / sizeof(samplerates[0]))
+			return FALSE;
+	}
+
+	IFCONFIG = samplerates[i].ifcfg;
+
+	AUTOPTRSETUP = 7;
+	AUTOPTRH2 = 0xE4;
+	AUTOPTRL2 = 0x00;
+
+	/*
+	 * The program for low-speed, e.g. 1 MHz, is:
+	 * wait 24, CTL2=0, FIFO
+	 * wait 23, CTL2=1
+	 * jump 0, CTL2=1
+	 *
+	 * The program for 24 MHz is:
+	 * wait 1, CTL2=0, FIFO
+	 * jump 0, CTL2=1
+	 *
+	 * The program for 30/48 MHz is:
+	 * jump 0, CTL2=Z, FIFO, LOOP
+	 */
+
+	EXTAUTODAT2 = samplerates[i].wait0;
+	EXTAUTODAT2 = samplerates[i].wait1;
+	EXTAUTODAT2 = 1;
+	EXTAUTODAT2 = 0;
+	EXTAUTODAT2 = 0;
+	EXTAUTODAT2 = 0;
+	EXTAUTODAT2 = 0;
 	EXTAUTODAT2 = 0;
-    return TRUE;
-}
 
-// set *alt_ifc to the current alt interface for ifc
-BOOL handle_get_interface(BYTE ifc, BYTE* alt_ifc) {
-    (void) ifc; // ignore unused parameter
-    *alt_ifc=altiface;
-    return TRUE;
+	EXTAUTODAT2 = samplerates[i].opc0;
+	EXTAUTODAT2 = samplerates[i].opc1;
+	EXTAUTODAT2 = 1;
+	EXTAUTODAT2 = 0;
+	EXTAUTODAT2 = 0;
+	EXTAUTODAT2 = 0;
+	EXTAUTODAT2 = 0;
+	EXTAUTODAT2 = 0;
+
+	EXTAUTODAT2 = samplerates[i].out0;
+	EXTAUTODAT2 = 0x44;
+	EXTAUTODAT2 = 0x44;
+	EXTAUTODAT2 = 0x00;
+	EXTAUTODAT2 = 0x00;
+	EXTAUTODAT2 = 0x00;
+	EXTAUTODAT2 = 0x00;
+	EXTAUTODAT2 = 0x00;
+
+	EXTAUTODAT2 = 0;
+	EXTAUTODAT2 = 0;
+	EXTAUTODAT2 = 0;
+	EXTAUTODAT2 = 0;
+	EXTAUTODAT2 = 0;
+	EXTAUTODAT2 = 0;
+	EXTAUTODAT2 = 0;
+	EXTAUTODAT2 = 0;
+
+	for (i = 0; i < 96; i++)
+		EXTAUTODAT2 = 0;
+
+	return TRUE;
 }
-// return TRUE if you set the interface requested
-// NOTE this function should reconfigure and reset the endpoints
-// according to the interface descriptors you provided.
-BOOL handle_set_interface(BYTE ifc,BYTE alt_ifc) {  
-    if (ifc == 0) {
-      select_interface(alt_ifc);
-    }
-    return TRUE;
+
+/* Set *alt_ifc to the current alt interface for ifc. */
+BOOL handle_get_interface(BYTE ifc, BYTE *alt_ifc)
+{
+	(void)ifc;
+
+	*alt_ifc = altiface;
+
+	return TRUE;
 }
 
-// handle getting and setting the configuration
-// 1 is the default.  We don't support multiple configurations.
-BYTE handle_get_configuration() { 
-    return 0;
+/*
+ * Return TRUE if you set the interface requested.
+ *
+ * Note: This function should reconfigure and reset the endpoints
+ * according to the interface descriptors you provided.
+ */
+BOOL handle_set_interface(BYTE ifc,BYTE alt_ifc)
+{
+	if (ifc == 0)
+		select_interface(alt_ifc);
+
+	return TRUE;
 }
 
-BOOL handle_set_configuration(BYTE cfg) { 
-    (void) cfg; // ignore unused parameter
-    return TRUE;
+BYTE handle_get_configuration(void)
+{
+	/* We only support configuration 0. */
+	return 0;
 }
 
-BOOL handle_vendorcommand(BYTE cmd) {
-    stop_sampling();
-    // Set Red LED
-    PC0 = 0;
-    PC1 = 1;
-    ledcounter = 1000;
-    switch (cmd) {
-    case 0xe0:
-    case 0xe1:
-	EP0BCH=0;
-	EP0BCL=0;
-	while (EP0CS & bmEPBUSY);
-	set_voltage(cmd - 0xe0, EP0BUF[0]);
-	return TRUE;
-    case 0xe2:
-	EP0BCH=0;
-	EP0BCL=0;
-	while (EP0CS & bmEPBUSY);
-	set_samplerate(EP0BUF[0]);
-	return TRUE;
-    case 0xe3:
-	EP0BCH=0;
-	EP0BCL=0;
-	while (EP0CS & bmEPBUSY);
-	if (EP0BUF[0] == 1)
-	    start_sampling();
-	return TRUE;
-    case 0xe4:
-	EP0BCH=0;
-	EP0BCL=0;
-	while (EP0CS & bmEPBUSY);
-	set_numchannels(EP0BUF[0]);
+BOOL handle_set_configuration(BYTE cfg)
+{
+	/* We only support configuration 0. */
+	(void)cfg;
+
 	return TRUE;
-    }
-    return FALSE; // not handled by handlers
 }
 
-void main_init() {
-    EP4CFG = 0;
-    EP8CFG = 0;
-
-    // in idle mode tristate all outputs
-    GPIFIDLECTL = 0x00;
-    GPIFCTLCFG = 0x80;
-    GPIFWFSELECT = 0x00;
-    GPIFREADYSTAT = 0x00;
-
-    stop_sampling();
-    set_voltage(0, 1);
-    set_voltage(1, 1);
-    set_samplerate(1);
-    set_numchannels(2);
-    select_interface(0);
+BOOL handle_vendorcommand(BYTE cmd)
+{
+	stop_sampling();
+
+	/* Set red LED. */
+	PC0 = 0;
+	PC1 = 1;
+	ledcounter = 1000;
+
+	switch (cmd) {
+	case 0xe0:
+	case 0xe1:
+		EP0BCH = 0;
+		EP0BCL = 0;
+		while (EP0CS & bmEPBUSY);
+		set_voltage(cmd - 0xe0, EP0BUF[0]);
+		return TRUE;
+	case 0xe2:
+		EP0BCH = 0;
+		EP0BCL = 0;
+		while (EP0CS & bmEPBUSY);
+		set_samplerate(EP0BUF[0]);
+		return TRUE;
+	case 0xe3:
+		EP0BCH = 0;
+		EP0BCL = 0;
+		while (EP0CS & bmEPBUSY);
+		if (EP0BUF[0] == 1)
+			start_sampling();
+		return TRUE;
+	case 0xe4:
+		EP0BCH = 0;
+		EP0BCL = 0;
+		while (EP0CS & bmEPBUSY);
+		set_numchannels(EP0BUF[0]);
+		return TRUE;
+	}
+
+	return FALSE; /* Not handled by handlers. */
+}
+
+void main_init(void)
+{
+	EP4CFG = 0;
+	EP8CFG = 0;
+
+	/* In idle mode tristate all outputs. */
+	GPIFIDLECTL = 0x00;
+	GPIFCTLCFG = 0x80;
+	GPIFWFSELECT = 0x00;
+	GPIFREADYSTAT = 0x00;
+
+	stop_sampling();
+
+	set_voltage(0, 1);
+	set_voltage(1, 1);
+	set_samplerate(1);
+	set_numchannels(2);
+	select_interface(0);
 }