diff --git a/dynamixel_base/include/dynamixel_slave.h b/dynamixel_base/include/dynamixel_slave.h
index d19c55cebca0f7822327cb58d8423a4188fc6710..9254389dae21c86327d034ad9c377f0fc58c5c09 100644
--- a/dynamixel_base/include/dynamixel_slave.h
+++ b/dynamixel_base/include/dynamixel_slave.h
@@ -64,17 +64,32 @@ typedef struct TDynamixelSlave
    * \brief
    *
    */
-  unsigned char rx_buffer1[MAX_DYN_SLAVE_RX_BUFFER_LEN];
+  unsigned char rx_buffer[MAX_DYN_SLAVE_RX_BUFFER_LEN];
   /**
    * \brief
    *
    */
-  unsigned char rx_buffer2[MAX_DYN_SLAVE_RX_BUFFER_LEN];
+  unsigned short int rx_buffer_read_ptr;
   /**
    * \brief
    *
    */
-  unsigned char current_rx_buffer;
+  unsigned short int rx_buffer_write_ptr;
+  /**
+   * \brief
+   *
+   */
+  unsigned char rx_state;
+  /**
+   * \brief
+   *
+   */
+  unsigned short int rx_current_length;
+  /**
+   * \brief
+   *
+   */
+  unsigned short int rx_total_length;
   /**
    * \brief
    *
@@ -120,6 +135,11 @@ typedef struct TDynamixelSlave
    *
    */
   unsigned char (*abort_dma)(void *hal_dev);
+  /**
+   * \brief
+   *
+   */
+  unsigned short int (*get_rx_num_data)(void);
 }TDynamixelSlave;
 
 /* public functions */
diff --git a/dynamixel_base/src/dynamixel_slave.c b/dynamixel_base/src/dynamixel_slave.c
index 65d7bc4ddb133cd3f289932be97e7dd49d33a6e9..54937ed1472fd45ffc00ef11415febe5077d963b 100644
--- a/dynamixel_base/src/dynamixel_slave.c
+++ b/dynamixel_base/src/dynamixel_slave.c
@@ -11,6 +11,14 @@ void dyn_slave_send_cb(TDynamixelSlave *slave)
   }
 }
 
+void dyn_slave_receive_cb(TDynamixelSlave *slave)
+{
+  if(slave!=0x00000000)
+  {
+    slave->rx_dma(slave->hal_dev,slave->rx_buffer,MAX_DYN_SLAVE_RX_BUFFER_LEN);
+  }
+}
+
 void dummy_dyn_slave_set_tx_mode(void)
 {
 
@@ -46,6 +54,11 @@ unsigned char dummy_dyn_slave_abort_dma(void *hal_dev)
   return DYN_SUCCESS;
 }
 
+unsigned short int dummy_dyn_slave_get_rx_num_data(void)
+{
+  return 0;
+}
+
 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)
@@ -73,75 +86,164 @@ void dyn_slave_send_status_packet(TDynamixelSlave *slave,unsigned char address,u
   }
 }
 
-unsigned char dyn_slave_check_new_packet(TDynamixelSlave *slave)
+unsigned short int dyn_slave_get_num_data(TDynamixelSlave *slave)
 {
-  unsigned char *buffer;
+  unsigned short int num;
+
+  slave->rx_buffer_write_ptr=slave->get_rx_num_data();
+
+  if(slave->rx_buffer_write_ptr>=slave->rx_buffer_read_ptr)
+    num=slave->rx_buffer_write_ptr-slave->rx_buffer_read_ptr;
+  else
+    num=(MAX_DYN_SLAVE_RX_BUFFER_LEN-slave->rx_buffer_read_ptr)+slave->rx_buffer_write_ptr;
+
+  return num;
+}
+
+void dyn_slave_increment_rx_ptr(TDynamixelSlave *slave)
+{
+  if(slave->rx_buffer_read_ptr==(MAX_DYN_SLAVE_RX_BUFFER_LEN-1))
+    slave->rx_buffer_read_ptr=0;
+  else
+    slave->rx_buffer_read_ptr++;
+}
+
+unsigned char dyn_slave_get_next_packet(TDynamixelSlave *slave,unsigned char *packet)
+{
+  unsigned char byte,new_packet=0x00;
+  unsigned short int i,num=0;
 
   if(slave!=0x00000000)
   {
-    if(slave->current_rx_buffer==0)
-      buffer=slave->rx_buffer1;
-    else
-      buffer=slave->rx_buffer2;
-    if(slave->version==DYN_VER1)
+    num=dyn_slave_get_num_data(slave);
+/*  
+    if(num==0)
     {
-      if(buffer[0]!=0xFF) return 0x00;
-      if(buffer[1]!=0xFF) return 0x00;
-      if(dyn_check_checksum(buffer)!=0xFF) return 0x00;
-      return 0x01;
+      if(time_is_timeout(&slave->time))
+      {
+        slave->rx_state=0;
+        slave->rx_current_length=0;
+        slave->rx_total_length=0;
+      }
     }
     else
+      time_set_timeout(&slave->time,slave->rx_timeout_ms*1000);
+*/
+    for(i=0;i<num;i++)
     {
-      if(buffer[0]!=0xFF) return 0x00;
-      if(buffer[1]!=0xFF) return 0x00;
-      if(buffer[2]!=0xFD) return 0x00;
-      if(dyn2_get_length(buffer)>MAX_DYN_SLAVE_RX_BUFFER_LEN) return 0x00;
-      if(dyn2_check_checksum(buffer)!=0x01) return 0x00;
-      return 0x01;
+      byte=slave->rx_buffer[slave->rx_buffer_read_ptr];
+      switch(slave->rx_state)
+      {
+        case 0: if(byte==0xFF)
+                {
+                  slave->rx_state++;
+                  packet[slave->rx_current_length]=byte;
+                  slave->rx_current_length++;
+                }
+                break;
+        case 1: if(byte==0xFF)
+                {
+                  slave->rx_state++;
+                  packet[slave->rx_current_length]=byte;
+                  slave->rx_current_length++;
+                }
+                else 
+                {
+                  slave->rx_state=0;
+                  slave->rx_current_length=0;
+                }
+                break;
+        case 2: if(byte==0xFD)// version 2 header
+                {
+                  if(slave->version==DYN_VER2)// the module is configured for version 2
+                  {
+                    slave->rx_state++;
+                    packet[slave->rx_current_length]=byte;
+                    slave->rx_current_length++;
+                  }
+                  else
+                  {
+                    slave->rx_state=0;
+                    slave->rx_current_length=0;
+                  }
+                } 
+                else 
+                {
+                  if(byte!=0xFF)
+                  {
+                    slave->rx_state++;
+                    packet[slave->rx_current_length]=byte;
+                    slave->rx_current_length++;
+                  }
+                }
+                break;
+        case 3: packet[slave->rx_current_length]=byte;
+                slave->rx_current_length++;
+                if(slave->version==DYN_VER1)
+                {
+                  slave->rx_total_length=byte;
+                  slave->rx_state=7;
+                }
+                else
+                  slave->rx_state++;
+                break;
+        case 4: packet[slave->rx_current_length]=byte;
+                slave->rx_current_length++;
+                slave->rx_state++;
+                break;
+        case 5: packet[slave->rx_current_length]=byte;
+                slave->rx_current_length++;
+                slave->rx_total_length=byte;
+                slave->rx_state++;
+                break;
+        case 6: packet[slave->rx_current_length]=byte;
+                slave->rx_current_length++;
+                slave->rx_total_length+=(byte<<8);
+                slave->rx_state++;
+                break;
+        case 7: packet[slave->rx_current_length]=byte;
+                slave->rx_current_length++;
+                slave->rx_total_length--;
+                if(slave->rx_total_length==0)
+                {
+                  new_packet=0x01;
+                  slave->rx_state=0;
+                  slave->rx_current_length=0;
+                }
+                break;
+        default: break;
+      }
+      dyn_slave_increment_rx_ptr(slave);
+      if(new_packet)
+        break;
     }
   }
-  else
-    return 0x00;
+
+  return new_packet;
 }
 
 void dyn_slave_loop(TDynamixelSlave *slave)
 {
-  unsigned char send_status=0xFF,error,data[MAX_DYN_SLAVE_TX_BUFFER_LEN];
+  unsigned char send_status=0xFF,error,data_in[MAX_DYN_SLAVE_RX_BUFFER_LEN],data_out[MAX_DYN_SLAVE_RX_BUFFER_LEN];
   unsigned short int length,i;
-  unsigned char *buffer;
 
   if(slave!=0x00000000)
   {
-    if(dyn_slave_check_new_packet(slave))// check if a new instruction packet has been received
+    while(dyn_slave_get_next_packet(slave,data_in))
     {
-      // cancel current DMA transfer
-      slave->abort_dma(slave->hal_dev);
-      // change current_buffer
-      if(slave->current_rx_buffer==0)
-      {
-        buffer=slave->rx_buffer1;
-        slave->current_rx_buffer=1;
-        slave->rx_dma(slave->hal_dev,slave->rx_buffer2,MAX_DYN_SLAVE_RX_BUFFER_LEN);
-      }
-      else
-      {
-        buffer=slave->rx_buffer2;
-        slave->current_rx_buffer=0;
-        slave->rx_dma(slave->hal_dev,slave->rx_buffer1,MAX_DYN_SLAVE_RX_BUFFER_LEN);
-      }
       // 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],buffer,&error,(unsigned char *)&length,data);
+          send_status=dyn_v1_slave_loop(slave->slave_devices[i],data_in,&error,(unsigned char *)&length,data_out);
         else
-          send_status=dyn_v2_slave_loop(slave->slave_devices[i],buffer,&error,&length,data);
+          send_status=dyn_v2_slave_loop(slave->slave_devices[i],data_in,&error,&length,data_out);
         if(send_status==0x01)
-          dyn_slave_send_status_packet(slave,slave->slave_devices[i]->address,error,length,data,slave->slave_devices[i]->return_delay);
+          dyn_slave_send_status_packet(slave,slave->slave_devices[i]->address,error,length,data_out,slave->slave_devices[i]->return_delay);
       }
       if(send_status==0xFF)// packet has not been processed
       {
-        if(slave->on_relay(slave->version,buffer,slave->tx_buffer)==DYN_SUCCESS)
+        if(slave->on_relay(slave->version,data_out,slave->tx_buffer)==DYN_SUCCESS)
         {   
           // set the tx mode, if necessary           
           slave->set_tx_mode();
@@ -154,27 +256,6 @@ void dyn_slave_loop(TDynamixelSlave *slave)
         else
           dyn_slave_send_cb(slave);
       }
-      // erase header of the current buffer
-      for(i=0;i<MAX_DYN_SLAVE_RX_BUFFER_LEN;i++)
-        buffer[i]=0x00;
-    }
-    else
-    {
-      if(time_is_timeout(&slave->time))
-      {
-        if(slave->hal_dev!=0x00000000)
-        {
-          // cancel DMA reception 
-          slave->abort_dma(slave->hal_dev);
-          // enable reception by DMa 
-          if(slave->current_rx_buffer==0)
-            buffer=slave->rx_buffer1;
-          else
-            buffer=slave->rx_buffer2;
-          slave->rx_dma(slave->hal_dev,buffer,MAX_DYN_SLAVE_RX_BUFFER_LEN);
-        }
-        time_set_timeout(&slave->time,slave->rx_timeout_ms*1000);
-      }
     }
   }
 }
@@ -186,7 +267,11 @@ unsigned char dyn_slave_init(TDynamixelSlave *slave,void *hal_dev,TScheduler *sc
 
   slave->hal_dev=hal_dev;
   slave->version=version;
-  slave->current_rx_buffer=0;
+  slave->rx_buffer_read_ptr=0;
+  slave->rx_buffer_write_ptr=0;
+  slave->rx_state=0;
+  slave->rx_current_length=0;
+  slave->rx_total_length=0;
   /* initialize the internal callbacks */
   slave->set_tx_mode=dummy_dyn_slave_set_tx_mode;
   slave->set_rx_mode=dummy_dyn_slave_set_rx_mode;
@@ -217,17 +302,13 @@ unsigned char dyn_slave_init(TDynamixelSlave *slave,void *hal_dev,TScheduler *sc
 
 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;
-    slave->rx_dma(slave->hal_dev,buffer,MAX_DYN_SLAVE_RX_BUFFER_LEN);
+    slave->rx_dma(slave->hal_dev,slave->rx_buffer,MAX_DYN_SLAVE_RX_BUFFER_LEN);
+    slave->rx_buffer_read_ptr=0;
+    slave->rx_buffer_write_ptr=0;
   }
   /* start timeout */
   time_set_timeout(&slave->time,slave->rx_timeout_ms*1000);