#include "main.h"
#include "cmsis_os.h"
#include "FreeRTOS.h"
#include "task.h"
#include "timers.h"
#include "event_groups.h"
/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "string.h"
#include "stdio.h"
#include "usart.h"
#include "config.h"
#include "rkg.h"
#include "cang.h"
#include "kzq.h"
#include "level.h"
#include "tem.h"
#include "angle.h"
#include "xyf.h"
#include "hdf.h"
#include "bgy.h"
#include "yqhs.h"
#include "iap.h"
#include "stmflash.h"
#include "md5c.h"
//////////////////////////////////////////////////////////////////////////////////
extern void __set_FAULTMASK(uint32_t faultMask);
//////////////////////////////////////////////////////////////////////////////////

iapfun jump2app;
uint16_t iapbuf[1024];

__asm void MSR_MSP(uint32_t addr) 
{
    MSR MSP, r0 			//set Main Stack value
    BX r14
}

int App2_MD5_Check(uint32_t addr,unsigned int all_len)
{
	unsigned char digest[16];
//	unsigned char *md5_ptr=(unsigned char *)(StartMode_Addr+72);
	unsigned int i,update_len;
	MD5_CTX md5c;
	all_len -= 16;
	MD5Init(&md5c);
	for(i=0;i<all_len;)
	{
		if(all_len>(i+512))
			update_len = 512;
		else
			update_len = all_len-i;
		memcpy(iapbuf,(const void *)(addr+i),update_len);
		MD5Update (&md5c, (char *)iapbuf, update_len);
		i += update_len;
		HAL_GPIO_TogglePin(WDI_sp706_kanmemgou_GPIO_Port, WDI_sp706_kanmemgou_Pin);
	}
	MD5Final(digest,&md5c);
	//memcpy(iapbuf,(const void *)(addr+all_len),16);
	for(i=0;i<16;++i)
	{
		if(digest[i]!=*(unsigned char *)(addr+all_len+i))
			break;
	}
	if(i>=16)
		return 1;
	else
		return 0;
}
// appxaddr:应用程序的起始地址
// appbuf:应用程序CODE.
// appsize:应用程序大小(字节).
void iap_write_appbin(uint32_t appxaddr, uint8_t *appbuf, uint32_t appsize)
{
	uint32_t t;
	uint16_t i = 0;
	uint16_t temp;
	uint32_t fwaddr = appxaddr; //当前写入的地址
	uint8_t *dfu = appbuf;
	for (t = 0; t < appsize; t += 2)
	{
		temp = (uint16_t)dfu[1] << 8;
		temp += (uint16_t)dfu[0];
		dfu += 2; //偏移2个字节
		iapbuf[i++] = temp;
		if (i == 1024)
		{
			i = 0;
			HAL_GPIO_TogglePin(WDI_sp706_kanmemgou_GPIO_Port, WDI_sp706_kanmemgou_Pin);
			STMFLASH_Write(fwaddr, iapbuf, 1024);
			fwaddr += 2048; //偏移2048  16=2*8.所以要乘以2.
		}
		
	}
	if (i)
		STMFLASH_Write(fwaddr, iapbuf, i); //将最后的一些内容字节写进去.
}

//跳转到应用程序段
// appxaddr:用户代码起始地址.
void iap_load_app(uint32_t appxaddr)
{
	if (((*(uint32_t *)appxaddr) & 0x2FFE0000) == 0x20000000) //检查栈顶地址是否合法.
	{
		jump2app = (iapfun) * (uint32_t *)(appxaddr + 4); //用户代码区第二个字为程序开始地址(复位地址)
		MSR_MSP(*(uint32_t *)appxaddr);					  //初始化APP堆栈指针(用户代码区的第一个字用于存放栈顶地址)
		jump2app();										  //跳转到APP.
	}
}

/**
 * @bieaf 进行BootLoader的启动
 *
 * @param none
 * @return none
 */
void Start_BootLoader(void)
{
	unsigned int update_flag = 0xAAAAAAAA; ///< 对应bootloader的启动步骤
	unsigned int ModeStart;
	unsigned int i,all_len;
	uint16_t ModeStart1[2];
	HAL_GPIO_TogglePin(WDI_sp706_kanmemgou_GPIO_Port, WDI_sp706_kanmemgou_Pin);
	Flash_ReadBytes(ModeStart1, StartMode_Addr, 2);
	ModeStart = ModeStart1[0];
	ModeStart <<= 16;
	ModeStart |= ModeStart1[1];
	switch (ModeStart) ///< 读取是否启动应用程序 */
	{
	case Startup_Normal: ///< 正常启动 */   //在APP2中可以设定此标志位   使得下次重启之后进入APP1
	{
		; // printf("> Normal start......\r\n");
		break;
	}
	case Startup_Update: /*启动最新的程序 */
		HAL_GPIO_TogglePin(WDI_sp706_kanmemgou_GPIO_Port, WDI_sp706_kanmemgou_Pin);
		for (i = 0, all_len = 0; i < 4; ++i)
		{
				all_len = all_len << 8;
				all_len |= *(unsigned char *)(StartMode_Addr + 68 + i);
		}
		if (App2_MD5_Check(Application_Buffer_Addr,all_len))
			{
				iap_write_appbin(ADD_UPDATE_PROG,(uint8_t *)Application_Buffer_Addr,all_len);
				HAL_GPIO_TogglePin(WDI_sp706_kanmemgou_GPIO_Port, WDI_sp706_kanmemgou_Pin);
				Flash_WriteBytes((uint16_t*)(StartMode_Addr + 68), StartMode_Addr+132, 2);
			}
			Set_App2_Flag();
	case Startup_APP2: /*启动最新的程序 */
		for (i = 0, all_len = 0; i < 4; ++i)
			{
				all_len = all_len << 8;
				all_len |= *(unsigned char *)(StartMode_Addr + 132 + i);
			}
			if ((all_len<0x20000) && App2_MD5_Check(ADD_UPDATE_PROG,all_len))
			{
				__set_FAULTMASK(0);			   //先关闭全局中断
				iap_load_app(ADD_UPDATE_PROG); //执行FLASH APP2代码
			}
		break;
	default: ///< 启动失败
	{
		return;
	}
	}
}

// extern uint8_t ACK_Arr[16];//={'A','C','K'};
// extern uint8_t ERR_Arr[8];//={'E','R','R'};
// extern void Set_Update_Down(void);

uint8_t YmodemID;
uint16_t packIndex, packTotalNum, packIndexbak; //当前包号，总包数
uint16_t crcIAP, crcTerminal, crcIAP1;
uint32_t FileLength_IAP = 0;  //文件长度
uint16_t FileBuffArray[512];  // MD5校验
uint16_t FileBuffArray1[512]; // MD5校验
uint32_t AddrToWrite = Application_Buffer_Addr;
uint32_t DataReadFlash, DataReadFlash1, update_flag;
void Set_Update_Down(void)
{
	uint16_t update_flag1[2];
	update_flag = Startup_Update; ///< 对应bootloader的启动步骤
	update_flag1[0] = (uint16_t)update_flag;
	update_flag1[1] = (uint16_t)(update_flag >> 16);
	Flash_WriteBytes(update_flag1, StartMode_Addr, 2);
	__set_FAULTMASK(1);
	NVIC_SystemReset();
}

void Res_Update_Down(void)
{
	uint16_t update_flag1[2];
	update_flag = Startup_Normal; ///< 对应bootloader的启动步骤
	update_flag1[0] = (uint16_t)update_flag;
	update_flag1[1] = (uint16_t)(update_flag >> 16);
	Flash_WriteBytes(update_flag1, StartMode_Addr, 2);
	__set_FAULTMASK(1);
	NVIC_SystemReset();
}
void Set_App2_Flag(void)
{
	uint16_t update_flag1[2];
	update_flag = Startup_APP2; ///< 对应bootloader的启动步骤
	update_flag1[1] = (uint16_t)update_flag;
	update_flag1[0] = (uint16_t)(update_flag >> 16);
	Flash_WriteBytes(update_flag1, StartMode_Addr, 2);
	//__set_FAULTMASK(1);
	//NVIC_SystemReset();
}
void Process_CMD_IAP_Update(void)
{

	int lenRx1, lenRx2;
	int i;
	uint16_t CP_CNT = 0, j = 0;
	static uint16_t update_frame_num = 0;
	for (i = 0; i < 512; i++)
	{
		FileBuffArray[i] = 0;
		FileBuffArray1[i] = 0;
	}
	packIndex = (USART2_RX_BUF002[5] << 8) + USART2_RX_BUF002[6];
	packTotalNum = (USART2_RX_BUF002[7] << 8) + USART2_RX_BUF002[8];
	lenRx1 = data_lengthU2;
	YmodemID = USART2_RX_BUF002[4]; //只能是01,02
	data_lengthU2 = 0;
	uint8_t ERR_Arr[8] = "error\r\n";
	uint8_t ACK_Arr[16] = "acknowlege";

	HAL_GPIO_TogglePin(WDI_sp706_kanmemgou_GPIO_Port, WDI_sp706_kanmemgou_Pin);
	switch (YmodemID)
	{
	case 0x01: //起始帧与结束帧解析  根据包序号判别该帧为起始帧还是结束帧
		if (packIndex == 1)
		{ //起始帧处理
			FileLength_IAP = (USART2_RX_BUF002[73] << 24) + (USART2_RX_BUF002[74] << 16) + (USART2_RX_BUF002[75] << 8) + (USART2_RX_BUF002[76]);
			crcIAP = LIB_CRC_MODBUS(&USART2_RX_BUF002[4], 133); //
			crcTerminal = (USART2_RX_BUF002[lenRx1 - 5] << 8) + USART2_RX_BUF002[lenRx1 - 6];
			if (crcIAP == crcTerminal)
			{
				for (i = 0; i < 88; ++i)
				{
					if (*(unsigned char *)(StartMode_Addr + i + 4) != USART2_RX_BUF002[9 + i])
						break;
				}
				if (i < 88)
				{
					for (i = 0; i < 44; i++)
					{
						FileBuffArray[i] = (uint16_t)(USART2_RX_BUF002[i * 2 + 10] << 8 | USART2_RX_BUF002[i * 2 + 9]);
					}
					HAL_GPIO_TogglePin(WDI_sp706_kanmemgou_GPIO_Port, WDI_sp706_kanmemgou_Pin);
					Flash_WriteBytes(FileBuffArray, StartMode_Addr + 4, 44);
					HAL_GPIO_TogglePin(WDI_sp706_kanmemgou_GPIO_Port, WDI_sp706_kanmemgou_Pin);
					delay_sys_us(80);
					packIndexbak = 0;
					AddrToWrite = Application_Buffer_Addr;
					HAL_GPIO_TogglePin(WDI_sp706_kanmemgou_GPIO_Port, WDI_sp706_kanmemgou_Pin);
				}
				else
				{
					if(packIndexbak>1)
						packIndexbak-=1;
				}

				{
					static uint16_t txLen;
					for (i = 0; i < 1024; i++)
					{
						USART2_RX_BUF002[i] = 0;
					}
					txLen = sprintf((char *)FileBuffArray, "file=%d\r\n", packIndexbak);
					HAL_GPIO_WritePin(GPIOA, con03_uart2_kongzhiqi_Pin, GPIO_PIN_RESET); //
					delay_sys_us(80);
					HAL_UART_Transmit_IT(&huart2,(char *)FileBuffArray,txLen);		
					while (huart2.gState == HAL_UART_STATE_BUSY_TX)
					{
						osDelay(1);
					}
					delay_sys_us(80);
					delay_sys_us(80);
					HAL_GPIO_WritePin(GPIOA, con03_uart2_kongzhiqi_Pin, GPIO_PIN_SET); //
				}
			}
			else
			{
				for (i = 0; i < 1024; i++)
				{
					USART2_RX_BUF002[i] = 0;
				}
				HAL_GPIO_WritePin(GPIOA, con03_uart2_kongzhiqi_Pin, GPIO_PIN_RESET); //
				delay_sys_us(80);
				HAL_UART_Transmit(&huart2, ERR_Arr, strlen((char *)ERR_Arr), 100);
				delay_sys_us(80);
				HAL_GPIO_WritePin(GPIOA, con03_uart2_kongzhiqi_Pin, GPIO_PIN_SET); //
				__NOP();
			}
			__NOP();
		}
		else
		{
			__NOP();
		}
		break;
	case 0x02:														//有效载荷帧数据处理
		crcIAP = LIB_CRC_MODBUS(&USART2_RX_BUF002[4], lenRx1 - 10); //
		crcTerminal = (USART2_RX_BUF002[lenRx1 - 5] << 8) + USART2_RX_BUF002[lenRx1 - 6];
		if ((crcIAP == crcTerminal) && (((packIndexbak + 1) == packIndex) || ((packIndexbak) == packIndex)))
		{
			if ((packIndexbak + 1) == packIndex)
			{
				lenRx2 = (lenRx1 - 15) % 2;
				if (!lenRx2)
				{
					for (i = 0; i < ((lenRx1 - 15) / 2); i++)
					{
						FileBuffArray[i] = (uint16_t)(USART2_RX_BUF002[i * 2 + 10] << 8 | USART2_RX_BUF002[i * 2 + 9]);
						FileBuffArray1[i] = (uint16_t)(USART2_RX_BUF002[i * 2 + 10] << 8 | USART2_RX_BUF002[i * 2 + 9]);
					}
					Flash_WriteBytes(FileBuffArray, AddrToWrite, ((lenRx1 - 15) / 2));
					HAL_GPIO_TogglePin(WDI_sp706_kanmemgou_GPIO_Port, WDI_sp706_kanmemgou_Pin);
					Flash_ReadBytes(FileBuffArray1, AddrToWrite, ((lenRx1 - 15) / 2));
					HAL_GPIO_TogglePin(WDI_sp706_kanmemgou_GPIO_Port, WDI_sp706_kanmemgou_Pin);
				}
				else
				{
					for (i = 0; i < ((lenRx1 - 16) / 2); i++)
					{
						FileBuffArray[i] = (uint16_t)(USART2_RX_BUF002[i * 2 + 10] << 8 | USART2_RX_BUF002[i * 2 + 9]);
						FileBuffArray1[i] = (uint16_t)(USART2_RX_BUF002[i * 2 + 10] << 8 | USART2_RX_BUF002[i * 2 + 9]);
					}
					FileBuffArray[(lenRx1 - 16) / 2] = USART2_RX_BUF002[(lenRx1 - 16 + 10)] << 8;
					FileBuffArray1[(lenRx1 - 16) / 2] = USART2_RX_BUF002[(lenRx1 - 16 + 10)] << 8;
					HAL_GPIO_TogglePin(WDI_sp706_kanmemgou_GPIO_Port, WDI_sp706_kanmemgou_Pin);
					Flash_WriteBytes(FileBuffArray, AddrToWrite, ((lenRx1 - 15 + 1) / 2));
					HAL_GPIO_TogglePin(WDI_sp706_kanmemgou_GPIO_Port, WDI_sp706_kanmemgou_Pin);
					Flash_ReadBytes(FileBuffArray1, AddrToWrite, ((lenRx1 - 15 + 1) / 2));
					HAL_GPIO_TogglePin(WDI_sp706_kanmemgou_GPIO_Port, WDI_sp706_kanmemgou_Pin);
				}
				AddrToWrite += lenRx1 - 15;
				DataReadFlash = *((uint32_t *)(AddrToWrite));
				DataReadFlash1 = (*(__IO uint32_t *)AddrToWrite);
				HAL_GPIO_TogglePin(WDI_sp706_kanmemgou_GPIO_Port, WDI_sp706_kanmemgou_Pin);
			}
			if (DataReadFlash == DataReadFlash1)
			{
				__NOP();
				HAL_GPIO_TogglePin(WDI_sp706_kanmemgou_GPIO_Port, WDI_sp706_kanmemgou_Pin);
				packIndexbak = packIndex;
			}

			{
				int txLen;
				for (i = 0; i < 1024; i++)
				{
					USART2_RX_BUF002[i] = 0;
				}
				txLen = sprintf((char *)USART2_RX_BUF002, "%s=%d\r\n", ACK_Arr, packIndex);
				HAL_GPIO_WritePin(GPIOA, con03_uart2_kongzhiqi_Pin, GPIO_PIN_RESET); //
				delay_sys_us(80);
				HAL_UART_Transmit_IT(&huart2,USART2_RX_BUF002,txLen);		
				while (huart2.gState == HAL_UART_STATE_BUSY_TX)
				{
					osDelay(1);
				}
				HAL_GPIO_WritePin(GPIOA, con03_uart2_kongzhiqi_Pin, GPIO_PIN_SET); //
			}
			__NOP();
		}
		else
		{
			HAL_GPIO_WritePin(GPIOA, con03_uart2_kongzhiqi_Pin, GPIO_PIN_RESET); //
			delay_sys_us(80);
			HAL_UART_Transmit(&huart2, ERR_Arr, strlen((char *)ERR_Arr), 100);
			delay_sys_us(80);
			HAL_GPIO_WritePin(GPIOA, con03_uart2_kongzhiqi_Pin, GPIO_PIN_SET); //
			__NOP();
		}
		break;
	case 3:
		//	if(packIndex>2)
		{														 //结束帧处理
			int i,all_len;
			crcIAP1 = LIB_CRC_MODBUS(&USART2_RX_BUF002[4], 133); //
			crcTerminal = (USART2_RX_BUF002[lenRx1 - 5] << 8) + USART2_RX_BUF002[lenRx1 - 6];
			if (crcIAP1 == crcTerminal)
			{
				int txLen;
				HAL_GPIO_WritePin(GPIOA, con03_uart2_kongzhiqi_Pin, GPIO_PIN_RESET); //
				delay_sys_us(80);
				txLen = sprintf((char *)USART2_RX_BUF002, "%s\r\n", ACK_Arr);
				HAL_UART_Transmit_IT(&huart2,USART2_RX_BUF002,txLen);		
				while (huart2.gState == HAL_UART_STATE_BUSY_TX)
				{
					osDelay(1);
				}
				HAL_GPIO_WritePin(GPIOA, con03_uart2_kongzhiqi_Pin, GPIO_PIN_SET); //
				for (i = 0, all_len = 0; i < 4; ++i)
				{
					all_len = all_len << 8;
					all_len |= *(unsigned char *)(StartMode_Addr + 68 + i);
				}
			if (1)//App2_MD5_Check(Application_Buffer_Addr,all_len))
			{
				txLen = sprintf((char *)USART2_RX_BUF002, "md5 check=ok\r\n");
				HAL_GPIO_WritePin(GPIOA, con03_uart2_kongzhiqi_Pin, GPIO_PIN_RESET); //
				delay_sys_us(80);
				HAL_UART_Transmit_IT(&huart2,USART2_RX_BUF002,txLen);		
				while (huart2.gState == HAL_UART_STATE_BUSY_TX)
				{
					osDelay(1);
				}
				HAL_GPIO_WritePin(GPIOA, con03_uart2_kongzhiqi_Pin, GPIO_PIN_SET); //
				Set_Update_Down(); //设定标志位  下次启动时进行程序拷贝
			}
			else
			{
				txLen = sprintf((char *)USART2_RX_BUF002,"md5 check=error\r\n");
			}
				HAL_GPIO_WritePin(GPIOA, con03_uart2_kongzhiqi_Pin, GPIO_PIN_RESET); //
				delay_sys_us(80);
				HAL_UART_Transmit_IT(&huart2,USART2_RX_BUF002,txLen);		
				while (huart2.gState == HAL_UART_STATE_BUSY_TX)
				{
					osDelay(1);
				}
				HAL_GPIO_WritePin(GPIOA, con03_uart2_kongzhiqi_Pin, GPIO_PIN_SET); //
			}
			else
			{
				HAL_GPIO_WritePin(GPIOA, con03_uart2_kongzhiqi_Pin, GPIO_PIN_RESET); //
				delay_sys_us(80);
				HAL_UART_Transmit(&huart2, ERR_Arr, strlen((char *)ERR_Arr), 100);
				delay_sys_us(80);
				HAL_GPIO_WritePin(GPIOA, con03_uart2_kongzhiqi_Pin, GPIO_PIN_SET); //
				__NOP();
			}
			__NOP();
		}
		break;
	default:
		break;
	}
}