/**
 * \brief Test interrupt is getting triggered in various Sleep mode.
 *
 * This function put the device in Idle and Power Save sleep mode and check
 * whether the ADC conversion complete interrupt is executed only in Idle sleep
 * mode.
 * The device will wakeup from power save mode when Timer/Counter2 overflow
 * occur.
 *
 * \param test Current test case.
 */
static void run_sleep_trigger_test(const struct test_case *test)
{
    /* Disable Global interrupt */
    cpu_irq_disable();
    /* Initialize the lock counts */
    sleepmgr_init();
    /* Initialize the ADC */
    adc_initialisation();
    /* Initialize the Timer/Counter2 */
    timer2_initialisation();
    /* Lock Idle Sleep mode */
    sleepmgr_lock_mode(SLEEPMGR_IDLE);
    /* Clear Timer/Counter2 Register */
    TCNT2 = 0;
    /* Wait for TCNT2 register to get updated */
    while (ASSR & (1 << TCN2UB)) {
    }
    /* Start ADC Conversion */
    adc_start_conversion();
    /* Enable Global interrupt */
    cpu_irq_enable();
    /* Go to sleep in the deepest allowed mode */
    sleepmgr_enter_sleep();
    /* Unlock Idle Sleep mode */
    sleepmgr_unlock_mode(SLEEPMGR_IDLE);
    /* Lock Power Save mode */
    sleepmgr_lock_mode(SLEEPMGR_PSAVE);
    /* Clear Timer/Counter2 Register */
    TCNT2 = 0;
    /* Wait for TCNT2 register to get updated */
    while (ASSR & (1 << TCN2UB)) {
    }
    /* Start ADC Conversion */
    adc_start_conversion();
    /* Go to sleep in the deepest allowed mode */
    sleepmgr_enter_sleep();
    /* Disable ADC */
    adc_disable();
    /* Unlock Power Save mode */
    sleepmgr_unlock_mode(SLEEPMGR_PSAVE);

    /* Disable Global interrupt */
    cpu_irq_disable();

    test_assert_true(test, trigger_count == 2,
                     "ADC interrupt trigger failed.");
}

/**
 * \brief Enable TC
 *
 * Enables the TC.
 *
 * \param tc Pointer to TC module
 *
 * \note
 * unmask TC clock (sysclk), but does not configure the TC clock source.
 */
void tc_enable(volatile void *tc)
{
	irqflags_t iflags = cpu_irq_save();

#ifdef TCC0
	if ((uintptr_t) tc == (uintptr_t) & TCC0) {
		sysclk_enable_module(SYSCLK_PORT_C, SYSCLK_TC0);
		sysclk_enable_module(SYSCLK_PORT_C, SYSCLK_HIRES);
	} else
#endif
#ifdef TCC1
	if ((uintptr_t) tc == (uintptr_t) & TCC1) {
		sysclk_enable_module(SYSCLK_PORT_C, SYSCLK_TC1);
		sysclk_enable_module(SYSCLK_PORT_C, SYSCLK_HIRES);
	} else
#endif
#ifdef TCD0
	if ((uintptr_t) tc == (uintptr_t) & TCD0) {
		sysclk_enable_module(SYSCLK_PORT_D, SYSCLK_TC0);
		sysclk_enable_module(SYSCLK_PORT_D, SYSCLK_HIRES);
	} else
#endif
#ifdef TCD1
	if ((uintptr_t) tc == (uintptr_t) & TCD1) {
		sysclk_enable_module(SYSCLK_PORT_D, SYSCLK_TC1);
		sysclk_enable_module(SYSCLK_PORT_D, SYSCLK_HIRES);
	} else
#endif
#ifdef TCE0
	if ((uintptr_t) tc == (uintptr_t) & TCE0) {
		sysclk_enable_module(SYSCLK_PORT_E, SYSCLK_TC0);
		sysclk_enable_module(SYSCLK_PORT_E, SYSCLK_HIRES);
	} else
#endif
#ifdef TCE1
	if ((uintptr_t) tc == (uintptr_t) & TCE1) {
		sysclk_enable_module(SYSCLK_PORT_E, SYSCLK_TC1);
		sysclk_enable_module(SYSCLK_PORT_E, SYSCLK_HIRES);
	} else
#endif
#ifdef TCF0
	if ((uintptr_t) tc == (uintptr_t) & TCF0) {
		sysclk_enable_module(SYSCLK_PORT_F, SYSCLK_TC0);
		sysclk_enable_module(SYSCLK_PORT_F, SYSCLK_HIRES);
	} else
#endif
#ifdef TCF1
	if ((uintptr_t) tc == (uintptr_t) & TCF1) {
		sysclk_enable_module(SYSCLK_PORT_F, SYSCLK_TC1);
		sysclk_enable_module(SYSCLK_PORT_F, SYSCLK_HIRES);
	} else
#endif
	{
		cpu_irq_restore(iflags);
		return;
	}
	sleepmgr_lock_mode(SLEEPMGR_IDLE);
	cpu_irq_restore(iflags);
}

/*! \brief Main File Section:
 *          - Initialization (CPU, Controller Task,... )
 *          - Main loop with task management (ADC, DAC, CAN and GUI)
 */
int main(void)
{
	irq_initialize_vectors();

	/* Initialize the board.
	 * The board-specific conf_board.h file contains the configuration of
	 * the board initialization.
	 */
	board_init();

	/* Initialize the clocks.
	 * The clock-specific conf_clocks.h file contains the configuration of
	 * the clocks initialization.
	 */
	sysclk_init();

	// Initialize the sleep manager
	sleepmgr_init();
	sleepmgr_lock_mode(SLEEPMGR_IDLE);

        /* Initialize the required Task.
	 * - Initialize the DAC task to start the signal generator,
	 * - Initialize the ADC task to start the scope acquisition,
	 * - Initialize the Noise Task to add digital noise to the signal,
	 * - Initialize the Filter Task to remove the digital noise of the signal,
	 * - Initialize the GUI Task to display signals as a scope on the LCD,
	 * - Initialize the Remote Task to display signals as a scope on the LCD,
	 */
	dac_task_init();
	adc_task_init();
	noise_task_init();
	filter_task_init();
	gui_task_init();
	controller_task_init();
	remote_task_init();

	cpu_irq_enable();

	// Free running scheduler loop
	while (true) {
		// Enter Sleep Mode
		sleepmgr_enter_sleep();
		// Call ADC task
		adc_task();
		// Call DAC task
		dac_task();
		// Call Noise task
		noise_task();
		// Filter Task
		filter_task();
		// Call Gui task for update
		gui_task();
		// Call Controller Task for control Update
		controller_task();
		// Send data to the PC Application
		remote_task();
	}
}

void app_touch_init(void)
{
#ifdef QTOUCH_STUDIO_MASKS
	SNS_array[0][0] = 0x50;
	SNS_array[0][1] = 0x00;
	SNS_array[1][0] = 0x00;
	SNS_array[1][1] = 0x00;

	SNSK_array[0][0] = 0xA0;
	SNSK_array[0][1] = 0x00;
	SNSK_array[1][0] = 0x00;
	SNSK_array[1][1] = 0x00;
#endif

	/* Configures the sensors as keys and assigns the channel numbers.
	 * The sensor is wired up with SNS=PF6 and SNSK=PF7
	 * When using "pin reconfigurability" this will result in channel 0
	 * because it is the first and only channel that is used.
	 * For the standard qtouch library setup we would need to use
	 * channel 3 since we are using the last two pins on the port.
	 */
	qt_enable_key(CHANNEL_0, NO_AKS_GROUP, 10u, HYST_6_25);
	qt_enable_key(CHANNEL_1, NO_AKS_GROUP, 10u, HYST_6_25);

	qt_init_sensing();

	/* This will fill the default threshold values in the configuration
	 * data structure. But User can change the values of these parameters.
	 */
	qt_config_data.qt_di              = DEF_QT_DI;
	qt_config_data.qt_neg_drift_rate  = DEF_QT_NEG_DRIFT_RATE;
	qt_config_data.qt_pos_drift_rate  = DEF_QT_POS_DRIFT_RATE;
	qt_config_data.qt_max_on_duration = DEF_QT_MAX_ON_DURATION;
	qt_config_data.qt_drift_hold_time = DEF_QT_DRIFT_HOLD_TIME;
	qt_config_data.qt_recal_threshold = DEF_QT_RECAL_THRESHOLD;
	qt_config_data.qt_pos_recal_delay = DEF_QT_POS_RECAL_DELAY;

	/* Initialize the timer counter */
	tc_enable(&TCC0);
	tc_write_period(&TCC0, TIMER_PERIOD);
	tc_write_clock_source(&TCC0, TC_CLKSEL_DIV8_gc);
	tc_set_cca_interrupt_level(&TCC0, PMIC_LVL_LOW);
	tc_set_cca_interrupt_callback(&TCC0, app_touch_tc_interrupt_callback);

	/*
	 * Set up callback function. This function is called after the library
	 * has made capacitive measurements, but before it has processed them.
	 * The user can use this hook to apply filter functions to the measured
	 * signal values.(Possibly to fix sensor layout faults)
	 */
	qt_filter_callback = NULL;

#ifdef _DEBUG_INTERFACE_
	QDebug_Init();
#endif
	sleepmgr_lock_mode(SLEEPMGR_IDLE);
}

/**
 * \brief Enable DMA controller
 *
 * \note This function will do a soft reset of the DMA controller, clearing all
 * previous configuration.
 */
void dma_enable(void)
{
	sysclk_enable_module(SYSCLK_PORT_GEN, SYSCLK_DMA);
	sleepmgr_lock_mode(SLEEPMGR_IDLE);

	/* Reset DMA controller just to make sure everything is from scratch */
	DMA.CTRL = DMA_RESET_bm;
	DMA.CTRL = DMA_ENABLE_bm;
}

/**
 * \brief Enable ADC
 *
 * Enables the ADC and locks IDLE mode for the sleep manager.
 *
 * \param adc Pointer to ADC module
 *
 * \note To ensure accurate conversions, please wait for at least
 * the specified start-up time between enabling the ADC module, and starting
 * a conversion. For most XMEGA devices the start-up time is specified
 * to be a maximum of 24 ADC clock cycles. Please verify the start-up time for
 * the device in use.
 */
void adc_enable(ADC_t *adc)
{
	irqflags_t flags = cpu_irq_save();
	adc_enable_clock(adc);
	adc->CTRLA |= ADC_ENABLE_bm;
	cpu_irq_restore(flags);

	sleepmgr_lock_mode(SLEEPMGR_IDLE);
}

/**
 * \brief Enable DAC
 *
 * Enables the DAC for conversions, and locks the sleep mode needed for
 * the DAC to operate.
 *
 * \param dac Pointer to DAC module.
 */
void dac_enable(DAC_t *dac)
{
	irqflags_t flags;

	flags = cpu_irq_save();
	sleepmgr_lock_mode(SLEEPMGR_IDLE);
	dac_enable_clock(dac);
	dac->CTRLA |= DAC_ENABLE_bm;
	cpu_irq_restore(flags);
}

/**
 * \brief Enable EDMA controller
 *
 * \note Before enabling the EDMA controller, this function will do a soft
 *  reset, clearing all previous configurations.
 *
 * \param channelmode Channel configuration mode given by a EDMA_CHMODE_t type
 */
void edma_enable(EDMA_CHMODE_t channelmode)
{

	sysclk_enable_module(SYSCLK_PORT_GEN, SYSCLK_EDMA);
	sleepmgr_lock_mode(SLEEPMGR_IDLE);

	/* Reset DMA controller just to make sure everything is from scratch */
	EDMA.CTRL = EDMA_RESET_bm;
	EDMA.CTRL = EDMA_ENABLE_bm | channelmode;
}

/*! \brief Authorize or not the CPU powerdown mode
 *
 * \param b_enable   true to authorize powerdown mode
 */
static void udd_sleep_mode(bool b_idle)
{
	if (!b_idle && udd_b_idle) {
		sleepmgr_unlock_mode(USBB_SLEEP_MODE_USB_IDLE);
	}
	if (b_idle && !udd_b_idle) {
		sleepmgr_lock_mode(USBB_SLEEP_MODE_USB_IDLE);
	}
	udd_b_idle = b_idle;
}

/**
 * Lock sleep mode for VBus PIO pin change detection
 */
static void udd_vbus_monitor_sleep_mode(bool b_lock)
{
	if (b_lock && !b_vbus_sleep_lock) {
		b_vbus_sleep_lock = true;
		sleepmgr_lock_mode(SLEEPMGR_SLEEP_WFI);
	}
	if (!b_lock && b_vbus_sleep_lock) {
		b_vbus_sleep_lock = false;
		sleepmgr_unlock_mode(SLEEPMGR_SLEEP_WFI);
	}
}

void ui_init(void)
{
	sleepmgr_lock_mode(SLEEPMGR_SLEEP_WFI);
	/* Set handler for push button */
	pio_handler_set(WAKEUP_PIO, WAKEUP_PIO_ID, WAKEUP_PIO_MASK,
		WAKEUP_PIO_ATTR, ui_wakeup_handler);
	/* Enable IRQ for button */
	NVIC_EnableIRQ((IRQn_Type) WAKEUP_PIO_ID);
	/* Initialize LED */
	LED_Off(LED0);
}

/**
 * \brief Enable the GLOC module.
 *
 * \param dev_inst Device structure pointer.
 *
 */
void gloc_enable(struct gloc_dev_inst *const dev_inst)
{
	struct genclk_config gencfg;

	sysclk_enable_peripheral_clock(dev_inst->hw_dev);
	sleepmgr_lock_mode(SLEEPMGR_SLEEP_0);
	genclk_config_defaults(&gencfg, GLOC_GCLK_NUM);
	genclk_enable_source(CONFIG_GLOC_GENCLK_SRC);
	genclk_config_set_source(&gencfg, CONFIG_GLOC_GENCLK_SRC);
	genclk_config_set_divider(&gencfg, CONFIG_GLOC_GENCLK_DIV);
	genclk_enable(&gencfg, GLOC_GCLK_NUM);
}

/**
 * \brief Initialize the RTC
 *
 * Start up the RTC and start counting from 0
 *
 * \note The RTC clock source used by the RTC module should be set up before
 *       calling this function. 
 */
void rtc_init(void)
{
	sysclk_enable_module(SYSCLK_PORT_GEN, SYSCLK_RTC);
	RTC.PER = 0xffff;
	RTC.CNT = 0;
	/* Since overflow interrupt is needed all the time we limit sleep to
	 * power-save.
	 */
	sleepmgr_lock_mode(SLEEPMGR_PSAVE);
	RTC.INTCTRL = RTC_OVERFLOW_INT_LEVEL;
	RTC.CTRL = CONFIG_RTC_PRESCALER;
}

/**
 * \brief Enable the AES.
 *
 * \param dev_inst Device structure pointer.
 *
 */
void aes_enable(struct aes_dev_inst *const dev_inst)
{
	struct genclk_config gencfg;

	sysclk_enable_peripheral_clock(dev_inst->hw_dev);
	sleepmgr_lock_mode(SLEEPMGR_SLEEP_0);
	dev_inst->hw_dev->AESA_CTRL = AESA_CTRL_ENABLE;
	genclk_config_defaults(&gencfg, AESA_GCLK_NUM);
	genclk_enable_source(CONFIG_AESA_GENERIC_SRC);
	genclk_config_set_source(&gencfg, CONFIG_AESA_GENERIC_SRC);
	genclk_config_set_divider(&gencfg, CONFIG_AESA_GENERIC_DIV);
	genclk_enable(&gencfg, AESA_GCLK_NUM);
}

void udd_enable(void)
{
	irqflags_t flags;

	flags = cpu_irq_save();

#if SAMG55
	matrix_set_usb_device();
#endif

	// Enable USB hardware
	udd_enable_periph_ck();
	sysclk_enable_usb();
	// Cortex, uses NVIC, no need to register IRQ handler
	NVIC_SetPriority((IRQn_Type) ID_UDP, UDD_USB_INT_LEVEL);
	NVIC_EnableIRQ((IRQn_Type) ID_UDP);

	// Reset internal variables
#if (0!=USB_DEVICE_MAX_EP)
	udd_ep_job_table_reset();
#endif

	// Always authorize asynchronous USB interrupts to exit of sleep mode
	pmc_set_fast_startup_input(PMC_FSMR_USBAL);

#ifndef UDD_NO_SLEEP_MGR
	// Initialize the sleep mode authorized for the USB suspend mode
	udd_b_idle = false;
	sleepmgr_lock_mode(UDP_SLEEP_MODE_USB_SUSPEND);
#endif

#if UDD_VBUS_IO
	/* Initialize VBus monitor */
	udd_vbus_init(udd_vbus_handler);
	udd_vbus_monitor_sleep_mode(true);
	/* Force Vbus interrupt when Vbus is always high
	 * This is possible due to a short timing between a Host mode stop/start.
	 */
	if (Is_udd_vbus_high()) {
		udd_vbus_handler(USB_VBUS_PIO_ID, USB_VBUS_PIO_MASK);
	}
#else
#  ifndef USB_DEVICE_ATTACH_AUTO_DISABLE
	udd_attach();
#  endif
#endif

	cpu_irq_restore(flags);
}

/*! \brief Main function.
 */
int main(void)
{
	uint8_t tx_buf[] = "\n\rHello AVR world ! : ";
	uint8_t i;

	/* Initialize the board.
	 * The board-specific conf_board.h file contains the configuration of
	 * the board initialization.
	 */
	board_init();
	sysclk_init();
	pmic_init();
	cpu_irq_enable();
	sleepmgr_init();
	sleepmgr_lock_mode(SLEEPMGR_STDBY);

	/* USART options. */
	static usart_xmegae_rs232_options_t USART_SERIAL_OPTIONS = {
		.baudrate = USART_SERIAL_EXAMPLE_BAUDRATE,
		.charlength = USART_SERIAL_CHAR_LENGTH,
		.paritytype = USART_SERIAL_PARITY,
		.stopbits = USART_SERIAL_STOP_BIT,
		.start_frame_detection = false,
		.one_wire = false,
		.pec_length = USART_SERIAL_VARIABLE_CHAR_LENGTH,
		.pec_action = USART_PECACT_PERC01_gc,
		.encoding_type = USART_DECTYPE_DATA_gc,
		.encoding_stream = USART_LUTACT_OFF_gc,
	};

	/* Initialize usart driver in RS232 mode */
	usart_xmegae_init_rs232(USART_SERIAL_EXAMPLE, &USART_SERIAL_OPTIONS);
	usart_set_rx_interrupt_level(USART_SERIAL_EXAMPLE, USART_INT_LVL_LO);

	/* Send "message header" */
	for (i = 0; i < sizeof(tx_buf); i++) {
		usart_putchar(USART_SERIAL_EXAMPLE, tx_buf[i]);
		while (!usart_tx_is_complete(USART_SERIAL_EXAMPLE)) {
		}
		usart_clear_tx_complete(USART_SERIAL_EXAMPLE);
	}

	/* Incoming character is process under interrupt
	 * main loop simply enters sleep mode */
	while (true) {
		sleepmgr_enter_sleep();
	}
}

/* Override the default definition of vPortSetupTimerInterrupt() that is weakly
defined in the FreeRTOS Cortex-M3 port layer with a version that configures the
asynchronous timer (AST) to generate the tick interrupt. */
void vPortSetupTimerInterrupt( void )
{
struct ast_config ast_conf;

	/* Ensure the AST can bring the CPU out of sleep mode. */
	sleepmgr_lock_mode( SLEEPMGR_RET );

	/* Ensure the 32KHz oscillator is enabled. */
	if( osc_is_ready( OSC_ID_OSC32 ) == pdFALSE )
	{
		osc_enable( OSC_ID_OSC32 );
		osc_wait_ready( OSC_ID_OSC32 );
	}

	/* Enable the AST itself. */
	ast_enable( AST );

	ast_conf.mode = AST_COUNTER_MODE;  /* Simple up counter. */
	ast_conf.osc_type = AST_OSC_32KHZ;
	ast_conf.psel = 0; /* No prescale so the actual frequency is 32KHz/2. */
	ast_conf.counter = 0;
	ast_set_config( AST, &ast_conf );

	/* The AST alarm interrupt is used as the tick interrupt.  Ensure the alarm
	status starts clear. */
	ast_clear_interrupt_flag( AST, AST_INTERRUPT_ALARM );

	/* Enable wakeup from alarm 0 in the AST and power manager.  */
	ast_enable_wakeup( AST, AST_WAKEUP_ALARM );
	bpm_enable_wakeup_source( BPM, ( 1 << BPM_BKUPWEN_AST ) );

	/* Tick interrupt MUST execute at the lowest interrupt priority. */
	NVIC_SetPriority( AST_ALARM_IRQn, configLIBRARY_LOWEST_INTERRUPT_PRIORITY);
	ast_enable_interrupt( AST, AST_INTERRUPT_ALARM );
	NVIC_ClearPendingIRQ( AST_ALARM_IRQn );
	NVIC_EnableIRQ( AST_ALARM_IRQn );

	/* Automatically clear the counter on interrupt. */
	ast_enable_counter_clear_on_alarm( AST, portAST_ALARM_CHANNEL );

	/* Start with the tick active and generating a tick with regular period. */
	ast_write_alarm0_value( AST, ulAlarmValueForOneTick );
	ast_write_counter_value( AST, 0 );

	/* See the comments where xMaximumPossibleSuppressedTicks is declared. */
	xMaximumPossibleSuppressedTicks = ULONG_MAX / ulAlarmValueForOneTick;
}

/**
 * \brief RTC compare ISR
 *
 * This function updates the sleep mode locks so that the device cycles through
 * the different sleep modes.
 */
static void lowpower_interrupt(void)
{
	static uint8_t state = 0;

	switch (state) {
	/* The device starts out in active mode. Go to Idle. */
	default:
	case 0:
		sleepmgr_unlock_mode(SLEEPMGR_ACTIVE);
		sleepmgr_lock_mode(SLEEPMGR_IDLE);
		++state;
		break;

	/* Go to extended Standby */
	case 1:
		sleepmgr_unlock_mode(SLEEPMGR_IDLE);
		sleepmgr_lock_mode(SLEEPMGR_ESTDBY);
		++state;
		break;

	/* Go to power save */
	case 2:
		sleepmgr_unlock_mode(SLEEPMGR_ESTDBY);
		sleepmgr_lock_mode(SLEEPMGR_PSAVE);
		++state;
		break;

	/* Go to standby */
	case 3:
		sleepmgr_unlock_mode(SLEEPMGR_PSAVE);
		sleepmgr_lock_mode(SLEEPMGR_STDBY);
		++state;
		break;

	/* Go to power down */
	case 4:
		sleepmgr_unlock_mode(SLEEPMGR_STDBY);
		sleepmgr_lock_mode(SLEEPMGR_PDOWN);
		++state;
		break;

	/* Go back to idle */
	case 5:
		sleepmgr_unlock_mode(SLEEPMGR_PDOWN);
		sleepmgr_lock_mode(SLEEPMGR_IDLE);
		state = 1;
		break;
	}
}

/**
 * \brief Test the usart sleepwalking in wait mode.
 */
static void usart_sleepwalking_test_wait(void)
{
	enum sleepmgr_mode current_sleep_mode = SLEEPMGR_WAIT;

	puts("Test in wait mode, press number '0' to '9' to wake up.\r");

	/* Wait for the puts operation to finish. */
	delay_ms(50);

	/* The sleep manage will set the clock to 24MRC in wait mode,
	 * reconfig the divisor */
	usart_set_async_baudrate(CONSOLE_UART, CONF_UART_BAUDRATE, OSC_MAINCK_24M_RC_HZ);

	/* Wait for the clock stable. */
	delay_ms(5);

	/* Set the wakeup condition */
	usart_set_sleepwalking(CONSOLE_UART, '0', true, true, '9');

	/* Enable the sleepwalking in PMC */
	pmc_enable_sleepwalking(CONSOLE_UART_ID);

	/* Enter wait mode. */
	sleepmgr_init();
	sleepmgr_lock_mode(current_sleep_mode);
	sleepmgr_enter_sleep();

	/* Config the divisor to the original setting */
	usart_set_async_baudrate(CONSOLE_UART, CONF_UART_BAUDRATE, sysclk_get_peripheral_hz());

	/* Wait for the clock stable. */
	delay_ms(5);

	puts("A character is received, system wake up.\r\n\r");

}

/** \brief Manages the sleep mode following the USB state
 *
 * \param new_state  New USB state
 */
static void udd_sleep_mode(enum udd_usb_state_enum new_state)
{
	enum sleepmgr_mode sleep_mode[] = {
		SLEEPMGR_ACTIVE,  /* UDD_STATE_OFF (not used) */
		SLEEPMGR_IDLE_2,  /* UDD_STATE_SUSPEND */
		SLEEPMGR_IDLE_1,  /* UDD_STATE_SUSPEND_LPM */
		SLEEPMGR_IDLE_0,  /* UDD_STATE_IDLE */
	};

	static enum udd_usb_state_enum udd_state = UDD_STATE_OFF;

	if (udd_state == new_state) {
		return; // No change
	}
	if (new_state != UDD_STATE_OFF) {
		/* Lock new limit */
		sleepmgr_lock_mode(sleep_mode[new_state]);
	}
	if (udd_state != UDD_STATE_OFF) {
		/* Unlock old limit */
		sleepmgr_unlock_mode(sleep_mode[udd_state]);
	}
	udd_state = new_state;
}