diff --git a/dynamixel_base/include/dynamixel_slave.h b/dynamixel_base/include/dynamixel_slave.h
index badb06401d5f55d74a1cb5b9e7f5c76cbf1f28cd..311b35cba0d2705363bedda99b2d6fcebbfff972 100644
--- a/dynamixel_base/include/dynamixel_slave.h
+++ b/dynamixel_base/include/dynamixel_slave.h
@@ -5,7 +5,7 @@
 extern "C" {
 #endif
 
-#include "comm.h"
+#include "stm32_time.h"
 #include "dynamixel.h"
 #include "dynamixel2.h"
 #include "scheduler.h"
@@ -29,21 +29,22 @@ extern "C" {
  */
 typedef struct TDynamixelSlave
 {
+  TTime time;
   /**
    * \brief
    *
    */
-  TScheduler *scheduler;
+  void *hal_dev;
   /**
    * \brief
    *
    */
-  sched_channel_t sch_channel;
+  TScheduler *scheduler;
   /**
    * \brief
    *
    */
-  TComm *comm_dev;
+  sched_channel_t sch_channel;
   /**
    * \brief
    *
@@ -58,32 +59,32 @@ typedef struct TDynamixelSlave
    * \brief
    *
    */
-  unsigned short int received_bytes;
+  unsigned short int rx_timeout_ms;
   /**
    * \brief
    *
    */
-  unsigned short int rx_timeout_ms;
+  unsigned char tx_buffer[MAX_DYN_SLAVE_TX_BUFFER_LEN];
   /**
    * \brief
    *
    */
-  unsigned short int op_length;
+  unsigned char rx_buffer1[MAX_DYN_SLAVE_RX_BUFFER_LEN];
   /**
    * \brief
    *
    */
-  unsigned char tx_buffer[MAX_DYN_SLAVE_TX_BUFFER_LEN];
+  unsigned char rx_buffer2[MAX_DYN_SLAVE_RX_BUFFER_LEN];
   /**
    * \brief
    *
    */
-  unsigned char rx_buffer[MAX_DYN_SLAVE_RX_BUFFER_LEN];
+  unsigned char current_rx_buffer;
   /**
    * \brief
    *
    */
-  unsigned char user_buffer[MAX_DYN_SLAVE_RX_BUFFER_LEN];
+  unsigned char rx_dma_mode;
   /**
    * \brief
    *
@@ -108,12 +109,42 @@ typedef struct TDynamixelSlave
    * \brief
    *
    */
-  void (*set_baudrate)(TComm *comm_dev,unsigned int baudrate);
+  void (*set_baudrate)(void *hal_dev,unsigned int baudrate);
   /**
    * \brief
    *
    */
   unsigned char (*on_relay)(TDynVersion version,unsigned char *inst_pkt,unsigned char *status_pkt);
+  /**
+   * \brief
+   *
+   */
+  unsigned char (*tx_irq)(void *hal_dev, const unsigned char *data, unsigned short int length);
+  /**
+   * \brief
+   *
+   */
+  unsigned char (*rx_irq)(void *hal_dev, const unsigned char *data, unsigned short int length);
+  /**
+   * \brief
+   *
+   */
+  unsigned char (*abort_irq)(void *hal_dev);
+  /**
+   * \brief
+   *
+   */
+  unsigned char (*tx_dma)(void *hal_dev, const unsigned char *data, unsigned short int length);
+  /**
+   * \brief
+   *
+   */
+  unsigned char (*rx_dma)(void *hal_dev, const unsigned char *data, unsigned short int length);
+  /**
+   * \brief
+   *
+   */
+  unsigned char (*abort_dma)(void *hal_dev);
 }TDynamixelSlave;
 
 /* public functions */
@@ -121,7 +152,19 @@ typedef struct TDynamixelSlave
  * \brief
  *
  */
-unsigned char dyn_slave_init(TDynamixelSlave *slave,TComm *dev,TScheduler *scheduler,sched_channel_t ch,TDynVersion version);
+void dyn_slave_receive_cb(TDynamixelSlave *dyn_slave);
+
+/**
+ * \brief
+ *
+ */
+void dyn_slave_send_cb(TDynamixelSlave *slave);
+
+/**
+ * \brief
+ *
+ */
+unsigned char dyn_slave_init(TDynamixelSlave *slave,void *hal_dev,TScheduler *scheduler,sched_channel_t ch,TDynVersion version);
 /**
  * \brief
  *
@@ -134,7 +177,7 @@ unsigned char dyn_slave_add_device(TDynamixelSlave *slave, TDynamixelSlaveDevice
 static inline void dyn_slave_set_baudrate(TDynamixelSlave *slave,unsigned int baudrate)
 {
   if(slave!=0x00000000)
-    slave->set_baudrate(slave->comm_dev,baudrate);
+    slave->set_baudrate(slave->hal_dev,baudrate);
 }
 /**
  * \brief
@@ -160,21 +203,18 @@ static inline TDynVersion dyn_slave_get_version(TDynamixelSlave *slave)
  * \brief
  *
  */
-static inline void dyn_slave_start(TDynamixelSlave *slave)
-{
-  if(slave->scheduler!=0x00000000)
-    scheduler_enable_channel(slave->scheduler,slave->sch_channel);
-}
+void dyn_slave_start(TDynamixelSlave *slave);
+
 /**
  * \brief
  *
  */
-static inline void dyn_slave_stop(TDynamixelSlave *slave)
-{
-  if(slave->scheduler!=0x00000000)
-    scheduler_disable_channel(slave->scheduler,slave->sch_channel);
-}
+void dyn_slave_stop(TDynamixelSlave *slave);
 
+/**
+ * \brief
+ *
+ */
 void dyn_slave_loop(TDynamixelSlave *slave);
 
 #ifdef __cplusplus
diff --git a/dynamixel_base/src/dynamixel_slave.c b/dynamixel_base/src/dynamixel_slave.c
index fce15f76604c0ce607eb31edc98c5d0a818d2e5f..fdb179c35204ac7f575aaac4cb9a60f01caa14e8 100644
--- a/dynamixel_base/src/dynamixel_slave.c
+++ b/dynamixel_base/src/dynamixel_slave.c
@@ -1,141 +1,126 @@
 #include "dynamixel_slave.h"
 
 // communication functions
-unsigned char dyn_slave_irq_receive_cb(TDynamixelSlave *dyn_slave,unsigned char byte)
+void dyn_slave_receive_cb(TDynamixelSlave *slave)
 {
-  if(dyn_slave!=0x00000000)
+  unsigned char *buffer;
+
+  if(slave!=0x00000000)
   {
-    switch(dyn_slave->received_bytes)
+    if(slave->rx_dma_mode==0x01)
     {
-      case 0: if(byte==0xFF)
-              {
-                dyn_slave->rx_buffer[dyn_slave->received_bytes]=byte;
-                dyn_slave->received_bytes++;
-              }
-              if(dyn_slave->comm_dev->time!=0x00000000)
-                time_set_timeout(dyn_slave->comm_dev->time,dyn_slave->rx_timeout_ms*1000);
-              break;
-      case 1: if(byte==0xFF)
-              {
-                dyn_slave->rx_buffer[dyn_slave->received_bytes]=byte;
-                dyn_slave->received_bytes++;
-              }
-              else 
-                dyn_slave->received_bytes--;
-              break;
-      case 2: if(byte==0xFD)// version 2 header
-              {
-                if(dyn_slave->version==DYN_VER2)// the module is configured for version 2
-                {
-                  dyn_slave->rx_buffer[dyn_slave->received_bytes]=byte;
-                  dyn_slave->received_bytes++;
-                }
-                else
-                  dyn_slave->received_bytes=0;// ignore packet and restart synchronization
-              } 
-              else if(byte!=0xFF)
-              {
-                dyn_slave->rx_buffer[dyn_slave->received_bytes]=byte;
-                dyn_slave->received_bytes++;
-              }
-              break;
-      case 3: dyn_slave->rx_buffer[dyn_slave->received_bytes]=byte;
-              if(dyn_slave->version==DYN_VER1)
-              {
-                dyn_slave->op_length=byte;
-                dyn_slave->received_bytes=0;
-                /* finish reception by IRQ */
-                comm_cancel_irq_receive(dyn_slave->comm_dev);
-                /* enable dma RX */
-                comm_receive_dma(dyn_slave->comm_dev,&dyn_slave->rx_buffer[4],dyn_slave->op_length);
-              }
-              else
-                dyn_slave->received_bytes++;
-              break;
-      case 4: dyn_slave->rx_buffer[dyn_slave->received_bytes]=byte;
-              dyn_slave->received_bytes++;
-              break;
-      case 5: dyn_slave->rx_buffer[dyn_slave->received_bytes]=byte;
-              dyn_slave->received_bytes++;
-              dyn_slave->op_length=byte;
-              break;
-      case 6: dyn_slave->rx_buffer[dyn_slave->received_bytes]=byte;
-              dyn_slave->received_bytes++;
-              dyn_slave->op_length+=(byte<<8);
-              dyn_slave->received_bytes=0;
-              /* finish reception by IRQ */
-              comm_cancel_irq_receive(dyn_slave->comm_dev);
-              /* enable dma RX */
-              comm_receive_dma(dyn_slave->comm_dev,&dyn_slave->rx_buffer[7],dyn_slave->op_length);
-              break;
-      default: break;
+      if(slave->current_rx_buffer==0)
+      {
+        buffer=slave->rx_buffer2;
+        slave->current_rx_buffer=1;
+      }
+      else
+      {
+        buffer=slave->rx_buffer1;
+        slave->current_rx_buffer=0;
+      }
+      slave->rx_dma_mode=0x00;
+      if(slave->hal_dev!=0x00000000)
+      {
+        if(slave->version==DYN_VER1)
+          slave->rx_irq(slave->hal_dev,buffer,4);
+        else
+          slave->rx_irq(slave->hal_dev,buffer,7);
+      }
+      time_set_timeout(&slave->time,slave->rx_timeout_ms*1000);
+      slave->packet_ready=0x01;
+    }
+    else
+    {
+      if(slave->current_rx_buffer==0)
+        buffer=slave->rx_buffer1;
+      else
+        buffer=slave->rx_buffer2;
+      slave->rx_dma_mode=0x01;
+      if(slave->version==DYN_VER1)
+      {
+        if(buffer[0]!=0xFF) return;
+        if(buffer[1]!=0xFF) return;
+        if(slave->hal_dev!=0x00000000)
+          slave->rx_dma(slave->hal_dev,&buffer[4],buffer[3]);
+      }
+      else
+      {
+        if(buffer[0]!=0xFF) return;
+        if(buffer[1]!=0xFF) return;
+        if(buffer[2]!=0xFD) return;
+        if(slave->hal_dev!=0x00000000)
+          slave->rx_dma(slave->hal_dev,&buffer[7],buffer[5]+(buffer[6]<<8));
+      }
     }
   }
-
-  return 0x00;
 }
 
-unsigned char dyn_slave_dma_send_cb(TDynamixelSlave *dyn_slave)
+void dyn_slave_send_cb(TDynamixelSlave *slave)
 {
-  if(dyn_slave!=0x00000000)
+  if(slave!=0x00000000)
   {
     // enable tx interrupts
-    dyn_slave->set_rx_mode();  
+    slave->set_rx_mode();  
   }
+}
+
+void dummy_dyn_slave_set_tx_mode(void)
+{
 
-  return 0x00;
 }
 
-unsigned char dyn_slave_dma_receive_cb(TDynamixelSlave *dyn_slave)
+void dummy_dyn_slave_set_rx_mode(void)
 {
-  unsigned short int i,length;
 
-  if(dyn_slave!=0x00000000)
-  {
-    comm_receive_irq(dyn_slave->comm_dev,0);// reenable reception by IRQ
-    if(dyn_slave->comm_dev->time!=0x00000000)
-      time_cancel_timeout(dyn_slave->comm_dev->time);
-    if(dyn_slave->version==DYN_VER1)
-      length=dyn_slave->op_length+4;
-    else
-      length=dyn_slave->op_length+7;
-    for(i=0;i<length;i++)
-      dyn_slave->user_buffer[i]=dyn_slave->rx_buffer[i];
-    dyn_slave->packet_ready=0x01;
-  }
+}
+
+void dummy_dyn_slave_set_baudrate(void *hal_dev,unsigned int baudrate)
+{
 
-  return 0x00;
 }
 
-void dummy_dyn_slave_set_tx_mode(void)
+unsigned char dummy_dyn_slave_on_relay(TDynVersion version,unsigned char *inst_pkt,unsigned char *status_pkt)
 {
+  return DYN_NO_DEVICE;
+}
 
+unsigned char dummy_dyn_slave_tx_irq(void *hal_dev, const unsigned char *data, unsigned short int length)
+{
+  return DYN_SUCCESS;
 }
 
-void dummy_dyn_slave_set_rx_mode(void)
+unsigned char dummy_dyn_slave_rx_irq(void *hal_dev, const unsigned char *data, unsigned short int length)
 {
+  return DYN_SUCCESS;
+}
 
+unsigned char dummy_dyn_slave_tx_dma(void *hal_dev, const unsigned char *data, unsigned short int length)
+{
+  return DYN_SUCCESS;
 }
 
-void dummy_dyn_slave_set_baudrate(TComm *comm_dev,unsigned int baudrate)
+unsigned char dummy_dyn_slave_rx_dma(void *hal_dev, const unsigned char *data, unsigned short int length)
 {
+  return DYN_SUCCESS;
+}
 
+unsigned char dummy_dyn_slave_abort_irq(void *hal_dev)
+{
+  return DYN_SUCCESS;
 }
 
-unsigned char dummy_dyn_slave_on_relay(TDynVersion version,unsigned char *inst_pkt,unsigned char *status_pkt)
+unsigned char dummy_dyn_slave_abort_dma(void *hal_dev)
 {
-  return DYN_NO_DEVICE;
+  return DYN_SUCCESS;
 }
 
 void dyn_slave_send_status_packet(TDynamixelSlave *slave,unsigned char address,unsigned char error,unsigned short int length, unsigned char *data,unsigned char return_delay)
 {
   if(slave!=0x00000000)
   {
-    // cancel any transmission pending 
-    comm_do_dma_send(slave->comm_dev);
     if(return_delay>0)
-      if(slave->comm_dev->time!=0x00000000)
-        time_delay_us(slave->comm_dev->time,return_delay<<1);
+      time_delay_us(&slave->time,return_delay<<1);
     if(slave->version==DYN_VER1)
     {
       // create the status packet
@@ -143,7 +128,7 @@ void dyn_slave_send_status_packet(TDynamixelSlave *slave,unsigned char address,u
       // set the tx mode, if necessary
       slave->set_tx_mode();
       // start transmission by DMA
-      comm_send_dma(slave->comm_dev,slave->tx_buffer,dyn_get_length(slave->tx_buffer)+4);
+      slave->tx_dma(slave->hal_dev,slave->tx_buffer,dyn_get_length(slave->tx_buffer)+4);
     }
     else
     {
@@ -152,7 +137,7 @@ void dyn_slave_send_status_packet(TDynamixelSlave *slave,unsigned char address,u
       // set the tx mode, if necessary
       slave->set_tx_mode();
       // start transmission by DMA
-      comm_send_dma(slave->comm_dev,slave->tx_buffer,dyn2_get_length(slave->tx_buffer)+7);
+      slave->tx_dma(slave->hal_dev,slave->tx_buffer,dyn2_get_length(slave->tx_buffer)+7);
     }
   }
 }
@@ -161,75 +146,89 @@ void dyn_slave_loop(TDynamixelSlave *slave)
 {
   unsigned char i,send_status,error,data[MAX_DYN_SLAVE_TX_BUFFER_LEN];
   unsigned short int length;
+  unsigned char *buffer;
 
   if(slave!=0x00000000)
   {
     if(slave->packet_ready)// check if a new instruction packet has been received
     {
       slave->packet_ready=0x00;
+      if(slave->current_rx_buffer==0)
+        buffer=slave->rx_buffer2;
+      else
+        buffer=slave->rx_buffer1;
       // check address
       for(i=0;i<slave->num_slave_devices;i++)
       {
         if(slave->version==DYN_VER1)
-          send_status=dyn_v1_slave_loop(slave->slave_devices[i],slave->user_buffer,&error,(unsigned char *)&length,data);
+          send_status=dyn_v1_slave_loop(slave->slave_devices[i],buffer,&error,(unsigned char *)&length,data);
         else
-          send_status=dyn_v2_slave_loop(slave->slave_devices[i],slave->user_buffer,&error,&length,data);
+          send_status=dyn_v2_slave_loop(slave->slave_devices[i],buffer,&error,&length,data);
         if(send_status==0x01)
           dyn_slave_send_status_packet(slave,slave->slave_devices[i]->address,error,length,data,slave->slave_devices[i]->return_delay);
       }
       if(send_status==0xFF)// packet has not been processed
       {
-        if(slave->on_relay(slave->version,slave->user_buffer,slave->tx_buffer)==DYN_SUCCESS)
+        if(slave->on_relay(slave->version,buffer,slave->tx_buffer)==DYN_SUCCESS)
         {   
           // set the tx mode, if necessary           
           slave->set_tx_mode();
           // start transmission by DMA
           if(slave->version==DYN_VER1)
-            comm_send_dma(slave->comm_dev,slave->tx_buffer,dyn_get_length(slave->tx_buffer)+4);         
+            slave->tx_dma(slave->hal_dev,slave->tx_buffer,dyn_get_length(slave->tx_buffer)+4);
           else
-            comm_send_dma(slave->comm_dev,slave->tx_buffer,dyn2_get_length(slave->tx_buffer)+7);         
+            slave->tx_dma(slave->hal_dev,slave->tx_buffer,dyn2_get_length(slave->tx_buffer)+7);
         } 
       }
     }
     else
     {
-      if(slave->comm_dev->time!=0x00000000)
+      if(time_is_timeout(&slave->time))
       {
-        if(time_is_timeout(slave->comm_dev->time))
-        { 
+        if(slave->hal_dev!=0x00000000)
+        {
           /* cancel any IRQ or DMA reception */
-          comm_cancel_irq_receive(slave->comm_dev);
-          comm_cancel_dma_receive(slave->comm_dev);
-          slave->received_bytes=0;
+          slave->abort_irq(slave->hal_dev);
+          slave->abort_dma(slave->hal_dev);
           /* enable reception by IRQ */
-          comm_receive_irq(slave->comm_dev,0);
+          if(slave->current_rx_buffer==0)
+            buffer=slave->rx_buffer1;
+          else
+            buffer=slave->rx_buffer2;
+          if(slave->version==DYN_VER1)
+            slave->rx_irq(slave->hal_dev,buffer,4);
+          else
+            slave->rx_irq(slave->hal_dev,buffer,7);
         }
+        time_set_timeout(&slave->time,slave->rx_timeout_ms*1000);
       }
     }
   }
 }
 
 /* public functions */
-unsigned char dyn_slave_init(TDynamixelSlave *slave,TComm *dev,TScheduler *scheduler,sched_channel_t ch,TDynVersion version)
+unsigned char dyn_slave_init(TDynamixelSlave *slave,void *hal_dev,TScheduler *scheduler,sched_channel_t ch,TDynVersion version)
 {
   unsigned char i;
 
-  /* assign communication functions */
-  dev->irq_receive_cb=(unsigned char (*)(void *,unsigned char))dyn_slave_irq_receive_cb;
-  dev->dma_send_cb=(unsigned char (*)(void *))dyn_slave_dma_send_cb;
-  dev->dma_receive_cb=(unsigned char (*)(void *))dyn_slave_dma_receive_cb;
-  slave->comm_dev=dev;
+  slave->hal_dev=hal_dev;
   slave->version=version;
-  dev->data=slave;
+  slave->current_rx_buffer=0;
+  slave->rx_dma_mode=0x00;
   /* initialize the internal callbacks */
   slave->set_tx_mode=dummy_dyn_slave_set_tx_mode;
   slave->set_rx_mode=dummy_dyn_slave_set_rx_mode;
   slave->set_baudrate=dummy_dyn_slave_set_baudrate;
   slave->on_relay=dummy_dyn_slave_on_relay;
+  slave->tx_irq=dummy_dyn_slave_tx_irq;
+  slave->rx_irq=dummy_dyn_slave_rx_irq;
+  slave->abort_irq=dummy_dyn_slave_abort_irq;
+  slave->tx_dma=dummy_dyn_slave_tx_dma;
+  slave->rx_dma=dummy_dyn_slave_rx_dma;
+  slave->abort_dma=dummy_dyn_slave_abort_dma;
   /* initialize internal variables */
   slave->packet_ready=0x00;
   slave->rx_timeout_ms=50;
-  slave->received_bytes=0x00;
   /* initialize slave devices */
   slave->num_slave_devices=0;
   for(i=0;i<MAX_NUM_SLAVE_DEVICES;i++)
@@ -244,12 +243,44 @@ unsigned char dyn_slave_init(TDynamixelSlave *slave,TComm *dev,TScheduler *sched
   slave->scheduler=scheduler;
 
   slave->set_rx_mode();
-  /* enable reception by IRQ */
-  comm_receive_irq(slave->comm_dev,0);
 
   return 0x01;
 }
 
+void dyn_slave_start(TDynamixelSlave *slave)
+{
+  unsigned char *buffer;
+
+  if(slave->scheduler!=0x00000000)
+    scheduler_enable_channel(slave->scheduler,slave->sch_channel);
+  if(slave->hal_dev!=0x00000000)
+  {
+    if(slave->current_rx_buffer==0)
+      buffer=slave->rx_buffer1;
+    else
+      buffer=slave->rx_buffer2;
+    if(slave->version==DYN_VER1)
+      slave->rx_irq(slave->hal_dev,buffer,4);
+    else
+      slave->rx_irq(slave->hal_dev,buffer,7);
+  }
+  /* start timeout */
+  time_set_timeout(&slave->time,slave->rx_timeout_ms*1000);
+}
+
+void dyn_slave_stop(TDynamixelSlave *slave)
+{
+  if(slave->scheduler!=0x00000000)
+    scheduler_disable_channel(slave->scheduler,slave->sch_channel);
+  if(slave->hal_dev!=0x00000000)
+  {
+    slave->abort_irq(slave->hal_dev);
+    slave->abort_dma(slave->hal_dev);
+  }
+  /* cancel timeout */
+  time_cancel_timeout(&slave->time);
+}
+
 unsigned char dyn_slave_add_device(TDynamixelSlave *slave, TDynamixelSlaveDevice *device)
 {
   if(slave!=0x00000000)