+static void spi_send_byte(uint8_t byte)
+{
+ SS_LOW();
+ SPDR = byte;
+ /* Wait for transmission complete (active loop is fine because
+ * the clock is high) */
+ while(!(SPSR & (1<<SPIF)));
+ SS_HIGH();
+}
+
+static void spi_send_one_servo(uint8_t num, uint16_t val)
+{
+ union spi_byte0 byte0;
+ union spi_byte1 byte1;
+
+ byte0.val_msb = val >> 7;
+ if (num < N_SERVO)
+ byte0.cmd_num = num + 1;
+ else
+ byte0.cmd_num = num;
+ byte0.zero = 0;
+ byte1.one = 1;
+ byte1.val_lsb = val;
+
+ /* save the second byte */
+ spi_servo_tx.next_byte = byte1.u8;
+
+ /* send the first byte */
+ spi_send_byte(byte0.u8);
+}
+
+static void decode_rx_servo(union spi_byte0 byte0, union spi_byte1 byte1)
+{
+ uint8_t num;
+ uint16_t val;
+
+ num = byte0.cmd_num - 1;
+ if (num >= N_SERVO)
+ return;
+
+ val = byte0.val_msb;
+ val <<= 7;
+ val |= byte1.val_lsb;
+
+ spi_servo_rx.servo[num] = val;
+}
+
+/* called by the scheduler */
+static void spi_servo_cb(void *arg)
+{
+ uint8_t idx;
+ union spi_byte0 byte0;
+ union spi_byte1 byte1;
+
+ (void)arg;
+
+ /* get the value from the slave */
+ byte0.u8 = SPDR;
+ byte1.u8 = byte0.u8;
+ if (byte0.zero == 0) {
+ spi_servo_rx.prev_byte = byte0.u8;
+ }
+ else {
+ byte0.u8 = spi_servo_rx.prev_byte;
+ decode_rx_servo(byte0, byte1);
+ }
+
+ /* if next byte is set, send it */
+ if (spi_servo_tx.next_byte != 0) {
+ spi_send_byte(spi_servo_tx.next_byte);
+ spi_servo_tx.next_byte = 0;
+ return;
+ }
+
+ /* if there is no updated servo, send 0 and return. */
+ if (spi_servo_tx.cmd_mask == 0) {
+ spi_send_byte(0);
+ return;
+ }
+
+ /* else find it and send it */
+ idx = spi_servo_tx.cur_idx;
+ while (1) {
+ idx++;
+ if (idx == N_SERVO)
+ idx = BYPASS_ENABLE;
+ else if (idx >= sizeof(uint16_t) * 8)
+ idx = 0;
+
+ if (spi_servo_tx.cmd_mask & (1 << (uint16_t)idx))
+ break;
+ }
+
+ spi_send_one_servo(idx, spi_servo_tx.servo[idx]);
+ spi_servo_tx.cmd_mask &= (~(1 << idx));
+ spi_servo_tx.cur_idx = idx;
+}
+