Index: os/hal/ports/AVR/MEGA/LLD/TIMv1/hal_pwm_lld.c
===================================================================
--- os/hal/ports/AVR/MEGA/LLD/TIMv1/hal_pwm_lld.c	(revision 11065)
+++ os/hal/ports/AVR/MEGA/LLD/TIMv1/hal_pwm_lld.c	(working copy)
@@ -35,6 +35,10 @@
 /* Driver local definitions.                                                 */
 /*===========================================================================*/
 
+#define TIM1_CS_OFFSET CS10
+#define TIM1_WGM_OFFSET1 WGM10
+#define TIM1_WGM_OFFSET2 WGM12
+
 typedef struct {
   volatile uint8_t *tccra;
   volatile uint8_t *tccrb;
@@ -46,6 +50,7 @@
   volatile uint8_t *ocrcl;
   volatile uint8_t *tifr;
   volatile uint8_t *timsk;
+  volatile uint16_t *icr;
 } timer_registers_t;
 
 static timer_registers_t regs_table[]=
@@ -52,22 +57,28 @@
 {
 #if AVR_PWM_USE_TIM1 || defined(__DOXYGEN__)
 #if defined(OCR1C)
-  {&TCCR1A, &TCCR1B, &OCR1AH, &OCR1AL, &OCR1BH, &OCR1BL, &OCR1CH, &OCR1CL, &TIFR1, &TIMSK1},
+  {&TCCR1A, &TCCR1B, &OCR1AH, &OCR1AL, &OCR1BH, &OCR1BL, &OCR1CH, &OCR1CL,
+   &TIFR1, &TIMSK1, &ICR1},
 #else
-  {&TCCR1A, &TCCR1B, &OCR1AH, &OCR1AL, &OCR1BH, &OCR1BL, NULL, NULL, &TIFR1, &TIMSK1},
+  {&TCCR1A, &TCCR1B, &OCR1AH, &OCR1AL, &OCR1BH, &OCR1BL, NULL, NULL,
+   &TIFR1, &TIMSK1, &ICR1},
 #endif
 #endif
 #if AVR_PWM_USE_TIM2 || defined(__DOXYGEN__)
-  {&TCCR2A, &TCCR2B, &OCR2A, &OCR2A, &OCR2B, &OCR2B, NULL, NULL, &TIFR2, &TIMSK2},
+  {&TCCR2A, &TCCR2B, &OCR2A, &OCR2A, &OCR2B, &OCR2B, NULL, NULL,
+   &TIFR2, &TIMSK2, NULL},
 #endif
 #if AVR_PWM_USE_TIM3 || defined(__DOXYGEN__)
-  {&TCCR3A, &TCCR3B, &OCR3AH, &OCR3AL, &OCR3BH, &OCR3BL, &OCR3CH, &OCR3CL, &TIFR3, &TIMSK3},
+  {&TCCR3A, &TCCR3B, &OCR3AH, &OCR3AL, &OCR3BH, &OCR3BL, &OCR3CH, &OCR3CL,
+   &TIFR3, &TIMSK3, &ICR3},
 #endif
 #if AVR_PWM_USE_TIM4 || defined(__DOXYGEN__)
-  {&TCCR4A, &TCCR4B, &OCR4AH, &OCR4AL, &OCR4CH, &OCR4CL, &OCR4CH, &OCR4CL, &TIFR4, &TIMSK4},
+  {&TCCR4A, &TCCR4B, &OCR4AH, &OCR4AL, &OCR4CH, &OCR4CL, &OCR4CH, &OCR4CL,
+   &TIFR4, &TIMSK4, &ICR4},
 #endif
 #if AVR_PWM_USE_TIM5 || defined(__DOXYGEN__)
-  {&TCCR5A, &TCCR5B, &OCR5AH, &OCR5AL, &OCR5BH, &OCR5BL, &OCR5CH, &OCR5CL, &TIFR5, &TIMSK5},
+  {&TCCR5A, &TCCR5B, &OCR5AH, &OCR5AL, &OCR5BH, &OCR5BL, &OCR5CH, &OCR5CL,
+   &TIFR5, &TIMSK5, &ICR5},
 #endif
 };
 
@@ -306,41 +317,33 @@
 #if AVR_PWM_USE_TIM1 || defined(__DOXYGEN__)
   pwmObjectInit(&PWMD1);
   PWMD1.channels = PWM_CHANNELS;
-  TCCR1A = (1 << WGM11) | (1 << WGM10);
-  TCCR1B = (0 << WGM13) | (1 << WGM12);
 #endif
 
 #if AVR_PWM_USE_TIM2 || defined(__DOXYGEN__)
   pwmObjectInit(&PWMD2);
   PWMD2.channels = PWM_CHANNELS;
-  TCCR2A = (1 << WGM21) | (1 << WGM20);
-  TCCR2B = (0 << WGM22);
 #endif
 
 #if AVR_PWM_USE_TIM3 || defined(__DOXYGEN__)
   pwmObjectInit(&PWMD3);
   PWMD3.channels = PWM_CHANNELS;
-  TCCR3A = (1 << WGM31) | (1 << WGM30);
-  TCCR3B = (0 << WGM33) | (1 << WGM32);
 #endif
 
 #if AVR_PWM_USE_TIM4 || defined(__DOXYGEN__)
   pwmObjectInit(&PWMD4);
   PWMD4.channels = PWM_CHANNELS;
-  TCCR4A = (1 << WGM41) | (1 << WGM40);
-  TCCR4B = (0 << WGM43) | (1 << WGM42);
 #endif
 
 #if AVR_PWM_USE_TIM5 || defined(__DOXYGEN__)
   pwmObjectInit(&PWMD5);
   PWMD5.channels = PWM_CHANNELS;
-  TCCR5A = (1 << WGM51) | (1 << WGM50);
-  TCCR5B = (0 << WGM53) | (1 << WGM52);
 #endif
 }
 
 /**
  * @brief   Configures and activates the PWM peripheral.
+ * @note    We do not use the period value in Timer2 in order to
+ *          be able to use both PWM channels
  *
  * @param[in] pwmp      pointer to the @p PWMDriver object
  *
@@ -349,11 +352,33 @@
 void pwm_lld_start(PWMDriver *pwmp) {
 
   if (pwmp->state == PWM_STOP) {
+    uint8_t cs_value, wgm_value;
 
 #if AVR_PWM_USE_TIM2 || defined(__DOXYGEN__)
     if (pwmp == &PWMD2) {
-      TCCR2B &= ~((1 << CS22) | (1 << CS21));
-      TCCR2B |= (1 << CS20);
+      /* for now only fast pwm is supported */
+      wgm_value = 0x3;
+      cs_value = 1;
+
+      /* period is fixed for timer2 */
+      PWMD2.period = 0xFF;
+
+      /* A prescaler value can only be a suitable power of 2 (1, 8, 32,
+         64, 128 256 or 1024), so we choose the one that makes F_CPU
+         divided by it equal to the given frequency (fallback value is
+         1, to keep compatibility with old code */
+      const uint8_t log_ratio_timer2[] = {0, 3, 5, 6, 7, 8, 10};
+      uint8_t n;
+      for (n=0; n<sizeof(log_ratio_timer2)/sizeof(uint8_t); n++) {
+        if (pwmp->config->frequency == (F_CPU >> log_ratio_timer2[n])) {
+              cs_value = n + 1;
+              break;
+            }
+      }
+
+      TCCR2A = (wgm_value & 0x3) << TIM1_WGM_OFFSET1;
+      TCCR2B = ((cs_value << TIM1_CS_OFFSET) |
+                ((wgm_value >> 2) << TIM1_WGM_OFFSET2));
       if (pwmp->config->callback != NULL)
         TIMSK2 |= (1 << TOIE2);
       return;
@@ -360,12 +385,29 @@
     }
 #endif
 
+    /* for now only fast pwm is supported */
+    wgm_value = 0xE;
+    cs_value = 0x5;
+
+    /* A prescaler value can only be a suitable power of 2 (1, 8, 64,
+       256 or 1024), so we choose the one that makes F_CPU divided by
+       it equal to the given frequency (fallback value is 1024, to
+       keep compatibility with old code */
+    const uint8_t log_ratio_timer1[] = {0, 3, 6, 8, 10};
+    uint8_t n;
+    for (n=0; n<sizeof(log_ratio_timer1)/sizeof(uint8_t); n++) {
+      if (pwmp->config->frequency == (F_CPU >> log_ratio_timer1[n])) {
+        cs_value = n + 1;
+        break;
+      }
+    }
+
     uint8_t i = timer_index(pwmp);
 
-    /* TODO: support other prescaler options */
-
-    *regs_table[i].tccrb &= ~(1 << CS11);
-    *regs_table[i].tccrb |= (1 << CS12) | (1 << CS10);
+    *regs_table[i].icr = pwmp->period;
+    *regs_table[i].tccra = (wgm_value & 0x3) << TIM1_WGM_OFFSET1;
+    *regs_table[i].tccrb = ((cs_value << TIM1_CS_OFFSET) |
+                            ((wgm_value >> 2) << TIM1_WGM_OFFSET2));
     if (pwmp->config->callback != NULL)
       *regs_table[i].timsk = (1 << TOIE1);
   }
@@ -380,8 +422,17 @@
  */
 void pwm_lld_stop(PWMDriver *pwmp) {
 
+#if AVR_PWM_USE_TIM2 || defined(__DOXYGEN__)
+  if (pwmp == &PWMD2) {
+    TCCR2A = 0;
+    TCCR2B = 0;
+    TIMSK2 = 0;
+    return;
+  }
+#endif
   uint8_t i = timer_index(pwmp);
-  *regs_table[i].tccrb &= ~((1 << CS12) | (1 << CS11) | (1 << CS10));
+  *regs_table[i].tccra = 0;
+  *regs_table[i].tccrb = 0;
   *regs_table[i].timsk = 0;
 }
 
@@ -402,6 +453,16 @@
  * @notapi
  */
 void pwm_lld_change_period(PWMDriver *pwmp, pwmcnt_t period) {
+
+#if AVR_PWM_USE_TIM2 || defined(__DOXYGEN__)
+  /* Can't change period in timer2 */
+  if (pwmp == &PWMD2) {
+    PWMD2.period = 0xFF;
+    return;
+  }
+#endif
+  uint8_t i = timer_index(pwmp);
+  *regs_table[i].icr = period;
 }
 
 /**
@@ -423,8 +484,6 @@
                             pwmcnt_t width) {
 
   uint16_t val = width;
-  if (val > MAX_PWM_VALUE)
-    val = MAX_PWM_VALUE;
 
 #if AVR_PWM_USE_TIM2 || defined(__DOXYGEN__)
   if (pwmp == &PWMD2) {
@@ -432,16 +491,16 @@
                    7 - 2*channel,
                    6 - 2*channel,
                    pwmp->config->channels[channel].mode);
-    TIMSK2 |= (1 << (channel + 1));
     /* Timer 2 is 8 bit */
     if (val > 0xFF)
       val = 0xFF;
-    if (pwmp->config->channels[channel].callback) {
-      switch (channel) {
-      case 0: OCR2A = val; break;
-      case 1: OCR2B = val; break;
-      }
+    switch (channel) {
+    case 0: OCR2A = val; break;
+    case 1: OCR2B = val; break;
     }
+    TIFR2 = 1 << (OCF2A + channel);
+    if (pwmp->config->channels[channel].callback)
+      TIMSK2 |= (1 << (OCIE2A + channel));
     return;
   }
 #endif
@@ -467,7 +526,7 @@
   }
   *ocrh = val >> 8;
   *ocrl = val & 0xFF;
-  *regs_table[i].tifr |= (1 << (channel + 1));
+  *regs_table[i].tifr = (1 << (channel + 1));
   if (pwmp->config->channels[channel].callback != NULL)
     *regs_table[i].timsk |= (1 << (channel + 1));
 }
Index: os/hal/ports/AVR/MEGA/LLD/TIMv1/hal_pwm_lld.h
===================================================================
--- os/hal/ports/AVR/MEGA/LLD/TIMv1/hal_pwm_lld.h	(revision 11065)
+++ os/hal/ports/AVR/MEGA/LLD/TIMv1/hal_pwm_lld.h	(working copy)
@@ -69,8 +69,6 @@
   #endif
 #endif
 
-#define MAX_PWM_VALUE           0x3FF
-
 /*===========================================================================*/
 /* Derived constants and error checks.                                       */
 /*===========================================================================*/
@@ -129,7 +127,7 @@
    * @note    The low level can use assertions in order to catch invalid
    *          frequency specifications.
    */
-  uint16_t                  frequency;
+  uint32_t                  frequency;
   /**
    * @brief   PWM period in ticks.
    * @note    The low level can use assertions in order to catch invalid
Index: testhal/AVR/MEGA/PWM/Makefile_nano
===================================================================
--- testhal/AVR/MEGA/PWM/Makefile_nano	(nonexistent)
+++ testhal/AVR/MEGA/PWM/Makefile_nano	(working copy)
@@ -0,0 +1,316 @@
+
+##############################################################################
+#
+# @file   Makefile.
+#
+# @brief  AVR Make file, it can be use to build, and program an application to
+#         an AVR MCU like atmega328p, atmega2560 and so on.
+#
+# @author Theodore Ateba, tf.ateba@gmail.com
+#
+##############################################################################
+
+##############################################################################
+# Building and programming global options.
+# NOTE: Can be overridden externally.
+#
+
+# Compiler options here.
+ifeq ($(USE_OPT),)
+  USE_OPT = -Os
+endif
+
+# C specific options here (added to USE_OPT).
+ifeq ($(USE_COPT),)
+  USE_COPT =
+endif
+
+# C++ specific options here (added to USE_OPT).
+ifeq ($(USE_CPPOPT),)
+  USE_CPPOPT =
+endif
+
+# Enable this if you want to see the full log while compiling.
+ifeq ($(USE_VERBOSE_COMPILE),)
+  USE_VERBOSE_COMPILE = no
+endif
+
+# If enabled, this option makes the build process faster by not compiling
+# modules not used in the current configuration.
+ifeq ($(USE_SMART_BUILD),)
+  USE_SMART_BUILD = yes
+endif
+
+# If enable, this option arase the counter cycle after device programming.
+ifeq ($(USE_AVRDUDE_ERASE_COUNTER),)
+	USE_AVRDUDE_ERASE_COUNTER = no
+endif
+
+# If enable, this option perform a verification after device programming.
+ifeq ($(USE_AVRDUDE_NO_VERIFY),)
+	USE_AVRDUDE_NO_VERIFY = no
+endif
+
+# If enabled, this option increase the programming verbosity level.
+ifeq ($(USE_VERBOSE_PROGRAMMATION),)
+  USE_VERBOSE_PROGRAMMATION = no
+endif
+
+# Enable this if you want to use AVRDUDE programmer.
+ifeq ($(USE_AVRDUDE_PROGRAMMER),)
+  USE_AVRDUDE_PROGRAMMER = yes
+endif
+
+# Enable this if you want to use DFU programmer.
+ifeq ($(USE_DFU_PROGRAMMER),)
+  USE_DFU_PROGRAMMER = no
+endif
+
+# Enable this if you want to use MICRONUCLEUS programmer.
+ifeq ($(USE_MICRONUCLEUS_PROGRAMMER),)
+  USE_MICRONUCLEUS_PROGRAMMER = no
+endif
+
+#
+# Building and programming global options.
+##############################################################################
+
+##############################################################################
+# Project, sources and paths.
+#
+
+# Define project name here.
+PROJECT = ch
+
+# Imported source files and paths
+CHIBIOS = ../../../..
+# HAL-OSAL files (optional).
+include $(CHIBIOS)/os/hal/hal.mk
+include $(CHIBIOS)/os/hal/boards/ARDUINO_NANO/board.mk
+include $(CHIBIOS)/os/hal/ports/AVR/MEGA/ATMEGAxx/platform.mk
+include $(CHIBIOS)/os/hal/osal/rt/osal.mk
+# RTOS files (optional).
+include $(CHIBIOS)/os/rt/rt.mk
+include $(CHIBIOS)/os/common/ports/AVR/compilers/GCC/mk/port.mk
+# Other files (optional).
+include $(CHIBIOS)/os/hal/lib/streams/streams.mk
+
+# List C source files here. (C dependencies are automatically generated.)
+CSRC =  $(KERNSRC)                      \
+        $(PORTSRC)                      \
+        $(OSALSRC)                      \
+        $(HALSRC)                       \
+        $(PLATFORMSRC)                  \
+        $(BOARDSRC)                     \
+        $(STREAMSSRC)                   \
+        $(CHIBIOS)/os/various/evtimer.c \
+        main.c
+
+# List C++ sources file here.
+CPPSRC =
+
+INCDIR =  $(CHIBIOS)/os/license $(PORTINC) $(KERNINC)     \
+          $(HALINC) $(OSALINC) $(PLATFORMINC) $(TESTINC)  \
+          $(BOARDINC) $(STREAMSINC) $(CHIBIOS)/os/various
+
+#
+# Project, sources and paths.
+##############################################################################
+
+##############################################################################
+# Compiler settings.
+#
+
+# Micro-Controller Unit.
+MCU  = atmega328p
+
+# MCU frequency (Hz).
+F_CPU = 16000000
+
+# Output format. (can be srec, ihex, binary)
+FORMAT = ihex
+
+# C and C++ Compiler name.
+TRGT = avr-
+CC   = $(TRGT)gcc
+CPPC = $(TRGT)g++
+
+# Enable loading with g++ only if you need C++ runtime support.
+# NOTE: You can use C++ even without C++ support if you are careful. C++
+#       runtime support makes code size explode.
+LD    = $(TRGT)gcc
+CP    = $(TRGT)objcopy
+AR    = $(TRGT)ar rcs
+OD    = $(TRGT)objdump
+NM    = $(TRGT)nm
+SZ    = $(TRGT)size
+HEX   = $(CP) -O ihex
+BIN   = $(CP) -O binary
+
+# Size of the elf binary file.
+ELFSIZE = $(SZ) --mcu=$(MCU) --format=avr $(BUILDDIR)/$(PROJECT).elf
+
+# MCU specific options here.
+MOPT =
+
+# Define C warning options here.
+CWARN = -Wall -Wstrict-prototypes
+
+# Define C++ warning options here.
+CPPWARN =
+
+#
+# Compiler settings.
+##############################################################################
+
+##############################################################################
+# Start of user section.
+#
+
+# List all user C define here, like -D_DEBUG=1.
+UDEFS =
+
+# Define ASM defines here.
+UADEFS =
+
+# List all user directories here.
+UINCDIR =
+
+# List the user directory to look for the libraries here.
+ULIBDIR =
+
+# List all user libraries here.
+ULIBS =
+
+#
+# End of user defines.
+##############################################################################
+
+##############################################################################
+# Start of programming Options.
+#
+
+# List of available AVR programmer.
+AVRDUDE_PROGRAMMER    = avrdude
+AVRDUDE_PROGRAMMER_ID = arduino
+DFU_PROGRAMMER        = dfu-programmer
+MICRONUCLEUS          = micronucleus
+
+# Set the AVR programmer according to the selection..
+ifeq ($(USE_AVRDUDE_PROGRAMMER),yes)
+	AVR_PROGRAMMER = $(AVRDUDE_PROGRAMMER)
+else ifeq ($(USE_DFU_PROGRAMMER),yes)
+	AVR_PROGRAMMER = $(DFU_PROGRAMMER)
+else ifeq ($(USE_MICRONUCLEUS_PROGRAMMER),yes)
+	AVR_PROGRAMMER = $(MICRONUCLEUS_PROGRAMMER)
+else
+  $(error ERROR: Please you need to configure the AVR programmer!)
+endif
+
+# AVR serial port.
+AVRDUDE_PORT = /dev/ttyUSB0
+
+AVRDUDE_WRITE_FLASH = -D -U flash:w:$(BUILDDIR)/$(PROJECT).hex
+
+# Check if the counter cycle erase must be performed after device programming.
+ifeq ($(USE_AVRDUDE_ERASE_COUNTER),yes)
+	AVRDUDE_ERASE_COUNTER = -y
+endif
+
+# Check if a verification must be performed after device programming.
+ifeq ($(USE_AVRDUDE_NO_VERIFY),no)
+	AVRDUDE_NO_VERIFY = -V
+endif
+
+# Check verbosity level activation.
+ifeq ($(USE_VERBOSE_PROGRAMMATION),yes)
+	AVRDUDE_VERBOSE = -v -v
+endif
+
+# AVR programmer flags for AVRDUDE programmer.
+ifeq ($(AVR_PROGRAMMER),$(AVRDUDE_PROGRAMMER))
+AVRDUDE_FLAGS = -p $(MCU)
+AVRDUDE_FLAGS += -P $(AVRDUDE_PORT)
+AVRDUDE_FLAGS += -b 57600
+AVRDUDE_FLAGS += -c $(AVRDUDE_PROGRAMMER_ID)
+AVRDUDE_FLAGS += $(AVRDUDE_NO_VERIFY)
+AVRDUDE_FLAGS += $(AVRDUDE_VERBOSE)
+AVRDUDE_FLAGS += $(AVRDUDE_ERASE_COUNTER)
+endif
+
+# AVR programmer flags for DFU programmer.
+ifeq ($(AVR_PROGRAMMER),$(DFU_PROGRAMMER))
+DFU_WRITE_FLASH = flash --force
+DFU_ERASE_FLASH = erase
+DFU_RESET=reset
+endif
+
+# AVR programmer flags for MICRONUCLEUS programmer.
+ifeq ($(AVR_PROGRAMMER),$(MICRONUCLEUS_PROGRAMMER))
+MICRONUCLEUS_TIMEOUT_ARG = --timeout 60
+MICRONUCLEUS_RUN_ARG = --run
+MICRONUCLEUS_TYPE_ARG = --type raw
+MICRONUCLEUS_DUMP_PROGRESS = --dump-progress
+MICRONUCLEUS_FLAGS=$(MICRONUCLEUS_TYPE_ARG)
+MICRONUCLEUS_FLAGS+=$(MICRONUCLEUS_TIMEOUT_ARG)
+MICRONUCLEUS_FLAGS+=$(MICRONUCLEUS_RUN_ARG)
+endif
+
+#
+# End of Programming Options.
+##############################################################################
+
+##############################################################################
+# Include file.
+#
+
+RULESPATH = $(CHIBIOS)/os/common/ports/AVR/compilers/GCC
+include $(RULESPATH)/rules.mk
+
+#
+# End of include file.
+##############################################################################
+
+##############################################################################
+# Programming rules
+#
+
+# AVRDUDE programming rules.
+ifeq ($(AVR_PROGRAMMER),$(AVRDUDE_PROGRAMMER))
+program: $(BUILDDIR)/$(PROJECT).hex
+	@echo
+	@echo Programming $(MCU) device.
+	$(AVR_PROGRAMMER) $(AVRDUDE_FLAGS) $(AVRDUDE_WRITE_FLASH) $<
+	@echo Done.
+endif
+
+# DFU programming rules.
+ifeq ($(AVR_PROGRAMMER),$(DFU_PROGRAMMER))
+program: $(BUILDDIR)/$(PROJECT).hex
+	@echo
+	@echo Programming $(MCU) device.
+	$(AVR_PROGRAMMER) $(MCU) $(DFU_WRITE_FLASH) $<
+	$(AVR_PROGRAMMER) $(MCU) $(DFU_RESET)
+	@echo Done.
+
+erase:
+	@echo
+	@echo Erasing $(MCU) device.
+	$(AVR_PROGRAMMER) $(MCU) $(DFU_ERASE_FLASH)
+	@echo Done.
+endif
+
+# MICRONUCLEUS programming rules.
+ifeq ($(AVR_PROGRAMMER),$(MICRONUCLEUS_PROGRAMMER))
+program: $(BUILDDIR)/$(PROJECT).bin
+	@echo
+	@echo Programming $(MCU) device.
+	$(AVR_PROGRAMMER) $(MICRONUCLEUS_FLAGS) $<
+	@echo Done.
+endif
+
+#
+# End of programming rules.
+##############################################################################
+
+# EOF
Index: testhal/AVR/MEGA/PWM/chconf.h
===================================================================
--- testhal/AVR/MEGA/PWM/chconf.h	(revision 11065)
+++ testhal/AVR/MEGA/PWM/chconf.h	(working copy)
@@ -59,7 +59,7 @@
  *          The value one is not valid, timeouts are rounded up to
  *          this value.
  */
-#define CH_CFG_ST_TIMEDELTA                 2
+#define CH_CFG_ST_TIMEDELTA                 0
 
 /** @} */
 
Index: testhal/AVR/MEGA/PWM/main.c
===================================================================
--- testhal/AVR/MEGA/PWM/main.c	(revision 11065)
+++ testhal/AVR/MEGA/PWM/main.c	(working copy)
@@ -22,6 +22,9 @@
 #include "ch.h"
 #endif
 
+#define PERIOD_VALUE1 0x7FFF
+#define PERIOD_VALUE2 0xFF
+
 #ifdef _NIL_
 THD_WORKING_AREA(waThread1, 128);
 THD_FUNCTION(Thread1, arg) {
@@ -45,9 +48,53 @@
    * more can be done in this thread so we first initialize PWM subsystem.
    */
 
+#if AVR_PWM_USE_TIM1 && PWM_CHANNELS == 2
+  static PWMConfig pwm1cfg = {
+    F_CPU,
+    PERIOD_VALUE1,
+    NULL,
+    {
+      {PWM_OUTPUT_ACTIVE_HIGH, NULL},
+      {PWM_OUTPUT_ACTIVE_HIGH, NULL}
+    },
+  };
+
+  /* PB1-2 are timer 1 pwm channel outputs for ATMega328p */
+  palSetGroupMode(IOPORT2, 0x3, 1, PAL_MODE_OUTPUT_PUSHPULL);
+
+  pwmStart(&PWMD1, &pwm1cfg);
+
+  /* channel 0 with 75% duty cycle and 1 with 25% */
+  pwmEnableChannel(&PWMD1, 0, (PERIOD_VALUE1 >> 2)*3);
+  pwmEnableChannel(&PWMD1, 1, PERIOD_VALUE1 >> 2);
+#endif
+
+#if AVR_PWM_USE_TIM2
+  static PWMConfig pwm2cfg = {
+    F_CPU >> 5,
+    PERIOD_VALUE2,
+    NULL,
+    {
+      {PWM_OUTPUT_ACTIVE_HIGH, NULL},
+      {PWM_OUTPUT_ACTIVE_HIGH, NULL}
+    },
+  };
+
+  /* PB3 and PD3 are timer 2 pwm channel outputs for ATMega328p */
+  palSetPadMode(IOPORT2, 3, PAL_MODE_OUTPUT_PUSHPULL);
+  palSetPadMode(IOPORT4, 3, PAL_MODE_OUTPUT_PUSHPULL);
+
+  pwmStart(&PWMD2, &pwm2cfg);
+
+  /* channel 0 with 80% duty cycle and 1 with 20% */
+  pwmEnableChannel(&PWMD2, 0, PERIOD_VALUE2/5*4);
+  pwmEnableChannel(&PWMD2, 1, PERIOD_VALUE2/5);
+#endif
+
+#if AVR_PWM_USE_TIM3
   static PWMConfig pwm3cfg = {
-    1023,   /* Not real clock */
-    1023,   /* Maximum PWM count */
+    F_CPU,
+    PERIOD_VALUE1,
     NULL,
     {
       {PWM_OUTPUT_ACTIVE_HIGH, NULL},
@@ -62,11 +109,11 @@
   palSetPadMode(IOPORT5, 5, PAL_MODE_OUTPUT_PUSHPULL);
 
   pwmStart(&PWMD3, &pwm3cfg);
-
   /* channel 0 with 50% duty cycle, 1 with 25% and 2 with 75% */
-  pwmEnableChannel(&PWMD3, 0, 511);
-  pwmEnableChannel(&PWMD3, 1, 255);
-  pwmEnableChannel(&PWMD3, 2, 767);
+  pwmEnableChannel(&PWMD3, 0, PERIOD_VALUE1 >> 1);
+  pwmEnableChannel(&PWMD3, 1, PERIOD_VALUE1 >> 2);
+  pwmEnableChannel(&PWMD3, 2, (PERIOD_VALUE1 >> 2)*3);
+#endif
 
   chSysInit();
 
Index: testhal/AVR/MEGA/PWM/mcuconf.h
===================================================================
--- testhal/AVR/MEGA/PWM/mcuconf.h	(revision 11065)
+++ testhal/AVR/MEGA/PWM/mcuconf.h	(working copy)
@@ -66,9 +66,9 @@
 /*
  * PWM driver system settings.
  */
-#define AVR_PWM_USE_TIM1                   FALSE
-#define AVR_PWM_USE_TIM2                   FALSE
-#define AVR_PWM_USE_TIM3                   TRUE
+#define AVR_PWM_USE_TIM1                   TRUE
+#define AVR_PWM_USE_TIM2                   TRUE
+#define AVR_PWM_USE_TIM3                   FALSE
 #define AVR_PWM_USE_TIM4                   FALSE
 #define AVR_PWM_USE_TIM5                   FALSE
 
