#define SPI_IDLE   0x00
#define SPI_WAIT   0x01
#define SPI_BUSY   0x02
#define SPI_DONE   0x03
#define SPI_ERR    0x04

#define CMD_NOP        0x00
#define CMD_CONTINUE   0x01
#define CMD_FINISH     0x02
#define CMD_CLEAR      0x8A

#define CMD_SET_SERVOS 0x10
#define CMD_SET_LED    0x11
#define CMD_SET_LASER  0x12

#define CMD_GET_SERVOS 0x20

struct spi_buffer {
   unsigned int cmd;
   unsigned int length;
   unsigned int checksum;
   unsigned int data[255];
};

volatile unsigned int *pwm=(unsigned int *)0x40000000;
volatile struct spi_buffer *spi_buffer=(struct spi_buffer *)0x50000000;

volatile unsigned int *gpio_set=(unsigned int *)0x20000030;
volatile unsigned int *gpio_clear=(unsigned int *)0x20000040;
volatile unsigned int *gpio_in=(unsigned int *)0x20000050;

volatile unsigned int *counter=(unsigned int *)0x20000060;

int main() {
   int i,length;
   unsigned char cmd;
   unsigned char checksum;
   unsigned char led=0, pulsing_led=0;

   for(i=0;i<24;i++) pwm[i]=0;
   
   while(1) {
      if(((*counter)>>17)&0x100) {
         pulsing_led=((*counter)>>18)&0x7f;
      } else {
         pulsing_led=0x7f-(((*counter)>>18)&0xff);
      }
      if(led>((*counter)&0xff)) { *gpio_set=0x01; } else { *gpio_clear=0x01; }
      if(pulsing_led>((*counter)&0xff)) { *gpio_set=0x02; } else { *gpio_clear=0x02; }
      cmd=spi_buffer->cmd;
      switch(cmd) {
          case CMD_FINISH:
             spi_buffer->cmd=SPI_IDLE;
             break;
         case CMD_CLEAR:
            spi_buffer->cmd=SPI_BUSY;
            spi_buffer->length=0x00;
            spi_buffer->checksum=0x00;
            for(i=1;i<2000;i++) {
               spi_buffer->data[i]=0x00;
            }
            for(i=0;i<24;i++) {
               pwm[i]=0;
            }
            spi_buffer->cmd=SPI_IDLE;
            break;
         case CMD_SET_SERVOS:
            spi_buffer->cmd=SPI_WAIT;
            while(spi_buffer->cmd!=CMD_CONTINUE);
            spi_buffer->cmd=SPI_BUSY;
            checksum=0;
            length=spi_buffer->length;
            for(i=0;i<length;i++) {
               checksum^=spi_buffer->data[i];
            }
            if(checksum != spi_buffer->checksum ) {
               spi_buffer->cmd=SPI_ERR;
            } else {
               for(i=0;i<24;i++) {
                  pwm[i]=(spi_buffer->data[4*i+1]<<8)|spi_buffer->data[4*i];
               }           
               spi_buffer->cmd=SPI_DONE;
            }
            break;
         case CMD_SET_LED:
            spi_buffer->cmd=SPI_WAIT;
            while(spi_buffer->cmd!=CMD_CONTINUE);
            spi_buffer->cmd=SPI_BUSY;
            checksum=0;
            length=spi_buffer->length;
            for(i=0;i<length;i++) {
               checksum^=spi_buffer->data[i];
            }
            if(checksum != spi_buffer->checksum ) {
               spi_buffer->cmd=SPI_ERR;
            } else {
               led=spi_buffer->data[0];
               spi_buffer->cmd=SPI_DONE;
            }
         case CMD_SET_LASER:
            spi_buffer->cmd=SPI_WAIT;
            while(spi_buffer->cmd!=CMD_CONTINUE);
            spi_buffer->cmd=SPI_BUSY;
            checksum=0;
            length=spi_buffer->length;
            for(i=0;i<length;i++) {
               checksum^=spi_buffer->data[i];
            }
            if(checksum != spi_buffer->checksum ) {
               spi_buffer->cmd=SPI_ERR;
            } else {
               if(spi_buffer->data[0]) {
                  *gpio_set=0x04;
               } else {
                  *gpio_clear=0x04;
               }
               spi_buffer->cmd=SPI_DONE;
            }

/*         case CMD_GET_SERVOS:
            spi_buffer->cmd=SPI_BUSY;
            spi_buffer->length=0x00;
            spi_buffer->checksum=0x00;
            for(i=0;i<24;i++) {
               spi_buffer->data[2*i]=pwm[i];
               spi_buffer->data[2*i+1]=pwm[i]>>8;
            }
            spi_buffer->cmd=SPI_DONE;
            break;
*/
         default:
            break;
      }
   }
   return 0;
}