+#include <aversive.h>
+#include <aversive/wait.h>
+#include <stdint.h>
+
+#include "spi_servo.h"
+
+#define BYPASS_ENABLE 14
+#define BYPASS_DISABLE 15
+
+/*
+ * SPI protocol:
+ *
+ * A command is stored on 2 bytes. The first one has its msb to 0, and the
+ * second one to 1. The first received byte contains the command number, and the
+ * msb of the servo value. The second byte contains the lsb of the servo value.
+ *
+ * Commands 0 to NB_SERVO are to set the value of servo.
+ * Command 14 is to enable bypass mode.
+ * Command 15 is to disable bypass mode.
+ */
+static volatile union {
+ uint8_t u8;
+ struct {
+ /* inverted: little endian */
+ uint8_t val_msb:3;
+ uint8_t cmd_num:4;
+ uint8_t zero:1;
+ };
+} byte0;
+
+static volatile union {
+ uint8_t u8;
+ struct {
+ /* inverted: little endian */
+ uint8_t val_lsb:7;
+ uint8_t one:1;
+ };
+} byte1;
+
+void spi_servo_init(void)
+{
+ /* real SS ! */
+ DDRK = 0x2;
+ /* SCK, SS & MOSI */
+ DDRB = 0x7;
+
+ /* remove power reduction on spi */
+ PRR0 &= ~(1 << PRSPI);
+
+ /* Enable SPI, Master, set clock rate fck/64 */
+ SPCR = (1<<SPE)|(1<<MSTR)|(1<<SPR1);
+
+ PORTK |= (1<<1);
+
+ spi_servo_set(BYPASS_DISABLE, 0);
+}
+
+void spi_servo_set(uint8_t num, uint16_t val)
+{
+ byte0.val_msb = val >> 7;
+ byte0.cmd_num = num;
+ byte0.zero = 0;
+ byte1.one = 1;
+ byte1.val_lsb = val;
+
+ PORTK &= ~(1<<1);
+ SPDR = byte0.u8;
+ /* Wait for transmission complete */
+ while(!(SPSR & (1<<SPIF)));
+ PORTK |= (1<<1);
+
+ _delay_loop_1(5);
+ PORTK &= ~(1<<1);
+
+ SPDR = byte1.u8;
+ /* Wait for transmission complete */
+ while(!(SPSR & (1<<SPIF)));
+ PORTK |= (1<<1);
+}
+
+void spi_servo_bypass(uint8_t enable)
+{
+ if (enable)
+ spi_servo_set(BYPASS_ENABLE, 0);
+ else
+ spi_servo_set(BYPASS_DISABLE, 0);
+}