src/hokuyo/lib/ring_buffer.c

   1 /*!\r
   2   \file\r
   3   \brief \83\8a\83\93\83O\83o\83b\83t\83@\r
   4 \r
   5   \author Satofumi KAMIMURA\r
   6 \r
   7   $Id: ring_buffer.c 1557 2009-12-01 12:38:06Z satofumi $\r
   8 */\r
   9 \r
  10 #include "ring_buffer.h"\r
  11 \r
  12 \r
  13 void ring_initialize(ringBuffer_t *ring, char *buffer, const int shift_length)\r
  14 {\r
  15   ring->buffer = buffer;\r
  16   ring->buffer_size = 1 << shift_length;\r
  17   ring_clear(ring);\r
  18 }\r
  19 \r
  20 \r
  21 void ring_clear(ringBuffer_t *ring)\r
  22 {\r
  23   ring->first = 0;\r
  24   ring->last = 0;\r
  25 }\r
  26 \r
  27 \r
  28 int ring_size(const ringBuffer_t *ring)\r
  29 {\r
  30   int first = ring->first;\r
  31   int last = ring->last;\r
  32 \r
  33   return (last >= first) ? last - first : ring->buffer_size - (first - last);\r
  34 }\r
  35 \r
  36 \r
  37 int ring_capacity(const ringBuffer_t *ring)\r
  38 {\r
  39   return ring->buffer_size - 1;\r
  40 }\r
  41 \r
  42 \r
  43 static void charmove(char *dest, const char *src, int n)\r
  44 {\r
  45   const char *last_p = dest + n;\r
  46   while (dest < last_p) {\r
  47     *dest++ = *src++;\r
  48   }\r
  49 }\r
  50 \r
  51 \r
  52 int ring_write(ringBuffer_t *ring, const char *data, int size)\r
  53 {\r
  54   int free_size = ring_capacity(ring) - ring_size(ring);\r
  55   int push_size = (size > free_size) ? free_size : size;\r
  56 \r
  57   // \83f\81[\83^\94z\92u\r
  58   if (ring->first <= ring->last) {\r
  59     // last \82©\82ç buffer_size \8fI\92[\82Ü\82Å\82É\94z\92u\r
  60     int left_size = 0;\r
  61     int to_end = ring->buffer_size - ring->last;\r
  62     int move_size = (to_end > push_size) ? push_size : to_end;\r
  63 \r
  64     charmove(&ring->buffer[ring->last], data, move_size);\r
  65     ring->last += move_size;\r
  66     ring->last &= (ring->buffer_size -1);\r
  67 \r
  68     left_size = push_size - move_size;\r
  69     if (left_size > 0) {\r
  70       // 0 \82©\82ç first \82Ì\91O\82Ü\82Å\82ð\94z\92u\r
  71       charmove(ring->buffer, &data[move_size], left_size);\r
  72       ring->last = left_size;\r
  73     }\r
  74   } else {\r
  75     // last \82©\82ç first \82Ì\91O\82Ü\82Å\94z\92u\r
  76     charmove(&ring->buffer[ring->last], data, size);\r
  77     ring->last += push_size;\r
  78   }\r
  79   return push_size;\r
  80 }\r
  81 \r
  82 \r
  83 int ring_read(ringBuffer_t *ring, char *buffer, int size)\r
  84 {\r
  85   // \83f\81[\83^\8eæ\93¾\r
  86   int now_size = ring_size(ring);\r
  87   int pop_size = (size > now_size) ? now_size : size;\r
  88 \r
  89   if (ring->first <= ring->last) {\r
  90     charmove(buffer, &ring->buffer[ring->first], pop_size);\r
  91     ring->first += pop_size;\r
  92 \r
  93   } else {\r
  94     // first \82©\82ç buffer_size \8fI\92[\82Ü\82Å\82ð\94z\92u\r
  95     int left_size = 0;\r
  96     int to_end = ring->buffer_size - ring->first;\r
  97     int move_size = (to_end > pop_size) ? pop_size : to_end;\r
  98     charmove(buffer, &ring->buffer[ring->first], move_size);\r
  99 \r
 100     ring->first += move_size;\r
 101     ring->first &= (ring->buffer_size -1);\r
 102 \r
 103     left_size = pop_size - move_size;\r
 104     if (left_size > 0) {\r
 105       // 0 \82©\82ç last \82Ì\91O\82Ü\82Å\82ð\94z\92u\r
 106       charmove(&buffer[move_size], ring->buffer, left_size);\r
 107 \r
 108       ring->first = left_size;\r
 109     }\r
 110   }\r
 111   return pop_size;\r
 112 }\r