/**
 ******************************************************************************
 *
 * @file       ahrs_spi_program_slave.c
 * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
 * @brief      AHRS programming over SPI link - slave(AHRS) end.
 *
 * @see        The GNU Public License (GPL) Version 3
 *
 *****************************************************************************/
/*
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
 * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
 * for more details.
 *
 * You should have received a copy of the GNU General Public License along
 * with this program; if not, write to the Free Software Foundation, Inc.,
 * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
 */

#include <stdint.h>
#include <string.h>

#include "pios_opahrs_proto.h"
#include "pios_spi.h"
#include "STM32103CB_AHRS.h"

#include "ahrs_bl.h"
#include "ahrs_spi_program_slave.h"
#include "ahrs_spi_program.h"

static AhrsProgramPacket txBuf;
static AhrsProgramPacket rxBuf;
static bool done = false;

static void ProcessPacket();

#define WAIT_IF_RECEIVING() while(!(GPIOB->IDR & GPIO_Pin_12)){}; //NSS must be high
//Number of crc failures to allow before giving up
#define PROGRAM_PACKET_TRIES 4

void AhrsProgramReceive(uint32_t spi_id) {
	done = false;
	memset(&txBuf, 0, sizeof(AhrsProgramPacket));
	//wait for a program request
	int count = PROGRAM_PACKET_TRIES;
	while (1) {
		WAIT_IF_RECEIVING();
		while ((PIOS_SPI_Busy(spi_id) != 0)) {
		};
		memset(&rxBuf, 'a', sizeof(AhrsProgramPacket));
		int32_t res = PIOS_SPI_TransferBlock(spi_id, NULL, (uint8_t*) &rxBuf,
				SPI_PROGRAM_REQUEST_LENGTH + 1, NULL);

		if (res == 0 && memcmp(&rxBuf, SPI_PROGRAM_REQUEST,
				SPI_PROGRAM_REQUEST_LENGTH) == 0) {
			break;
		}
		if (count-- == 0) {
			return;
		}
	}

	if (!StartProgramming()) {
		//Couldn't erase FLASH. Nothing we can do.
		return;
	}

	//send ack
	memcpy(&txBuf, SPI_PROGRAM_ACK, SPI_PROGRAM_REQUEST_LENGTH);
	WAIT_IF_RECEIVING();
	while (0 != PIOS_SPI_TransferBlock(spi_id, (uint8_t*) &txBuf, NULL,
			SPI_PROGRAM_REQUEST_LENGTH + 1, NULL)) {
	};

	txBuf.type = PROGRAM_NULL;

	while (!done) {
		WAIT_IF_RECEIVING();
		if (0 == PIOS_SPI_TransferBlock(spi_id, (uint8_t*) &txBuf,
				(uint8_t*) &rxBuf, sizeof(AhrsProgramPacket), NULL)) {

			uint32_t crc = GenerateCRC(&rxBuf);
			if (crc != rxBuf.crc || txBuf.packetId == rxBuf.packetId) {
				continue;
			}
			ProcessPacket();
			txBuf.packetId = rxBuf.packetId;
			txBuf.crc = GenerateCRC(&txBuf);
		}
	}
}

void ProcessPacket() {
	switch (rxBuf.type) {
	case PROGRAM_NULL:
		txBuf.type = PROGRAM_NULL;
		break;

	case PROGRAM_WRITE:
		if (WriteData(rxBuf.address, rxBuf.data, rxBuf.size)) {
			txBuf.type = PROGRAM_ACK;
			txBuf.size = rxBuf.size;
		} else {
			txBuf.type = PROGRAM_ERR;
		}
		break;

	case PROGRAM_READ:
		if (ReadData(rxBuf.address, txBuf.data, rxBuf.size)) {
			txBuf.type = PROGRAM_ACK;
			txBuf.size = rxBuf.size;
		} else {
			txBuf.type = PROGRAM_ERR;
		}
		break;

	case PROGRAM_REBOOT:
		if (0 == memcmp(rxBuf.data, REBOOT_CONFIRMATION,
				REBOOT_CONFIRMATION_LENGTH)) {
			done = true;
			txBuf.type = PROGRAM_ACK;
		} else {
			txBuf.type = PROGRAM_ERR;
		}
		break;

	default:
		txBuf.type = PROGRAM_ERR;
	}
}