+#if 0
+#define N_SERVO 6
+#define SERVO_NBITS 10
+uint16_t servos[N_SERVO] = { 0x123, 0x234, 0x321, 0x123, 0x234, 0x321 };
+
+int8_t servo2buf(uint8_t *buf, uint8_t len, uint8_t servo_mask,
+ uint8_t pow, uint8_t seq)
+{
+ uint8_t i = 0, num;
+ uint8_t remain_bits;
+ uint8_t servo_bits = 0;
+ uint16_t servo_val;
+ uint8_t tmp;
+
+ if (len < 2)
+ return -1;
+
+ memset(buf, 0, len);
+ buf[i++] = servo_mask;
+ buf[i++] = ((seq & 0x1f) << 3) | (pow & 0x7);
+ remain_bits = 8;
+
+ for (num = 0; num < N_SERVO; num++) {
+ if (!(servo_mask & (1 << num)))
+ continue;
+ servo_val = servos[num];
+ servo_bits = SERVO_NBITS;
+
+ tmp = (servo_val >> (servo_bits - remain_bits));
+ tmp &= ((1 << remain_bits) - 1);
+ if (i >= len)
+ return -1;
+ buf[i++] |= tmp;
+
+ servo_bits = 10 - remain_bits;
+ tmp = servo_val & ((1 << servo_bits) - 1);
+ tmp <<= (8 - servo_bits);
+ if (i >= len)
+ return -1;
+ buf[i] = tmp;
+
+ if (servo_bits == 8) {
+ i++;
+ remain_bits = 8;
+ }
+ else
+ remain_bits = 8 - servo_bits;
+ }
+
+ if (remain_bits != 8)
+ i++;
+
+ return i;
+}
+
+int8_t buf2servo(uint8_t *buf, uint8_t len)
+{
+ uint8_t mask, count = 0;
+ uint8_t num = 0;
+ uint8_t pow, seq;
+ uint16_t val;
+
+ if (len < 2)
+ return -1;
+
+ mask = buf[0];
+ if (mask > 0x3f)
+ return -1;
+ pow = buf[1] & 0x07;
+ seq = buf[1] >> 5;
+
+ for (num = 0; num < N_SERVO; num++) {
+ if ((1<<num) & mask)
+ count++;
+ }
+ switch (count) {
+ case 1: if (len != 4) return -1; break;
+ case 2: if (len != 5) return -1; break;
+ case 3: if (len != 6) return -1; break;
+ case 4: if (len != 7) return -1; break;
+ case 5: if (len != 9) return -1; break;
+ case 6: if (len != 10) return -1; break;
+ default: return -1;
+ }
+
+ for (num = 0; ((1<<num) & mask) == 0; num++) {
+ if (num >= N_SERVO)
+ return 0;
+ }
+
+ val = buf[2];
+ val <<= 2;
+ val |= (buf[3] >> 6);
+
+ for (num++; ((1<<num) & mask) == 0; num++) {
+ if (num >= N_SERVO)
+ return 0;
+ }
+
+ val = buf[3] & 0x3f;
+ val <<= 4;
+ val |= (buf[4] >> 4);
+
+ for (num++; ((1<<num) & mask) == 0; num++) {
+ if (num >= N_SERVO)
+ return 0;
+ }
+
+ val = buf[4] & 0xf;
+ val <<= 6;
+ val |= (buf[5] >> 2);
+
+ for (num++; ((1<<num) & mask) == 0; num++) {
+ if (num >= N_SERVO)
+ return 0;
+ }
+
+ val = buf[5] & 0x3;
+ val <<= 8;
+ val |= (buf[6]);
+
+ for (num++; ((1<<num) & mask) == 0; num++) {
+ if (num >= N_SERVO)
+ return 0;
+ }
+
+ val = buf[7];
+ val <<= 2;
+ val |= (buf[8] >> 6);
+
+ for (num++; ((1<<num) & mask) == 0; num++) {
+ if (num >= N_SERVO)
+ return 0;
+ }
+
+ val = buf[8];
+ val <<= 4;
+ val |= (buf[9] >> 4);
+
+ return 0;
+}
+#endif