X-Git-Url: http://git.linex4red.de/pub/USBasp.git/blobdiff_plain/7e74bd3ea2f8de6c6a78fc8e2527c806b2dd8476..f9c0357b731e5ca4a5456056bbbe2e4a8a2dc9fe:/Projects/Incomplete/AVRISP/Lib/V2Protocol.c

diff --git a/Projects/Incomplete/AVRISP/Lib/V2Protocol.c b/Projects/Incomplete/AVRISP/Lib/V2Protocol.c
index 2feefc8db..7bb8f71ca 100644
--- a/Projects/Incomplete/AVRISP/Lib/V2Protocol.c
+++ b/Projects/Incomplete/AVRISP/Lib/V2Protocol.c
@@ -58,6 +58,14 @@ void V2Protocol_ProcessCommand(void)
 		case CMD_LEAVE_PROGMODE_ISP:
 			V2Protocol_Command_LeaveISPMode();
 			break;
+		case CMD_PROGRAM_FLASH_ISP:
+		case CMD_PROGRAM_EEPROM_ISP:
+			V2Protocol_Command_ProgramMemory(V2Command);			
+			break;
+		case CMD_READ_FLASH_ISP:
+		case CMD_READ_EEPROM_ISP:
+			V2Protocol_Command_ReadMemory(V2Command);
+			break;
 		case CMD_CHIP_ERASE_ISP:
 			V2Protocol_Command_ChipErase();
 			break;
@@ -185,13 +193,14 @@ static void V2Protocol_Command_EnterISPMode(void)
 
 	V2Protocol_DelayMS(Enter_ISP_Params.ExecutionDelayMS);	  
 	SPI_Init(V2Protocol_GetSPIPrescalerMask() | SPI_SCK_LEAD_RISING | SPI_SAMPLE_LEADING | SPI_MODE_MASTER);
-	V2Protocol_ChangeTargetResetLine(true);
-	V2Protocol_DelayMS(Enter_ISP_Params.PinStabDelayMS);
 		
 	while (Enter_ISP_Params.SynchLoops-- && (ResponseStatus == STATUS_CMD_FAILED))
 	{
 		uint8_t ResponseBytes[4];
 
+		V2Protocol_ChangeTargetResetLine(true);
+		V2Protocol_DelayMS(Enter_ISP_Params.PinStabDelayMS);
+
 		for (uint8_t RByte = 0; RByte < sizeof(ResponseBytes); RByte++)
 		{
 			V2Protocol_DelayMS(Enter_ISP_Params.ByteDelay);
@@ -200,7 +209,14 @@ static void V2Protocol_Command_EnterISPMode(void)
 		
 		/* Check if polling disabled, or if the polled value matches the expected value */
 		if (!Enter_ISP_Params.PollIndex || (ResponseBytes[Enter_ISP_Params.PollIndex - 1] == Enter_ISP_Params.PollValue))
-		  ResponseStatus = STATUS_CMD_OK;
+		{
+			ResponseStatus = STATUS_CMD_OK;
+		}
+		else
+		{
+			V2Protocol_ChangeTargetResetLine(false);
+			V2Protocol_DelayMS(Enter_ISP_Params.PinStabDelayMS);
+		}
 	}
 
 	Endpoint_Write_Byte(CMD_ENTER_PROGMODE_ISP);
@@ -231,6 +247,106 @@ static void V2Protocol_Command_LeaveISPMode(void)
 	Endpoint_ClearIN();
 }
 
+static void V2Protocol_Command_ProgramMemory(uint8_t V2Command)
+{
+	struct
+	{
+		uint16_t BytesToWrite;
+		uint8_t  ProgrammingMode;
+		uint8_t  DelayMS;
+		uint8_t  ProgrammingCommands[3];
+		uint8_t  PollValue1;
+		uint8_t  PollValue2;
+	} Write_Memory_Params;
+	
+	uint8_t ProgrammingStatus = STATUS_CMD_OK;
+	
+	Endpoint_Read_Stream_LE(&Write_Memory_Params, sizeof(Write_Memory_Params));
+	Write_Memory_Params.BytesToWrite = SwapEndian_16(Write_Memory_Params.BytesToWrite);
+	
+	if (Write_Memory_Params.ProgrammingMode & PROG_MODE_PAGED_WRITES_MASK)
+	{
+		for (uint16_t CurrentByte = 0; CurrentByte < Write_Memory_Params.BytesToWrite; CurrentByte++)
+		{
+			if ((V2Command == CMD_READ_FLASH_ISP) && (CurrentByte & 0x01))
+			  Write_Memory_Params.ProgrammingCommands[0] ^= READ_WRITE_ODD_BYTE_MASK;
+			  
+			SPI_SendByte(Write_Memory_Params.ProgrammingCommands[0]);
+			SPI_SendByte(CurrentAddress >> 8);
+			SPI_SendByte(CurrentAddress & 0xFF);
+			SPI_SendByte(Endpoint_Read_Byte());
+			
+			// TODO - Correct Polling
+
+			if (((V2Command == CMD_PROGRAM_FLASH_ISP) && (CurrentByte & 0x01)) || (V2Command == CMD_PROGRAM_EEPROM_ISP))
+			  CurrentAddress++;
+		}
+		
+		/* If the current page must be committed, send the PROGRAM PAGE command to the target */
+		if (Write_Memory_Params.ProgrammingMode & PROG_MODE_COMMIT_PAGE_MASK)
+		{
+			SPI_SendByte(Write_Memory_Params.ProgrammingCommands[1]);
+			SPI_SendByte(CurrentAddress >> 8);
+			SPI_SendByte(CurrentAddress & 0xFF);
+			SPI_SendByte(0x00);
+		}
+	}
+	else
+	{			
+		// TODO - Read in programming data, write to device		
+	}
+	
+	Endpoint_ClearOUT();
+	Endpoint_SetEndpointDirection(ENDPOINT_DIR_IN);
+
+	Endpoint_Write_Byte(V2Command);
+	Endpoint_Write_Byte(ProgrammingStatus);
+	
+	Endpoint_ClearIN();
+}
+
+static void V2Protocol_Command_ReadMemory(uint8_t V2Command)
+{
+	struct
+	{
+		uint16_t BytesToRead;
+		uint8_t  ReadMemoryCommand;
+	} Read_Memory_Params;
+	
+	Endpoint_Read_Stream_LE(&Read_Memory_Params, sizeof(Read_Memory_Params));
+	Read_Memory_Params.BytesToRead = SwapEndian_16(Read_Memory_Params.BytesToRead);
+	
+	Endpoint_ClearOUT();
+	Endpoint_SetEndpointDirection(ENDPOINT_DIR_IN);
+	
+	Endpoint_Write_Byte(V2Command);
+	Endpoint_Write_Byte(STATUS_CMD_OK);
+	
+	for (uint16_t CurrentByte = 0; CurrentByte < Read_Memory_Params.BytesToRead; CurrentByte++)
+	{
+		if ((V2Command == CMD_READ_FLASH_ISP) && (CurrentByte & 0x01))
+		  Read_Memory_Params.ReadMemoryCommand ^= READ_WRITE_ODD_BYTE_MASK;
+
+		SPI_SendByte(Read_Memory_Params.ReadMemoryCommand);
+		SPI_SendByte(CurrentAddress >> 8);
+		SPI_SendByte(CurrentAddress & 0xFF);
+		Endpoint_Write_Byte(SPI_ReceiveByte());
+		
+		/* Check if the endpoint bank is currently full */
+		if (!(Endpoint_IsReadWriteAllowed()))
+		{
+			Endpoint_ClearIN();
+			Endpoint_WaitUntilReady();
+		}
+		
+		if (((V2Command == CMD_READ_FLASH_ISP) && (CurrentByte & 0x01)) || (V2Command == CMD_READ_EEPROM_ISP))
+		  CurrentAddress++;
+	}
+
+	Endpoint_Write_Byte(STATUS_CMD_OK);
+	Endpoint_ClearIN();
+}
+
 static void V2Protocol_Command_ChipErase(void)
 {
 	struct
@@ -280,7 +396,7 @@ static void V2Protocol_Command_ReadFuseLockSigOSCCAL(uint8_t V2Command)
 		
 	Endpoint_Write_Byte(V2Command);
 	Endpoint_Write_Byte(STATUS_CMD_OK);
-	Endpoint_Write_Byte(ResponseBytes[Read_FuseLockSigOSCCAL_Params.RetByte]);
+	Endpoint_Write_Byte(ResponseBytes[Read_FuseLockSigOSCCAL_Params.RetByte - 1]);
 	Endpoint_Write_Byte(STATUS_CMD_OK);
 	Endpoint_ClearIN();
 }